dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: message planned battery action with warning together

This commit is contained in:
Matthias Grob 2022-01-26 20:27:51 +01:00 committed by Daniel Agar
parent 973116c7a4
commit ae0bb8371d
3 changed files with 43 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/commander/Commander.cpp
View File

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

41
src/modules/commander/state_machine_helper.cpp
View File

@ -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:

4
src/modules/commander/state_machine_helper.h
View File

@ -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);