Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+8 -1
View File
@@ -3890,7 +3890,14 @@ void Commander::battery_status_check()
// execute battery failsafe if the state has gotten worse while we are armed // execute battery failsafe if the state has gotten worse while we are armed
if (battery_warning_level_increased_while_armed) { if (battery_warning_level_increased_while_armed) {
warn_user_about_battery(&_mavlink_log_pub, _battery_warning); 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 // Handle shutdown request from emergency battery action
src/modules/commander/state_machine_helper.cpp
+9 -14
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, 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) 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 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; || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND;
const bool already_landing_or_rtl = already_landing 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:
case LOW_BAT_ACTION::RETURN_OR_LAND: case LOW_BAT_ACTION::RETURN_OR_LAND:
if (!already_landing_or_rtl) { if (!already_landing_or_rtl) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; ret = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
} }
break; break;
case LOW_BAT_ACTION::LAND: case LOW_BAT_ACTION::LAND:
if (!already_landing) { if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; ret = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
} }
break; 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) { switch (param_com_low_bat_act) {
case LOW_BAT_ACTION::RETURN: case LOW_BAT_ACTION::RETURN:
if (!already_landing_or_rtl) { if (!already_landing_or_rtl) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; ret = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
} }
break; 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::RETURN_OR_LAND:
case LOW_BAT_ACTION::LAND: case LOW_BAT_ACTION::LAND:
if (!already_landing) { if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; ret = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
} }
break; break;
@@ -1201,6 +1194,8 @@ void act_on_battery_failsafe(commander_state_s &internal_state, const uint8_t ba
break; break;
} }
return ret;
} }
void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -143,7 +143,7 @@ typedef enum LOW_BAT_ACTION {
} low_battery_action_t; } 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);
void act_on_battery_failsafe(commander_state_s &commander_state, const uint8_t battery_warning, 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); const low_battery_action_t param_com_low_bat_act);
// COM_IMB_PROP_ACT parameter values // COM_IMB_PROP_ACT parameter values