ManualPositionSmoothVel - Split large function into smaller ones, split XY and Z axes

Next step is to move as much as possible to a library in order to
reuse the Z axis in the Altitude FlightTask

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

@@ -48,12 +48,14 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
 	const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
 	const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
 
-	for (int i = 0; i < 3; ++i) {
+	for (int i = 0; i < 2; ++i) {
-		_smoothing[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
+		_smoothing_xy[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
 	}
 
-	_initEkfResetCounters();
+	_smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2));
+
 	_resetPositionLock();
+	_initEkfResetCounters();
 
 	return ret;
 }
@@ -63,13 +65,13 @@ void FlightTaskManualPositionSmoothVel::reActivate()
 	// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
 	// using the generated jerk, reset the z derivatives to zero
 	for (int i = 0; i < 2; ++i) {
-		_smoothing[i].reset(0.f, _velocity(i), _position(i));
+		_smoothing_xy[i].reset(0.f, _velocity(i), _position(i));
 	}
 
-	_smoothing[2].reset(0.f, 0.f, _position(2));
+	_smoothing_z.reset(0.f, 0.f, _position(2));
 
-	_initEkfResetCounters();
 	_resetPositionLock();
+	_initEkfResetCounters();
 }
 
 void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
@@ -97,177 +99,264 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se
 }
 
 void FlightTaskManualPositionSmoothVel::_resetPositionLock()
+{
+	_resetPositionLockXY();
+	_resetPositionLockZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_resetPositionLockXY()
 {
 	_position_lock_xy_active = false;
-	_position_lock_z_active = false;
 	_position_setpoint_xy_locked(0) = NAN;
 	_position_setpoint_xy_locked(1) = NAN;
+}
+
+void FlightTaskManualPositionSmoothVel::_resetPositionLockZ()
+{
+	_position_lock_z_active = false;
 	_position_setpoint_z_locked = NAN;
 }
 
 void FlightTaskManualPositionSmoothVel::_initEkfResetCounters()
+{
+	_initEkfResetCountersXY();
+	_initEkfResetCountersZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_initEkfResetCountersXY()
 {
 	_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
 	_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+}
+
+void FlightTaskManualPositionSmoothVel::_initEkfResetCountersZ()
+{
 	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
 	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
 }
 
+void FlightTaskManualPositionSmoothVel::_updateSetpoints()
+{
+	// Reset trajectories if EKF did a reset
+	_checkEkfResetCounters();
+
+	// Update state
+	_updateTrajectories();
+
+	// Set max accel/vel/jerk
+	// Has to be done before _updateTrajDurations()
+	_updateTrajConstraints();
+
+	// Get yaw setpont, un-smoothed position setpoints
+	// Has to be done before _checkPositionLock()
+	FlightTaskManualPosition::_updateSetpoints();
+	_velocity_target_xy = Vector2f(_velocity_setpoint);
+	_velocity_target_z = _velocity_setpoint(2);
+
+	// Lock or unlock position
+	// Has to be done before _updateTrajDurations()
+	_checkPositionLock();
+
+	// Update durations and sync XY
+	_updateTrajDurations();
+
+	// Fill the jerk, acceleration, velocity and position setpoint vectors
+	_setOutputState();
+}
+
 void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
 {
 	// Check if a reset event has happened.
+	_checkEkfResetCountersXY();
+	_checkEkfResetCountersZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersXY()
+{
 	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
-		_smoothing[0].setCurrentPosition(_position(0));
+		_smoothing_xy[0].setCurrentPosition(_position(0));
-		_smoothing[1].setCurrentPosition(_position(1));
+		_smoothing_xy[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_xy[0].setCurrentVelocity(_velocity(0));
-		_smoothing[1].setCurrentVelocity(_velocity(1));
+		_smoothing_xy[1].setCurrentVelocity(_velocity(1));
 		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
 	}
+}
 
+void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersZ()
+{
 	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
-		_smoothing[2].setCurrentPosition(_position(2));
+		_smoothing_z.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));
+		_smoothing_z.setCurrentVelocity(_velocity(2));
 		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
 	}
 }
 
-void FlightTaskManualPositionSmoothVel::_updateSetpoints()
+void FlightTaskManualPositionSmoothVel::_updateTrajectories()
 {
-	Vector3f pos_sp_smooth;
+	_updateTrajectoriesXY();
+	_updateTrajectoriesZ();
+}
 
-	for (int i = 0; i < 3; ++i) {
+void FlightTaskManualPositionSmoothVel::_updateTrajectoriesXY()
-		_smoothing[i].updateTraj(_deltatime);
+{
+	for (int i = 0; i < 2; ++i) {
+		_smoothing_xy[i].updateTraj(_deltatime);
 
-		_jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
+		_traj_xy.j(i) = _smoothing_xy[i].getCurrentJerk();
-		_acceleration_setpoint(i) = _smoothing[i].getCurrentAcceleration();
+		_traj_xy.a(i) = _smoothing_xy[i].getCurrentAcceleration();
-		_vel_sp_smooth(i) = _smoothing[i].getCurrentVelocity();
+		_traj_xy.v(i) = _smoothing_xy[i].getCurrentVelocity();
-		pos_sp_smooth(i) = _smoothing[i].getCurrentPosition();
+		_traj_xy.x(i) = _smoothing_xy[i].getCurrentPosition();
 	}
+}
 
-	/* Get yaw setpont, un-smoothed position setpoints.*/
+void FlightTaskManualPositionSmoothVel::_updateTrajectoriesZ()
-	FlightTaskManualPosition::_updateSetpoints();
+{
+	_smoothing_z.updateTraj(_deltatime);
 
-	/* Update constraints */
+	_traj_z.j = _smoothing_z.getCurrentJerk();
-	_smoothing[0].setMaxAccel(_param_mpc_acc_hor_max.get());
+	_traj_z.a = _smoothing_z.getCurrentAcceleration();
-	_smoothing[1].setMaxAccel(_param_mpc_acc_hor_max.get());
+	_traj_z.v = _smoothing_z.getCurrentVelocity();
-	_smoothing[0].setMaxVel(_constraints.speed_xy);
+	_traj_z.x = _smoothing_z.getCurrentPosition();
-	_smoothing[1].setMaxVel(_constraints.speed_xy);
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
+{
+	_updateTrajConstraintsXY();
+	_updateTrajConstraintsZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY()
+{
+	for (int i = 0; i < 2; i++) {
+		_smoothing_xy[i].setMaxJerk(_param_mpc_jerk_max.get());
+		_smoothing_xy[i].setMaxAccel(_param_mpc_acc_hor_max.get());
+		_smoothing_xy[i].setMaxVel(_constraints.speed_xy);
+	}
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ()
+{
+	_smoothing_z.setMaxJerk(_param_mpc_jerk_max.get());
 
 	if (_velocity_setpoint(2) < 0.f) { // up
-		_smoothing[2].setMaxAccel(_param_mpc_acc_up_max.get());
+		_smoothing_z.setMaxAccel(_param_mpc_acc_up_max.get());
-		_smoothing[2].setMaxVel(_constraints.speed_up);
+		_smoothing_z.setMaxVel(_constraints.speed_up);
 
 	} else { // down
-		_smoothing[2].setMaxAccel(_param_mpc_acc_down_max.get());
+		_smoothing_z.setMaxAccel(_param_mpc_acc_down_max.get());
-		_smoothing[2].setMaxVel(_constraints.speed_down);
+		_smoothing_z.setMaxVel(_constraints.speed_down);
+	}
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajDurations()
+{
+	_updateTrajDurationsXY();
+	_updateTrajDurationsZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajDurationsXY()
+{
+	for (int i = 0; i < 2; ++i) {
+		_smoothing_xy[i].updateDurations(_velocity_target_xy(i));
 	}
 
-	float jerk[3] = {_param_mpc_jerk_max.get(), _param_mpc_jerk_max.get(), _param_mpc_jerk_max.get()};
+	VelocitySmoothing::timeSynchronization(_smoothing_xy, 2);
+}
 
-	_checkEkfResetCounters();
+void FlightTaskManualPositionSmoothVel::_updateTrajDurationsZ()
+{
+	_smoothing_z.updateDurations(_velocity_target_z);
+}
 
-	/* Check for position unlock
+void FlightTaskManualPositionSmoothVel::_checkPositionLock()
+{
+	/**
 	 * 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
+	 * is continuous. We know that the output of the position loop (part of the velocity setpoint)
-	 * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller
+	 * 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 Vector2f velocity_target_xy = Vector2f(_velocity_setpoint);
+	_checkPositionLockXY();
-	const float velocity_target_z = _velocity_setpoint(2);
+	_checkPositionLockZ();
+}
 
-	if (velocity_target_xy.length() > FLT_EPSILON) {
+void FlightTaskManualPositionSmoothVel::_checkPositionLockXY()
+{
+	if (_traj_xy.v.length() < 0.1f &&
+	    _traj_xy.a.length() < .2f &&
+	    _velocity_target_xy.length() <= FLT_EPSILON) {
+		// Lock position
+		_position_lock_xy_active = true;
+		_position_setpoint_xy_locked = _traj_xy.x;
+
+	} else {
+		// Unlock position
 		if (_position_lock_xy_active) {
-			_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(
+			_smoothing_xy[0].setCurrentVelocity(_velocity_setpoint_feedback(
 					0)); // Start the trajectory at the current velocity setpoint
-			_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
+			_smoothing_xy[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
 			_position_setpoint_xy_locked(0) = NAN;
 			_position_setpoint_xy_locked(1) = NAN;
 		}
 
 		_position_lock_xy_active = false;
+		_smoothing_xy[0].setCurrentPosition(_position(0));
+		_smoothing_xy[1].setCurrentPosition(_position(1));
 	}
+}
 
-	if (fabsf(velocity_target_z) > FLT_EPSILON) {
+void FlightTaskManualPositionSmoothVel::_checkPositionLockZ()
+{
+	if (fabsf(_traj_z.v) < 0.1f &&
+	    fabsf(_traj_z.a) < .2f &&
+	    fabsf(_velocity_target_z) <= FLT_EPSILON) {
+		// Lock position
+		_position_lock_z_active = true;
+		_position_setpoint_z_locked = _traj_z.x;
+
+	} else {
+		// Unlock position
 		if (_position_lock_z_active) {
-			_smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback(
+			_smoothing_z.setCurrentVelocity(_velocity_setpoint_feedback(
 								2)); // Start the trajectory at the current velocity setpoint
 			_position_setpoint_z_locked = NAN;
 		}
 
 		_position_lock_z_active = false;
+		_smoothing_z.setCurrentPosition(_position(2));
 	}
+}
 
-	for (int i = 0; i < 3; ++i) {
+void FlightTaskManualPositionSmoothVel::_setOutputState()
-		_smoothing[i].setMaxJerk(jerk[i]);
+{
-		_smoothing[i].updateDurations(_velocity_setpoint(i));
+	_setOutputStateXY();
-	}
+	_setOutputStateZ();
+}
 
-	VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
+void FlightTaskManualPositionSmoothVel::_setOutputStateXY()
-
+{
-	if (!_position_lock_xy_active) {
-		_smoothing[0].setCurrentPosition(_position(0));
-		_smoothing[1].setCurrentPosition(_position(1));
-	}
-
-	if (!_position_lock_z_active) {
-		_smoothing[2].setCurrentPosition(_position(2));
-	}
-
-	// Check for position lock transition
-	if (Vector2f(_vel_sp_smooth).length() < 0.1f &&
-	    Vector2f(_acceleration_setpoint).length() < .2f &&
-	    velocity_target_xy.length() <= FLT_EPSILON) {
-		_position_lock_xy_active = true;
-	}
-
-	if (fabsf(_vel_sp_smooth(2)) < 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_xy_active) {
-		_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
-		_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
-
-		// 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.
 	for (int i = 0; i < 2; i++) {
-			if (fabsf(_velocity_setpoint(i)) < 1e-3f && fabsf(_acceleration_setpoint(0)) < FLT_EPSILON) {
+		_jerk_setpoint(i) = _traj_xy.j(i);
-				_velocity_setpoint(i) = 0.f;
+		_acceleration_setpoint(i) = _traj_xy.a(i);
-				_acceleration_setpoint(i) = 0.f;
+		_velocity_setpoint(i) = _traj_xy.v(i);
-				_smoothing[i].setCurrentVelocity(0.f);
+		_position_setpoint(i) = _position_setpoint_xy_locked(i);
-				_smoothing[i].setCurrentAcceleration(0.f);
-			}
-		}
 	}
+}
 
-	if (_position_lock_z_active) {
+void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
-		_position_setpoint_z_locked = pos_sp_smooth(2);
+{
-
+	_jerk_setpoint(2) = _traj_z.j;
-		if (fabsf(_velocity_setpoint(2)) < 1e-3f && fabsf(_acceleration_setpoint(2)) < FLT_EPSILON) {
+	_acceleration_setpoint(2) = _traj_z.a;
-			_velocity_setpoint(2) = 0.f;
+	_velocity_setpoint(2) = _traj_z.v;
-			_acceleration_setpoint(2) = 0.f;
-			_smoothing[2].setCurrentVelocity(0.f);
-			_smoothing[2].setCurrentAcceleration(0.f);
-		}
-	}
-
-	_velocity_setpoint = _vel_sp_smooth; // Feedforward
-	_position_setpoint(0) = _position_setpoint_xy_locked(0);
-	_position_setpoint(1) = _position_setpoint_xy_locked(1);
 	_position_setpoint(2) = _position_setpoint_z_locked;
 }

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

@@ -42,6 +42,8 @@
 #include "FlightTaskManualPosition.hpp"
 #include "VelocitySmoothing.hpp"
 
+using matrix::Vector2f;
+
 class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 {
 public:
@@ -61,17 +63,52 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
+
 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 */
 
-	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
+	void _resetPositionLock();
-	matrix::Vector3f _vel_sp_smooth;
+	void _resetPositionLockXY();
+	void _resetPositionLockZ();
+
+	void _initEkfResetCounters();
+	void _initEkfResetCountersXY();
+	void _initEkfResetCountersZ();
+
+	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
+	void _checkEkfResetCountersXY();
+	void _checkEkfResetCountersZ();
+
+	void _updateTrajectories();
+	void _updateTrajectoriesXY();
+	void _updateTrajectoriesZ();
+
+	void _updateTrajConstraints();
+	void _updateTrajConstraintsXY();
+	void _updateTrajConstraintsZ();
+
+	void _updateTrajDurations();
+	void _updateTrajDurationsXY();
+	void _updateTrajDurationsZ();
+
+	void _checkPositionLock();
+	void _checkPositionLockXY();
+	void _checkPositionLockZ();
+
+	void _setOutputState();
+	void _setOutputStateXY();
+	void _setOutputStateZ();
+
+	VelocitySmoothing _smoothing_xy[2]; ///< Smoothing in x and y directions
+	VelocitySmoothing _smoothing_z; ///< Smoothing in z direction
+
+	Vector2f _velocity_target_xy;
+	float _velocity_target_z{0.f};
+
 	bool _position_lock_xy_active{false};
 	bool _position_lock_z_active{false};
-	matrix::Vector2f _position_setpoint_xy_locked;
+
+	Vector2f _position_setpoint_xy_locked;
 	float _position_setpoint_z_locked{NAN};
 
 	/* counters for estimator local position resets */
@@ -81,4 +118,18 @@ private:
 		uint8_t z;
 		uint8_t vz;
 	} _reset_counters{0, 0, 0, 0};
+
+	struct {
+		Vector2f j;
+		Vector2f a;
+		Vector2f v;
+		Vector2f x;
+	} _traj_xy;
+
+	struct {
+		float j;
+		float a;
+		float v;
+		float x;
+	} _traj_z{0.f, 0.f, 0.f, NAN};
 };