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

Navigator/Commander: make GPS failsafe consitent: switch to Descend also for FW and VTOL

Browse Source 
- remove GPS failsafe mode
- for VTOL: transition to hover in Descend (unless NAV_FORCE_VT is not set)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-25 11:56:09 +02:00
committed by Roman Bapst
parent b77487d69c
commit f02786d112
15 changed files with 59 additions and 459 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+9 -34
View File
@@ -119,7 +119,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"6: unallocated",
"7: unallocated",
"AUTO_LANDENGFAIL",
"AUTO_LANDGPSFAIL",
"ACRO",
"11: UNUSED",
"DESCEND",
@@ -870,13 +869,8 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -934,21 +928,14 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
return;
} else {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (status_flags.condition_local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return;
if (status_flags.condition_local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
return;
}
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
return;
} else if (status_flags.condition_local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
}
return;
}
// FALLTHROUGH
@@ -1016,13 +1003,7 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm
// If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -1095,13 +1076,7 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &
// If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) {
//TODO: Add case for rover
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;