[转]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#中的栅格变量,也就是下面的图下控件

[转]Mission PLanner地面站构架分析之MAVLINK航点写入_第1张图片
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();//得到飞行器"家"的参数

  1. if (MainV2.comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))//这个wps是个航点字典,里面保存了每个要航点的姿态,参数和航点指令,具体可以看到 mavlink_mission_item_t这个类
  2. MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);//这条指令是把Commands表格列表的参数转换成对应的mavlink_mission_item_t里面的参数值,主要是一些字符串转换。
  3. 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
[转]Mission PLanner地面站构架分析之MAVLINK航点写入_第2张图片
Paste_Image.png

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

  1. foreach (var temp in commandlist)循环把所有的的航点传入飞控
    // try send the wp
  2. 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的代码中做了比较好的逻辑判断,我们可以分析懂代码后,做一些移植工作,来满自己的定制化需要。
  3. port.setWPACK();//mavlinkinterface.cs结束航点设置
    以上就是saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)这个委托的分析,可以用断点来调试跟踪下。前面两章实现了航点的读出和写入,接下来考虑用移植代码实现!

你可能感兴趣的:([转]Mission PLanner地面站构架分析之MAVLINK航点写入)