From d54d9ae922cdb910d288eefcc85eaf4e4a37b55b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 26 Sep 2023 14:28:40 +1000 Subject: [PATCH] ArduCopter: accept several mode-changing commands via command_int ... as well as via command_long --- ArduCopter/GCS_Mavlink.cpp | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 427c84f1da..82fbfa2f27 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -795,6 +795,25 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i return handle_MAV_CMD_DO_WINCH(packet); #endif + case MAV_CMD_NAV_LOITER_UNLIM: + if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_VTOL_LAND: + case MAV_CMD_NAV_LAND: + if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + default: return GCS_MAVLINK::handle_command_int_packet(packet, msg); } @@ -846,25 +865,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_FAILED; #endif - case MAV_CMD_NAV_LOITER_UNLIM: - if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_VTOL_LAND: - case MAV_CMD_NAV_LAND: - if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - default: return GCS_MAVLINK::handle_command_long_packet(packet, msg); }