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

state_machine_helper: reuse battery action conditions

Browse Source
This commit is contained in:
Matthias Grob
2022-01-14 12:29:21 +01:00
committed by Daniel Agar
parent 95b5bc0d84
commit f49bd9956b
1 changed files with 16 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+16 -19
View File
@@ -1119,9 +1119,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case battery_status_s::BATTERY_WARNING_CRITICAL:
mavlink_log_critical(mavlink_log_pub, "Critical %s, return encouraged\t", battery_level);
mavlink_log_critical(mavlink_log_pub, "Critical %s, return now\t", battery_level);
events::send(events::ID("commander_bat_crit"), {events::Log::Critical, events::LogInternal::Info},
"Critical battery level, return encouraged");
"Critical battery level, return now");
break;
case battery_status_s::BATTERY_WARNING_EMERGENCY:
@@ -1140,23 +1140,25 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
}
// 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
|| internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND;
const bool already_landing_or_rtl = already_landing
|| internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
switch (battery_warning) {
case battery_status_s::BATTERY_WARNING_CRITICAL:
switch (low_battery_action) {
case LOW_BAT_ACTION::RETURN:
case LOW_BAT_ACTION::RETURN_OR_LAND:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (rtl_possible) {
if (!already_landing_or_rtl) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.timestamp = hrt_absolute_time();
}
} else {
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
}
@@ -1165,8 +1167,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::LAND:
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
}
@@ -1181,17 +1182,14 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case battery_status_s::BATTERY_WARNING_EMERGENCY:
switch (low_battery_action) {
case LOW_BAT_ACTION::RETURN:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (rtl_possible) {
if (!already_landing_or_rtl) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.timestamp = hrt_absolute_time();
}
} else {
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
}
@@ -1201,8 +1199,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case LOW_BAT_ACTION::RETURN_OR_LAND:
case LOW_BAT_ACTION::LAND:
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (!already_landing) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
}