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.

src/lib/flight_tasks/FlightTasks.cpp

@@ -36,6 +36,16 @@ const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
 	}
 }
 
+const ekf_reset_counters_s FlightTasks::getResetCounters()
+{
+	if (isAnyTaskActive()) {
+		return _current_task.task->getResetCounters();
+
+	} else {
+		return FlightTask::zero_reset_counters;
+	}
+}
+
 const vehicle_constraints_s FlightTasks::getConstraints()
 {
 	if (isAnyTaskActive()) {
@@ -64,7 +74,8 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 	}
 
 	// Save current setpoints for the next FlightTask
-	vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint();
+	const vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint();
+	const ekf_reset_counters_s last_reset_counters = getResetCounters();
 
 	if (_initTask(new_task_index)) {
 		// invalid task
@@ -84,6 +95,8 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::ActivationFailed;
 	}
 
+	_current_task.task->setResetCounters(last_reset_counters);
+
 	return FlightTaskError::NoError;
 }
 

src/lib/flight_tasks/FlightTasks.hpp

@@ -81,6 +81,12 @@ public:
 	 */
 	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
+	/**
+	 * Get the local frame and attitude reset counters of the estimator from the current task
+	 * @return the reset counters
+	 */
+	const ekf_reset_counters_s getResetCounters();
+
 	/**
 	 * Get task dependent constraints
 	 * @return setpoint constraints that has to be respected by the position controller

src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp

@@ -42,8 +42,9 @@ using namespace matrix;
 
 bool FlightTaskAutoFollowMe::update()
 {
+	bool ret = FlightTaskAuto::update();
 	_position_setpoint = _target;
 	matrix::Vector2f vel_sp = _getTargetVelocityXY();
 	_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
-	return true;
-}
+	return ret;
+}

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -49,6 +49,7 @@ bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpo
 
 bool FlightTaskAutoMapper::update()
 {
+	bool ret = FlightTaskAuto::update();
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
 
@@ -107,7 +108,7 @@ bool FlightTaskAutoMapper::update()
 	// update previous type
 	_type_previous = _type;
 
-	return true;
+	return ret;
 }
 
 void FlightTaskAutoMapper::_reset()

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp

@@ -48,6 +48,8 @@ bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint
 
 bool FlightTaskDescend::update()
 {
+	bool ret = FlightTask::update();
+
 	if (PX4_ISFINITE(_velocity(2))) {
 		// land with landspeed
 		_velocity_setpoint(2) = _param_mpc_land_speed.get();
@@ -59,5 +61,5 @@ bool FlightTaskDescend::update()
 		_acceleration_setpoint(2) = .3f;
 	}
 
-	return true;
+	return ret;
 }

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -49,6 +49,8 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin
 
 bool FlightTaskFailsafe::update()
 {
+	bool ret = FlightTask::update();
+
 	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
 		// stay at current position setpoint
 		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
@@ -72,5 +74,5 @@ bool FlightTaskFailsafe::update()
 		_acceleration_setpoint(2) = NAN;
 	}
 
-	return true;
+	return ret;
 }

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -6,6 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
 const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
 
+const ekf_reset_counters_s FlightTask::zero_reset_counters = {};
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
@@ -13,7 +14,6 @@ bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
-	_initEkfResetCounters();
 	_time_stamp_activate = hrt_absolute_time();
 	_gear = empty_landing_gear_default_keep;
 	return true;
@@ -37,17 +37,13 @@ bool FlightTask::updateInitialize()
 
 	_evaluateVehicleLocalPosition();
 	_evaluateDistanceToGround();
-	_checkEkfResetCounters();
 	return true;
 }
 
-void FlightTask::_initEkfResetCounters()
+bool FlightTask::update()
 {
-	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
-	_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
-	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
-	_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
-	_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
+	_checkEkfResetCounters();
+	return true;
 }
 
 void FlightTask::_checkEkfResetCounters()

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -55,6 +55,14 @@
 #include <uORB/topics/home_position.h>
 #include <lib/weather_vane/WeatherVane.hpp>
 
+struct ekf_reset_counters_s {
+	uint8_t xy;
+	uint8_t vxy;
+	uint8_t z;
+	uint8_t vz;
+	uint8_t quat;
+};
+
 class FlightTask : public ModuleParams
 {
 public:
@@ -97,7 +105,7 @@ public:
 	 * To be called regularly in the control loop cycle to execute the task
 	 * @return true on success, false on error
 	 */
-	virtual bool update() = 0;
+	virtual bool update();
 
 	/**
 	 * Call after update()
@@ -113,6 +121,9 @@ public:
 	 */
 	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
+	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
+	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
+
 	/**
 	 * Get vehicle constraints.
 	 * The constraints can vary with task.
@@ -139,6 +150,11 @@ public:
 	 */
 	static const vehicle_local_position_setpoint_s empty_setpoint;
 
+	/**.
+	 * All counters are set to 0.
+	 */
+	static const ekf_reset_counters_s zero_reset_counters;
+
 	/**
 	 * Empty constraints.
 	 * All constraints are set to NAN.
@@ -193,8 +209,8 @@ protected:
 	/**
 	 * Monitor the EKF reset counters and
 	 * call the appropriate handling functions in case of a reset event
+	 * TODO: add the delta values to all the handlers
 	 */
-	void _initEkfResetCounters();
 	void _checkEkfResetCounters();
 	virtual void _ekfResetHandlerPositionXY() {};
 	virtual void _ekfResetHandlerVelocityXY() {};
@@ -234,14 +250,7 @@ protected:
 	matrix::Vector3f _velocity_setpoint_feedback;
 	matrix::Vector3f _acceleration_setpoint_feedback;
 
-	/* Counters for estimator local position resets */
-	struct {
-		uint8_t xy;
-		uint8_t vxy;
-		uint8_t z;
-		uint8_t vz;
-		uint8_t quat;
-	} _reset_counters{};
+	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
 
 	/**
 	 * Vehicle constraints.

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -361,10 +361,11 @@ bool FlightTaskManualAltitude::_checkTakeoff()
 
 bool FlightTaskManualAltitude::update()
 {
+	bool ret = FlightTaskManual::update();
 	_updateConstraintsFromEstimator();
 	_scaleSticks();
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
 
-	return true;
+	return ret;
 }

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -68,6 +68,8 @@ bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s last_setpoin
 
 bool FlightTaskOffboard::update()
 {
+	bool ret = FlightTask::update();
+
 	// reset setpoint for every loop
 	_resetSetpoints();
 
@@ -106,7 +108,7 @@ bool FlightTaskOffboard::update()
 		}
 
 		// don't have to continue
-		return true;
+		return ret;
 
 	} else {
 		_position_lock.setAll(NAN);
@@ -124,7 +126,7 @@ bool FlightTaskOffboard::update()
 		}
 
 		// don't have to continue
-		return true;
+		return ret;
 
 	} else {
 		_position_lock.setAll(NAN);
@@ -144,7 +146,7 @@ bool FlightTaskOffboard::update()
 		}
 
 		// don't have to continue
-		return true;
+		return ret;
 
 	} else {
 		_position_lock.setAll(NAN);
@@ -155,7 +157,7 @@ bool FlightTaskOffboard::update()
 		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
 		_velocity_setpoint.setNaN();
 		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-		return true;
+		return ret;
 	}
 
 	// Possible inputs:
@@ -240,5 +242,5 @@ bool FlightTaskOffboard::update()
 	// use default conditions of upwards position or velocity to take off
 	_constraints.want_takeoff = _checkTakeoff();
 
-	return true;
+	return ret;
 }

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -162,7 +162,7 @@ bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint)
 bool FlightTaskOrbit::update()
 {
 	// update altitude
-	FlightTaskManualAltitudeSmooth::update();
+	bool ret = FlightTaskManualAltitudeSmooth::update();
 
 	// stick input adjusts parameters within a fixed time frame
 	const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f);
@@ -186,7 +186,7 @@ bool FlightTaskOrbit::update()
 	// publish information to UI
 	sendTelemetry();
 
-	return true;
+	return ret;
 }
 
 void FlightTaskOrbit::generate_circle_approach_setpoints()

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp

@@ -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;
 }

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp

@@ -92,7 +92,11 @@ public:
 		_state.x = pos;
 		_trajectory[0].setCurrentPosition(pos(0));
 		_trajectory[1].setCurrentPosition(pos(1));
-		// TODO: check lock/unlock in case of EKF reset
+		_position_estimate = pos;
+
+		if (_position_lock_active) {
+			_position_setpoint_locked = pos;
+		}
 	}
 	Vector2f getCurrentPosition() const { return _position_setpoint_locked; }
 
@@ -108,15 +112,15 @@ private:
 
 	bool _position_lock_active{false};
 
-	Vector2f _position_setpoint_locked;
+	Vector2f _position_setpoint_locked{};
 
-	Vector2f _velocity_setpoint_feedback;
-	Vector2f _position_estimate;
+	Vector2f _velocity_setpoint_feedback{};
+	Vector2f _position_estimate{};
 
 	struct {
 		Vector2f j;
 		Vector2f a;
 		Vector2f v;
 		Vector2f x;
-	} _state;
+	} _state{};
 };

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp

@@ -84,6 +84,11 @@ public:
 	{
 		_state.x = pos;
 		_trajectory.setCurrentPosition(pos);
+		_position_estimate = pos;
+
+		if (_position_lock_active) {
+			_position_setpoint_locked = pos;
+		}
 	}
 	float getCurrentPosition() const { return _position_setpoint_locked; }
 	void setCurrentPositionEstimate(float pos) { _position_estimate = pos; }
@@ -99,17 +104,17 @@ private:
 
 	bool _position_lock_active{false};
 
-	float _position_setpoint_locked;
+	float _position_setpoint_locked{};
 
-	float _velocity_setpoint_feedback;
-	float _position_estimate;
+	float _velocity_setpoint_feedback{};
+	float _position_estimate{};
 
 	struct {
 		float j;
 		float a;
 		float v;
 		float x;
-	} _state;
+	} _state{};
 
 	float _max_accel_up{0.f};
 	float _max_accel_down{0.f};