通过代码看MAVLink协议 (二)

来源：互联网发布：win10笔记本修改Mac 编辑：程序博客网时间：2024/06/05 17:48

通过代码看MAVLink协议 (二)

接下来看一个500多行的函数= =，先擦擦汗。

[code] public byte[] readPacket()

没错就是他！今天要啃下这根骨头，吗？

[code]            byte[] buffer = new byte[270];            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;            //Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream.BytesToRead);

一些声明啦，等我看懂再说- =

读日志什么的先不看了，看传感器数据才是重点。

[code]BaseStream.Read(buffer, count, 1);

这句就是读到buffer里边啦，不过就读了第一个包头

[code]// check if looks like a mavlink packet and check for exclusions and write to console// 检验是不是看起来像个mavlink包，排除，写到控制台//如果不是0xFE,或者'U'if (buffer[0] != 254 && buffer[0] != 'U'){//如果是可打印ASCII字符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 = "";    }    // 控制台写个buffer[0]    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;}

上边处理完坏字符的情况后面，就是好的结果了。

[code]// check for a header// 检验包头if (buffer[0] == 254 || buffer[0] == 'U'){    // 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 < 5)        {            if (DateTime.Now > to)            {                log.InfoFormat("MAVLINK: 2 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);        }        //继续读数据帧的1-5个字节        int read = BaseStream.Read(buffer, 1, 5);        count = read;        if (rawlogfile != null && rawlogfile.CanWrite)            rawlogfile.Write(buffer, 1, read);    }    // packet length    // 包长度：数据帧长度+头长度+校验-2    length = buffer[1] + 6 + 2 - 2; // data + header + checksum - U - length    // 如果长度大于5，或者读日志模式    if (count >= 5 || logreadmode)    {        try        {            if (logreadmode)            {            }            else            {            //就是长度大于5，= =这样写真的有必要吗                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)                {                //继续读，从第7个字符开始（含），读长度-4个字符，也就是数据啦                    int read = BaseStream.Read(buffer, 6, length - 4);                    if (rawlogfile != null && rawlogfile.CanWrite)                    {                        // write only what we read, temp is the whole packet, so 6-end                        rawlogfile.Write(buffer, 6, read);                    }                }            }            count = length + 2;        }        catch        {            break;        }        break;    }}

这个lock就完了，上限300个包

[code]        count++;        if (count == 299)            break;    }    //Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream.BytesToRead);} // end readlock

读入数据之后，就该分析数据了吧

[code]// resize the packet to the correct length// 重新调整数组大小为合适的长度 Array.Resize<byte>(ref buffer, count);// add byte count// 字节计数_bytesReceivedSubj.OnNext(buffer.Length);// update bps statistics// 更新比特率策略// 如果bps时间的秒数和现在时间的秒数不相等，不是读日志模式，端口打开if (bpstime.Second != DateTime.Now.Second && !logreadmode && BaseStream.IsOpen){    //打印一句bps损失，剩余多少，收一次垃圾    Console.Write("bps {0} loss {1} left {2} mem {3}      \n", bps1, MAV.synclost, BaseStream.BytesToRead,        System.GC.GetTotalMemory(false)/1024/1024.0);    bps2 = bps1; // prev sec    bps1 = 0; // current sec    bpstime = DateTime.Now;}//这个比特率下，buffer的长度bps1 += buffer.Length;// calc crc// 计算crc，循环冗余校验，两句话ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);// calc extra bit of crc for mavlink 1.0if (buffer.Length > 5 && buffer[0] == 254){    crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);}

通过查表，检验信息长度

[code]// check message length vs table// 如果buffer的长度大于5，buffer[1]的长度不是这个表里面的什么东西// 这一步实际上是查表，在mavlink的github上的test.h中有定义，但是在这个文件中我怎么都找不到= =希望知道的人给我指出一条明路if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]]){    if (MAVLINK_MESSAGE_LENGTHS[buffer[5]] == 0)     // pass for unknown packets    //不一样说明不知道包的类型，因为第五个字节是消息ID，所以如果这个是0的话，说明这个还没有定义……刚才提到的test.h文件中除了第一项有数据之外，所有的全是0，所以是个简单的例子啊。。。    {        log.InfoFormat("unknown packet type {0}", buffer[5]);    }    else    //否则是个坏包，有头但是丢了数据了，长度不对    {        log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, buffer[5]);        //下面是检测是不是v0.9的，基本都不用了吧……        if (buffer.Length == 11 && buffer[0] == 'U' && buffer[5] == 0)         // check for 0.9 hb packet        {            string message =                "Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n";            Console.WriteLine(message);            if (logreadmode)                logplaybackfile.BaseStream.Seek(0, SeekOrigin.End);            throw new Exception(message);        }        //不是包或者坏包，函数返回        return new byte[0];    }}

下来就是校验CRC了

[code]// check crc// 校验CRC//如果buffer长度小于5，或者crc的低位和刚才校验的结果不一样，再或者crc的高位和刚才的高位校验结果不一样的话if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) ||    buffer[buffer.Length - 2] != (crc & 0xff)){    //分两种情况，一种packetno=-1，另一种大于0，分别对应buffer长度小于5和大于5两种情况    int packetno = -1;    if (buffer.Length > 5)    {        packetno = buffer[5];    }    //大于5的情况，crc校验失败，坏包    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));    //读日志模式的话写点什么东西    if (logreadmode)        log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position);    //返回失败信息，空字符    return new byte[0];}

0 0