/// <summary>
/// Constructs a read register packet.
/// </summary>
/// <param name="registerData">
/// <see cref="RegisterData"/> instance containing register address to be read.
/// </param>
/// <returns>
/// Constructed and encoded read register packet
/// </returns>
public static byte[] ConstructReadRegisterPacket(RegisterData registerData)
{
byte[] decodedPacket = new byte[] { (byte)PacketHeaders.ReadRegister,
(byte)((ushort)registerData.Address >> 8),
(byte)registerData.Address,
0 };
decodedPacket = InsertChecksum(decodedPacket);
return(PacketEncoding.EncodePacket(decodedPacket));
}
/// <summary>
/// Constructs an encoded command packet.
/// </summary>
/// <param name="commandCode">
/// Command code. See <see cref="CommandCodes"/>.
/// </param>
/// <returns>
/// Constructed and encoded command packet.
/// </returns>
public static byte[] ConstructCommandPacket(CommandCodes commandCode)
{
byte[] decodedPacket = new byte[] { (byte)PacketHeaders.Command,
(byte)((ushort)commandCode >> 8),
(byte)commandCode,
0 };
decodedPacket = InsertChecksum(decodedPacket);
return(PacketEncoding.EncodePacket(decodedPacket));
}
/// <summary>
/// Constructs an encoded digital I/O packet.
/// </summary>
/// <param name="digitalIOdata">
/// DigitalIOdata object containing output states to be set.
/// </param>
/// <returns>
/// Constructed and encoded digital I/O packet.
/// </returns>
public static byte[] ConstructDigitalIOpacket(DigitalIOdata digitalIOdata)
{
byte[] decodedPacket = new byte[] { (byte)PacketHeaders.DigitalIOdata,
0x00,
digitalIOdata.State.ConvertToByte(),
0 };
decodedPacket = InsertChecksum(decodedPacket);
return(PacketEncoding.EncodePacket(decodedPacket));
}
/// <summary>
/// Constructs a write date/time packet.
/// </summary>
/// <param name="dateTimeData">
/// <see cref="DateTimeData"/> instance containing data to be written.
/// </param>
/// <returns>
/// Constructed and encoded write date/time packet
/// </returns>
public static byte[] ConstructWriteDateTimePacket(DateTimeData dateTimeData)
{
byte[] decodedPacket = new byte[] { (byte)PacketHeaders.WriteDateTime,
(byte)(((((dateTimeData.Year - 2000) % 100) / 10) << 4) | ((dateTimeData.Year - 2000) % 10)),
(byte)(((((dateTimeData.Month) % 100) / 10) << 4) | (dateTimeData.Month % 10)),
(byte)(((((dateTimeData.Day) % 100) / 10) << 4) | (dateTimeData.Day % 10)),
(byte)(((((dateTimeData.Hours) % 100) / 10) << 4) | (dateTimeData.Hours % 10)),
(byte)(((((dateTimeData.Minutes) % 100) / 10) << 4) | (dateTimeData.Minutes % 10)),
(byte)(((((dateTimeData.Seconds) % 100) / 10) << 4) | (dateTimeData.Seconds % 10)),
0 };
decodedPacket = InsertChecksum(decodedPacket);
return(PacketEncoding.EncodePacket(decodedPacket));
}
/// <summary>
/// Constructs an encoded PWM output data packet.
/// </summary>
/// <param name="PWMoutputData">
/// PWM output data.
/// </param>
/// <returns>
/// Constructed and encoded PWM output data packet.
/// </returns>
public static byte[] ConstructPWMoutputPacket(PWMoutputData PWMoutputData)
{
byte[] decodedPacket = new byte[] { (byte)PacketHeaders.PWMoutputData,
(byte)(PWMoutputData.AX0 >> 8),
(byte)PWMoutputData.AX0,
(byte)(PWMoutputData.AX2 >> 8),
(byte)PWMoutputData.AX2,
(byte)(PWMoutputData.AX4 >> 8),
(byte)PWMoutputData.AX4,
(byte)(PWMoutputData.AX6 >> 8),
(byte)PWMoutputData.AX6,
0 };
decodedPacket = InsertChecksum(decodedPacket);
return(PacketEncoding.EncodePacket(decodedPacket));
}
/// <summary>
/// Deconstructs packet from and encoded byte array and return data object.
/// </summary>
/// <param name="encodedPacket">
/// Byte array containing the encoded packet.
/// </param>
/// <returns>
/// <see cref="xIMUdata"/> object deconstructed from packet.
/// </returns>
/// <exception cref="System.Exception">
/// Thrown when deconstruction of an invalid packet is attempted.
/// </exception>
public static xIMUdata DeconstructPacket(byte[] encodedPacket)//解码函数
{
// Decode packet
if (encodedPacket.Length < 4)
{
throw new Exception("Too few bytes in packet.");
}
if (encodedPacket.Length > 30)
{
throw new Exception("Too many bytes in packet.");
}
byte[] decodedPacket = PacketEncoding.DecodePacket(encodedPacket);//1. encodedPacket --> decodedPacket
// Confirm checksum
byte checksum = 0;
for (int i = 0; i < (decodedPacket.Length - 1); i++)
{
checksum += decodedPacket[i];
}
if (checksum != decodedPacket[decodedPacket.Length - 1])
{
throw new Exception("Invalid checksum.");
}
// Interpret packet according to header
switch (decodedPacket[0])
{
case ((byte)PacketHeaders.Error):
if (decodedPacket.Length != 4)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new ErrorData((ushort)Concat(decodedPacket[1], decodedPacket[2])));
case ((byte)PacketHeaders.Command):
if (decodedPacket.Length != 4)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new CommandData((ushort)Concat(decodedPacket[1], decodedPacket[2])));
case ((byte)PacketHeaders.WriteRegister):
if (decodedPacket.Length != 6)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new RegisterData((ushort)Concat(decodedPacket[1], decodedPacket[2]), (ushort)Concat(decodedPacket[3], decodedPacket[4])));
case ((byte)PacketHeaders.WriteDateTime):
if (decodedPacket.Length != 8)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new DateTimeData((int)(10 * ((decodedPacket[1] & 0xF0) >> 4) + (decodedPacket[1] & 0x0F)) + 2000,
(int)(10 * ((decodedPacket[2] & 0xF0) >> 4) + (decodedPacket[2] & 0x0F)),
(int)(10 * ((decodedPacket[3] & 0xF0) >> 4) + (decodedPacket[3] & 0x0F)),
(int)(10 * ((decodedPacket[4] & 0xF0) >> 4) + (decodedPacket[4] & 0x0F)),
(int)(10 * ((decodedPacket[5] & 0xF0) >> 4) + (decodedPacket[5] & 0x0F)),
(int)(10 * ((decodedPacket[6] & 0xF0) >> 4) + (decodedPacket[6] & 0x0F))));
case ((byte)PacketHeaders.RawBatteryAndThermometerData):
if (decodedPacket.Length != 6)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new RawBatteryAndThermometerData(Concat(decodedPacket[1], decodedPacket[2]), Concat(decodedPacket[3], decodedPacket[4])));
case ((byte)PacketHeaders.CalBatteryAndThermometerData):
if (decodedPacket.Length != 6)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new CalBatteryAndThermometerData(FixedFloat.FixedToFloat(Concat(decodedPacket[1], decodedPacket[2]), Qvals.CalibratedBattery),
FixedFloat.FixedToFloat(Concat(decodedPacket[3], decodedPacket[4]), Qvals.CalibratedThermometer)));
case ((byte)PacketHeaders.RawInertialAndMagneticData):
if (decodedPacket.Length != 20)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new RawInertialAndMagneticData(new short[] { Concat(decodedPacket[1], decodedPacket[2]), Concat(decodedPacket[3], decodedPacket[4]), Concat(decodedPacket[5], decodedPacket[6]) },
new short[] { Concat(decodedPacket[7], decodedPacket[8]), Concat(decodedPacket[9], decodedPacket[10]), Concat(decodedPacket[11], decodedPacket[12]) },
new short[] { Concat(decodedPacket[13], decodedPacket[14]), Concat(decodedPacket[15], decodedPacket[16]), Concat(decodedPacket[17], decodedPacket[18]) }));
case ((byte)PacketHeaders.CalInertialAndMagneticData): //读取、分析数据
if (decodedPacket.Length != 20)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new CalInertialAndMagneticData(new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[1], decodedPacket[2]), Qvals.CalibratedGyroscope),
FixedFloat.FixedToFloat(Concat(decodedPacket[3], decodedPacket[4]), Qvals.CalibratedGyroscope),
FixedFloat.FixedToFloat(Concat(decodedPacket[5], decodedPacket[6]), Qvals.CalibratedGyroscope) },
new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[7], decodedPacket[8]), Qvals.CalibratedAccelerometer),
FixedFloat.FixedToFloat(Concat(decodedPacket[9], decodedPacket[10]), Qvals.CalibratedAccelerometer),
FixedFloat.FixedToFloat(Concat(decodedPacket[11], decodedPacket[12]), Qvals.CalibratedAccelerometer) },
new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[13], decodedPacket[14]), Qvals.CalibratedMagnetometer),
FixedFloat.FixedToFloat(Concat(decodedPacket[15], decodedPacket[16]), Qvals.CalibratedMagnetometer),
FixedFloat.FixedToFloat(Concat(decodedPacket[17], decodedPacket[18]), Qvals.CalibratedMagnetometer) }));
case ((byte)PacketHeaders.QuaternionData):
if (decodedPacket.Length != 10)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new QuaternionData(new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[1], decodedPacket[2]), Qvals.Quaternion), FixedFloat.FixedToFloat(Concat(decodedPacket[3], decodedPacket[4]), Qvals.Quaternion),
FixedFloat.FixedToFloat(Concat(decodedPacket[5], decodedPacket[6]), Qvals.Quaternion), FixedFloat.FixedToFloat(Concat(decodedPacket[7], decodedPacket[8]), Qvals.Quaternion) }));
case ((byte)PacketHeaders.DigitalIOdata):
if (decodedPacket.Length != 4)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new DigitalIOdata(decodedPacket[1], decodedPacket[2]));
case ((byte)PacketHeaders.RawAnalogueInputData):
if (decodedPacket.Length != 18)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new RawAnalogueInputData(Concat(decodedPacket[1], decodedPacket[2]), Concat(decodedPacket[3], decodedPacket[4]), Concat(decodedPacket[5], decodedPacket[6]), Concat(decodedPacket[7], decodedPacket[8]),
Concat(decodedPacket[9], decodedPacket[10]), Concat(decodedPacket[11], decodedPacket[12]), Concat(decodedPacket[13], decodedPacket[14]), Concat(decodedPacket[15], decodedPacket[16])));
case ((byte)PacketHeaders.CalAnalogueInputData):
if (decodedPacket.Length != 18)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new CalAnalogueInputData(FixedFloat.FixedToFloat(Concat(decodedPacket[1], decodedPacket[2]), Qvals.CalibratedAnalogueInput), FixedFloat.FixedToFloat(Concat(decodedPacket[3], decodedPacket[4]), Qvals.CalibratedAnalogueInput), FixedFloat.FixedToFloat(Concat(decodedPacket[5], decodedPacket[6]), Qvals.CalibratedAnalogueInput), FixedFloat.FixedToFloat(Concat(decodedPacket[7], decodedPacket[8]), Qvals.CalibratedAnalogueInput),
FixedFloat.FixedToFloat(Concat(decodedPacket[9], decodedPacket[10]), Qvals.CalibratedAnalogueInput), FixedFloat.FixedToFloat(Concat(decodedPacket[11], decodedPacket[12]), Qvals.CalibratedAnalogueInput), FixedFloat.FixedToFloat(Concat(decodedPacket[13], decodedPacket[14]), Qvals.CalibratedAnalogueInput), FixedFloat.FixedToFloat(Concat(decodedPacket[15], decodedPacket[16]), Qvals.CalibratedAnalogueInput)));
case ((byte)PacketHeaders.PWMoutputData):
if (decodedPacket.Length != 10)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new PWMoutputData((ushort)Concat(decodedPacket[1], decodedPacket[2]), (ushort)Concat(decodedPacket[3], decodedPacket[4]), (ushort)Concat(decodedPacket[5], decodedPacket[6]), (ushort)Concat(decodedPacket[7], decodedPacket[8])));
case ((byte)PacketHeaders.RawADXL345busData):
if (decodedPacket.Length != 26)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new RawADXL345busData(new short[] { Concat(decodedPacket[1], decodedPacket[2]), Concat(decodedPacket[3], decodedPacket[4]), Concat(decodedPacket[5], decodedPacket[6]) },
new short[] { Concat(decodedPacket[7], decodedPacket[8]), Concat(decodedPacket[9], decodedPacket[10]), Concat(decodedPacket[11], decodedPacket[12]) },
new short[] { Concat(decodedPacket[13], decodedPacket[14]), Concat(decodedPacket[15], decodedPacket[16]), Concat(decodedPacket[17], decodedPacket[18]) },
new short[] { Concat(decodedPacket[19], decodedPacket[20]), Concat(decodedPacket[21], decodedPacket[22]), Concat(decodedPacket[23], decodedPacket[24]) }));
case ((byte)PacketHeaders.CalADXL345busData):
if (decodedPacket.Length != 26)
{
throw new Exception("Invalid number of bytes for packet header.");
}
return(new CalADXL345busData(new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[1], decodedPacket[2]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[3], decodedPacket[4]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[5], decodedPacket[6]), Qvals.CalibratedADXL345) },
new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[7], decodedPacket[8]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[9], decodedPacket[10]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[11], decodedPacket[12]), Qvals.CalibratedADXL345) },
new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[13], decodedPacket[14]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[15], decodedPacket[16]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[17], decodedPacket[18]), Qvals.CalibratedADXL345) },
new float[] { FixedFloat.FixedToFloat(Concat(decodedPacket[19], decodedPacket[20]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[21], decodedPacket[22]), Qvals.CalibratedADXL345), FixedFloat.FixedToFloat(Concat(decodedPacket[23], decodedPacket[24]), Qvals.CalibratedADXL345) }));
default:
throw new Exception("Unknown packet header.");
}
}
