From f49bd9956b3d6f2f9e021ee2b0ac92f1e1d627c9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 14 Jan 2022 12:29:21 +0100 Subject: [PATCH] state_machine_helper: reuse battery action conditions --- .../commander/state_machine_helper.cpp | 35 +++++++++---------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4b1857e76e..7df2ab2b9a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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(); }