轉(zhuǎn)自http://www.amovauto.com/portal.php?mod=view&aid=47
我們知道了怎么利用mavlink讀取航點(diǎn)灭抑，那么如果我們要寫(xiě)入航點(diǎn)梗掰，也是一樣的道理哮翘。這里有兩個(gè)重要的類分別是lightPlanner.cs和mavlinkinterface.cs。雙擊寫(xiě)入航點(diǎn)按鈕進(jìn)入實(shí)現(xiàn)函數(shù)，在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;//寫(xiě)入航點(diǎn)的后臺(tái)委托函數(shù)
frmProgressReporter.UpdateProgressAndStatus(-1, "Sending WP's");
ThemeManager.ApplyThemeTo(frmProgressReporter);
frmProgressReporter.RunBackgroundOperationAsync();
frmProgressReporter.Dispose();
MainMap.Focus();
}
上面的寫(xiě)入航點(diǎn)按鈕的單擊事件里面虾宇，我們實(shí)際上可以看成是兩部分鹃愤，一開(kāi)始是做航點(diǎn)列表的參數(shù)邏輯準(zhǔn)確性判斷簇搅，第二部分是做開(kāi)啟航點(diǎn)寫(xiě)入代碼。
在MP里面寫(xiě)入航點(diǎn)和 Commands變量有關(guān)系软吐，這個(gè)是C#中的柵格變量瘩将，也就是下面的圖下控件
這個(gè)控件的行列數(shù)代表有多少航點(diǎn)。通過(guò)這個(gè)控件可以很容易的訪問(wèn)到每個(gè)元素的值凹耙，可以在前期做出一些規(guī)則判斷姿现，避免寫(xiě)入錯(cuò)誤的航點(diǎn)參數(shù)
byte cmd =(byte)(int)Enum.Parse(typeof (MAVLink.MAV_CMD),Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);這句比較有意思他是把柵格里面對(duì)應(yīng)的命令轉(zhuǎn)換為命令I(lǐng)D，至于這個(gè)MAVLINK的庫(kù)實(shí)現(xiàn)有多少命令我們可以參考MAVLink.MAV_CMD這里面有實(shí)現(xiàn)的MAVLINK的命令肖抱。在寫(xiě)入?yún)?shù)之前备典，做好參數(shù)的判斷是很重要的，飛行器收到不和邏輯的參數(shù)虐沥，是一件非常危險(xiǎn)的事情熊经，一定要避免。MP在前期做了一些規(guī)則判斷欲险，我們可以看到詳細(xì)的代碼镐依。
同理一個(gè)對(duì)話框后臺(tái)執(zhí)行的委托 saveWPs函數(shù)。來(lái)執(zhí)行寫(xiě)入航點(diǎn)天试，寫(xiě)入航點(diǎn)的代碼相對(duì)復(fù)雜槐壳，我們來(lái)看看1. MAVLinkInterface port = MainV2.comPort;//得到和MAVLINK相關(guān)的一個(gè)全局類，MAVlink協(xié)議的實(shí)現(xiàn)就是依靠這個(gè)類喜每，這個(gè)類比較重要务唐。2. Locationwp home = new Locationwp();//得到飛行器"家"的參數(shù)
- if (MainV2.comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))//這個(gè)wps是個(gè)航點(diǎn)字典，里面保存了每個(gè)要航點(diǎn)的姿態(tài)带兜，參數(shù)和航點(diǎn)指令枫笛，具體可以看到 mavlink_mission_item_t這個(gè)類
- MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);//這條指令是把Commands表格列表的參數(shù)轉(zhuǎn)換成對(duì)應(yīng)的mavlink_mission_item_t里面的參數(shù)值，主要是一些字符串轉(zhuǎn)換刚照。
- port.setWPTotal(totalwpcountforupload);//上傳總共的航點(diǎn)6. var homeans = port.setWP(home, (ushort)a, MAVLink.MAV_FRAME.GLOBAL, 0, 1, use_int);//設(shè)置家的航點(diǎn)我們可以在mavlinkinterface.cs看到port.setWP的具體實(shí)現(xiàn)刑巧，至于詳細(xì)的讀寫(xiě)文章在這里:http://qgroundcontrol.org/mavlink/waypoint_protocol
圖1.2 寫(xiě)入航點(diǎn)協(xié)議流程
以上的代碼只是實(shí)現(xiàn)把"家"的位置上傳給APM，繼續(xù)看如下的代碼，下面是采用
- foreach (var temp in commandlist)循環(huán)把所有的的航點(diǎn)傳入飛控
// try send the wp - MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(a), frame, 0, 1, use_int);//嘗試寫(xiě)入航點(diǎn)啊楚，注意看這個(gè)setWP會(huì)有返回值ans吠冤，不同發(fā)回值，表示這次傳輸是否成功
MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR
MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE
MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID
這是設(shè)置航點(diǎn)的返回值恭理，通過(guò)這些值的判斷拯辙，就知道本次航點(diǎn)設(shè)置是否正確。在具體的mavlinkinterface.cs中我們可以分析到如圖1.2所示的傳輸步驟細(xì)節(jié)颜价。但是MP的代碼中做了比較好的邏輯判斷涯保，我們可以分析懂代碼后，做一些移植工作拍嵌，來(lái)滿自己的定制化需要遭赂。 - port.setWPACK();//mavlinkinterface.cs結(jié)束航點(diǎn)設(shè)置
以上就是saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)這個(gè)委托的分析，可以用斷點(diǎn)來(lái)調(diào)試跟蹤下横辆。前面兩章實(shí)現(xiàn)了航點(diǎn)的讀出和寫(xiě)入撇他，接下來(lái)考慮用移植代碼實(shí)現(xiàn)！
