AltitudeSmoothVel - Use ManualVelocitySmoothingZ class instead of re-implementing the logic

src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -51,7 +51,6 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
 	_smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);
 
 	_initEkfResetCounters();
-	_resetPositionLock();
 
 	return ret;
 }
@@ -63,7 +62,6 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
 	_smoothing.reset(0.f, 0.f, _position(2));
 
 	_initEkfResetCounters();
-	_resetPositionLock();
 }
 
 void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
@@ -78,19 +76,34 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se
 	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
 }
 
-void FlightTaskManualAltitudeSmoothVel::_resetPositionLock()
-{
-	// Always start unlocked
-	_position_lock_z_active = false;
-	_position_setpoint_z_locked = NAN;
-}
-
 void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters()
 {
 	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
 	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
 }
 
+void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
+{
+	// Reset trajectories if EKF did a reset
+	_checkEkfResetCounters();
+
+	// Set max accel/vel/jerk
+	// Has to be done before _updateTrajectories()
+	_updateTrajConstraints();
+
+	_smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2));
+	_smoothing.setCurrentPositionEstimate(_position(2));
+
+
+	// Get yaw setpoint, un-smoothed position setpoints
+	FlightTaskManualAltitude::_updateSetpoints();
+
+	_smoothing.update(_deltatime, _velocity_setpoint(2));
+
+	// Fill the jerk, acceleration, velocity and position setpoint vectors
+	_setOutputState();
+}
+
 void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
 {
 	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
@@ -104,85 +117,22 @@ void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
 	}
 }
 
-void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
+void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
 {
-	float pos_sp_smooth;
+	_smoothing.setMaxJerk(_param_mpc_jerk_max.get());
 
-	_smoothing.updateTraj(_deltatime);
+	_smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get());
+	_smoothing.setMaxVelUp(_constraints.speed_up);
 
+	_smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get());
+	_smoothing.setMaxVelDown(_constraints.speed_down);
+}
+
+
+void FlightTaskManualAltitudeSmoothVel::_setOutputState()
+{
 	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
-	_vel_sp_smooth = _smoothing.getCurrentVelocity();
+	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
-	pos_sp_smooth = _smoothing.getCurrentPosition();
+	_position_setpoint(2) = _smoothing.getCurrentPosition();
-
-	/* Get yaw setpont, un-smoothed position setpoints.*/
-	FlightTaskManualAltitude::_updateSetpoints();
-
-	/* Update constraints */
-	if (_velocity_setpoint(2) < 0.f) { // up
-		_smoothing.setMaxAccel(_param_mpc_acc_up_max.get());
-		_smoothing.setMaxVel(_constraints.speed_up);
-
-	} else { // down
-		_smoothing.setMaxAccel(_param_mpc_acc_down_max.get());
-		_smoothing.setMaxVel(_constraints.speed_down);
-	}
-
-	float jerk = _param_mpc_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
-	 * 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.
-	 */
-
-	const float velocity_target_z = _velocity_setpoint(2);
-
-	if (fabsf(velocity_target_z) > FLT_EPSILON) {
-		if (_position_lock_z_active) {
-			_smoothing.setCurrentVelocity(_velocity_setpoint_feedback(
-							      2)); // Start the trajectory at the current velocity setpoint
-			_position_setpoint_z_locked = NAN;
-		}
-
-		_position_lock_z_active = false;
-	}
-
-	_smoothing.setMaxJerk(jerk);
-	_smoothing.updateDurations(_velocity_setpoint(2));
-
-	if (!_position_lock_z_active) {
-		_smoothing.setCurrentPosition(_position(2));
-	}
-
-	_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward
-
-	if (fabsf(_vel_sp_smooth) < 0.1f &&
-	    fabsf(_acceleration_setpoint(2)) < .2f &&
-	    fabsf(velocity_target_z) <= FLT_EPSILON) {
-		_position_lock_z_active = true;
-	}
-
-	// Set valid position setpoint while in position lock.
-	// When the position lock condition above is false, it does not
-	// mean that the unlock condition is true. This is why
-	// we are checking the lock flag here.
-	if (_position_lock_z_active) {
-		_position_setpoint_z_locked = pos_sp_smooth;
-
-		// If the velocity setpoint is smaller than 1mm/s and that the acceleration is 0, force the setpoints
-		// to zero. This is required because the generated velocity is never exactly zero and if the drone hovers
-		// for a long period of time, thr drift of the position setpoint will be noticeable.
-		if (fabsf(_velocity_setpoint(2)) < 1e-3f && fabsf(_acceleration_setpoint(2)) < FLT_EPSILON) {
-			_velocity_setpoint(2) = 0.f;
-			_acceleration_setpoint(2) = 0.f;
-			_smoothing.setCurrentVelocity(0.f);
-			_smoothing.setCurrentAcceleration(0.f);
-		}
-	}
-
-	_position_setpoint(2) = _position_setpoint_z_locked;
 }

src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -40,7 +40,7 @@
 #pragma once
 
 #include "FlightTaskManualAltitude.hpp"
-#include "VelocitySmoothing.hpp"
+#include "ManualVelocitySmoothingZ.hpp"
 
 class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 {
@@ -64,14 +64,13 @@ protected:
 private:
 
 	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-	void _resetPositionLock();
+
 	void _initEkfResetCounters();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
+	void _updateTrajConstraints();
+	void _setOutputState();
 
-	VelocitySmoothing _smoothing; ///< Smoothing in z direction
+	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
-	float _vel_sp_smooth;
-	bool _position_lock_z_active{false};
-	float _position_setpoint_z_locked{NAN};
 
 	/* counters for estimator local position resets */
 	struct {