/// <summary>
/// Sets 16-bit register value from float interpreted using fixed-point precision defined in <see cref="Qvals"/>.
/// </summary>
/// <param name="floatValue">
/// Register value represented as float.
/// </param>
public void SetValueFromFloat(float floatValue)
{
Value = (ushort)FixedFloat.FloatToFixed(floatValue, LookupQval());
}
/// <summary>
/// Converts 16-bit register value to float using fixed-point precision defined in <see cref="Qvals"/>.
/// </summary>
/// <returns>
/// Register value represented as float.
/// </returns>
public float ConvertValueToFloat()
{
return(FixedFloat.FixedToFloat((short)Value, LookupQval()));
}
/// <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.");
}
}
