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:
Julian Oes
2019-10-18 16:18:19 +02:00
parent 5f53ae1253
commit de90543d6f
7 changed files with 184 additions and 7 deletions
+1 -2
View File
@@ -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
+1 -5
View File
@@ -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: