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

state_machine_helper: change main state directly also for low battery RTL

Browse Source
This commit is contained in:
Matthias Grob
2022-01-14 15:13:21 +01:00
committed by Daniel Agar
parent f49bd9956b
commit 367b6123e1
1 changed files with 11 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+11 -23
View File
@@ -1140,28 +1140,21 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
} }
// Failsafe action // Failsafe action
const bool rtl_possible = status_flags.condition_global_position_valid && status_flags.condition_home_position_valid;
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
|| internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
// The main state is directly changed for the action because we need the fallbacks by the navigation state.
switch (battery_warning) { switch (battery_warning) {
case battery_status_s::BATTERY_WARNING_CRITICAL: case battery_status_s::BATTERY_WARNING_CRITICAL:
switch (low_battery_action) { switch (low_battery_action) {
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 (rtl_possible) { if (!already_landing_or_rtl) {
if (!already_landing_or_rtl) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time(); internal_state.timestamp = hrt_absolute_time();
}
} else {
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
}
} }
break; break;
@@ -1169,6 +1162,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
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; internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time(); internal_state.timestamp = hrt_absolute_time();
} }
@@ -1182,17 +1176,10 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case battery_status_s::BATTERY_WARNING_EMERGENCY: case battery_status_s::BATTERY_WARNING_EMERGENCY:
switch (low_battery_action) { switch (low_battery_action) {
case LOW_BAT_ACTION::RETURN: case LOW_BAT_ACTION::RETURN:
if (rtl_possible) { if (!already_landing_or_rtl) {
if (!already_landing_or_rtl) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time(); internal_state.timestamp = hrt_absolute_time();
}
} else {
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
}
} }
break; break;
@@ -1201,6 +1188,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
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; internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time(); internal_state.timestamp = hrt_absolute_time();
} }