From 8eed43b515d0625892359a82cb1ed7d26f977dcb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 14 Jan 2022 15:30:56 +0100 Subject: [PATCH] Commander: put battery warning and failsafe action in separate functions --- src/modules/commander/Commander.cpp | 4 ++-- src/modules/commander/state_machine_helper.cpp | 12 +++++++----- src/modules/commander/state_machine_helper.h | 7 +++---- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6178a32194..dd19373d18 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3889,8 +3889,8 @@ 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) { - battery_failsafe(&_mavlink_log_pub, _status, _status_flags, _internal_state, _battery_warning, - (low_battery_action_t)_param_com_low_bat_act.get()); + warn_user_about_battery(&_mavlink_log_pub, _battery_warning); + act_on_battery_failsafe(_internal_state, _battery_warning, (low_battery_action_t)_param_com_low_bat_act.get()); } // Handle shutdown request from emergency battery action diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5354b06e52..81cd00bf09 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1104,9 +1104,7 @@ void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsaf } } -void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, - const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning, - const low_battery_action_t low_battery_action) +void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning) { static constexpr char battery_level[] = "battery level"; @@ -1138,7 +1136,11 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta case battery_status_s::BATTERY_WARNING_NONE: break; // no warning } +} +void act_on_battery_failsafe(commander_state_s &internal_state, const uint8_t battery_warning, + const low_battery_action_t param_com_low_bat_act) +{ // Failsafe action const bool already_landing = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND; @@ -1148,7 +1150,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta // 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_CRITICAL: - switch (low_battery_action) { + switch (param_com_low_bat_act) { case LOW_BAT_ACTION::RETURN: case LOW_BAT_ACTION::RETURN_OR_LAND: if (!already_landing_or_rtl) { @@ -1174,7 +1176,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta break; case battery_status_s::BATTERY_WARNING_EMERGENCY: - switch (low_battery_action) { + switch (param_com_low_bat_act) { case LOW_BAT_ACTION::RETURN: if (!already_landing_or_rtl) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b189cb21fc..f899d15202 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -142,10 +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 battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, - const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning, - const low_battery_action_t low_bat_action); - +void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning); +void act_on_battery_failsafe(commander_state_s &commander_state, const uint8_t battery_warning, + const low_battery_action_t param_com_low_bat_act); // COM_IMB_PROP_ACT parameter values enum class imbalanced_propeller_action_t {