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