fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 07:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: put battery warning and failsafe action in separate functions

Browse Source
This commit is contained in:
Matthias Grob
2022-01-31 20:27:06 -05:00
committed by Daniel Agar
parent 367b6123e1
commit 8eed43b515
3 changed files with 12 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+2 -2
View File
@@ -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
src/modules/commander/state_machine_helper.cpp
+7 -5
View File
@@ -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;
src/modules/commander/state_machine_helper.h
+3 -4
View File
@@ -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 {