diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0238ffb68e..879fdf27b0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3885,19 +3885,17 @@ void Commander::battery_status_check() // execute battery failsafe if the state has gotten worse while we are armed if (battery_warning_level_increased_while_armed) { - warn_user_about_battery(&_mavlink_log_pub, _battery_warning); - _battery_failsafe_timestamp = hrt_absolute_time(); - uint8_t failsafe_action = get_battery_failsafe_action(_internal_state, _battery_warning, (low_battery_action_t)_param_com_low_bat_act.get()); + warn_user_about_battery(&_mavlink_log_pub, _battery_warning, + failsafe_action, _param_com_bat_act_t.get(), + main_state_str(failsafe_action), navigation_mode(failsafe_action)); + _battery_failsafe_timestamp = hrt_absolute_time(); + + // Switch to loiter to wait for the reaction delay if (_param_com_bat_act_t.get() > 0.f && failsafe_action != commander_state_s::MAIN_STATE_MAX) { - mavlink_log_critical(&_mavlink_log_pub, "Executing %s in %d seconds", - main_state_str(failsafe_action), static_cast(_param_com_bat_act_t.get())); - events::send(events::ID("commander_low_bat_action"), {events::Log::Critical, events::LogInternal::Warning}, - "Executing {1} in {2} seconds", - navigation_mode(failsafe_action), static_cast(_param_com_bat_act_t.get())); main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 37c5221e3c..4edfc608a9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1104,7 +1104,9 @@ void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsaf } } -void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning) +void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning, + const uint8_t failsafe_action, const float param_com_bat_act_t, + const char *failsafe_action_string, const events::px4::enums::navigation_mode_t failsafe_action_navigation_mode) { static constexpr char battery_level[] = "battery level"; @@ -1114,18 +1116,39 @@ void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t batter mavlink_log_critical(mavlink_log_pub, "Low %s, return advised\t", battery_level); events::send(events::ID("commander_bat_low"), {events::Log::Warning, events::LogInternal::Info}, "Low battery level, return advised"); + break; case battery_status_s::BATTERY_WARNING_CRITICAL: - mavlink_log_critical(mavlink_log_pub, "Critical %s, return now\t", battery_level); - events::send(events::ID("commander_bat_crit"), {events::Log::Critical, events::LogInternal::Info}, - "Critical battery level, return now"); + if (failsafe_action == commander_state_s::MAIN_STATE_MAX) { + mavlink_log_critical(mavlink_log_pub, "Critical %s, return now\t", battery_level); + events::send(events::ID("commander_bat_crit"), {events::Log::Critical, events::LogInternal::Info}, + "Critical battery level, return now"); + + } else { + mavlink_log_critical(mavlink_log_pub, "Critical %s, executing %s in %d seconds\t", battery_level, + failsafe_action_string, static_cast(param_com_bat_act_t)); + events::send(events::ID("commander_bat_crit_act"), {events::Log::Critical, events::LogInternal::Info}, + "Critical battery level, executing {1} in {2} seconds", + failsafe_action_navigation_mode, static_cast(param_com_bat_act_t)); + } + break; case battery_status_s::BATTERY_WARNING_EMERGENCY: - mavlink_log_emergency(mavlink_log_pub, "Dangerous %s, land now\t", battery_level); - events::send(events::ID("commander_bat_emerg"), {events::Log::Emergency, events::LogInternal::Info}, - "Dangerous battery level, land now"); + if (failsafe_action == commander_state_s::MAIN_STATE_MAX) { + mavlink_log_emergency(mavlink_log_pub, "Dangerous %s, land now\t", battery_level); + events::send(events::ID("commander_bat_emerg"), {events::Log::Emergency, events::LogInternal::Info}, + "Dangerous battery level, land now"); + + } else { + mavlink_log_emergency(mavlink_log_pub, "Dangerous %s, executing %s in %d seconds\t", battery_level, + failsafe_action_string, static_cast(param_com_bat_act_t)); + events::send(events::ID("commander_bat_emerg_act"), {events::Log::Emergency, events::LogInternal::Info}, + "Dangerous battery level, executing {1} in {2} seconds", + failsafe_action_navigation_mode, static_cast(param_com_bat_act_t)); + } + break; case battery_status_s::BATTERY_WARNING_FAILED: @@ -1150,6 +1173,10 @@ uint8_t get_battery_failsafe_action(const commander_state_s &internal_state, con // The main state is directly changed for the action because we need the fallbacks by the navigation state. switch (battery_warning) { + case battery_status_s::BATTERY_WARNING_NONE: + case battery_status_s::BATTERY_WARNING_LOW: + break; + case battery_status_s::BATTERY_WARNING_CRITICAL: switch (param_com_low_bat_act) { case LOW_BAT_ACTION::RETURN: diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index cf1ed4b434..995c0d4fb3 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -142,7 +142,9 @@ typedef enum LOW_BAT_ACTION { RETURN_OR_LAND = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels } low_battery_action_t; -void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning); +void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning, + const uint8_t failsafe_action, const float param_com_bat_act_t, + const char *failsafe_action_string, const events::px4::enums::navigation_mode_t failsafe_action_navigation_mode); uint8_t get_battery_failsafe_action(const commander_state_s &internal_state, const uint8_t battery_warning, const low_battery_action_t param_com_low_bat_act);