From 9b9163813da27501f200cfceebe518260eb30d34 Mon Sep 17 00:00:00 2001 From: bazooka joe Date: Fri, 10 Jul 2020 23:15:15 -0700 Subject: [PATCH] avoid re-initiate RTL or land in battery emergency if the mode is already running --- .../commander/state_machine_helper.cpp | 57 +++++++++++++------ 1 file changed, 39 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a6a8c4441e..75d766a23c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1015,22 +1015,33 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta case LOW_BAT_ACTION::RETURN_OR_LAND: 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); + if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + + 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 { - 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); + if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + 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: - 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); + if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + 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; } @@ -1048,14 +1059,21 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta case LOW_BAT_ACTION::RETURN: 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_dangerous); + if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + 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_dangerous); + } } else { - 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_dangerous); + if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + 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_dangerous); + } } break; @@ -1064,9 +1082,12 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta // FALLTHROUGH case LOW_BAT_ACTION::LAND: - 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_dangerous); + if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + 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_dangerous); + } break; }