FlightTask: Fix ekf2 reset race condition during task switch (#14692)

* FlightTask: Fix ekf2 reset race condition during task switch

During a loss of GPS data when using GPS as primary height source,
the height is reset to baro and the local position gets invalid at the
same time. This triggers a switch to altitude flight task and a setpoint
reset.
This combination of events had the effect to ignore the height reset,
the large sudden height error could create an abrupt change of altitude
or even a crash.
The ekf2 reset is now done at the beginning of each update call.
This commit is contained in:
Mathieu Bresciani
2020-04-22 13:18:35 +02:00
committed by GitHub
parent e82880d6d7
commit be2bb4a479
14 changed files with 85 additions and 42 deletions
@@ -60,6 +60,7 @@ void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &set
bool FlightTaskTransition::update()
{
bool ret = FlightTask::update();
_acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
// demand zero vertical velocity and level attitude
// tailsitters will override attitude and thrust setpoint
@@ -68,5 +69,5 @@ bool FlightTaskTransition::update()
_velocity_setpoint(2) = 0.0f;
_yaw_setpoint = NAN;
return true;
return ret;
}