[转]Mission PLanner地面站构架分析之MAVLINK航点写入

转自http://www.amovauto.com/portal.php?mod=view&aid=47
我们知道了怎么利用mavlink读取航点，那么如果我们要写入航点，也是一样的道理。这里有两个重要的类分别是lightPlanner.cs和mavlinkinterface.cs。双击写入航点按钮进入实现函数，在FlightPlanner.cs中

 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();
 }

上面的写入航点按钮的单击事件里面，我们实际上可以看成是两部分，一开始是做航点列表的参数逻辑准确性判断，第二部分是做开启航点写入代码。

在MP里面写入航点和 Commands变量有关系，这个是C#中的栅格变量，也就是下面的图下控件

Paste_Image.png

这个控件的行列数代表有多少航点。通过这个控件可以很容易的访问到每个元素的值，可以在前期做出一些规则判断，避免写入错误的航点参数
byte cmd =(byte)(int)Enum.Parse(typeof (MAVLink.MAV_CMD),Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);这句比较有意思他是把栅格里面对应的命令转换为命令ID，至于这个MAVLINK的库实现有多少命令我们可以参考MAVLink.MAV_CMD这里面有实现的MAVLINK的命令。在写入参数之前，做好参数的判断是很重要的，飞行器收到不和逻辑的参数，是一件非常危险的事情，一定要避免。MP在前期做了一些规则判断，我们可以看到详细的代码。
同理一个对话框后台执行的委托 saveWPs函数。来执行写入航点，写入航点的代码相对复杂，我们来看看1. MAVLinkInterface port = MainV2.comPort;//得到和MAVLINK相关的一个全局类，MAVlink协议的实现就是依靠这个类，这个类比较重要。2. Locationwp home = new Locationwp();//得到飞行器"家"的参数

3. if (MainV2.comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))//这个wps是个航点字典，里面保存了每个要航点的姿态，参数和航点指令，具体可以看到 mavlink_mission_item_t这个类
4. MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);//这条指令是把Commands表格列表的参数转换成对应的mavlink_mission_item_t里面的参数值，主要是一些字符串转换。
5. port.setWPTotal(totalwpcountforupload);//上传总共的航点6. var homeans = port.setWP(home, (ushort)a, MAVLink.MAV_FRAME.GLOBAL, 0, 1, use_int);//设置家的航点我们可以在mavlinkinterface.cs看到port.setWP的具体实现，至于详细的读写文章在这里:http://qgroundcontrol.org/mavlink/waypoint_protocol

Paste_Image.png

图1.2 写入航点协议流程
以上的代码只是实现把"家"的位置上传给APM，继续看如下的代码，下面是采用

7. foreach (var temp in commandlist)循环把所有的的航点传入飞控
   // try send the wp
8. MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(a), frame, 0, 1, use_int);//尝试写入航点，注意看这个setWP会有返回值ans，不同发回值，表示这次传输是否成功
   MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR
   MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE
   MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID
   这是设置航点的返回值，通过这些值的判断，就知道本次航点设置是否正确。在具体的mavlinkinterface.cs中我们可以分析到如图1.2所示的传输步骤细节。但是MP的代码中做了比较好的逻辑判断，我们可以分析懂代码后，做一些移植工作，来满自己的定制化需要。
9. port.setWPACK();//mavlinkinterface.cs结束航点设置
   以上就是saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)这个委托的分析，可以用断点来调试跟踪下。前面两章实现了航点的读出和写入，接下来考虑用移植代码实现！