mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 21:33:33 +08:00
FlightTasks: add Descend task to land without GPS
This adds a flight task to catch the case where we want to do an emergency descent without GPS but only a baro. Previously, this would lead to the navigator land class being called without position estimates which then made the flight tasks fail and react with a flight task failsafe. This however meant that landed was never detected and a couple of confusing error messages. This applies if NAV_RCL_ACT is set to 3 "land".
This commit is contained in:
@@ -3197,8 +3197,7 @@ Commander::update_control_mode()
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
/* TODO: check if this makes sense */
|
||||
control_mode.flag_control_auto_enabled = true;
|
||||
control_mode.flag_control_auto_enabled = false;
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = true;
|
||||
control_mode.flag_control_climb_rate_enabled = true;
|
||||
|
||||
@@ -845,6 +845,28 @@ MulticopterPositionControl::start_flight_task()
|
||||
// we want to be in this mode, reset the failure count
|
||||
_task_failure_count = 0;
|
||||
}
|
||||
|
||||
} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
|
||||
|
||||
// Emergency descend
|
||||
should_disable_task = false;
|
||||
FlightTaskError error = FlightTaskError::NoError;
|
||||
|
||||
error = _flight_tasks.switchTask(FlightTaskIndex::Descend);
|
||||
|
||||
if (error != FlightTaskError::NoError) {
|
||||
if (prev_failure_count == 0) {
|
||||
PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error));
|
||||
}
|
||||
|
||||
task_failure = true;
|
||||
_task_failure_count++;
|
||||
|
||||
} else {
|
||||
// we want to be in this mode, reset the failure count
|
||||
_task_failure_count = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// manual position control
|
||||
|
||||
@@ -591,11 +591,6 @@ Navigator::run()
|
||||
_precland.set_mode(PrecLandMode::Required);
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
_pos_sp_triplet_published_invalid_once = false;
|
||||
navigation_mode_new = &_land;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
|
||||
_pos_sp_triplet_published_invalid_once = false;
|
||||
navigation_mode_new = &_dataLinkLoss;
|
||||
@@ -620,6 +615,7 @@ Navigator::run()
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO:
|
||||
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
|
||||
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB:
|
||||
|
||||
Reference in New Issue
Block a user