From 63c88fea5822a85d80ec28e2492250f657f7e6ac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 19 Sep 2023 18:37:08 +1000 Subject: [PATCH] Plane: handle DO_PARACHUTE as both COMMAND_LONG and COMMAND_INT --- ArduPlane/GCS_Mavlink.cpp | 18 +++++++++++++----- ArduPlane/GCS_Mavlink.h | 1 + 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d4b5d199e7..4140bb1ea4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -984,6 +984,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_DO_CHANGE_SPEED: return handle_command_DO_CHANGE_SPEED(packet); +#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: + return handle_MAV_CMD_DO_PARACHUTE(packet); +#endif + #if HAL_QUADPLANE_ENABLED case MAV_CMD_DO_MOTOR_TEST: return handle_MAV_CMD_DO_MOTOR_TEST(packet); @@ -1059,8 +1064,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l plane.autotune_enable(!is_zero(packet.param1)); return MAV_RESULT_ACCEPTED; + default: + return GCS_MAVLINK::handle_command_long_packet(packet, msg); + } +} + #if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: +MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) +{ // configure or release parachute switch ((uint16_t)packet.param1) { case PARACHUTE_DISABLE: @@ -1087,12 +1098,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l break; } return MAV_RESULT_FAILED; +} #endif - default: - return GCS_MAVLINK::handle_command_long_packet(packet, msg); - } -} #if HAL_QUADPLANE_ENABLED MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 5d4b8533b1..27bb6e16c6 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -56,6 +56,7 @@ private: MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet); MAV_RESULT handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet); MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet); bool try_send_message(enum ap_message id) override;