zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

state_machine_helper: reuse battery action conditions

master
Matthias Grob 3 years ago committed by Daniel Agar
parent
95b5bc0d84
commit
f49bd9956b
1 changed files with 16 additions and 19 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 35
      src/modules/commander/state_machine_helper.cpp

35
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -1119,9 +1119,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta @@ -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 @@ -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 @@ -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 @@ -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 @@ -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();
}

Write Preview
Loading…
Cancel
Save