From 11ffb059aebf611c6c6aaef4252596dc635de79a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Sep 2023 19:04:24 +1000 Subject: [PATCH] Plane: accept MAV_CMD_DO_AUTOTUNE_ENABLE as both long and int --- ArduPlane/GCS_Mavlink.cpp | 15 ++++++++++----- ArduPlane/GCS_Mavlink.h | 1 + 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 4140bb1ea4..bfa02114b8 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -954,6 +954,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in { switch(packet.command) { + case MAV_CMD_DO_AUTOTUNE_ENABLE: + return handle_MAV_CMD_DO_AUTOTUNE_ENABLE(packet); + case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); @@ -1059,16 +1062,18 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l case MAV_CMD_DO_GO_AROUND: return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; - case MAV_CMD_DO_AUTOTUNE_ENABLE: - // param1 : enable/disable - plane.autotune_enable(!is_zero(packet.param1)); - return MAV_RESULT_ACCEPTED; - default: return GCS_MAVLINK::handle_command_long_packet(packet, msg); } } +MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet) +{ + // param1 : enable/disable + plane.autotune_enable(!is_zero(packet.param1)); + return MAV_RESULT_ACCEPTED; +} + #if PARACHUTE == ENABLED MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 27bb6e16c6..c6643c39b9 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -54,6 +54,7 @@ private: void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_AUTOTUNE_ENABLE(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);