//scan DIO slave status
private void systemScan(FlowVar fv)
{
//while (true)
//{
if (keyOfIOScan)
{
ushort uValue = 0, uRet = 0;
uRet = CEtherCAT_DLL.CS_ECAT_Slave_DIO_Get_Input_Value(dIOPara.CardNo, dIOPara.NodeNo, g_uESCSlotID, ref uValue);
if (uRet == CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
{
for (int nBit = 0; nBit < 16; nBit++)
{
Status1[nBit] = BitConverterEx.TestB(uValue, (byte)nBit);
Status2[nBit / 8, nBit % 8] = BitConverterEx.TestB(uValue, (byte)nBit);
}
}
else
{
throw new Exception("CS_ECAT_Slave_DIO_Get_Input_Value, " + GetEtherCATErrorCode(uRet));
}
//Thread.Sleep(15);
//if (Environment.ProcessorCount == 1 || (++_loops % 100) == 0)
//{
// Thread.Sleep(1);
//}
//else
//{
// Thread.SpinWait(_iterations);
//}
}
//}
}
public static int AttachMessageBody(ref byte[] messageBuffer, byte[] messageBody, int bodyOffset, int bodyLength)
{
if (messageBuffer == null)
{
return(-1);
}
int composeLength = minimalHeadLength;
if (messageBuffer.Length < composeLength)
{
return(-2);
}
int headLength = messageBuffer[2];
if (headLength != composeLength && headLength != composeLength + dynamicEncryptBlockLength)
{
return(-3);
}
composeLength = headLength + bodyLength;
if (messageBuffer.Length < composeLength)
{
return(-4);
}
byte[] bodyLengthBytes = BitConverterEx.GetBytes((short)bodyLength, true);
messageBuffer[10] = bodyLengthBytes[0];
messageBuffer[11] = bodyLengthBytes[1];
System.Buffer.BlockCopy(messageBody, bodyOffset, messageBuffer, headLength, bodyLength);
// @TODO: 如果需要加密，要计算消息体的密文或者散列值的密文
// @TODO: 如果需要压缩，要压缩消息体
// @TODO: 如果消息体过大，需要拆分消息体
return(composeLength);
}
//scan DIO slave status
private void systemScan(FlowVar fv)
{
//while (true)
//{
if (keyOfIOScan)
{
ushort uValue = BitConverterEx.ConvertToUInt16(Status1);
if (uValue != _lastOutputValue)
{
_lastOutputValue = uValue;
ushort uRet = CEtherCAT_DLL.CS_ECAT_Slave_DIO_Set_Output_Value(dIOPara.CardNo, dIOPara.NodeNo, g_uESCSlotID, uValue);
if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
{
throw new Exception("CS_ECAT_Slave_DIO_Set_Output_Value, " + GetEtherCATErrorCode(uRet));
}
//Thread.Sleep(15);
//if (Environment.ProcessorCount == 1 || (++_loops % 100) == 0)
//{
// Thread.Sleep(1);
//}
//else
//{
// Thread.SpinWait(_iterations);
//}
}
}
//}
}
public void WriteFloat(double value)
{
this.ThrowIfDisposed();
this.PrepareValue();
// ReSharper disable once CompareOfFloatsByEqualityOperator
if (!this.forceFloat64 && value == (float)value)
{
// TODO: Increase testing coverage
this.containerStack.IncreaseCurrentContainerLength(5);
this.dataBuffer.WriteByte(TidFloatByte | 4);
this.dataBuffer.WriteUint32(BitConverterEx.SingleToInt32Bits((float)value));
}
else
{
this.containerStack.IncreaseCurrentContainerLength(9);
this.dataBuffer.WriteByte(TidFloatByte | 8);
if (double.IsNaN(value))
{
// Double.NaN is different between C# and Java
// For consistency, map NaN to the long value for NaN in Java
this.dataBuffer.WriteUint64(0x7ff8000000000000L);
}
else
{
this.dataBuffer.WriteUint64(BitConverter.DoubleToInt64Bits(value));
}
}
this.FinishValue();
}
public void StringGetBytes()
{
String data = "What is the answer to life the universe and everything?";
// This function should fail on Strings
byte[] actual = BitConverterEx.GetBytes(data);
}
//scan m1x1 slave IO status
private void systemScan()
{
while (true)
{
if (keyOfIOStatus)
{
U32 status = 0;
I16 rt = CCMNet.CS_mnet_m1_get_io_status(RingNoOfMNet, axisPara.SlaveIP, ref status);
this.status.RDY = BitConverterEx.TestB(status, 0);
this.status.ALM = BitConverterEx.TestB(status, 1);
this.status.LimitP = BitConverterEx.TestB(status, 2);
this.status.LimitN = BitConverterEx.TestB(status, 3);
this.status.ORG = BitConverterEx.TestB(status, 4);
this.status.DIR = BitConverterEx.TestB(status, 5);
this.status.EMG = BitConverterEx.TestB(status, 6);
this.status.PCS = BitConverterEx.TestB(status, 7);
this.status.ERC = BitConverterEx.TestB(status, 8);
this.status.ZPhase = BitConverterEx.TestB(status, 9);
this.status.CLR = BitConverterEx.TestB(status, 10);
this.status.Latch = BitConverterEx.TestB(status, 11);
this.status.SD = BitConverterEx.TestB(status, 12);
this.status.INP = BitConverterEx.TestB(status, 13);
this.status.SVON = BitConverterEx.TestB(status, 14);
this.status.RALM = BitConverterEx.TestB(status, 15);
if (this.status.ALM || this.status.EMG)
{
this.status.Home = false;
}
Thread.Sleep(50);
}
}
}
private void Awake()
{
if (Instance != null)
{
if (LogUtil.ShowWarning != null)
{
LogUtil.ShowWarning("已经启用过了ConnectorDaemon，不能重复启用");
}
}
else
{
_instance = this;
}
// 初始化心跳数据包
heartBeatPack[0] = 0xC8;
heartBeatPack[1] = 0xEF;
byte[] lenBytes = BitConverterEx.GetBytes((short)8, true);
heartBeatPack[2] = lenBytes[0];
heartBeatPack[3] = lenBytes[1];
heartBeatPack[4] = 1; // 版本
heartBeatPack[5] = 1; // 序列化类型，2-flatbuffers
heartBeatPack[6] = heartBeatModuleID;
heartBeatPack[7] = heartBeatFunctionID;
}
//scan DIO slave status
private void systemScan(FlowVar fv)
{
//while (true)
//{
if (keyOfIOScan)
{
I16 status;
for (byte portNo = 0; portNo < 4; portNo++)
{
status = CCMNet.CS_mnet_io_input(RingNoOfMNet, dIOPara.SlaveIP, portNo);
for (int i = 0; i < 8; i++)
{
Status1[portNo * 8 + i] = BitConverterEx.TestB(status, (byte)i);
Status2[portNo, i] = BitConverterEx.TestB(status, (byte)i);
}
}
//Thread.Sleep(15);
//if (Environment.ProcessorCount == 1 || (++_loops % 100) == 0)
//{
// Thread.Sleep(1);
//}
//else
//{
// Thread.SpinWait(_iterations);
//}
}
//}
}
public PacketBuffer Add(double[] dwarr)
{
buffer.AddRange(BitConverterEx.GetBytes((short)dwarr.Length, true));
foreach (double dw in dwarr)
{
buffer.AddRange(BitConverterEx.GetBytes(dw, true));
}
return(this);
}
public PacketBuffer Add(float[] farr)
{
buffer.AddRange(BitConverterEx.GetBytes((short)farr.Length, true));
foreach (float f in farr)
{
buffer.AddRange(BitConverterEx.GetBytes(f, true));
}
return(this);
}
public PacketBuffer Add(int[] narr)
{
buffer.AddRange(BitConverterEx.GetBytes((short)narr.Length, true));
foreach (int n in narr)
{
buffer.AddRange(BitConverterEx.GetBytes(n, true));
}
return(this);
}
public static void ParseActions(byte[] actions, int id, out SlamObservation observation)
{
int offset = 0;
var covisible = new List <SlamObservation.CovisibleInfo>();
observation = new SlamObservation(covisible)
{
Orientation = Quaternion.identity,
};
SlamPoint obsPoint = observation;
obsPoint.id = id;
obsPoint.color = Color.gray;
while (offset != actions.Length)
{
Debug.Assert(offset <= actions.Length, $"[SlamObservationPackageObject.ParseActions] offset ({offset}) out of range");
ActionType type = (ActionType)actions[offset++];
if (type == ActionType.Create)
{
obsPoint.isNew = true;
}
if (type == ActionType.Create || type == ActionType.Move)
{
obsPoint.position = BitConverterEx.ToVector3(actions, offset, ref offset);
observation.Orientation = BitConverterEx.ToQuaternion(actions, offset, ref offset);
}
if (type == ActionType.Tint)
{
obsPoint.color = BitConverterEx.ToRGBColor32(actions, offset, ref offset);
}
if (type == ActionType.Remove)
{
obsPoint.color = Color.red;
obsPoint.isRemoved = true;
}
if (type == ActionType.Connect)
{
covisible.Add(
new SlamObservation.CovisibleInfo()
{
id = BitConverterEx.ToInt32(actions, offset, ref offset),
sharedPointsCount = BitConverterEx.ToInt32(actions, offset, ref offset)
});
}
if (type == ActionType.Message)
{
int countOfMsgBytes = BitConverterEx.ToInt32(actions, offset, ref offset);
if (countOfMsgBytes >= MAX_MESSAGE_LENGTH_IN_BYTES)
{
throw new Exception();
}
obsPoint.message = countOfMsgBytes > 0 ? Encoding.ASCII.GetString(actions, offset, countOfMsgBytes) : "";
offset += sizeof(byte) * MAX_MESSAGE_LENGTH_IN_BYTES;
}
}
observation.Point = obsPoint;
}
public PacketBuffer Add(bool[] barr)
{
buffer.AddRange(BitConverterEx.GetBytes((short)barr.Length, true));
foreach (bool b in barr)
{
buffer.AddRange(BitConverterEx.GetBytes(b));
}
return(this);
}
public void StringSetBytes()
{
byte[] data = new byte[]
{
42, 0, 0, 128
};
// This function should fail on Strings
String actual = BitConverterEx.SetBytes <String>(data);
}
public PacketBuffer Send(IConnectionManager mgr)
{
buffer.InsertRange(0, BitConverterEx.GetBytes((short)(buffer.Count), true));
mgr.Send(classID, methodID, buffer.ToArray());
if (LogUtil.ShowDebug != null && LogUtil.ShowWarning != null)
{
LogUtil.ShowWarning("[网络消息中心][发送]" + ToString());
}
return(this);
}
public void Vector3GetBytes()
{
Vector3 data = new Vector3(2.0f, 1.0f, 3.0f);
byte[] expected = new byte[]
{
0, 0, 0, 64, 0, 0, 128, 63, 0, 0, 64, 64
};
byte[] actual = BitConverterEx.GetBytes(data);
Assert.AreEqual(expected, actual);
}
public void UInt32SetBytes()
{
UInt32 expected = 2147483690;
byte[] data = new byte[]
{
42, 0, 0, 128
};
UInt32 actual = BitConverterEx.SetBytes <UInt32>(data);
Assert.AreEqual(expected, actual);
}
public void UInt32GetBytes()
{
UInt32 data = 2147483690;
byte[] expected = new byte[]
{
42, 0, 0, 128
};
byte[] actual = BitConverterEx.GetBytes(data);
Assert.AreEqual(expected, actual);
}
public void Int32SetBytes()
{
Int32 expected = 42;
byte[] data = new byte[]
{
42, 0, 0, 0
};
Int32 actual = BitConverterEx.SetBytes <Int32>(data);
Assert.AreEqual(expected, actual);
}
public void Int32GetBytes()
{
Int32 data = 42;
byte[] expected = new byte[]
{
42, 0, 0, 0
};
byte[] actual = BitConverterEx.GetBytes(data);
Assert.AreEqual(expected, actual);
}
public void DoubleSetBytes()
{
Double expected = System.Math.PI;
byte[] data = new byte[]
{
24, 45, 68, 84, 251, 33, 9, 64
};
Double actual = BitConverterEx.SetBytes <Double>(data);
Assert.AreEqual(expected, actual);
}
public void ColorExGetBytes()
{
ColorEx data = ColorEx.BurlyWood;
byte[] expected = new byte[]
{
0, 0, 128, 63, 222, 222, 94, 63, 184, 184, 56, 63, 136, 135, 7, 63
};
byte[] actual = BitConverterEx.GetBytes(data);
Assert.AreEqual(expected, actual);
}
public void Vector4SetBytes()
{
Vector4 expected = new Vector4(2.0f, 1.0f, 3.0f, 4.0f);
byte[] data = new byte[]
{
0, 0, 0, 64, 0, 0, 128, 63, 0, 0, 64, 64, 0, 0, 128, 64
};
Vector4 actual = BitConverterEx.SetBytes <Vector4>(data);
Assert.AreEqual(expected, actual);
}
public void ColorExSetBytes()
{
ColorEx expected = new ColorEx(ColorEx.BurlyWood);
byte[] data = new byte[]
{
0, 0, 128, 63, 222, 222, 94, 63, 184, 184, 56, 63, 136, 135, 7, 63
};
ColorEx actual = BitConverterEx.SetBytes <ColorEx>(data);
Assert.AreEqual(expected.ToString(), actual.ToString());
}
public void SingleGetBytes()
{
Single data = (Single)System.Math.PI;
byte[] expected = new byte[]
{
219, 15, 73, 64
};
byte[] actual = BitConverterEx.GetBytes(data);
Assert.AreEqual(expected, actual);
}
public void SingleSetBytes()
{
Single expected = (Single)System.Math.PI;
byte[] data = new byte[]
{
219, 15, 73, 64
};
Single actual = BitConverterEx.SetBytes <Single>(data);
Assert.AreEqual(expected, actual);
}
private byte getCheckDigit(List <byte> cmdCombined)
{
bool[] bits = BitConverterEx.GetBits(cmdCombined[0]);
for (int j = 1; j < cmdCombined.Count; j++)
{
for (byte i = 0; i < 8; i++)
{
bits[i] ^= BitConverterEx.TestB(cmdCombined[j], i);
}
}
return(BitConverterEx.ConvertToByte(bits));
}
public void DoubleGetBytes()
{
Double data = System.Math.PI;
byte[] expected = new byte[]
{
24, 45, 68, 84, 251, 33, 9, 64
};
byte[] actual = BitConverterEx.GetBytes(data);
Assert.AreEqual(expected, actual);
}
//TODO: add handling of LucenFieldAttribute
//TODO: make sure LuceneFieldAttribute has all necessary stuff for FieldType
public static Field ToField <TObject>(this TObject @object, string name)
{
switch (Type.GetTypeCode(@object.GetType()))
{
case TypeCode.Boolean:
return(new StringField(name, Convert.ToBoolean(@object) ? "true" : "false", Constants.DefaultStore));
case TypeCode.Char:
return(new StringField(name, Convert.ToChar(@object).ToString(), Constants.DefaultStore));
case TypeCode.DateTime:
return(new StringField(name, DateTools.DateToString(Convert.ToDateTime(@object), DateTools.Resolution.MILLISECOND), Constants.DefaultStore));
case TypeCode.DBNull:
return(new StringField(name, "dbnull", Constants.DefaultStore));
case TypeCode.Single:
case TypeCode.Double:
return(new DoubleField(name, Convert.ToDouble(@object), Constants.DefaultStore));
case TypeCode.Byte:
case TypeCode.Int16:
case TypeCode.Int32:
case TypeCode.SByte:
return(new Int32Field(name, Convert.ToInt32(@object), Constants.DefaultStore));
case TypeCode.UInt16:
case TypeCode.UInt32:
case TypeCode.Int64:
return(new Int64Field(name, Convert.ToInt64(@object), Constants.DefaultStore));
case TypeCode.UInt64:
return(new Field(name,
BitConverter.GetBytes(Convert.ToUInt64(@object)),
new FieldType {
IsIndexed = true, NumericType = NumericType.INT64, IsStored = Constants.DefaultStore == Field.Store.YES
}));
case TypeCode.Decimal:
return(new Field(name,
BitConverterEx.GetBytes(Convert.ToDecimal(@object)),
new FieldType {
IsIndexed = true, NumericType = NumericType.DOUBLE, IsStored = Constants.DefaultStore == Field.Store.YES
}));
case TypeCode.String:
return(new StringField(name, Convert.ToString(@object), Constants.DefaultStore));
default:
return(null);
}
}
private static BinaryTextData RandomInt(BinaryTextData value_min, BinaryTextData value_max)
{
var value_min_i_temp = (value_min != null) ? (value_min.ToUint64()) : (0);
var value_max_i_temp = (value_max != null) ? (value_max.ToUint64()) : (0xFF);
var value_min_i = (int)Math.Min(value_min_i_temp, value_max_i_temp);
var value_max_i = (int)Math.Min(Math.Max(value_min_i_temp, value_max_i_temp), 0xFFFFFFF);
var random = new Random();
return(new BinaryTextData(
BitConverterEx.GetBytesBigEndian(
(ulong)random.Next(value_min_i, value_max_i))));
}
