通过代码看MAVLink协议 (三)

这是接着上一次的接收写的= =果然解析是最难的啊。

// packet is now verified
// 包现在已经确认了

//系统id，组件id
byte sysid = buffer[3];
byte compid = buffer[4];

// update packet loss statistics
// 更新包损失策略信息
// 如果不是读日志模式，并且MAVlist里面的时间加5秒还比现在的时间小
if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.Now)
{
    //计时器更新，lost，notlost都乘0.8
    MAVlist[sysid, compid].packetlosttimer = DateTime.Now;
    MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost*0.8f);
    MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost*0.8f);
}
//读日志模式，并且加上5秒后小于上一次日志读取的时间
else if (logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < lastlogread)
{
    //更新时间，lost，notlost乘0.8
    MAVlist[sysid, compid].packetlosttimer = lastlogread;
    MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost*0.8f);
    MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost*0.8f);
}

抽出想要的数据

// extract wp's from stream
// 获取航点信息
if (buffer.Length >= 5)
{
    getWPsfromstream(ref buffer, sysid, compid);
}

// if its a gcs packet - dont process further
// 如果是个地面站包，不进行进一步处理
if (buffer.Length >= 5 && (sysid == 255 || sysid == 253) && logreadmode) // gcs packet
{
    //返回这个信号
    return buffer; // new byte[0];
}

// 3dr radios dont send a hb, so no mavstate is ever created, this overrides that behavior
// 3dr雷达不传送hb信号，所以不会产生mav状态，这个取代了这个行为
// 如果包系统id是51，组件id是68，并且MAVlist有这两个值，交给另外的函数
if (sysid == 51 && compid == 68 && !MAVlist.Contains(51, 68))
{
    // create an item - hidden
    MAVlist.AddHiddenList(sysid, compid);
}
// 这个是个wifi芯片，也做相应的设置
// esp8266 no hb, provide param interface however
if (compid == (byte)MAV_COMPONENT.MAV_COMP_ID_UDP_BRIDGE && !MAVlist.Contains(sysid, (byte)MAV_COMPONENT.MAV_COMP_ID_UDP_BRIDGE))
{
    // create an item - visible
    MAVlist[sysid, compid] = MAVlist[sysid, compid];
    MAVlist[sysid, compid].sysid = sysid;
    MAVlist[sysid, compid].compid = compid;
}

下面看到很长的一段我好虚，在try里面包括着

if ((buffer[0] == 'U' || buffer[0] == 254) && buffer.Length >= buffer[1])
{
    // check if we lost pacakets based on seqno
    // （哈哈他这里居然有单词写错了让我笑一会儿）检测我们是不是漏掉了序列中的包，就是检测丢包吧
    // 包序号是buffer[2]
    byte packetSeqNo = buffer[2];
    // 期望的包序号，假设以前的包都正常接收，所以接受数量+1然后再模余个255
    int expectedPacketSeqNo = ((MAVlist[sysid, compid].recvpacketcount + 1)%0x100);
    {
        // the seconds part is to work around a 3dr radio bug sending dup seqno's
        //第二部分围绕3dr电台bug工作发送，吗
        //进来的包序号和期望的包序号不一样的话，并且和上一个包的序号也不一样的话。
        if (packetSeqNo != expectedPacketSeqNo && packetSeqNo != MAVlist[sysid, compid].recvpacketcount)
        {
            //同步失败
            MAVlist[sysid, compid].synclost++; // actualy sync loss's
            int numLost = 0;
            //两种情况又分开了= =
            if (packetSeqNo < ((MAVlist[sysid, compid].recvpacketcount + 1)))
                // recvpacketcount = 255 then   10 < 256 = true if was % 0x100 this would fail
            {
                //损失数量：255-期望包数+当前包数
                numLost = 0x100 - expectedPacketSeqNo + packetSeqNo;
            }
            else
            {
                //损失数量：当前包数-期望包数
                numLost = packetSeqNo - expectedPacketSeqNo;
            }
            //更新损失包数
            MAVlist[sysid, compid].packetslost += numLost;
            //什么时候丢的包？这个机制我还是不太懂
            WhenPacketLost.OnNext(numLost);
            log.InfoFormat("mav {2} seqno {0} exp {3} pkts lost {1}", packetSeqNo, numLost, sysid,
                expectedPacketSeqNo);
        }
        //否则就没丢包
        MAVlist[sysid, compid].packetsnotlost++;

        //Console.WriteLine("{0} {1}", sysid, packetSeqNo);
        //更新数量
        MAVlist[sysid, compid].recvpacketcount = packetSeqNo;
    }
    WhenPacketReceived.OnNext(1);

    // packet stats per mav
    // 每个飞行器的包状态
    // 对于消息buffer[5],如果 每秒的包数 这个值是无穷大的话，
    if (double.IsInfinity(MAVlist[sysid, compid].packetspersecond[buffer[5]]))
    // 给0
        MAVlist[sysid, compid].packetspersecond[buffer[5]] = 0;
    //然后算每秒的包数，算了公式我不看了
    MAVlist[sysid, compid].packetspersecond[buffer[5]] = (((1000/
                                                            ((DateTime.Now -                                  MAVlist[sysid, compid].packetspersecondbuild[buffer[5]]).TotalMilliseconds) + MAVlist[sysid, compid].packetspersecond[buffer[5]])/2));
    //然后更新一个计算时间     
    MAVlist[sysid, compid].packetspersecondbuild[buffer[5]] = DateTime.Now;

hb是心跳包什么的= =唉专业术语真是太坑了

// set seens sysid's based on hb packet - this will hide 3dr radio packets
// 发送看到的系统id基于心跳包-这将会屏蔽3dr电台的包
// 如果buffer[5]是一个心跳包的id
if (buffer[5] == (byte) MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
    //把buffer中的数据转换成心跳包结构体
    mavlink_heartbeat_t hb = buffer.ByteArrayToStructure<mavlink_heartbeat_t>(6);
    // not a gcs
    //不是地面站
    if (hb.type != (byte) MAV_TYPE.GCS)
    {
        // add a seen sysid
        // 添加一个飞机的系统id
        if (!MAVlist.Contains(sysid, compid))
        {
            // ensure its set from connect or log playback
            MAVlist.Create(sysid, compid);
            MAVlist[sysid, compid].aptype = (MAV_TYPE) hb.type;
            MAVlist[sysid, compid].apname = (MAV_AUTOPILOT) hb.autopilot;
            setAPType(sysid, compid);
        }

        // attach to the only remote device. / default to first device seen
        // 连接到唯一的远程设备：默认连接到第一个可看到的设备
        if (MAVlist.Count == 1)
        {
            sysidcurrent = sysid;
            compidcurrent = compid;
        }
    }
}

// only process for active mav
// 对活动的微型飞行器的唯一的进程
// 如果当前的系统和id和当前的组件id一样的话，接收包
if (sysidcurrent == sysid && compidcurrent == compid)
PacketReceived(buffer);
//调试模式
if (debugmavlink)
DebugPacket(buffer);
//状态文本
if (buffer[5] == (byte) MAVLink.MAVLINK_MSG_ID.STATUSTEXT) // status text
{
//buffer转结构体
var msg = buffer.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 + " " + sev + " " + logdata);

MAVlist[sysid, compid].cs.messages.Add(logdata);

bool printit = false;

// the change of severity and the autopilot version where introduced at the same time, so any version non 0 can be used
// copter 3.4+
// plane 3.4+
// 严格的变化，版本被引入的同时，可以使用3.4以上的版本，吗？
if (MAVlist[sysid, compid].cs.version.Major > 0 || MAVlist[sysid, compid].cs.version.Minor >= 4)
{
    if (sev <= (byte) MAV_SEVERITY.WARNING)
    {
        printit = true;
    }
}
else
{
    if (sev >= 3)
    {
        printit = true;
    }
}
//这段不看了
if (printit)
{
    MAVlist[sysid, compid].cs.messageHigh = logdata;
    MAVlist[sysid, compid].cs.messageHighTime = DateTime.Now;

    if (MainV2.speechEngine != null &&
        MainV2.speechEngine.IsReady &&
        Settings.Instance["speechenable"] != null &&
        Settings.Instance["speechenable"].ToString() == "True")
    {
        MainV2.speechEngine.SpeakAsync(logdata);
    }
}
}
if (lastparamset != DateTime.MinValue && lastparamset.AddSeconds(10) < DateTime.Now)
{
//上次参数设置时间=时间的最小值
lastparamset = DateTime.MinValue;

if (BaseStream.IsOpen)
{
    //执行这一句
    doCommand(MAV_CMD.PREFLIGHT_STORAGE, 0, 0, 0, 0, 0, 0, 0);
}
}
//又是从流中得到航点
getWPsfromstream(ref buffer, sysid, compid);