public void SendMessage(MavLinkMessage msg)
 {
     msg.SequenceNumber = seqno++;
     byte[] buffer = msg.Pack();
     if (this.port == null)
     {
         throw new Exception("Please call Start to provide the mavlink Port we are using");
     }
     this.port.Write(buffer, buffer.Length);
 }
        public MavlinkChannel()
        {
            // plug in our custom mavlink_simulator_telemetry message
            int  id   = MAVLink.mavlink_telemetry.MessageId;
            Type info = MavLinkMessage.GetMavlinkType((uint)id);

            if (info != null && typeof(MAVLink.mavlink_telemetry) != info)
            {
                throw new Exception("The custom messageid " + id + " is already defined, so we can't use it for mavlink_simulator_telemetry");
            }
            else
            {
                MAVLink.MAVLINK_MESSAGE_INFO[id] = typeof(MAVLink.mavlink_telemetry);
            }
        }
        void ReceiveThread()
        {
            byte[] buffer = new byte[1];

            MavLinkMessage msg        = null;
            ReadState      state      = ReadState.Init;
            int            payloadPos = 0;
            ushort         crc        = 0;

            System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
            watch.Start();

            try
            {
                while (this.port != null)
                {
                    int len = this.port.Read(buffer, 1);
                    if (len == 1)
                    {
                        byte b = buffer[0];
                        switch (state)
                        {
                        case ReadState.Init:
                            if (b == MagicMarker)
                            {
                                state      = ReadState.GotMagic;
                                msg        = new MavLinkMessage();
                                msg.Time   = (ulong)watch.ElapsedMilliseconds * 1000;   // must be in microseconds
                                msg.Magic  = MagicMarker;
                                payloadPos = 0;
                                crc        = 0;
                            }
                            break;

                        case ReadState.GotMagic:
                            msg.Length  = b;
                            msg.Payload = new byte[msg.Length];
                            state       = ReadState.GotLength;
                            break;

                        case ReadState.GotLength:
                            msg.SequenceNumber = b;
                            state = ReadState.GotSequenceNumber;
                            break;

                        case ReadState.GotSequenceNumber:
                            msg.SystemId = b;
                            state        = ReadState.GotSystemId;
                            break;

                        case ReadState.GotSystemId:
                            msg.ComponentId = b;
                            state           = ReadState.GotComponentId;
                            break;

                        case ReadState.GotComponentId:
                            msg.MsgId = (MAVLink.MAVLINK_MSG_ID)b;
                            if (msg.Length == 0)
                            {
                                // done!
                                state = ReadState.GotPayload;
                            }
                            else
                            {
                                state = ReadState.GotMessageId;
                            }
                            break;

                        case ReadState.GotMessageId:
                            // read in the payload.
                            msg.Payload[payloadPos++] = b;
                            if (payloadPos == msg.Length)
                            {
                                state = ReadState.GotPayload;
                            }
                            break;

                        case ReadState.GotPayload:
                            crc   = b;
                            state = ReadState.GotCrc1;
                            break;

                        case ReadState.GotCrc1:
                            crc = (ushort)((b << 8) + crc);
                            // ok, let's see if it's good.
                            msg.Crc = crc;
                            ushort found = msg.crc_calculate();
                            if (found != crc)
                            {
                                // bad crc!!
                            }
                            else
                            {
                                // try and deserialize the payload.
                                msg.Deserialize();
                                if (MessageReceived != null)
                                {
                                    MessageReceived(this, msg);
                                }
                            }
                            state = ReadState.Init;
                            break;
                        }
                    }
                }
            }
            catch (Exception)
            {
                // port was closed
            }
        }
示例#4
0
        void ReceiveThread()
        {
            byte[] buffer = new byte[1];

            const byte     MAVLINK_IFLAG_SIGNED        = 0x01;
            const int      MAVLINK_SIGNATURE_BLOCK_LEN = 13;
            MavLinkMessage msg              = null;
            ReadState      state            = ReadState.Init;
            int            payloadPos       = 0;
            ushort         crc              = 0;
            int            signaturePos     = 0;
            uint           msgid            = 0;
            int            msgIdPos         = 0;
            int            MaxPayloadLength = 255;
            bool           messageComplete  = false;

            System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
            watch.Start();

            try
            {
                while (this.port != null)
                {
                    int len = this.port.Read(buffer, 1);
                    if (len == 1)
                    {
                        byte b = buffer[0];
                        switch (state)
                        {
                        case ReadState.Init:
                            if (b == MavLinkMessage.Mavlink1Stx)
                            {
                                state           = ReadState.GotMagic;
                                msg             = new MavLinkMessage();
                                msg.Time        = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds
                                msg.Magic       = MavLinkMessage.Mavlink1Stx;
                                payloadPos      = 0;
                                msgIdPos        = 0;
                                msgid           = 0;
                                crc             = 0;
                                messageComplete = false;
                            }
                            else if (b == MavLinkMessage.Mavlink2Stx)
                            {
                                state           = ReadState.GotMagic;
                                msg             = new MavLinkMessage();
                                msg.Time        = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds
                                msg.Magic       = MavLinkMessage.Mavlink2Stx;
                                payloadPos      = 0;
                                msgIdPos        = 0;
                                msgid           = 0;
                                crc             = 0;
                                messageComplete = false;
                            }
                            break;

                        case ReadState.GotMagic:
                            if (b > MaxPayloadLength)
                            {
                                state = ReadState.Init;
                            }
                            else
                            {
                                if (msg.Magic == MavLinkMessage.Mavlink1Stx)
                                {
                                    msg.IncompatFlags = 0;
                                    msg.CompatFlags   = 0;
                                    state             = ReadState.GotCompatFlags;
                                }
                                else
                                {
                                    msg.Length  = b;
                                    msg.Payload = new byte[msg.Length];
                                    state       = ReadState.GotLength;
                                }
                            }
                            break;

                        case ReadState.GotLength:
                            msg.IncompatFlags = b;
                            state             = ReadState.GotIncompatFlags;
                            break;

                        case ReadState.GotIncompatFlags:
                            msg.CompatFlags = b;
                            state           = ReadState.GotCompatFlags;
                            break;

                        case ReadState.GotCompatFlags:
                            msg.SequenceNumber = b;
                            state = ReadState.GotSequenceNumber;
                            break;

                        case ReadState.GotSequenceNumber:
                            msg.SystemId = b;
                            state        = ReadState.GotSystemId;
                            break;

                        case ReadState.GotSystemId:
                            msg.ComponentId = b;
                            state           = ReadState.GotComponentId;
                            break;

                        case ReadState.GotComponentId:
                            if (msg.Magic == MavLinkMessage.Mavlink1Stx)
                            {
                                msg.MsgId = (MAVLink.MAVLINK_MSG_ID)b;
                                if (msg.Length == 0)
                                {
                                    // done!
                                    state = ReadState.GotPayload;
                                }
                                else
                                {
                                    state = ReadState.GotMessageId;
                                }
                            }
                            else
                            {
                                // msgid is 24 bits
                                switch (msgIdPos)
                                {
                                case 0:
                                    msgid = b;
                                    break;

                                case 1:
                                    msgid |= ((uint)b << 8);
                                    break;

                                case 2:
                                    msgid    |= ((uint)b << 16);
                                    msg.MsgId = (MAVLink.MAVLINK_MSG_ID)msgid;
                                    state     = ReadState.GotMessageId;
                                    break;
                                }
                                msgIdPos++;
                            }
                            break;

                        case ReadState.GotMessageId:
                            // read in the payload.
                            msg.Payload[payloadPos++] = b;
                            if (payloadPos == msg.Length)
                            {
                                state = ReadState.GotPayload;
                            }
                            break;

                        case ReadState.GotPayload:
                            crc   = b;
                            state = ReadState.GotCrc1;
                            break;

                        case ReadState.GotCrc1:
                            crc = (ushort)((b << 8) + crc);
                            // ok, let's see if it's good.
                            msg.Crc = crc;
                            ushort found = msg.crc_calculate();
                            if (found != crc)
                            {
                                // bad crc!!
                            }
                            else
                            {
                                if ((msg.IncompatFlags & MAVLINK_IFLAG_SIGNED) == MAVLINK_IFLAG_SIGNED)
                                {
                                    signaturePos  = 0;
                                    msg.Signature = new byte[MAVLINK_SIGNATURE_BLOCK_LEN];
                                    state         = ReadState.GetSignature;
                                }
                                else
                                {
                                    messageComplete = true;
                                }
                            }
                            break;

                        case ReadState.GetSignature:
                            msg.Signature[signaturePos++] = b;
                            if (signaturePos == MAVLINK_SIGNATURE_BLOCK_LEN)
                            {
                                // todo: check the signature.
                                messageComplete = true;
                            }
                            break;
                        }

                        if (messageComplete)
                        {
                            // try and deserialize the payload.
                            msg.Deserialize();
                            if (MessageReceived != null)
                            {
                                MessageReceived(this, msg);
                            }
                            // reset for next message.
                            state = ReadState.Init;
                        }
                    }
                }
            }
            catch (Exception)
            {
                // port was closed
            }
        }