From e979d24fff7523dbfa7271d232d5c6e94ad054cb Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
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:
--
GitLab