fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 23:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

avoid re-initiate RTL or land in battery emergency

Browse Source 
if the mode is already running
This commit is contained in:
bazooka joe
2020-07-10 23:15:15 -07:00
committed by Lorenz Meier
parent 6260c164e7
commit 9b9163813d
1 changed files with 39 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+39 -18
View File
@@ -1015,22 +1015,33 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case LOW_BAT_ACTION::RETURN_OR_LAND:
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);
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)) {
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 {
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);
if (!(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_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:
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);
if (!(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_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical);
}
break;
}
@@ -1048,14 +1059,21 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case LOW_BAT_ACTION::RETURN:
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_dangerous);
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)) {
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_dangerous);
}
} else {
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_dangerous);
if (!(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_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_dangerous);
}
}
break;
@@ -1064,9 +1082,12 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
// FALLTHROUGH
case LOW_BAT_ACTION::LAND:
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_dangerous);
if (!(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_LAND;
internal_state->timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_dangerous);
}
break;
}