forked from Archive/PX4-Autopilot
Commander: message planned battery action with warning together
This commit is contained in:
parent
973116c7a4
commit
ae0bb8371d
|
@ -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<uint16_t>(_param_com_bat_act_t.get()));
|
||||
events::send<events::px4::enums::navigation_mode_t, uint16_t>(events::ID("commander_low_bat_action"), {events::Log::Critical, events::LogInternal::Warning},
|
||||
"Executing {1} in {2} seconds",
|
||||
navigation_mode(failsafe_action), static_cast<uint16_t>(_param_com_bat_act_t.get()));
|
||||
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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<uint16_t>(param_com_bat_act_t));
|
||||
events::send<events::px4::enums::navigation_mode_t, uint16_t>(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<uint16_t>(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<uint16_t>(param_com_bat_act_t));
|
||||
events::send<events::px4::enums::navigation_mode_t, uint16_t>(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<uint16_t>(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:
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
Loading…
Reference in New Issue