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/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.