Ejemplo n.º 1
0
 /// <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));
 }
Ejemplo n.º 2
0
 /// <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));
 }
Ejemplo n.º 3
0
 /// <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));
 }
Ejemplo n.º 4
0
 /// <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));
 }
Ejemplo n.º 5
0
 /// <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));
 }
Ejemplo n.º 6
0
        /// <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.");
            }
        }