smooth takeoff - Support smooth takeoff triggered by jerk setpoint

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

@@ -49,10 +49,12 @@ bool FlightTaskManualPositionSmoothVel::activate()
 
 void FlightTaskManualPositionSmoothVel::reActivate()
 {
-	reset(Axes::XY);
+	// 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
+	reset(Axes::XYZ, true);
 }
 
-void FlightTaskManualPositionSmoothVel::reset(Axes axes)
+void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero)
 {
 	int count;
 
@@ -75,6 +77,11 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes)
 		_smoothing[i].reset(0.f, _velocity(i), _position(i));
 	}
 
+	// Set the z derivatives to zero
+	if (force_z_zero) {
+		_smoothing[2].reset(0.f, 0.f, _position(2));
+	}
+
 	_position_lock_xy_active = false;
 	_position_lock_z_active = false;
 	_position_setpoint_xy_locked(0) = NAN;

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

@@ -65,7 +65,12 @@ protected:
 private:
 
 	enum class Axes {XY, XYZ};
-	void reset(Axes axes);
+
+	/**
+	 * 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);
+
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
 	matrix::Vector3f _vel_sp_smooth;
 	bool _position_lock_xy_active{false};