转自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();//得到飞行器"家"的参数
- if (MainV2.comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))//这个wps是个航点字典，里面保存了每个要航点的姿态，参数和航点指令，具体可以看到 mavlink_mission_item_t这个类
- MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);//这条指令是把Commands表格列表的参数转换成对应的mavlink_mission_item_t里面的参数值，主要是一些字符串转换。
- 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，继续看如下的代码，下面是采用
- foreach (var temp in commandlist)循环把所有的的航点传入飞控
// try send the wp - 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的代码中做了比较好的逻辑判断，我们可以分析懂代码后，做一些移植工作，来满自己的定制化需要。 - port.setWPACK();//mavlinkinterface.cs结束航点设置
以上就是saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)这个委托的分析，可以用断点来调试跟踪下。前面两章实现了航点的读出和写入，接下来考虑用移植代码实现！
网友评论