Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

GPS failsafe fixed-wing: constant roll loiter while keeping altitude and airspeed, then descend #17547

Merged
merged 2 commits into from
Nov 5, 2021
Merged

GPS failsafe fixed-wing: constant roll loiter while keeping altitude and airspeed, then descend #17547

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dc693b1
Fixed-wing Position controller: add modes for auto altitude and auto …
sfuhrer Oct 25, 2021
03c05de
Navigator/Commander: make GPS failsafe consitent: switch to Descend a…
sfuhrer Oct 25, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions msg/vehicle_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,9 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure
uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down)
uint8 NAVIGATION_STATE_UNUSED = 9 # Free slot
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot
uint8 NAVIGATION_STATE_UNUSED1 = 11 # Free slot
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14
Expand Down
1 change: 0 additions & 1 deletion src/drivers/osd/atxxxx/atxxxx.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -409,7 +409,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
flight_mode = "FAILURE";
break;

Expand Down
1 change: 0 additions & 1 deletion src/drivers/rc_input/crsf_telemetry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,6 @@ bool CRSFTelemetry::send_flight_mode()
break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
flight_mode = "Failure";
break;

Expand Down
14 changes: 14 additions & 0 deletions src/lib/parameters/param_translation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -195,6 +195,20 @@ bool param_modify_on_import(bson_node_t node)
}
}

// 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R
{
if (strcmp("NAV_GPSF_LT", node->name) == 0) {
strcpy(node->name, "FW_GPSF_LT");
PX4_INFO("copying %s -> %s", "NAV_GPSF_LT", "FW_GPSF_LT");
}

if (strcmp("NAV_GPSF_R", node->name) == 0) {
strcpy(node->name, "FW_GPSF_R");
PX4_INFO("copying and inverting sign %s -> %s", "NAV_GPSF_R", "FW_GPSF_R");
}
}


// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)

if (node->type != BSON_INT32) {
Expand Down
20 changes: 7 additions & 13 deletions src/modules/commander/Commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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:
Expand All @@ -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;
Expand Down Expand Up @@ -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:
Expand Down
5 changes: 2 additions & 3 deletions src/modules/commander/ManualControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand Down
43 changes: 9 additions & 34 deletions src/modules/commander/state_machine_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"6: unallocated",
"7: unallocated",
"AUTO_LANDENGFAIL",
"AUTO_LANDGPSFAIL",
sfuhrer marked this conversation as resolved.
Show resolved Hide resolved
"ACRO",
"11: UNUSED",
"DESCEND",
Expand Down Expand Up @@ -868,13 +867,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;
sfuhrer marked this conversation as resolved.
Show resolved Hide resolved
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
Expand Down Expand Up @@ -932,21 +926,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;

} else if (status_flags.condition_local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
return;
}
if (status_flags.condition_local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;

} 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
Expand Down Expand Up @@ -1014,13 +1001,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;
Expand Down Expand Up @@ -1093,13 +1074,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;
Expand Down
18 changes: 9 additions & 9 deletions src/modules/flight_mode_manager/FlightModeManager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,16 +215,17 @@ void FlightModeManager::start_flight_task()
_task_failure_count = 0;
}

} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
// Auto related maneuvers
} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {

// Emergency descend
should_disable_task = false;
FlightTaskError error = FlightTaskError::NoError;

error = switchTask(FlightTaskIndex::AutoLineSmoothVel);
error = switchTask(FlightTaskIndex::Descend);

if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {
PX4_WARN("Auto activation failed with error: %s", errorToString(error));
PX4_WARN("Descend activation failed with error: %s", errorToString(error));
}

task_failure = true;
Expand All @@ -235,17 +236,16 @@ void FlightModeManager::start_flight_task()
_task_failure_count = 0;
}

} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {

// Emergency descend
} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
// Auto related maneuvers
should_disable_task = false;
FlightTaskError error = FlightTaskError::NoError;

error = switchTask(FlightTaskIndex::Descend);
error = switchTask(FlightTaskIndex::AutoLineSmoothVel);

if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {
PX4_WARN("Descend activation failed with error: %s", errorToString(error));
PX4_WARN("Auto activation failed with error: %s", errorToString(error));
}

task_failure = true;
Expand Down
Loading