fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 17:17:36 +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/Commander.cpp
+7 -13
View File
@@ -3304,8 +3304,8 @@ Commander::update_control_mode()
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
@@ -3321,14 +3321,8 @@ Commander::update_control_mode()
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
@@ -3337,7 +3331,7 @@ Commander::update_control_mode()
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_vehicle_control_mode.flag_control_auto_enabled = false;
_vehicle_control_mode.flag_control_auto_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
@@ -3390,8 +3384,8 @@ Commander::update_control_mode()
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
break;
default:
src/modules/commander/ManualControl.cpp
+2 -3
View File
@@ -69,9 +69,8 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
&& vehicle_control_mode.flag_control_offboard_enabled;
// in Descend and LandGPSFail manual override is enbaled independently of COM_RC_OVERRIDE
const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
// in Descend manual override is enbaled independently of COM_RC_OVERRIDE
const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {
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;