From f2dbb0ad3b97eea91e07a800c968b6d2172b8f4c Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Sun, 14 Jan 2018 23:23:14 +0530 Subject: [PATCH] commander : add support for precision landing measurements --- src/modules/commander/commander.cpp | 15 +++++++++++ .../commander/state_machine_helper.cpp | 25 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1049a22072..ccd395dbe1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -581,6 +581,8 @@ int commander_main(int argc, char *argv[]) new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF; } else if (!strcmp(argv[2], "auto:land")) { new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + } else if (!strcmp(argv[2], "auto:precland")) { + new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND; } else { warnx("argument %s unsupported.", argv[2]); } @@ -1109,6 +1111,18 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety } break; + case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, main_state_prev, &status_flags, &internal_state)) { + mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&mavlink_log_pub, "Precision landing denied, land manually"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + case vehicle_command_s::VEHICLE_CMD_MISSION_START: { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; @@ -3902,6 +3916,7 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3a14c983b8..5611ec1743 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -481,6 +481,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: + + /* need local and global position, and precision land only implemented for multicopters */ + if (status_flags->condition_local_position_valid && status_flags->condition_global_position_valid && status->is_rotary_wing) { + ret = TRANSITION_CHANGED; + } + + break; + case commander_state_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ @@ -849,6 +858,22 @@ bool set_nav_state(struct vehicle_status_s *status, break; + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: + + /* must be rotary wing plus same requirements as normal landing */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { + // nothing to do - everything done in check_invalid_pos_nav_state + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; + } + + break; + case commander_state_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */