/// <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."); } }