diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 744af09307..39d8c6198b 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -19,6 +19,7 @@ #include #include #include +#include // check if a message will fit in the payload space available #define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN) @@ -228,6 +229,7 @@ protected: virtual class AP_Camera *get_camera() const = 0; virtual AP_ServoRelayEvents *get_servorelayevents() const = 0; virtual AP_GPS *get_gps() const = 0; + virtual AP_AdvancedFailsafe *get_advanced_failsafe() const { return nullptr; }; bool waypoint_receiving; // currently receiving // the following two variables are only here because of Tracker @@ -271,6 +273,7 @@ protected: void handle_setup_signing(const mavlink_message_t *msg); uint8_t handle_preflight_reboot(const mavlink_command_long_t &packet, bool disable_overrides); MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet); + virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet); void handle_device_op_read(mavlink_message_t *msg); void handle_device_op_write(mavlink_message_t *msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9fbe04a6ee..4463f8b309 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1612,6 +1612,23 @@ uint8_t GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &packe return MAV_RESULT_UNSUPPORTED; } +/* + handle a flight termination request + */ +MAV_RESULT GCS_MAVLINK::handle_flight_termination(const mavlink_command_long_t &packet) +{ + AP_AdvancedFailsafe *failsafe = get_advanced_failsafe(); + if (failsafe == nullptr) { + return MAV_RESULT_UNSUPPORTED; + } + + bool should_terminate = packet.param1 > 0.5f; + + if (failsafe->gcs_terminate(should_terminate)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +} /* handle a R/C bind request (for spektrum) @@ -1963,6 +1980,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack result = handle_servorelay_message(packet); break; + case MAV_CMD_DO_FLIGHTTERMINATION: + result = handle_flight_termination(packet); + break; + default: result = MAV_RESULT_UNSUPPORTED; }