[Position|AutoLine]SmoothVel - Update position and velocity states of the trajectories in case of EKF reset. Until now, only the position reset on XY axis was properly handled. Now, xy, vxy, z and vz are handled

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -117,19 +117,39 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
 	return math::constrain(val, min, max);
 }
 
+void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters()
+{
+	// Check if a reset event has happened.
+	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
+		_trajectory[0].setCurrentPosition(_position(0));
+		_trajectory[1].setCurrentPosition(_position(1));
+		_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
+		_trajectory[0].setCurrentVelocity(_velocity(0));
+		_trajectory[1].setCurrentVelocity(_velocity(1));
+		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
+		_trajectory[2].setCurrentPosition(_position(2));
+		_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
+		_trajectory[2].setCurrentVelocity(_velocity(2));
+		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+	}
+}
+
 void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 {
 	// Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified
 	// that one is used as a velocity limit.
 	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
 
-	// Check if a reset event has happened.
+	_checkEkfResetCounters();
-	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
-		// Reset the XY axes
-		_trajectory[0].setCurrentPosition(_position(0));
-		_trajectory[1].setCurrentPosition(_position(1));
-		_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
-	}
 
 	if (PX4_ISFINITE(_position_setpoint(0)) &&
 	    PX4_ISFINITE(_position_setpoint(1))) {

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -69,12 +69,20 @@ protected:
 	void _setDefaultConstraints() override;
 
 	inline float _constrainOneSide(float val, float constrain);
+	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 	void _generateHeading();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
 	void _updateTrajConstraints();
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _generateTrajectory();
 	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
-	float _yaw_sp_prev{NAN};
+	float _yaw_sp_prev;
-	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+
+	/* counters for estimator local position resets */
+	struct {
+		uint8_t xy;
+		uint8_t vxy;
+		uint8_t z;
+		uint8_t vz;
+	} _reset_counters{0, 0, 0, 0};
 };

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -89,6 +89,32 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero)
 	_position_setpoint_z_locked = NAN;
 }
 
+void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
+{
+	// Check if a reset event has happened.
+	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
+		_smoothing[0].setCurrentPosition(_position(0));
+		_smoothing[1].setCurrentPosition(_position(1));
+		_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
+		_smoothing[0].setCurrentVelocity(_velocity(0));
+		_smoothing[1].setCurrentVelocity(_velocity(1));
+		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
+		_smoothing[2].setCurrentPosition(_position(2));
+		_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
+		_smoothing[2].setCurrentVelocity(_velocity(2));
+		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+	}
+}
+
 void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 {
 	/* Get yaw setpont, un-smoothed position setpoints.*/
@@ -111,6 +137,8 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 
 	float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()};
 
+	_checkEkfResetCounters();
+
 	/* Check for position unlock
 	 * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
 	 * is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null
@@ -161,16 +189,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 
 	VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
 
-	if (_position_lock_xy_active) {
-		// Check if a reset event has happened.
-		if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
-			// Reset the XY axes
-			_smoothing[0].setCurrentPosition(_position(0));
-			_smoothing[1].setCurrentPosition(_position(1));
-			_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
-		}
-	}
-
 	if (!_position_lock_xy_active) {
 		_smoothing[0].setCurrentPosition(_position(0));
 		_smoothing[1].setCurrentPosition(_position(1));

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -70,6 +70,7 @@ private:
 	 * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states
 	 */
 	void reset(Axes axes, bool force_z_zero = false);
+	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
 	matrix::Vector3f _vel_sp_smooth;
@@ -78,5 +79,11 @@ private:
 	matrix::Vector2f _position_setpoint_xy_locked;
 	float _position_setpoint_z_locked{NAN};
 
-	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+	/* counters for estimator local position resets */
+	struct {
+		uint8_t xy;
+		uint8_t vxy;
+		uint8_t z;
+		uint8_t vz;
+	} _reset_counters{0, 0, 0, 0};
 };