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

转载注意出处：http://www.amovauto.com/?p=601#more-601 阿木社区 玩也要玩的专业！

我们知道了怎么利用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#中的栅格变量，也就是下面的图下控件
                                       图1.1

这个控件的行列数代表有多少航点。通过这个控件可以很容易的访问到每个元素的值，可以在前期做出一些规则判断，避免写入错误的航点参数

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
图1.2 写入航点协议流程
以上的代码只是实现把"家"的位置上传给APM，继续看如下的代码，下面是采用
7. foreach (var temp in commandlist)循环把所有的的航点传入APM
// 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)这个委托的分析，可以用断点来调试跟踪下。前面两章实现了航点的读出和写入，接下来考虑用移植代码实现！

更多内容欢迎访问阿木社区 http://www.amovauto.com

QQ群:526221258