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; }