From a42674b0b05abe92486edb44ad16fd55d95c015e Mon Sep 17 00:00:00 2001 From: squilter Date: Wed, 12 Aug 2015 10:28:35 -0700 Subject: [PATCH] Copter: implement do_flighttermination --- ArduCopter/GCS_Mavlink.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 858ab145fd..22e7e815a0 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1224,6 +1224,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_DO_FLIGHTTERMINATION: + if (packet.param1 > 0.5f) { + copter.init_disarm_motors(); + result = MAV_RESULT_ACCEPTED; + } + break; + case MAV_CMD_DO_SET_ROI: // param1 : regional of interest mode (not supported) // param2 : mission index/ target id (not supported) @@ -1316,6 +1323,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; } } else if (is_zero(packet.param1) && (copter.mode_has_manual_throttle(copter.control_mode) || copter.ap.land_complete || is_equal(packet.param2,21196.0f))) { + // force disarming by setting param2 = 21196 is deprecated copter.init_disarm_motors(); result = MAV_RESULT_ACCEPTED; } else {