Mavlink地面站编写之五—MP源码多线程读写框架分析

转载请注明出处！阿木开源社区 玩也要玩的专业 http://www.amovauto.com/?p=743#more-743

对于MissionPlanner这种多任务的程序，我们知道要采用多线程的方式来实现，读数据，显示数据，写数据。这是就需要一些数据同步的问题。尤其是串口数据的读写的数据同步。数据读写模型，比如常见的生产者，消费者模型。这里不再多述。我们来分析下MP代码的读线程，写线程，来实现我们社区的开源地面站的显示数据和写入指令的功能。下面看看数据读取线程的代码，这个数据线程读取的代码在MainV2.CS中的1962行左右。

    while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&
                               port.giveComport == false)
                        {
                            try
                            {
                                port.readPacket();
                            }
                            catch (Exception ex)
                            {
                                log.Error(ex);
                            }
                        }
   // update currentstate of sysids on the port
                        foreach (var MAV in port.MAVlist.GetMAVStates())
                        {
                            try
                            {
                                MAV.cs.UpdateCurrentSettings(null, false, port, MAV);
                            }
                            catch (Exception ex)
                            {
                                log.Error(ex);
                            }
                        }

其中port.readPacket()这个语句是把数据读到一个数据缓冲区，显示线程就可以从这个数据缓冲区拿出来显示了，具体的代码参考mavlinkinterface的内容。这个函数里面有些数据的上锁，解锁的语句，是为了完成数据同步。这是读取数据，下面我们看看如果我们要实现发送指令或者航点给飞控，也就是写数据，我们应该怎么操作。写数据一定是在另外一个线程里面。在FlightPlanne.cs中的1863行，我们可以看到写入航点的操作

   private void BUT_write_Click(object sender, EventArgs e)
        {
            if ((altmode) CMB_altmode.SelectedValue == altmode.Absolute)
            {
                if (DialogResult.No ==
                    CustomMessageBox.Show("Absolute Alt is selected are you sure?", "Alt Mode", MessageBoxButtons.YesNo))
                {
                    CMB_altmode.SelectedValue = (int) altmode.Relative;
                }
            }

            // check for invalid grid data
            for (int a = 0; a < Commands.Rows.Count - 0; a++)
            {
                for (int b = 0; b < Commands.ColumnCount - 0; b++) { double answer; if (b >= 1 && b <= 7)
                    {
                        if (!double.TryParse(Commands[b, a].Value.ToString(), out answer))
                        {
                            CustomMessageBox.Show("There are errors in your mission");
                            return;
                        }
                    }

                    if (TXT_altwarn.Text == "")
                        TXT_altwarn.Text = (0).ToString();

                    if (Commands.Rows[a].Cells[Command.Index].Value.ToString().Contains("UNKNOWN"))
                        continue;

                    byte cmd =
                        (byte)
                            (int)
                                Enum.Parse(typeof (MAVLink.MAV_CMD),
                                    Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);

                    if (cmd < (byte) MAVLink.MAV_CMD.LAST &&
                        double.Parse(Commands[Alt.Index, a].Value.ToString()) < double.Parse(TXT_altwarn.Text))
                    {
                        if (cmd != (byte) MAVLink.MAV_CMD.TAKEOFF &&
                            cmd != (byte) MAVLink.MAV_CMD.LAND &&
                            cmd != (byte) MAVLink.MAV_CMD.RETURN_TO_LAUNCH)
                        {
                            CustomMessageBox.Show("Low alt on WP#" + (a + 1) +
                                                  "\nPlease reduce the alt warning, or increase the altitude");
                            return;
                        }
                    }
                }
            }

            ProgressReporterDialogue frmProgressReporter = new ProgressReporterDialogue
            {
                StartPosition = FormStartPosition.CenterScreen,
                Text = "Sending WP's"
            };

            frmProgressReporter.DoWork += saveWPs;
            frmProgressReporter.UpdateProgressAndStatus(-1, "Sending WP's");

            ThemeManager.ApplyThemeTo(frmProgressReporter);

            frmProgressReporter.RunBackgroundOperationAsync();

            frmProgressReporter.Dispose();

            MainMap.Focus();
        }

上面的函数是一些验证航点逻辑准确性的操作，frmProgressReporter.DoWork += saveWPs;这是实现了一个委托。而saveWPs这个函数是数据写入函数.port.setWPTotal（写入一共的航点数量）,port.setWP（写入航点）, port.setParam（设置参数）, port.setWPACK(给飞控应答)。

这里我们可以看到是遵循MAVLINK的读写协议的，首先地面站写入航点数量(port.setWPTotal)，地面站收到航点请求指令，地面站向飞控写入航点(port.setWP)，最后port.setWPACK完成应答.源码太长自行打开源码阅读参考。这是完整的写入数据的流程。其中我们还有注意一点就是 MainV2.comPort.giveComport = true;这句，在 saveWPs函数中，这是这个写入线程占用COM端口的标志，通过这句话，我发现MP的数据读写通信是半双工的。因为在数据的代码中我们会发现 while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&port.giveComport == false)这句话中的port.giveComport == false表示，没有数据写入，也就是COM没有占用，读数据才会有效，否则跳出数据读循环。也就是说MP的源码通port.giveComport 来判断是不可以读取数据。所以如果port.giveComport == true表示当前有数据在写入，就不会去读取数据。由此看出MP地面站的代码数据读写是半双工的。我也是在写社区地面站的时候发现这一点的，否则我采用mavlinkinterface的时候写入航点一直有问题，就出在写入数据的时候，不能读取数据。本人浅解欢迎批评指正！