From 24c58db9e65c87e3cbfcf82b3502bcfe35b00e31 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 28 Oct 2019 10:53:15 +0100 Subject: [PATCH] commander: fix battery failsafe without GPS This fixes the battery failsafe for the following corner cases: - Battery failsafe set to Return but we can't do RTL because we don't have a global position or home position. In this case we now switch to Land. Land might end up in Descend in the failsafe state machine later. - Battery failsafe set to Land but we can't land because we don't have a local position. In this case we switch to land anyway and then fall back to descend in the failsafe state machine later. The "fix" involves ignoring using the main_state_transition and implementing the guards in place. This is a hack for now but should cover the corner case until a more thorough refactor. The different failsafe state machines have involved over time from requirements and learnings based on developed solutions and products. The implementations in various places will need to get consolidated in the future. Tested in SITL for Return and Land with and without GPS. --- .../commander/state_machine_helper.cpp | 43 +++++++++---------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6055e0935f..33c8ebbb0f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1145,25 +1145,23 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta // FALLTHROUGH case LOW_BAT_ACTION::RETURN_OR_LAND: - // let us send the critical message even if already in RTL - if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, - internal_state)) { - mavlink_log_critical(mavlink_log_pub, "%s, Executing RTL", battery_critical); + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL; + internal_state->timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical); } else { - mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute RTL", battery_critical); + internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state->timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical); } break; case LOW_BAT_ACTION::LAND: - if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, - internal_state)) { - mavlink_log_critical(mavlink_log_pub, "%s, Landing immediately", battery_critical); - - } else { - mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute landing", battery_critical); - } + internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state->timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical); break; } @@ -1180,12 +1178,15 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta break; case LOW_BAT_ACTION::RETURN: - if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, - internal_state)) { - mavlink_log_emergency(mavlink_log_pub, "%s, Executing RTL", battery_dangerous); + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL; + internal_state->timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical); } else { - mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute RTL", battery_dangerous); + internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state->timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical); } break; @@ -1194,13 +1195,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta // FALLTHROUGH case LOW_BAT_ACTION::LAND: - if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, - internal_state)) { - mavlink_log_emergency(mavlink_log_pub, "%s, Landing immediately", battery_dangerous); - - } else { - mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute landing", battery_dangerous); - } + internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state->timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical); break; }