Manual Smoth Vel - Override reActivate to not reset Z axis (prevented…

… takeoff)

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

@@ -42,16 +42,42 @@ bool FlightTaskManualPositionSmoothVel::activate()
 {
 	bool ret = FlightTaskManualPosition::activate();
 
+	reset(Axes::XYZ);
+
+	return ret;
+}
+
+void FlightTaskManualPositionSmoothVel::reActivate()
+{
+	reset(Axes::XY);
+}
+
+void FlightTaskManualPositionSmoothVel::reset(Axes axes)
+{
+	int count;
+
+	switch (axes) {
+	case Axes::XY:
+		count = 2;
+		break;
+
+	case Axes::XYZ:
+		count = 3;
+		break;
+
+	default:
+		count = 0;
+		break;
+	}
+
 	// TODO: get current accel
-	for (int i = 0; i < 3; ++i) {
+	for (int i = 0; i < count; ++i) {
 		_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;
 }
 
 void FlightTaskManualPositionSmoothVel::_updateSetpoints()

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

@@ -50,6 +50,7 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
 	bool activate() override;
+	void reActivate() override;
 
 protected:
 
@@ -63,6 +64,8 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 				       )
 private:
 
+	enum class Axes {XY, XYZ};
+	void reset(Axes axes);
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
 	matrix::Vector3f _vel_sp_smooth;
 	bool _position_lock_xy_active{false};