diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 9399e78cc2875b897a282b10f8b60220889e54ce..fc4ffa7027bb99a67dd79d5f0c378161f83c2387 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/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()
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index b224ea65c5549f274ff4d17746f8b617846eddb9..9892f59cdaf5d66b01234c84fd0d23752ef84067 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -50,6 +50,7 @@ public:
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
 	bool activate() override;
+	void reActivate() override;
 
 protected:
 
@@ -63,6 +64,8 @@ protected:
 				       )
 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};