From 27a50275b6c2206ff7f43845b000a3818f983311 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 30 Oct 2016 16:10:02 -0400 Subject: [PATCH] commander NAV_RETURN_TO_LAUNCH change mode to RTL --- src/modules/commander/commander.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2240c1a7bb..58787d0a64 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1048,6 +1048,18 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; + case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { + mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&mavlink_log_pub, "Return to launch denied"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { /* ok, home set, use it to take off */ if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) { @@ -1058,7 +1070,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s mavlink_log_critical(&mavlink_log_pub, "Takeoff denied, disarm and re-try"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } break; @@ -1068,10 +1079,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(&mavlink_log_pub, "Landing denied, land manually."); + mavlink_log_critical(&mavlink_log_pub, "Landing denied, land manually"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } break;