通过代码看MAVLink协议 (二)
2016-04-01 13:00
309 查看
通过代码看MAVLink协议 (二)
接下来看一个500多行的函数= =,先擦擦汗。
没错就是他!今天要啃下这根骨头,吗?
一些声明啦,等我看懂再说- =
读日志什么的先不看了,看传感器数据才是重点。
这句就是读到buffer里边啦,不过就读了第一个包头
上边处理完坏字符的情况后面,就是好的结果了。
这个lock就完了,上限300个包
读入数据之后,就该分析数据了吧
通过查表,检验信息长度
下来就是校验CRC了
嗯没错,至此所有校验工作完成啦……下来才是重点……我先歇会儿,马上下一篇搞起。
接下来看一个500多行的函数= =,先擦擦汗。
public byte[] readPacket()
没错就是他!今天要啃下这根骨头,吗?
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);
一些声明啦,等我看懂再说- =
读日志什么的先不看了,看传感器数据才是重点。
BaseStream.Read(buffer, count, 1);
这句就是读到buffer里边啦,不过就读了第一个包头
// 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; }
上边处理完坏字符的情况后面,就是好的结果了。
// 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个包
count++; if (count == 299) break; } //Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream.BytesToRead); } // 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 // 更新比特率策略 // 如果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.0 if (buffer.Length > 5 && buffer[0] == 254) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc); }
通过查表,检验信息长度
// 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了
// 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]; }
嗯没错,至此所有校验工作完成啦……下来才是重点……我先歇会儿,马上下一篇搞起。
相关文章推荐
- java JDBC通用BaseDao升级版0.3
- [C#]自定义消息收发
- c++作业2
- 20 个顶尖的 Python 机器学习开源项目
- 【matlab】:matlab视频处理
- Java入门:一些初学者需要掌握的基础算法程序——二分查找
- java JDBC通用BaseDao升级版0.2
- C++实验2-模拟银行
- python爬虫小笔记/干货
- JAVA JDBC通用BaseDao版0.1
- Openfire4.0.2源码部署到Eclipse
- MAVEN 工程打包resources目录外的更多资源文件
- C/C++—— C++中一个空对象为什么还要占用一个字节空间
- java中Socket编程
- matlab文件读取相关操作
- java集合的讲解
- C++:移动构造和移动赋值
- 怎样写出无法维护的代码
- 浅谈Python装饰器
- C++之类型转换