fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 16:19:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: change battery failsafe to return action instead of executing

Browse Source
This commit is contained in:
Matthias Grob 2022-01-14 15:48:23 +01:00 committed by Daniel Agar
parent 8eed43b515
commit 3af315f2c3
3 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3890,7 +3890,14 @@ 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);
act_on_battery_failsafe(_internal_state, _battery_warning, (low_battery_action_t)_param_com_low_bat_act.get());
uint8_t failsafe_action = get_battery_failsafe_action(_internal_state, _battery_warning,
(low_battery_action_t)_param_com_low_bat_act.get());
if (failsafe_action != commander_state_s::MAIN_STATE_MAX) {
_internal_state.main_state = failsafe_action;
_internal_state.main_state_changes++;
_internal_state.timestamp = hrt_absolute_time();
}
}
// Handle shutdown request from emergency battery action

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

@ -1138,10 +1138,11 @@ void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t batter
}
}
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)
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)
{
// Failsafe action
uint8_t ret = commander_state_s::MAIN_STATE_MAX;
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;
const bool already_landing_or_rtl = already_landing
@ -1154,18 +1155,14 @@ void act_on_battery_failsafe(commander_state_s &internal_state, const uint8_t ba
case LOW_BAT_ACTION::RETURN:
case LOW_BAT_ACTION::RETURN_OR_LAND:
if (!already_landing_or_rtl) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
ret = commander_state_s::MAIN_STATE_AUTO_RTL;
}
break;
case LOW_BAT_ACTION::LAND:
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
ret = commander_state_s::MAIN_STATE_AUTO_LAND;
}
break;
@ -1179,9 +1176,7 @@ void act_on_battery_failsafe(commander_state_s &internal_state, const uint8_t ba
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;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
ret = commander_state_s::MAIN_STATE_AUTO_RTL;
}
break;
@ -1189,9 +1184,7 @@ void act_on_battery_failsafe(commander_state_s &internal_state, const uint8_t ba
case LOW_BAT_ACTION::RETURN_OR_LAND:
case LOW_BAT_ACTION::LAND:
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
ret = commander_state_s::MAIN_STATE_AUTO_LAND;
}
break;
@ -1201,6 +1194,8 @@ void act_on_battery_failsafe(commander_state_s &internal_state, const uint8_t ba
break;
}
return ret;
}
void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,

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

@ -143,8 +143,8 @@ typedef enum LOW_BAT_ACTION {
} low_battery_action_t;
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);
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);
// COM_IMB_PROP_ACT parameter values
enum class imbalanced_propeller_action_t {