From e979d24fff7523dbfa7271d232d5c6e94ad054cb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 3 Dec 2018 17:15:41 +0100 Subject: [PATCH] commander: fix orbit failsafes for data link & rc loss --- .../commander/state_machine_helper.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d2d3bba5f3..72891476c7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -588,16 +588,31 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ break; case commander_state_s::MAIN_STATE_ORBIT: - /* require local position */ if (status->engine_failure) { + // failsafe: on 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 if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + // failsafe: necessary position estimate lost (nothing to do - everything done in check_invalid_pos_nav_state) + } else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) { + // failsafe: just datalink is lost and we're in air + set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, + vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS); + + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + + } else if (rc_lost && !data_link_loss_act_configured && is_armed) { + // failsafe: RC is lost, datalink loss is not set up and rc loss is not disabled + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, + vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); } else { + // no failsafe, RC is not mandatory for orbit status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT; } + break; case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: