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