通过代码看MAVLink协议 (二)
通过代码看MAVLink协议 (二)
接下来看一个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];
}
嗯没错，至此所有校验工作完成啦……下来才是重点……我先歇会儿，马上下一篇搞起。