From 1069ab08605766e6ec888aad05b6577b2963b8da Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 12 Nov 2018 15:26:38 +0900 Subject: [PATCH] Rover: GCS invoked RTL returns failed when set-mode fails --- APMrover2/GCS_Mavlink.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 6162524f0d..bfaac69263 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -642,12 +642,16 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l switch (packet.command) { case MAV_CMD_NAV_RETURN_TO_LAUNCH: - rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND); - return MAV_RESULT_ACCEPTED; + if (rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; case MAV_CMD_MISSION_START: - rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND); - return MAV_RESULT_ACCEPTED; + if (rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1, 1.0f)) { @@ -725,7 +729,7 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { - return MAV_RESULT_UNSUPPORTED; + return MAV_RESULT_FAILED; } // send yaw change and target speed to guided mode controller