dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: fix battery failsafe without GPS 

This fixes the battery failsafe for the following corner cases:
- Battery failsafe set to Return but we can't do RTL because we don't
  have a global position or home position. In this case we now switch to
  Land. Land might end up in Descend in the failsafe state machine
  later.
- Battery failsafe set to Land but we can't land because we don't have a
  local position. In this case we switch to land anyway and then fall
  back to descend in the failsafe state machine later.

The "fix" involves ignoring using the main_state_transition and
implementing the guards in place. This is a hack for now but should
cover the corner case until a more thorough refactor.

The different failsafe state machines have involved over time from
requirements and learnings based on developed solutions and products.
The implementations in various places will need to get consolidated in
the future.

Tested in SITL for Return and Land with and without GPS.
This commit is contained in:
Julian Oes 2019-10-28 10:53:15 +01:00
parent 6e395fe885
commit 24c58db9e6
1 changed files with 20 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/modules/commander/state_machine_helper.cpp
View File

@ -1145,25 +1145,23 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
// FALLTHROUGH
case LOW_BAT_ACTION::RETURN_OR_LAND:
// let us send the critical message even if already in RTL
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, Executing RTL", battery_critical);
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute RTL", battery_critical);
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical);
}
break;
case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, Landing immediately", battery_critical);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute landing", battery_critical);
}
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical);
break;
}
@ -1180,12 +1178,15 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::RETURN:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, Executing RTL", battery_dangerous);
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute RTL", battery_dangerous);
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical);
}
break;
@ -1194,13 +1195,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
// FALLTHROUGH
case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, Landing immediately", battery_dangerous);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, Can't execute landing", battery_dangerous);
}
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical);
break;
}