Пример #1
0
        public byte[] ReadPacket(Stream BaseStream)
        {
            byte[] buffer = new byte[270];

            int readcount = 0;

            while (readcount < 200)
            {
                // read STX byte
                ReadWithTimeout(BaseStream, buffer, 0, 1);

                if (buffer[0] == MAVLink.MAVLINK_STX)
                {
                    break;
                }

                readcount++;
            }

            // read header
            ReadWithTimeout(BaseStream, buffer, 1, 5);

            // packet length
            int lengthtoread = buffer[1] + 6 + 2 - 2; // data + header + checksum - STX - length

            //read rest of packet
            ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - 4);

            // check message length vs table
            if (buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]])
            {
                badLength++;
                // bad or unknown packet
                return(null);
            }

            // resize the packet to the correct length
            Array.Resize <byte>(ref buffer, lengthtoread + 2);

            // calc crc
            ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

            // calc extra bit of crc for mavlink 1.0
            if (buffer.Length > 5 && buffer[0] == 254)
            {
                crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);
            }

            // check crc
            if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) ||
                buffer[buffer.Length - 2] != (crc & 0xff))
            {
                badCRC++;
                // crc fail
                return(null);
            }

            return(buffer);
        }
Пример #2
0
        void generatePacket(byte messageType, object indata)
        {
            if (!BaseStream.IsOpen)
            {
                return;
            }

            lock (writelock)
            {
                byte[] data;

                data = MavlinkUtil.StructureToByteArray(indata);

                byte[] packet = new byte[data.Length + 6 + 2];

                packet[0] = 254;
                packet[1] = (byte)data.Length;
                packet[2] = (byte)packetcount;

                packetcount++;

                packet[3] = 255; // this is always 255 - MYGCS
                packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
                packet[5] = messageType;

                int i = 6;
                foreach (byte b in data)
                {
                    packet[i] = b;
                    i++;
                }

                ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

                checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[messageType], checksum);

                byte ck_a = (byte)(checksum & 0xFF); ///< High byte
                byte ck_b = (byte)(checksum >> 8);   ///< Low byte

                packet[i] = ck_a;
                i        += 1;
                packet[i] = ck_b;
                i        += 1;

                if (BaseStream.IsOpen)
                {
                    BaseStream.Write(packet, 0, i);
                    _bytesSentSubj.OnNext(i);
                }
            }
        }
Пример #3
0
        public byte[] GenerateMAVLinkPacket_PX4(MAVLINK_MSG_ID messageType, object indata)
        {
            byte[] data;

            data = MavlinkUtil.StructureToByteArray(indata);

            byte[] packet = new byte[data.Length + 10 + 2];

            packet[0] = 0xfd;
            packet[1] = (byte)data.Length;
            packet[2] = 0;
            packet[3] = 0;
            packet[4] = 0x75;
            packet[5] = 255;               // this is always 255 - MYGCS
            packet[6] = 0;
            packet[7] = (byte)messageType; //messageType
            packet[8] = 0;                 //messageType extera
            packet[9] = 0;                 //messageType extera
            packetcount++;

            //packet[3] = 255; // this is always 255 - MYGCS
            //packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
            //packet[5] = (byte)messageType;


            int i = 10;

            foreach (byte b in data)
            {
                packet[i] = b;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 10);

            checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[(byte)messageType], checksum);

            byte ck_a = (byte)(checksum & 0xFF);   ///< High byte
            byte ck_b = (byte)(checksum >> 8);     ///< Low byte

            packet[i] = ck_a;
            i        += 1;
            packet[i] = ck_b;
            i        += 1;

            return(packet);
        }
Пример #4
0
        public byte[] GenerateMAVLinkPacket10(MAVLINK_MSG_ID messageType, object indata, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1)
        {
            byte[] data;

            data = MavlinkUtil.StructureToByteArray(indata);

            byte[] packet = new byte[data.Length + 6 + 2];

            packet[0] = MAVLINK_STX_MAVLINK1;
            packet[1] = (byte)data.Length;
            packet[2] = (byte)packetcount;
            if (sequence != -1)
            {
                packet[2] = (byte)sequence;
            }

            packetcount++;

            packet[3] = sysid; // this is always 255 - MYGCS
            packet[4] = compid;
            packet[5] = (byte)messageType;


            int i = 6;

            foreach (byte b in data)
            {
                packet[i] = b;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

            checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum);

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8);   ///< Low byte

            packet[i] = ck_a;
            i        += 1;
            packet[i] = ck_b;
            i        += 1;

            return(packet);
        }
Пример #5
0
        public byte[] GenerateMAVLinkPacket(MAVLINK_MSG_ID messageType, object indata)
        {
            byte[] data;

            data = MavlinkUtil.StructureToByteArray(indata);

            byte[] packet = new byte[data.Length + 6 + 2];

            packet[0] = 254;
            packet[1] = (byte)data.Length;
            packet[2] = (byte)packetcount;

            packetcount++;

            packet[3] = 255; // this is always 255 - MYGCS
            packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_Application1;
            packet[5] = (byte)messageType;


            int i = 6;

            foreach (byte b in data)
            {
                packet[i] = b;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

            checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[(byte)messageType], checksum);

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8);   ///< Low byte

            packet[i] = ck_a;
            i        += 1;
            packet[i] = ck_b;
            i        += 1;

            return(packet);
        }
Пример #6
0
        public byte[] GenerateMAVLinkPacket20(MAVLINK_MSG_ID messageType, object indata, bool sign = false, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1)
        {
            byte[] data;

            data = MavlinkUtil.StructureToByteArray(indata);

            MavlinkUtil.trim_payload(ref data);

            int extra = 0;

            if (sign)
            {
                extra = MAVLINK_SIGNATURE_BLOCK_LEN;
            }

            byte[] packet = new byte[data.Length + MAVLINK_NUM_NON_PAYLOAD_BYTES + extra];

            packet[0] = MAVLINK_STX;
            packet[1] = (byte)data.Length;
            packet[2] = 0;//incompat  signing
            if (sign)
            {
                packet[2] |= MAVLINK_IFLAG_SIGNED;
            }
            packet[3] = 0;//compat
            packet[4] = (byte)packetcount;
            if (sequence != -1)
            {
                packet[4] = (byte)sequence;
            }
            packetcount++;

            packet[5] = sysid;
            packet[6] = compid;
            packet[7] = (byte)((UInt32)messageType);
            packet[8] = (byte)((UInt32)messageType >> 8);
            packet[9] = (byte)((UInt32)messageType >> 16);

            int i = MAVLINK_NUM_HEADER_BYTES;

            foreach (byte b in data)
            {
                packet[i] = b;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, data.Length + MAVLINK_NUM_HEADER_BYTES);

            checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum);

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8);   ///< Low byte

            packet[i] = ck_a;
            i        += 1;
            packet[i] = ck_b;
            i        += 1;

            if (sign)
            {
                //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit

                /*
                 * 8 bits of link ID
                 * 48 bits of timestamp
                 * 48 bits of signature
                 */

                // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp)

                var timestamp = (UInt64)((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100);

                if (timestamp == lasttimestamp)
                {
                    timestamp++;
                }

                lasttimestamp = timestamp;

                var timebytes = BitConverter.GetBytes(timestamp);

                var sig = new byte[7];               // 13 includes the outgoing hash
                sig[0] = sendlinkid;
                Array.Copy(timebytes, 0, sig, 1, 6); // timestamp

                //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp);

                if (signingKey == null || signingKey.Length != 32)
                {
                    signingKey = new byte[32];
                }

                using (SHA256 signit = SHA256.Create())
                {
                    MemoryStream ms = new MemoryStream();
                    ms.Write(signingKey, 0, signingKey.Length);
                    ms.Write(packet, 0, i);
                    ms.Write(sig, 0, sig.Length);

                    var ctx = signit.ComputeHash(ms.GetBuffer());
                    // trim to 48
                    Array.Resize(ref ctx, 6);

                    foreach (byte b in sig)
                    {
                        packet[i] = b;
                        i++;
                    }

                    foreach (byte b in ctx)
                    {
                        packet[i] = b;
                        i++;
                    }
                }
            }

            return(packet);
        }
Пример #7
0
        public MAVLinkMessage ReadPacket(Stream BaseStream)
        {
            byte[] buffer = new byte[MAVLink.MAVLINK_MAX_PACKET_LEN];

            DateTime packettime = DateTime.MinValue;

            if (hasTimestamp)
            {
                byte[] datearray = new byte[8];

                int tem = BaseStream.Read(datearray, 0, datearray.Length);

                Array.Reverse(datearray);

                DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);

                UInt64 dateint = BitConverter.ToUInt64(datearray, 0);

                if ((dateint / 1000 / 1000 / 60 / 60) < 9999999)
                {
                    date1 = date1.AddMilliseconds(dateint / 1000);

                    packettime = date1.ToLocalTime();
                }
            }

            int readcount = 0;

            while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN)
            {
                // read STX byte
                ReadWithTimeout(BaseStream, buffer, 0, 1);

                if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
                {
                    break;
                }

                readcount++;
            }

            if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN)
            {
                return(null);

                throw new InvalidDataException("No header found in data");
            }

            var headerlength    = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
            var headerlengthstx = headerlength + 1;

            // read header
            try {
                ReadWithTimeout(BaseStream, buffer, 1, headerlength);
            }
            catch (EndOfStreamException)
            {
                return(null);
            }

            // packet length
            int lengthtoread = 0;

            if (buffer[0] == MAVLINK_STX)
            {
                lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
                if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
                {
                    lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
                }
            }
            else
            {
                lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
            }

            try
            {
                //read rest of packet
                ReadWithTimeout(BaseStream, buffer, headerlengthstx, lengthtoread - (headerlengthstx - 2));
            }
            catch (EndOfStreamException)
            {
                return(null);
            }

            // resize the packet to the correct length
            Array.Resize <byte>(ref buffer, lengthtoread + 2);

            MAVLinkMessage message = new MAVLinkMessage(buffer, packettime);

            // calc crc
            ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

            // calc extra bit of crc for mavlink 1.0+
            if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
            {
                crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
            }

            // check crc
            if ((message.crc16 >> 8) != (crc >> 8) ||
                (message.crc16 & 0xff) != (crc & 0xff))
            {
                badCRC++;
                // crc fail
                return(null);
            }

            return(message);
        }
Пример #8
0
        public MAVLinkMessage ReadPacket(byte[] buffer)
        {
            int readcount = 0;

            while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN)
            {
                // read STX byte

                if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
                {
                    break;
                }

                readcount++;
            }

            if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN)
            {
                throw new InvalidDataException("No header found in data");
            }

            var headerlength    = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
            var headerlengthstx = headerlength + 1;

            // read header

            // packet length
            int lengthtoread = 0;

            if (buffer[0] == MAVLINK_STX)
            {
                lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
                if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
                {
                    lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
                }
            }
            else
            {
                lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
            }

            //read rest of packet

            // resize the packet to the correct length
            Array.Resize <byte>(ref buffer, lengthtoread + 2);

            MAVLinkMessage message = new MAVLinkMessage(buffer);

            // calc crc
            ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

            // calc extra bit of crc for mavlink 1.0+
            if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
            {
                crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
            }

            // check crc
            if ((message.crc16 >> 8) != (crc >> 8) ||
                (message.crc16 & 0xff) != (crc & 0xff))
            {
                badCRC++;
                // crc fail
                return(null);
            }

            return(message);
        }
        public MAVLinkMessage ReadPacket(Stream BaseStream)
        {
            byte[] buffer = new byte[270];

            int readcount = 0;

            while (readcount < 200)
            {
                // read STX byte
                ReadWithTimeout(BaseStream, buffer, 0, 1);

                if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
                {
                    break;
                }

                readcount++;
            }

            var headerlength    = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
            var headerlengthstx = headerlength + 1;

            // read header
            ReadWithTimeout(BaseStream, buffer, 1, headerlength);

            // packet length
            int lengthtoread = 0;

            if (buffer[0] == MAVLINK_STX)
            {
                lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
                if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
                {
                    lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
                }
            }
            else
            {
                lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
            }

            //read rest of packet
            ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx - 2));

            // resize the packet to the correct length
            Array.Resize <byte>(ref buffer, lengthtoread + 2);

            MAVLinkMessage message = new MAVLinkMessage(buffer);

            // calc crc
            ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

            // calc extra bit of crc for mavlink 1.0+
            if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
            {
                crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
            }

            // check crc
            if ((message.crc16 >> 8) != (crc >> 8) ||
                (message.crc16 & 0xff) != (crc & 0xff))
            {
                badCRC++;
                // crc fail
                return(null);
            }

            return(message);
        }
Пример #10
0
        public MAVLinkMessage ReadPacket()
        {
            byte[]         buffer    = new byte[GlobalAppData.MAX_PACKET_LEN + 25];
            int            count     = 0;
            int            length    = 0;
            int            readcount = 0;
            MAVLinkMessage message   = null;

            DateTime start = DateTime.Now;

            lock (readlock)
            {
                while ((BaseStream != null && BaseStream.IsOpen) || logreadmode)
                {
                    try
                    {
                        if (readcount > 300)
                        {
                            break;
                        }

                        readcount++;
                        if (logreadmode)
                        {
                            message = ReadLog();
                            buffer  = message.buffer;
                            if (buffer == null || buffer.Length == 0)
                            {
                                return(MAVLinkMessage.Invalid);
                            }
                        }
                        else
                        {
                            if (BaseStream.ReadTimeout != 1200)
                            {
                                BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this.
                            }
                            DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                            while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
                            {
                                if (DateTime.Now > to)
                                {
                                    throw new TimeoutException("Timeout");
                                }

                                Task.Delay(1);
                            }
                            if (BaseStream.IsOpen)
                            {
                                BaseStream.Read(buffer, count, 1);
                                if (rawlogfile != null && rawlogfile.CanWrite)
                                {
                                    rawlogfile.WriteByte(buffer[count]);
                                }
                            }
                        }
                    }
                    catch (Exception ex)
                    {
                        break;
                    }

                    // check if looks like a mavlink packet and check for exclusions and write to console
                    if (buffer[0] != 0xfe && buffer[0] != 'U' && buffer[0] != 0xfd)
                    {
                        if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r')
                        {
                            // check for line termination
                            if (buffer[0] == '\r' || buffer[0] == '\n')
                            {
                                // check new line is valid
                                if (buildplaintxtline.Length > 3)
                                {
                                    plaintxtline = buildplaintxtline;
                                }

                                // reset for next line
                                buildplaintxtline = "";
                            }

                            TCPConsole.Write(buffer[0]);
                            buildplaintxtline += (char)buffer[0];
                        }

                        //_bytesReceivedSubj.OnNext(1);
                        count     = 0;
                        buffer[1] = 0;
                        continue;
                    }

                    // reset count on valid packet
                    readcount = 0;
                    // check for a header
                    if (buffer[0] == 0xfe || buffer[0] == 0xfd || buffer[0] == 'U')
                    {
                        var mavlinkv2 = buffer[0] == GlobalAppData.MAVLINK_STX ? true : false;

                        int headerlength    = mavlinkv2 ? GlobalAppData.MAVLINK_CORE_HEADER_LEN : GlobalAppData.MAVLINK_CORE_HEADER_MAVLINK1_LEN;
                        int headerlengthstx = headerlength + 1;

                        // if we have the header, and no other chars, get the length and packet identifiers
                        if (count == 0 && !logreadmode)
                        {
                            DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                            while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength)
                            {
                                if (DateTime.Now > to)
                                {
                                    throw new TimeoutException("Timeout");
                                }

                                Task.Delay(1);
                            }

                            int read = BaseStream.Read(buffer, 1, headerlength);
                            count = read;
                            if (rawlogfile != null && rawlogfile.CanWrite)
                            {
                                rawlogfile.Write(buffer, 1, read);
                            }
                        }

                        // packet length
                        if (buffer[0] == GlobalAppData.MAVLINK_STX)
                        {
                            length = buffer[1] + headerlengthstx +
                                     GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - magic - length
                            if ((buffer[2] & GlobalAppData.MAVLINK_IFLAG_SIGNED) > 0)
                            {
                                length += GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN;
                            }
                        }
                        else
                        {
                            length = buffer[1] + headerlengthstx +
                                     GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - U - length
                        }

                        if (count >= headerlength || logreadmode)
                        {
                            try
                            {
                                if (logreadmode)
                                {
                                }
                                else
                                {
                                    DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                                    while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - (headerlengthstx)))
                                    {
                                        if (DateTime.Now > to)
                                        {
                                            break;
                                        }

                                        Task.Delay(1);
                                    }

                                    if (BaseStream.IsOpen)
                                    {
                                        int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx));
                                        if (rawlogfile != null && rawlogfile.CanWrite)
                                        {
                                            // write only what we read, temp is the whole packet, so 6-end
                                            rawlogfile.Write(buffer, headerlengthstx, read);
                                        }
                                    }
                                }

                                count = length;
                            }
                            catch
                            {
                                break;
                            }
                            break;
                        }
                    }
                    count++;
                    if (count == 299)
                    {
                        break;
                    }
                }
            } // end readlock

            // resize the packet to the correct length
            Array.Resize <byte>(ref buffer, count);

            // add byte count
            //_bytesReceivedSubj.OnNext(buffer.Length);

            // update bps statistics
            if (_bpstime.Second != DateTime.Now.Second)
            {
                long btr = 0;
                if (BaseStream != null && BaseStream.IsOpen)
                {
                    btr = BaseStream.BytesToRead;
                }
                else if (logreadmode)
                {
                    btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position;
                }
                _bps2           = _bps1; // prev sec
                _bps1           = 0;     // current sec
                _bpstime        = DateTime.Now;
                _mavlink1count  = 0;
                _mavlink2count  = 0;
                _mavlink2signed = 0;
            }

            _bps1 += buffer.Length;

            if (buffer.Length == 0)
            {
                return(MAVLinkMessage.Invalid);
            }

            if (message == null)
            {
                message = new MAVLinkMessage(buffer, DateTime.UtcNow);
            }

            uint msgid = message.msgid;

            GlobalAppData.message_info msginfo = GlobalAppData.GetMessageInfo(msgid);

            // calc crc
            var    sigsize = (message.sig != null) ? GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN : 0;
            ushort crc     = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize - GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES);

            // calc extra bit of crc for mavlink 1.0/2.0
            if (message.header == 0xfe || message.header == 0xfd)
            {
                crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc);
            }

            // check message length size vs table (mavlink1 explicit size check | mavlink2 allow all, undersize 0 trimmed, and oversize unknown extension)
            if (!message.ismavlink2 && message.payloadlength != msginfo.minlength)
            {
                if (msginfo.length == 0) // pass for unknown packets
                {
                }
                else
                {
                    return(MAVLinkMessage.Invalid);
                }
            }

            // check crc
            if ((message.crc16 >> 8) != (crc >> 8) ||
                (message.crc16 & 0xff) != (crc & 0xff))
            {
                return(MAVLinkMessage.Invalid);
            }


            byte sysid       = message.sysid;
            byte compid      = message.compid;
            byte packetSeqNo = message.seq;

            // stat count
            if (message.buffer[0] == GlobalAppData.MAVLINK_STX)
            {
                _mavlink2count++;
            }
            else if (message.buffer[0] == GlobalAppData.MAVLINK_STX_MAVLINK1)
            {
                _mavlink1count++;
            }

            // if its a gcs packet - dont process further
            if (buffer.Length >= 5 && (sysid == 255 || sysid == 253) && logreadmode) // gcs packet
            {
                return(message);
            }

            return(message);
        }
        public object ReadPacket(Stream BaseStream)
        {
            byte[] buffer = new byte[270];

            int readcount = 0;

            while (readcount < 200)
            {
                // read STX byte
                BaseStream.Read(buffer, 0, 1);

                if (buffer[0] == MAVLink.MAVLINK_STX)
                {
                    break;
                }

                readcount++;
            }

            // read header
            int read = BaseStream.Read(buffer, 1, 5);

            // packet length
            int lengthtoread = buffer[1] + 6 + 2 - 2; // data + header + checksum - STX - length

            //read rest of packet
            read = BaseStream.Read(buffer, 6, lengthtoread - 4);

            // resize the packet to the correct length
            Array.Resize <byte>(ref buffer, lengthtoread + 2);

            // calc crc
            ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

            // calc extra bit of crc for mavlink 1.0
            if (buffer.Length > 5 && buffer[0] == 254)
            {
                crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);
            }

            // check message length vs table
            if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]])
            {
                // bad or unknown packet
                return(null);
            }

            // check crc
            if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) || buffer[buffer.Length - 2] != (crc & 0xff))
            {
                // crc fail
                return(null);
            }

            byte header = buffer[0];
            byte length = buffer[1];
            byte seq    = buffer[2];
            byte sysid  = buffer[3];
            byte compid = buffer[4];
            byte messid = buffer[5];

            //create the object spcified by the packet type
            object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]);

            // fill in the data of the object
            MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6);

            return(data);
        }
Пример #12
0
        public string CollectPacket(byte[] data)
        {
            int           dataIndex  = 0;
            StringBuilder nonMavData = new StringBuilder();

            while (dataIndex < data.Length)
            {
                byte dataByte = data[dataIndex++];
                if (collectCount == 0)
                {
                    // read STX byte
                    collectBuffer[0] = dataByte;
                    if (collectBuffer[0] == MAVLink.MAVLINK_STX || collectBuffer[0] == MAVLINK_STX_MAVLINK1)
                    {
                        collectCount     = 1;
                        collectGotHeader = false;
                    }
                    else
                    {
                        nonMavData.Append(Encoding.ASCII.GetString(new byte[] { dataByte }));
                    }
                    continue;
                }
                var headerlength    = collectBuffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
                var headerlengthstx = headerlength + 1;
                if (!collectGotHeader && collectCount < headerlengthstx)
                {
                    // read header
                    collectBuffer[collectCount++] = dataByte;
                    if (collectCount >= headerlengthstx)
                    {
                        collectGotHeader = true;
                        collectCount     = 6;
                    }
                    continue;
                }

                // packet length
                int lengthToRead;
                if (collectBuffer[0] == MAVLINK_STX)
                {
                    lengthToRead = collectBuffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
                    if ((collectBuffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
                    {
                        lengthToRead += MAVLINK_SIGNATURE_BLOCK_LEN;
                    }
                }
                else
                {
                    lengthToRead = collectBuffer[1] + headerlengthstx + 2 - 2;  // data + header + checksum - U - length
                }
                //read rest of packet
                //ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx - 2));
                if (collectCount < lengthToRead - (headerlengthstx - 2) + 6)
                {
                    collectBuffer[collectCount++] = dataByte;
                    if (collectCount < lengthToRead - (headerlengthstx - 2) + 6)
                    {
                        continue;
                    }
                }

                MAVLinkMessage message = new MAVLinkMessage(collectBuffer);

                // resize the packet to the correct length
                //Array.Resize<byte>(ref collectBuffer, lengthToRead + 2);

                // calc crc
                ushort crc = MavlinkCRC.crc_calculate(collectBuffer, collectCount - 2);

                // calc extra bit of crc for mavlink 1.0+
                if ((message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) &&
                    message.msgid < MAVLINK_MESSAGE_CRCS.Count)
                {
                    crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[message.msgid], crc);
                }

                // check crc
                if ((message.crc16 >> 8) != (crc >> 8) ||
                    (message.crc16 & 0xff) != (crc & 0xff))
                {
                    badCRC++;
                    OnMavError(MavError.BAD_CRC);
                }
                else
                {
                    OnMavMessageData(message);
                }
                collectCount = 0;
            }
            return(nonMavData.ToString());
        }
Пример #13
0
        public byte[] ReadPacket()
        {
            byte[] buffer    = new byte[260];
            int    count     = 0;
            int    length    = 0;
            int    readcount = 0;

            lastbad = new byte[2];

            BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this.

            DateTime start = DateTime.Now;

            lock (readlock)
            {
                while (BaseStream.IsOpen)
                {
                    try
                    {
                        if (readcount > 300)
                        {
                            //No valid mavlink package
                            break;
                        }
                        readcount++;
                        //MAV.cs.datetime = DateTime.Now;

                        DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                        // Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead);

                        while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
                        {
                            if (DateTime.Now > to)
                            {
                                //log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", BaseStream.BytesToRead, length);
                                throw new Exception("Timeout");
                            }
                            System.Threading.Thread.Sleep(1);
                            //Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream.BytesToRead);
                        }

                        if (BaseStream.IsOpen)
                        {
                            BaseStream.Read(buffer, count, 1);
                        }
                    }
                    catch (Exception e)
                    {
                        break;
                    }

                    // check if looks like a mavlink packet and check for exclusions and write to console
                    if (buffer[0] != 254)
                    {
                        if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r')
                        {
                            // check for line termination
                            if (buffer[0] == '\r' || buffer[0] == '\n')
                            {
                                // check new line is valid
                                if (buildplaintxtline.Length > 3)
                                {
                                    plaintxtline = buildplaintxtline;
                                }

                                // reset for next line
                                buildplaintxtline = "";
                            }
                            form.UpdateTextBox(((char)buffer[0]).ToString());
                            //TCPConsole.Write(buffer[0]);
                            //Console.Write((char)buffer[0]);
                            buildplaintxtline += (char)buffer[0];
                        }
                        _bytesReceivedSubj.OnNext(1);
                        count      = 0;
                        lastbad[0] = lastbad[1];
                        lastbad[1] = buffer[0];
                        buffer[1]  = 0;
                        continue;
                    }
                    // reset count on valid packet
                    readcount = 0;

                    // check for a header
                    if (buffer[0] == 254)
                    {
                        if (count == 0)
                        {
                            DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
                            while (BaseStream.IsOpen && BaseStream.BytesToRead < 5)
                            {
                                if (DateTime.Now > to)
                                {
                                    throw new Exception("Timeout");
                                }
                                System.Threading.Thread.Sleep(1);
                            }
                            int read = BaseStream.Read(buffer, 1, 5);
                            count = read;
                        }
                        // packet length
                        length = buffer[1] + 6 + 2 - 2; // data + header + checksum - U - length
                        if (count >= 5)
                        {
                            if (MAV.sysid != 0)
                            {
                                if (MAV.sysid != buffer[3] || MAV.compid != buffer[4])
                                {
                                    if (buffer[3] == '3' && buffer[4] == 'D')
                                    {
                                        // this is a 3dr radio rssi packet
                                    }
                                    else
                                    {
                                        //log.InfoFormat("Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}", buffer[3], buffer[4], MAV.sysid, MAV.compid);
                                        return(new byte[0]);
                                    }
                                }
                            }

                            try
                            {
                                DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                                while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - 4))
                                {
                                    if (DateTime.Now > to)
                                    {
                                        //log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}", BaseStream.BytesToRead, length);
                                        break;
                                    }
                                    System.Threading.Thread.Sleep(1);
                                }
                                if (BaseStream.IsOpen)
                                {
                                    int read = BaseStream.Read(buffer, 6, length - 4);
                                }
                                count = length + 2;
                            }
                            catch
                            {
                                break;
                            }

                            break;
                        }
                    }

                    count++;
                    if (count == 299)
                    {
                        break;
                    }
                }
            } // end readlock

            Array.Resize <byte>(ref buffer, count);

            _bytesReceivedSubj.OnNext(buffer.Length);

            if (packetlosttimer.AddSeconds(5) < DateTime.Now)
            {
                packetlosttimer = DateTime.Now;
                packetslost     = (packetslost * 0.8f);
                packetsnotlost  = (packetsnotlost * 0.8f);
            }

            if (bpstime.Second != DateTime.Now.Second && BaseStream.IsOpen)
            {
                Console.Write("bps {0} loss {1} left {2} mem {3}      \n", bps1, synclost, BaseStream.BytesToRead, System.GC.GetTotalMemory(false) / 1024 / 1024.0);
                bps2    = bps1; // prev sec
                bps1    = 0;    // current sec
                bpstime = DateTime.Now;
            }

            bps1 += buffer.Length;
            bps   = (bps1 + bps2) / 2;

            ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

            if (buffer.Length > 5 && buffer[0] == 254)
            {
                crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);
            }

            if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]])
            {
                if (MAVLINK_MESSAGE_LENGTHS[buffer[5]] == 0) // pass for unknown packets
                {
                }
                else
                {
                    //log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, buffer[5]);
                    if (buffer.Length == 11 && buffer[0] == 'U' && buffer[5] == 0)
                    {
                        string message = "Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n";
                        MessageBox.Show(message);
                        throw new Exception(message);
                    }
                    return(new byte[0]);
                }
            }

            if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) || buffer[buffer.Length - 2] != (crc & 0xff))
            {
                int packetno = -1;
                if (buffer.Length > 5)
                {
                    packetno = buffer[5];
                }
                if (packetno != -1 && buffer.Length > 5 && MAVLINK_MESSAGE_INFO[packetno] != null)
                {
                    //log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", buffer.Length, crc, packetno, MAVLINK_MESSAGE_INFO[packetno].ToString(), BitConverter.ToUInt16(buffer, buffer.Length - 2));
                    form.UpdateTextBox("Mavlink Bad Packet.");
                }
                return(new byte[0]);
            }

            try
            {
                if ((buffer[0] == 'U' || buffer[0] == 254) && buffer.Length >= buffer[1])
                {
                    if (buffer[3] == '3' && buffer[4] == 'D')
                    {
                        // Do nothing
                    }
                    else
                    {
                        byte packetSeqNo         = buffer[2];
                        int  expectedPacketSeqNo = ((MAV.recvpacketcount + 1) % 0x100);

                        if (packetSeqNo != expectedPacketSeqNo)
                        {
                            synclost++; // actualy sync loss's
                            int numLost = 0;

                            if (packetSeqNo < ((MAV.recvpacketcount + 1))) // recvpacketcount = 255 then   10 < 256 = true if was % 0x100 this would fail
                            {
                                numLost = 0x100 - expectedPacketSeqNo + packetSeqNo;
                            }
                            else
                            {
                                numLost = packetSeqNo - MAV.recvpacketcount;
                            }
                            packetslost += numLost;
                            WhenPacketLost.OnNext(numLost);

                            //log.InfoFormat("lost pkts new seqno {0} pkts lost {1}", packetSeqNo, numLost);
                        }

                        packetsnotlost++;
                        MAV.recvpacketcount = packetSeqNo;
                        WhenPacketReceived.OnNext(1);
                    }

                    if (double.IsInfinity(packetspersecond[buffer[5]]))
                    {
                        packetspersecond[buffer[5]] = 0;
                    }

                    packetspersecond[buffer[5]] = (((1000 / ((DateTime.Now - packetspersecondbuild[buffer[5]]).TotalMilliseconds) + packetspersecond[buffer[5]]) / 2));

                    packetspersecondbuild[buffer[5]] = DateTime.Now;

                    lock (writelock)
                    {
                        MAV.packets[buffer[5]] = buffer;
                        MAV.packetseencount[buffer[5]]++;
                    }

                    if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) // status text
                    {
                        var msg = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT].ByteArrayToStructure <MAVLink.mavlink_statustext_t>(6);

                        byte sev = msg.severity;

                        string logdata = Encoding.ASCII.GetString(msg.text);
                        int    ind     = logdata.IndexOf('\0');
                        if (ind != -1)
                        {
                            logdata = logdata.Substring(0, ind);
                        }
                        //log.Info(DateTime.Now + " " + logdata);

                        form.UpdateTextBox(logdata);
                        //MAV.cs.messages.Add(logdata);

                        if (sev >= 3)
                        {
                        }
                    }

                    // set ap type
                    if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
                    {
                        mavlink_heartbeat_t hb = buffer.ByteArrayToStructure <mavlink_heartbeat_t>(6);

                        if (hb.type != (byte)MAVLink.MAV_TYPE.GCS)
                        {
                            mavlinkversion = hb.mavlink_version;
                            MAV.aptype     = (MAV_TYPE)hb.type;
                            MAV.apname     = (MAV_AUTOPILOT)hb.autopilot;
                            //setAPType();
                        }
                    }
                }
            }
            catch { }

            if (buffer[3] == '3' && buffer[4] == 'D')
            {
                // dont update last packet time for 3dr radio packets
            }
            else
            {
                lastvalidpacket = DateTime.Now;
            }

            return(buffer);
        }
Пример #14
0
    /// <summary>
    /// Generate a Mavlink Packet and write to serial
    /// </summary>
    /// <param name="messageType">type number = MAVLINK_MSG_ID</param>
    /// <param name="indata">struct of data</param>
    public static byte[] GeneratePacket(IDrone drone, int messageType, object indata, bool forcemavlink2 = false, bool forcesigning = false)
    {
        lock (objLock)
        {
            var data   = MavlinkUtil.StructureToByteArray(indata);
            var packet = new byte[0];
            int i      = 0;

            // are we mavlink2 enabled for this sysid/compid
            if (!drone.MavLink2 && messageType < 256 && !forcemavlink2)
            {
                var info = MAVLink.MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.MsgId == messageType);
                if (data.Length != info.minlength)
                {
                    Array.Resize(ref data, (int)info.minlength);
                }

                //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
                packet = new byte[data.Length + 6 + 2];

                packet[0] = MAVLINK1_STX;
                packet[1] = (byte)data.Length;
                packet[2] = (byte)_packetCount;

                _packetCount++;

                packet[3] = gcssysid;
                packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
                packet[5] = (byte)messageType;

                i = 6;
                foreach (byte b in data)
                {
                    packet[i] = b;
                    i++;
                }

                ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

                checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum);


                byte ck_a = (byte)(checksum & 0xFF); ///< High byte
                byte ck_b = (byte)(checksum >> 8);   ///< Low byte

                packet[i] = ck_a;
                i        += 1;
                packet[i] = ck_b;
                i        += 1;
            }
            else
            {
                // trim packet for mavlink2
                MavlinkUtil.TrimePayload(ref data);

                packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN];

                packet[0] = MAVLINK2_STX;
                packet[1] = (byte)data.Length;
                packet[2] = 0;                     // incompat
                if (drone.Signing || forcesigning) // current mav
                {
                    packet[2] |= MAVLINK_IFLAG_SIGNED;
                }
                packet[3] = 0; // compat
                packet[4] = (byte)_packetCount;

                _packetCount++;

                packet[5] = gcssysid;
                packet[6] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
                packet[7] = (byte)(messageType & 0xff);
                packet[8] = (byte)((messageType >> 8) & 0xff);
                packet[9] = (byte)((messageType >> 16) & 0xff);

                i = 10;
                foreach (byte b in data)
                {
                    packet[i] = b;
                    i++;
                }

                ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + MAVLINK_NUM_HEADER_BYTES);

                checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum);

                byte ck_a = (byte)(checksum & 0xFF); ///< High byte
                byte ck_b = (byte)(checksum >> 8);   ///< Low byte

                packet[i] = ck_a;
                i        += 1;
                packet[i] = ck_b;
                i        += 1;

                if (drone.Signing || forcesigning)
                {
                    //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit

                    /*
                     * 8 bits of link ID
                     * 48 bits of timestamp
                     * 48 bits of signature
                     */

                    // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp)

                    var timestamp = (UInt64)((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100);

                    if (timestamp == drone.TimeStamp)
                    {
                        timestamp++;
                    }

                    drone.TimeStamp = timestamp;

                    var timebytes = BitConverter.GetBytes(timestamp);

                    var sig = new byte[7];               // 13 includes the outgoing hash
                    sig[0] = drone.SendLinkId;
                    Array.Copy(timebytes, 0, sig, 1, 6); // timestamp

                    //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp);

                    var signingKey = drone.SigningKey;

                    if (signingKey == null || signingKey.Length != 32)
                    {
                        signingKey = new byte[32];
                    }

                    using (SHA256Managed signit = new SHA256Managed())
                    {
                        signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0);
                        signit.TransformBlock(packet, 0, i, null, 0);
                        signit.TransformFinalBlock(sig, 0, sig.Length);
                        var ctx = signit.Hash;
                        // trim to 48
                        Array.Resize(ref ctx, 6);

                        foreach (byte b in sig)
                        {
                            packet[i] = b;
                            i++;
                        }

                        foreach (byte b in ctx)
                        {
                            packet[i] = b;
                            i++;
                        }
                    }
                }
            }

            return(packet);
        }
    }