Trajectory - Add position lock-unlock logic and proper initialization from controller feedback

src/lib/FlightTasks/FlightTasks.hpp

@@ -136,6 +136,8 @@ public:
 	 */
 	void reActivate();
 
+	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); }
+
 private:
 
 	/**

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -147,6 +147,8 @@ public:
 	 */
 	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {};
 
+	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
+
 protected:
 
 	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
@@ -197,6 +199,9 @@ protected:
 	float _yaw_setpoint;
 	float _yawspeed_setpoint;
 
+	matrix::Vector3f _velocity_setpoint_feedback;
+	matrix::Vector3f _thrust_setpoint_feedback;
+
 	/**
 	 * Vehicle constraints.
 	 * The constraints can vary with tasks.

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

@@ -47,6 +47,10 @@ bool FlightTaskManualPositionSmoothVel::activate()
 		_smoothing[i].reset(0.f, _velocity(i), _position(i));
 	}
 
+	_position_lock_xy_active = false;
+	_position_setpoint_xy_locked(0) = NAN;
+	_position_setpoint_xy_locked(1) = NAN;
+
 	return ret;
 }
 
@@ -81,9 +85,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 
 	if (_jerk_min.get() > FLT_EPSILON) {
 		if (vel_xy_sp.length() < FLT_EPSILON) { // Brake
-			jerk_xy = _jerk_min.get() + (_jerk_max.get() - _jerk_min.get());
-
-		} else if (vel_xy_sp.dot(vel_xy_sp_smooth) < -FLT_EPSILON) { // Reverse
 			jerk_xy = _jerk_max.get();
 
 		} else {
@@ -94,6 +95,23 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 	jerk[0] = jerk_xy;
 	jerk[1] = jerk_xy;
 
+	/* 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
+	 * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller
+	 * is used to set current velocity of the trajectory.
+	 */
+	Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0));
+	if (sticks_expo_xy.length() > FLT_EPSILON) {
+		if (_position_lock_xy_active) {
+			_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); // Start the trajectory at the current velocity setpoint
+			_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
+			_position_setpoint_xy_locked(0) = NAN;
+			_position_setpoint_xy_locked(1) = NAN;
+		}
+		_position_lock_xy_active = false;
+	}
+
 	for (int i = 0; i < 3; ++i) {
 		_smoothing[i].setMaxJerk(jerk[i]);
 		_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
@@ -101,11 +119,27 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 
 	VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
 
+	Vector3f pos_sp_smooth;
+	Vector3f accel_sp_smooth;
+
 	for (int i = 0; i < 3; ++i) {
-		float smoothed_position_setpoint;
+		_smoothing[i].setCurrentPosition(_position(i));
-		_smoothing[i].integrate(_position(i), _vel_sp_smooth(i), smoothed_position_setpoint);
+		_smoothing[i].integrate(accel_sp_smooth(i), _vel_sp_smooth(i), pos_sp_smooth(i));
-		//printf("\nTraj %d\tNew total time = %.3f", i, (double)_smoothing[i].getTotalTime());
+		_velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward
-		_position_setpoint(i) = smoothed_position_setpoint;
-		_velocity_setpoint(i) = _vel_sp_smooth(i);
 	}
+
+	// Check for position lock transition
+	Vector2f accel_setpoint_xy_smooth = Vector2f(&accel_sp_smooth(0));
+	if (vel_xy_sp_smooth.length() < 0.002f &&
+			accel_setpoint_xy_smooth.length() < .2f &&
+			sticks_expo_xy.length() <= FLT_EPSILON) {
+		if (!_position_lock_xy_active) {
+			_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
+			_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
+		}
+		_position_lock_xy_active = true;
+	}
+
+	_position_setpoint(0) = _position_setpoint_xy_locked(0);
+	_position_setpoint(1) = _position_setpoint_xy_locked(1);
 }

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

@@ -65,4 +65,6 @@ private:
 
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
 	matrix::Vector3f _vel_sp_smooth;
+	bool _position_lock_xy_active{false};
+	matrix::Vector2f _position_setpoint_xy_locked;
 };

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp

@@ -214,7 +214,7 @@ void VelocitySmoothing::updateDurations(float T123)
 	_T3 = T3;
 }
 
-void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
+void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
 				  float &pos_setpoint_smooth)
 {
 	/* Integrate the trajectory */
@@ -238,14 +238,8 @@ void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
 	_accel = accel_new;
 	_vel = vel_new;
 
-	/* Lock the position setpoint if the error is bigger than some value */
-	float x_err = pos_new - pos;
-
-	if (fabsf(x_err) <= max_pos_err) {
-		_pos = pos_new;
-	} // else: keep last position
-
 	/* set output variables */
+	accel_setpoint_smooth = _accel;
 	vel_setpoint_smooth = _vel;
 	pos_setpoint_smooth = _pos;
 }

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp

@@ -77,11 +77,11 @@ public:
 
 	/**
 	 * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
-	 * @param pos Current vehicle's position (used for position error clamping)
+	 * @param acc_setpoint_smooth returned smoothed acceleration setpoint
 	 * @param vel_setpoint_smooth returned smoothed velocity setpoint
 	 * @param pos_setpoint_smooth returned smoothed position setpoint
 	 */
-	void integrate(float pos, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
+	void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
 
 	/* Get / Set constraints (constraints can be updated at any time) */
 	float getMaxJerk() const { return _max_jerk; }
@@ -93,6 +93,9 @@ public:
 	float getMaxVel() const { return _max_vel; }
 	void setMaxVel(float max_vel) { _max_vel = max_vel; }
 
+	void setCurrentVelocity(const float vel) { _vel = vel; }
+	void setCurrentPosition(const float pos) { _pos = pos; }
+
 	/**
 	 * Synchronize several trajectories to have the same total time. This is required to generate
 	 * straight lines.

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -768,6 +768,7 @@ MulticopterPositionControl::run()
 			// Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller).
 			publish_local_pos_sp(local_pos_sp);
 
+			_flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller
 
 			// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
 			_att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());