diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 62fc5cb3f8..e3ac888075 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -2500,6 +2500,19 @@ public bool doHighLatency(bool onoff= false) return ans1; } + /// + /// Control ICE Engine + /// + /// true to enable, false to disable + /// + public bool doEngineControl(byte sysid, byte compid, bool onoff = false) + { + int param1 = onoff ? 1 : 0; + + return doCommand(sysid, compid, MAV_CMD.DO_ENGINE_CONTROL, + param1, 0, 0, 0, 0, 0, 0); + } + /// /// reboot the vehicle /// diff --git a/GCSViews/FlightData.cs b/GCSViews/FlightData.cs index 9b222c49ce..dc1dde5b42 100644 --- a/GCSViews/FlightData.cs +++ b/GCSViews/FlightData.cs @@ -192,7 +192,9 @@ public enum actions HighLatency_Enable, HighLatency_Disable, Toggle_Safety_Switch, - Do_Parachute + Do_Parachute, + Engine_Start, + Engine_Stop, } private Dictionary NIC_table = new Dictionary() @@ -1768,6 +1770,18 @@ private void BUTactiondo_Click(object sender, EventArgs e) ((Control)sender).Enabled = true; return; } + if (CMB_action.Text == actions.Engine_Start.ToString()) + { + MainV2.comPort.doEngineControl((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, true); + ((Control)sender).Enabled = true; + return; + } + if (CMB_action.Text == actions.Engine_Stop.ToString()) + { + MainV2.comPort.doEngineControl((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, false); + ((Control)sender).Enabled = true; + return; + } if (CMB_action.Text == actions.Battery_Reset.ToString()) {