示例#1
0
        void ih_PacketReceived(object sender, PacketReceivedEventArgs e)
        {
            switch (e.Packet.Type)
            {
            case PacketType.Ping:
                break;

            case PacketType.Pong:
                break;

            case PacketType.SSOutput:
                break;

            case PacketType.SSInput:
                break;

            case PacketType.Scalar16:
            {
                Scalar16Packet packet = (Scalar16Packet)e.Packet;
                SetRCGain(packet.Value);
                break;
            }

            case PacketType.Scalar32:
            {
                Scalar32Packet packet = (Scalar32Packet)e.Packet;
                switch (packet.ScalarType)
                {
                case Scalar32Type.Clock:
                    SetClock(packet.Value.UInt32Value);
                    break;

                case Scalar32Type.Heading:
                    SetHeading(packet.Value.SingleValue);
                    break;

                default:
                    break;
                }
                break;
            }

            case PacketType.Vector2_16:
                break;

            case PacketType.Vector2_32:
                break;

            case PacketType.Vector3_16:
            {
                Vector3_16Packet packet = (Vector3_16Packet)e.Packet;
                switch (packet.VectorType)
                {
                case Vector3_16Type.RotPid:
                    SetRotationRatePid(packet.Value.Int16X, packet.Value.Int16Y, packet.Value.Int16Z);
                    break;

                default:
                    break;
                }
                break;
            }

            case PacketType.Vector3_32:
            {
                Vector3_32Packet packet = (Vector3_32Packet)e.Packet;
                switch (packet.VectorType)
                {
                case Vector3_32Type.Gryo:
                    SetGyro(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
                    break;

                case Vector3_32Type.Attitude:
                    SetAttitude(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
                    break;

                case Vector3_32Type.Acceleration:
                    break;

                case Vector3_32Type.Velocity:
                    break;

                case Vector3_32Type.Position:
                    SetPosition(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
                    break;

                case Vector3_32Type.Gps:
                    break;

                case Vector3_32Type.AttitudePid:
                    SetAttitudePid(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
                    break;

                case Vector3_32Type.VelocityPid:
                    break;

                case Vector3_32Type.AltRatePid:
                    break;

                case Vector3_32Type.AltPid:
                    break;

                default:
                    break;
                }
                break;
            }

            case PacketType.Vector4_16:
            {
                Vector4Packet packet = (Vector4Packet)e.Packet;
                switch (packet.VectorType)
                {
                case Vector4Type.RCInput:
                    SetRCRaw(packet.Value.W, packet.Value.X, packet.Value.Y, packet.Value.Z);
                    break;

                case Vector4Type.ControlInput:
                    break;

                case Vector4Type.MotorOutput:
                    SetMotorOutput(packet.Value.W, packet.Value.X, packet.Value.Y, packet.Value.Z);
                    break;

                default:
                    break;
                }
                break;
            }

            default:
                break;
            }
        }
示例#2
0
        private void ParsePackets()
        {
            if (buffer == null || buffer.Length == 0)
            {
                return;
            }

            while (buffer.Length > 0)
            {
                PacketType pt  = (PacketType)(buffer[0] & 0x0F);
                int        len = buffer[0] >> 4;
                if (buffer.Length < len + 3)
                {
                    return;
                }
                if (!ValidateChecksum(buffer, len + 3))
                {
                    //throw new InvalidOperationException("Invalid cheksum");
                    byte[] newBuffer = new byte[buffer.Length - 1];
                    Buffer.BlockCopy(buffer, 1, newBuffer, 0, newBuffer.Length);
                    buffer = newBuffer;
                }
                else
                {
                    Packet p = null;
                    switch (pt)
                    {
                    case PacketType.Scalar16:
                    {
                        ushort value = BitConverter.ToUInt16(buffer, 2);
                        p = new Scalar16Packet(value);
                        break;
                    }

                    case PacketType.Scalar32:
                    {
                        Scalar32Type scalarType = (Scalar32Type)buffer[1];
                        Scalar32     value      = new Scalar32(BitConverter.ToUInt32(buffer, 2));
                        p = new Scalar32Packet(scalarType, value);
                        break;
                    }

                    case PacketType.Vector3_16:
                    {
                        Vector3_16Type type = (Vector3_16Type)buffer[1];
                        p = new Vector3_16Packet(type, new Vector3_16(buffer, 2));
                        break;
                    }

                    case PacketType.Vector3_32:
                    {
                        Vector3_32Type type = (Vector3_32Type)buffer[1];
                        p = new Vector3_32Packet(new Vector3_32(buffer, 2), type);
                        break;
                    }

                    case PacketType.Vector4_16:
                    {
                        Vector4Type type = (Vector4Type)buffer[1];
                        ushort      w    = BitConverter.ToUInt16(buffer, 2);
                        ushort      x    = BitConverter.ToUInt16(buffer, 4);
                        ushort      y    = BitConverter.ToUInt16(buffer, 6);
                        ushort      z    = BitConverter.ToUInt16(buffer, 8);
                        p = new Vector4Packet(type, new Vector4(w, x, y, z));
                        break;
                    }

                    default:
                        throw new InvalidOperationException(String.Format("Unknown type: {0}", pt));
                        break;
                    }

                    if (p != null)
                    {
                        EventHandler <PacketReceivedEventArgs> handler = PacketReceived;
                        if (handler != null)
                        {
                            handler(this, new PacketReceivedEventArgs(p));
                        }
                    }

                    byte[] newBuffer = new byte[buffer.Length - len - 3];
                    Buffer.BlockCopy(buffer, len + 3, newBuffer, 0, newBuffer.Length);
                    buffer = newBuffer;
                }
            }
        }