예제 #1
0
        /// <summary>
        /// 组装数据包
        /// </summary>
        /// <param name="packetData"></param>
        /// <returns></returns>
        private byte[] SendPacketLinkLayer(byte[] packetData)
        {
            /*
             * Byte order:
             *
             * 0   Packet start sign
             * 1	 Payload length	 0 - 255
             * 2	 Packet sequence	 0 - 255
             * 3	 System ID	 1 - 255
             * 4	 Component ID	 0 - 255
             * 5	 Message ID	 0 - 255
             * 6 to (n+6)	 Data	 (0 - 255) bytes
             * (n+7) to (n+8)	 Checksum (high byte, low byte)
             *
             */
            var outBytes = new byte[packetData.Length + 5];

            outBytes[0] = MavlinkSettings.ProtocolMarker;
            outBytes[1] = (byte)(packetData.Length - 3);  // 3 bytes for sequence, id, msg type which this
                                                          // layer does not concern itself with
            outBytes[2] = unchecked (txPacketSequence++);

            int i;

            for (i = 0; i < packetData.Length; i++)
            {
                outBytes[i + 3] = packetData[i];
            }

            // Check the CRC. Does not include the starting byte but does include the length
            var crc1 = Mavlink_Crc.Calculate(outBytes, 1, (UInt16)(packetData.Length + 2));

            if (MavlinkSettings.CrcExtra)
            {
                var possibleMsgId = outBytes[5];
                var extra         = MavLinkSerializer.Lookup[possibleMsgId];
                crc1 = Mavlink_Crc.CrcAccumulate(extra.CrcExtra, crc1);
            }

            byte crc_high = (byte)(crc1 & 0xFF);
            byte crc_low  = (byte)(crc1 >> 8);

            outBytes[i + 3] = crc_high;
            outBytes[i + 4] = crc_low;

            return(outBytes);
        }
예제 #2
0
        /// <summary>
        /// 解析数据包并触发事件
        /// </summary>
        public void ParseBytes(byte[] newlyReceived)
        {
            uint i = 0;

            // copy the old and new into a contiguous array
            // This is pretty inefficient...
            var bytesToProcess = new byte[newlyReceived.Length + leftovers.Length];
            int j = 0;

            for (i = 0; i < leftovers.Length; i++)
            {
                bytesToProcess[j++] = leftovers[i];
            }

            for (i = 0; i < newlyReceived.Length; i++)
            {
                bytesToProcess[j++] = newlyReceived[i];
            }

            i = 0;

            // we are going to loop and decode packets until we use up the data
            // at which point we will return. Hence one call to this method could
            // result in multiple packet decode events
            while (true)
            {
                // Hunt for the start char
                int huntStartPos = (int)i;

                while (i < bytesToProcess.Length && bytesToProcess[i] != MavlinkSettings.ProtocolMarker)
                {
                    i++;
                }

                if (i == bytesToProcess.Length)
                {
                    // No start byte found in all our bytes. Dump them, Exit.
                    leftovers = new byte[] { };
                    return;
                }

                if (i > huntStartPos)
                {
                    // if we get here then are some bytes which this code thinks are
                    // not interesting and would be dumped. For diagnostics purposes,
                    // lets pop these bytes up in an event.
                    if (BytesUnused != null)
                    {
                        var badBytes = new byte[i - huntStartPos];
                        Array.Copy(bytesToProcess, huntStartPos, badBytes, 0, (int)(i - huntStartPos));
                        BytesUnused(this, new PacketCRCFailEventArgs(badBytes, bytesToProcess.Length - huntStartPos));
                    }
                }

                // We need at least the minimum length of a packet to process it.
                // The minimum packet length is 8 bytes for acknowledgement packets without payload
                // if we don't have the minimum now, go round again
                if (bytesToProcess.Length - i < 8)
                {
                    leftovers = new byte[bytesToProcess.Length - i];
                    j         = 0;
                    while (i < bytesToProcess.Length)
                    {
                        leftovers[j++] = bytesToProcess[i++];
                    }
                    return;
                }

                /*
                 * Byte order:
                 *
                 * 0  Packet start sign
                 * 1	 Payload length	 0 - 255
                 * 2	 Packet sequence	 0 - 255
                 * 3	 System ID	 1 - 255
                 * 4	 Component ID	 0 - 255
                 * 5	 Message ID	 0 - 255
                 * 6 to (n+6)	 Data	 (0 - 255) bytes
                 * (n+7) to (n+8)	 Checksum (high byte, low byte) for v0.9, lowbyte, highbyte for 1.0
                 *
                 */
                UInt16 payLoadLength = bytesToProcess[i + 1];

                // Now we know the packet length,
                // If we don't have enough bytes in this packet to satisfy that packet lenghth,
                // then dump the whole lot in the leftovers and do nothing else - go round again
                if (payLoadLength > (bytesToProcess.Length - i - 8)) // payload + 'overhead' bytes (crc, system etc)
                {
                    // back up to the start char for next cycle
                    j = 0;

                    leftovers = new byte[bytesToProcess.Length - i];

                    for (; i < bytesToProcess.Length; i++)
                    {
                        leftovers[j++] = bytesToProcess[i];
                    }
                    return;
                }

                i++;

                // Check the CRC. Does not include the starting 'U' byte but does include the length
                var crc1 = Mavlink_Crc.Calculate(bytesToProcess, (UInt16)(i), (UInt16)(payLoadLength + 5));

                if (MavlinkSettings.CrcExtra)
                {
                    var possibleMsgId = bytesToProcess[i + 4];

                    if (!MavLinkSerializer.Lookup.ContainsKey(possibleMsgId))
                    {
                        // we have received an unknown message. In this case we don't know the special
                        // CRC extra, so we have no choice but to fail.

                        // The way we do this is to just let the procedure continue
                        // There will be a natural failure of the main packet CRC
                    }
                    else
                    {
                        var extra = MavLinkSerializer.Lookup[possibleMsgId];
                        crc1 = Mavlink_Crc.CrcAccumulate(extra.CrcExtra, crc1);
                    }
                }

                byte crcHigh = (byte)(crc1 & 0xFF);
                byte crcLow  = (byte)(crc1 >> 8);

                byte messageCrcHigh = bytesToProcess[i + 5 + payLoadLength];
                byte messageCrcLow  = bytesToProcess[i + 6 + payLoadLength];

                if (messageCrcHigh == crcHigh && messageCrcLow == crcLow)
                {
                    // This is used for data drop outs metrics, not packet windows
                    // so we should consider this here.
                    // We pass up to subscribers only as an advisory thing
                    var rxPacketSequence = bytesToProcess[++i];
                    i++;
                    var packet = new byte[payLoadLength + 3];  // +3 because we are going to send up the sys and comp id and msg type with the data

                    for (j = 0; j < packet.Length; j++)
                    {
                        packet[j] = bytesToProcess[i + j];
                    }

                    var debugArray = new byte[payLoadLength + 7];
                    Array.Copy(bytesToProcess, (int)(i - 3), debugArray, 0, debugArray.Length);

                    //OnPacketDecoded(packet, rxPacketSequence, debugArray);
                    //调用接收消息事件
                    ProcessPacketBytes(packet, rxPacketSequence);

                    PacketsReceived++;

                    // clear leftovers, just incase this is the last packet
                    leftovers = new byte[] { };

                    //  advance i here by j to avoid unecessary hunting
                    // todo: could advance by j + 2 I think?
                    i += (uint)(j + 2);
                }
                else
                {
                    var badBytes = new byte[i + 7 + payLoadLength];
                    Array.Copy(bytesToProcess, (int)(i - 1), badBytes, 0, payLoadLength + 7);

                    PacketFailedCRC?.Invoke(this, new PacketCRCFailEventArgs(badBytes, (int)(bytesToProcess.Length - i - 1)));

                    BadCrcPacketsReceived++;
                }
            }
        }