轉載注意出處：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