FlightTaskManualAcceleration: smooth position unlock

The velocity setpoint of the position controller
does a jump when unlocking position with a non-zero position error.

This is solved by using the velocity setpoint feedback to smoothly
take over.

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -70,7 +70,7 @@ bool FlightTaskManualAcceleration::update()
 	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
 				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
-			_deltatime);
+			Vector2f(_velocity_setpoint_feedback), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
 
 	_constraints.want_takeoff = _checkTakeoff();

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -66,7 +66,7 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
 }
 
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
-		const float dt)
+		const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
 	// maximum commanded acceleration and velocity
 	Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
@@ -98,7 +98,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
 	// Generate velocity setpoint by forward integrating commanded acceleration
 	_velocity_setpoint += _acceleration_setpoint * dt;
 
-	lockPosition(pos, dt);
+	lockPosition(pos, vel_sp_feedback, dt);
 }
 
 void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
@@ -147,7 +147,7 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
 	}
 }
 
-void StickAccelerationXY::lockPosition(const Vector3f &pos, const float dt)
+void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
 	if (_velocity_setpoint.norm_squared() < FLT_EPSILON) {
 		if (!PX4_ISFINITE(_position_setpoint(0))) {
@@ -155,7 +155,10 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const float dt)
 		}
 
 	} else {
-		_position_setpoint.setNaN();
+		if (PX4_ISFINITE(_position_setpoint(0))) {
-
+			_position_setpoint.setNaN();
+			// avoid velocity control jump because of remaining position error when unlocking
+			_velocity_setpoint = vel_sp_feedback;
+		}
 	}
 }

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -56,7 +56,7 @@ public:
 	void resetVelocity(const matrix::Vector2f &velocity);
 	void resetAcceleration(const matrix::Vector2f &acceleration);
 	void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,
-			       const float dt);
+			       const matrix::Vector2f &vel_sp_feedback, const float dt);
 	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
 
 private:
@@ -64,7 +64,7 @@ private:
 	matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
 				       const matrix::Vector2f &vel_sp);
 	void applyTiltLimit(matrix::Vector2f &acceleration);
-	void lockPosition(const matrix::Vector3f &pos, const float dt);
+	void lockPosition(const matrix::Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt);
 
 	SlewRate<float> _acceleration_slew_rate_x;
 	SlewRate<float> _acceleration_slew_rate_y;