コード例 #1
0
        private MAVLinkMessage ReadLog()
        {
            byte[] datearray        = new byte[8];
            bool   missingtimestamp = false;

            if (logplaybackfile.BaseStream is FileStream)
            {
                if (!streamfncache.ContainsKey(_logplaybackfile.BaseStream))
                {
                    streamfncache[_logplaybackfile.BaseStream] = new Tuple <string, long>(((FileStream)_logplaybackfile.BaseStream).Name.ToLower(), logplaybackfile.BaseStream.Length);
                }

                if (streamfncache[_logplaybackfile.BaseStream].Item1.EndsWith(".rlog"))
                {
                    missingtimestamp = true;
                }
            }

            else
            {
                if (!streamfncache.ContainsKey(_logplaybackfile.BaseStream))
                {
                    streamfncache[_logplaybackfile.BaseStream] = new Tuple <string, long>("", logplaybackfile.BaseStream.Length);
                }
            }

            if (!missingtimestamp)
            {
                int tem = logplaybackfile.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);

                try
                {
                    // array is reversed above
                    if (datearray[7] == 254 || datearray[7] == 253)
                    {
                        //rewind 8bytes
                        logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current);
                    }
                    else
                    {
                        if ((dateint / 1000 / 1000 / 60 / 60) < 9999999)
                        {
                            date1 = date1.AddMilliseconds(dateint / 1000);

                            lastlogread = date1.ToLocalTime();
                        }
                    }
                }
                catch
                {
                }
            }

            byte[] temp = new byte[0];

            byte byte0 = 0;
            byte byte1 = 0;
            byte byte2 = 0;

            var filelength = streamfncache[_logplaybackfile.BaseStream].Item2;
            var filepos    = logplaybackfile.BaseStream.Position;

            if (filelength == filepos)
            {
                return(MAVLinkMessage.Invalid);
            }

            int length = 5;
            int a      = 0;

            while (a < length)
            {
                if (filelength == filepos)
                {
                    return(MAVLinkMessage.Invalid);
                }

                var tempb = (byte)logplaybackfile.ReadByte();
                filepos++;

                switch (a)
                {
                case 0:
                    byte0 = tempb;
                    if (byte0 != 'U' && byte0 != GlobalAppData.MAVLINK_STX_MAVLINK1 && byte0 != GlobalAppData.MAVLINK_STX)
                    {
                        // seek to next valid
                        do
                        {
                            byte0 = logplaybackfile.ReadByte();
                        }while (byte0 != 'U' && byte0 != GlobalAppData.MAVLINK_STX_MAVLINK1 && byte0 != GlobalAppData.MAVLINK_STX);
                        a = 1;
                        continue;
                    }
                    break;

                case 1:
                    byte1 = tempb;
                    // handle length
                    {
                        int headerlength    = byte0 == GlobalAppData.MAVLINK_STX ? 9 : 5;
                        int headerlengthstx = headerlength + 1;

                        length = byte1 + headerlengthstx + 2;     // header + 2 checksum
                    }
                    break;

                case 2:
                    byte2 = tempb;
                    // handle signing and mavlink2
                    if (byte0 == GlobalAppData.MAVLINK_STX)
                    {
                        if ((byte2 & GlobalAppData.MAVLINK_IFLAG_SIGNED) > 0)
                        {
                            length += GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN;
                        }
                    }
                    // handle rest
                    {
                        temp    = new byte[length];
                        temp[0] = byte0;
                        temp[1] = byte1;
                        temp[2] = byte2;

                        var readto     = a + 1;
                        var readlength = length - (a + 1);
                        logplaybackfile.Read(temp, readto, readlength);
                        a = length;
                    }
                    break;
                }

                a++;
            }
            MAVLinkMessage tmp = new MAVLinkMessage(temp, lastlogread);

            //MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread;

            return(tmp);
        }
コード例 #2
0
        /// <summary>
        /// Print entire decoded packet to console
        /// </summary>
        /// <param name="datin">packet byte array</param>
        /// <returns>struct of data</returns>
        public object DebugPacket(MAVLinkMessage datin, ref string text, bool PrintToConsole, string delimeter = " ")
        {
            string textoutput = "";

            try
            {
                if (datin.Length > 5)
                {
                    textoutput =
                        string.Format(
                            "{0,2:X}{8}{1,2:X}{8}{2,2:X}{8}{3,2:X}{8}{4,2:X}{8}{5,2:X}{8}{6,2:X}{8}{7,6:X}{8}",
                            datin.header,
                            datin.payloadlength, datin.incompat_flags, datin.compat_flags, datin.seq, datin.sysid,
                            datin.compid, datin.msgid, delimeter);

                    object data = datin.data;

                    Type test = data.GetType();

                    {
                        textoutput = textoutput + test.Name + delimeter;

                        foreach (var field in test.GetFields())
                        {
                            // field.Name has the field's name.

                            object fieldValue = field.GetValue(data); // Get value

                            if (field.FieldType.IsArray)
                            {
                                textoutput = textoutput + field.Name + delimeter;
                                if (fieldValue.GetType() == typeof(byte[]))
                                {
                                    try
                                    {
                                        byte[] crap = (byte[])fieldValue;

                                        foreach (byte fiel in crap)
                                        {
                                            if (fiel == 0)
                                            {
                                                break;
                                            }
                                            else
                                            {
                                                textoutput = textoutput + (char)fiel;
                                            }
                                        }
                                    }
                                    catch
                                    {
                                    }
                                }
                                if (fieldValue.GetType() == typeof(short[]))
                                {
                                    try
                                    {
                                        short[] crap = (short[])fieldValue;

                                        foreach (short fiel in crap)
                                        {
                                            if (fiel == 0)
                                            {
                                                break;
                                            }
                                            else
                                            {
                                                textoutput = textoutput + Convert.ToString(fiel, 16) + "|";
                                            }
                                        }
                                    }
                                    catch
                                    {
                                    }
                                }
                                textoutput = textoutput + delimeter;
                            }
                            else
                            {
                                textoutput = textoutput + field.Name + delimeter + fieldValue.ToString() + delimeter;
                            }
                        }
                        var sig = "";
                        if (datin.sig != null)
                        {
                            sig = Convert.ToBase64String(datin.sig);
                        }

                        textoutput = textoutput + delimeter + "sig " + sig + delimeter + "Len" + delimeter + datin.Length + "\r\n";
                        if (PrintToConsole)
                        {
                            Console.Write(textoutput);
                        }

                        if (text != null)
                        {
                            text = textoutput;
                        }
                    }

                    return(data);
                }
            }
            catch
            {
                textoutput = textoutput + "\r\n";
            }

            return(null);
        }
コード例 #3
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);
        }