diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index cde73591bb7d..1315b39f9fa8 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -44,17 +44,13 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
 
 	// Check if the previous FlightTask provided setpoints
 	checkSetpoints(last_setpoint);
-	const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z);
-	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);
+	const Vector2f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y);
+	const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy);
+	const Vector2f pos_prev(last_setpoint.x, last_setpoint.y);
 
-	for (int i = 0; i < 2; ++i) {
-		_smoothing_xy[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
-	}
-
-	_smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2));
+	_smoothing_xy.reset(accel_prev, vel_prev, pos_prev);
+	_smoothing_z.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);
 
-	_resetPositionLock();
 	_initEkfResetCounters();
 
 	return ret;
@@ -64,13 +60,9 @@ 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_xy[i].reset(0.f, _velocity(i), _position(i));
-	}
-
+	_smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position));
 	_smoothing_z.reset(0.f, 0.f, _position(2));
 
-	_resetPositionLock();
 	_initEkfResetCounters();
 }
 
@@ -98,25 +90,6 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se
 	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
 }
 
-void FlightTaskManualPositionSmoothVel::_resetPositionLock()
-{
-	_resetPositionLockXY();
-	_resetPositionLockZ();
-}
-
-void FlightTaskManualPositionSmoothVel::_resetPositionLockXY()
-{
-	_position_lock_xy_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();
@@ -140,25 +113,16 @@ 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()
+	// Has to be done before _updateTrajectories()
 	_updateTrajConstraints();
+	_updateTrajVelFeedback();
+	_updateTrajCurrentPositionEstimate();
 
-	// Get yaw setpont, un-smoothed position setpoints
-	// Has to be done before _checkPositionLock()
+	// Get yaw setpoint, un-smoothed position setpoints
 	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();
+	_updateTrajectories(_velocity_setpoint);
 
 	// Fill the jerk, acceleration, velocity and position setpoint vectors
 	_setOutputState();
@@ -174,14 +138,12 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
 void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersXY()
 {
 	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
-		_smoothing_xy[0].setCurrentPosition(_position(0));
-		_smoothing_xy[1].setCurrentPosition(_position(1));
+		_smoothing_xy.setCurrentPosition(Vector2f(_position));
 		_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
 	}
 
 	if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
-		_smoothing_xy[0].setCurrentVelocity(_velocity(0));
-		_smoothing_xy[1].setCurrentVelocity(_velocity(1));
+		_smoothing_xy.setCurrentVelocity(Vector2f(_velocity));
 		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
 	}
 }
@@ -199,34 +161,6 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersZ()
 	}
 }
 
-void FlightTaskManualPositionSmoothVel::_updateTrajectories()
-{
-	_updateTrajectoriesXY();
-	_updateTrajectoriesZ();
-}
-
-void FlightTaskManualPositionSmoothVel::_updateTrajectoriesXY()
-{
-	for (int i = 0; i < 2; ++i) {
-		_smoothing_xy[i].updateTraj(_deltatime);
-
-		_traj_xy.j(i) = _smoothing_xy[i].getCurrentJerk();
-		_traj_xy.a(i) = _smoothing_xy[i].getCurrentAcceleration();
-		_traj_xy.v(i) = _smoothing_xy[i].getCurrentVelocity();
-		_traj_xy.x(i) = _smoothing_xy[i].getCurrentPosition();
-	}
-}
-
-void FlightTaskManualPositionSmoothVel::_updateTrajectoriesZ()
-{
-	_smoothing_z.updateTraj(_deltatime);
-
-	_traj_z.j = _smoothing_z.getCurrentJerk();
-	_traj_z.a = _smoothing_z.getCurrentAcceleration();
-	_traj_z.v = _smoothing_z.getCurrentVelocity();
-	_traj_z.x = _smoothing_z.getCurrentPosition();
-}
-
 void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
 {
 	_updateTrajConstraintsXY();
@@ -235,106 +169,38 @@ void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
 
 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);
-	}
+	_smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get());
+	_smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get());
+	_smoothing_xy.setMaxVel(_constraints.speed_xy);
 }
 
 void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ()
 {
 	_smoothing_z.setMaxJerk(_param_mpc_jerk_max.get());
 
-	if (_velocity_setpoint(2) < 0.f) { // up
-		_smoothing_z.setMaxAccel(_param_mpc_acc_up_max.get());
-		_smoothing_z.setMaxVel(_constraints.speed_up);
-
-	} else { // down
-		_smoothing_z.setMaxAccel(_param_mpc_acc_down_max.get());
-		_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));
-	}
+	_smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get());
+	_smoothing_z.setMaxVelUp(_constraints.speed_up);
 
-	VelocitySmoothing::timeSynchronization(_smoothing_xy, 2);
+	_smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get());
+	_smoothing_z.setMaxVelDown(_constraints.speed_down);
 }
 
-void FlightTaskManualPositionSmoothVel::_updateTrajDurationsZ()
+void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback()
 {
-	_smoothing_z.updateDurations(_velocity_target_z);
+	_smoothing_xy.setVelSpFeedback(Vector2f(_velocity_setpoint_feedback));
+	_smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2));
 }
 
-void FlightTaskManualPositionSmoothVel::_checkPositionLock()
+void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate()
 {
-	/**
-	 * 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.
-	 */
-	_checkPositionLockXY();
-	_checkPositionLockZ();
+	_smoothing_xy.setCurrentPositionEstimate(Vector2f(_position));
+	_smoothing_z.setCurrentPositionEstimate(_position(2));
 }
 
-void FlightTaskManualPositionSmoothVel::_checkPositionLockXY()
+void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target)
 {
-	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_xy[0].setCurrentVelocity(_velocity_setpoint_feedback(
-					0)); // Start the trajectory at the current velocity setpoint
-			_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));
-	}
-}
-
-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_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));
-	}
+	_smoothing_xy.update(_deltatime, Vector2f(vel_target));
+	_smoothing_z.update(_deltatime, vel_target(2));
 }
 
 void FlightTaskManualPositionSmoothVel::_setOutputState()
@@ -345,18 +211,16 @@ void FlightTaskManualPositionSmoothVel::_setOutputState()
 
 void FlightTaskManualPositionSmoothVel::_setOutputStateXY()
 {
-	for (int i = 0; i < 2; i++) {
-		_jerk_setpoint(i) = _traj_xy.j(i);
-		_acceleration_setpoint(i) = _traj_xy.a(i);
-		_velocity_setpoint(i) = _traj_xy.v(i);
-		_position_setpoint(i) = _position_setpoint_xy_locked(i);
-	}
+	_jerk_setpoint = _smoothing_xy.getCurrentJerk();
+	_acceleration_setpoint = _smoothing_xy.getCurrentAcceleration();
+	_velocity_setpoint = _smoothing_xy.getCurrentVelocity();
+	_position_setpoint = _smoothing_xy.getCurrentPosition();
 }
 
 void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
 {
-	_jerk_setpoint(2) = _traj_z.j;
-	_acceleration_setpoint(2) = _traj_z.a;
-	_velocity_setpoint(2) = _traj_z.v;
-	_position_setpoint(2) = _position_setpoint_z_locked;
+	_jerk_setpoint(2) = _smoothing_z.getCurrentJerk();
+	_acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration();
+	_velocity_setpoint(2) = _smoothing_z.getCurrentVelocity();
+	_position_setpoint(2) = _smoothing_z.getCurrentPosition();
 }
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index cc5814fc9eca..b6399645c5e8 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,9 +40,11 @@
 #pragma once
 
 #include "FlightTaskManualPosition.hpp"
-#include "VelocitySmoothing.hpp"
+#include "ManualVelocitySmoothingXY.hpp"
+#include "ManualVelocitySmoothingZ.hpp"
 
 using matrix::Vector2f;
+using matrix::Vector3f;
 
 class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 {
@@ -67,10 +69,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 private:
 	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
 
-	void _resetPositionLock();
-	void _resetPositionLockXY();
-	void _resetPositionLockZ();
-
 	void _initEkfResetCounters();
 	void _initEkfResetCountersXY();
 	void _initEkfResetCountersZ();
@@ -79,37 +77,21 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 	void _checkEkfResetCountersXY();
 	void _checkEkfResetCountersZ();
 
-	void _updateTrajectories();
-	void _updateTrajectoriesXY();
-	void _updateTrajectoriesZ();
-
 	void _updateTrajConstraints();
 	void _updateTrajConstraintsXY();
 	void _updateTrajConstraintsZ();
 
-	void _updateTrajDurations();
-	void _updateTrajDurationsXY();
-	void _updateTrajDurationsZ();
+	void _updateTrajVelFeedback();
+	void _updateTrajCurrentPositionEstimate();
 
-	void _checkPositionLock();
-	void _checkPositionLockXY();
-	void _checkPositionLockZ();
+	void _updateTrajectories(Vector3f vel_target);
 
 	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};
-
-	Vector2f _position_setpoint_xy_locked;
-	float _position_setpoint_z_locked{NAN};
+	ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
+	ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
 
 	/* counters for estimator local position resets */
 	struct {
@@ -118,18 +100,4 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 		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};
 };
diff --git a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
index 14cf505b344e..5cbaf2cfbe42 100644
--- a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
+++ b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
@@ -37,6 +37,8 @@ px4_add_library(FlightTaskUtility
 	ObstacleAvoidance.cpp
 	StraightLine.cpp
 	VelocitySmoothing.cpp
+	ManualVelocitySmoothingXY.cpp
+	ManualVelocitySmoothingZ.cpp
 )
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis)
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
new file mode 100644
index 000000000000..ef1a0554d24c
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ManualVelocitySmoothingXY.hpp"
+
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+void ManualVelocitySmoothingXY::reset(Vector2f accel, Vector2f vel, Vector2f pos)
+{
+	for (int i = 0; i < 2; i++) {
+		_smoothing[i].reset(accel(i), vel(i), pos(i));
+	}
+
+	resetPositionLock();
+}
+void ManualVelocitySmoothingXY::resetPositionLock()
+{
+	_position_lock_active = false;
+	_position_setpoint_locked(0) = NAN;
+	_position_setpoint_locked(1) = NAN;
+}
+
+void ManualVelocitySmoothingXY::update(float dt, Vector2f velocity_target)
+{
+	// Update state
+	updateTrajectories(dt);
+
+	// Lock or unlock position
+	// Has to be done before _updateTrajDurations()
+	checkPositionLock(velocity_target);
+
+	// Update durations and sync XY
+	updateTrajDurations(velocity_target);
+}
+
+void ManualVelocitySmoothingXY::updateTrajectories(float dt)
+{
+	for (int i = 0; i < 2; ++i) {
+		_smoothing[i].updateTraj(dt);
+
+		_state.j(i) = _smoothing[i].getCurrentJerk();
+		_state.a(i) = _smoothing[i].getCurrentAcceleration();
+		_state.v(i) = _smoothing[i].getCurrentVelocity();
+		_state.x(i) = _smoothing[i].getCurrentPosition();
+	}
+}
+
+void ManualVelocitySmoothingXY::updateTrajDurations(Vector2f velocity_target)
+{
+	for (int i = 0; i < 2; ++i) {
+		_smoothing[i].updateDurations(velocity_target(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_smoothing, 2);
+}
+
+void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target)
+{
+	/**
+	 * 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.
+	 */
+	if (_state.v.length() < 0.1f &&
+	    _state.a.length() < .2f &&
+	    velocity_target.length() <= FLT_EPSILON) {
+		// Lock position
+		_position_lock_active = true;
+		_position_setpoint_locked = _state.x;
+
+	} else {
+		// Unlock position
+		if (_position_lock_active) {
+			 // Start the trajectory at the current velocity setpoint
+			_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
+			_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
+			_position_setpoint_locked(0) = NAN;
+			_position_setpoint_locked(1) = NAN;
+		}
+
+		_position_lock_active = false;
+		_smoothing[0].setCurrentPosition(_position_estimate(0));
+		_smoothing[1].setCurrentPosition(_position_estimate(1));
+	}
+}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp
new file mode 100644
index 000000000000..363d514cd62d
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ManualVelocitySmoothingXY.hpp
+ *
+ */
+
+#pragma once
+
+#include "VelocitySmoothing.hpp"
+
+#include <matrix/matrix/math.hpp>
+
+using matrix::Vector2f;
+
+class ManualVelocitySmoothingXY
+{
+public:
+	ManualVelocitySmoothingXY() = default;
+
+	virtual ~ManualVelocitySmoothingXY() = default;
+
+	void reset(Vector2f accel, Vector2f vel, Vector2f pos);
+	void update(float dt, Vector2f velocity_target);
+
+	void setVelSpFeedback(const Vector2f fb) { _velocity_setpoint_feedback = fb; }
+
+	void setMaxJerk(const float max_jerk) {
+		_smoothing[0].setMaxJerk(max_jerk);
+		_smoothing[1].setMaxJerk(max_jerk);
+	}
+	void setMaxAccel(const float max_accel) {
+		_smoothing[0].setMaxAccel(max_accel);
+		_smoothing[1].setMaxAccel(max_accel);
+	}
+	void setMaxVel(const float max_vel) {
+		_smoothing[0].setMaxVel(max_vel);
+		_smoothing[1].setMaxVel(max_vel);
+	}
+
+	Vector2f getCurrentJerk() const { return _state.j; }
+	Vector2f getCurrentAcceleration() const { return _state.a; }
+	void setCurrentVelocity(const Vector2f vel) {
+		_state.v = vel;
+		_smoothing[0].setCurrentVelocity(vel(0));
+		_smoothing[1].setCurrentVelocity(vel(1));
+	}
+	Vector2f getCurrentVelocity() const { return _state.v; }
+	void setCurrentPosition(const Vector2f pos) {
+		_state.x = pos;
+		_smoothing[0].setCurrentPosition(pos(0));
+		_smoothing[1].setCurrentPosition(pos(1));
+	}
+	Vector2f getCurrentPosition() const { return _position_setpoint_locked; }
+	void setCurrentPositionEstimate(Vector2f pos) { _position_estimate = pos; }
+
+private:
+	void resetPositionLock();
+	void updateTrajectories(float dt);
+	void updateTrajConstraints();
+	void checkPositionLock(Vector2f velocity_target);
+	void updateTrajDurations(Vector2f velocity_target);
+
+	VelocitySmoothing _smoothing[2]; ///< Smoothing in x and y directions
+
+	bool _position_lock_active{false};
+
+	Vector2f _position_setpoint_locked;
+
+	Vector2f _velocity_setpoint_feedback;
+	Vector2f _position_estimate;
+
+	struct {
+		Vector2f j;
+		Vector2f a;
+		Vector2f v;
+		Vector2f x;
+	} _state;
+};
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
new file mode 100644
index 000000000000..187bcdd724db
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ManualVelocitySmoothingZ.hpp"
+
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos)
+{
+	_smoothing.reset(accel, vel, pos);
+
+	resetPositionLock();
+}
+void ManualVelocitySmoothingZ::resetPositionLock()
+{
+	_position_lock_active = false;
+	_position_setpoint_locked = NAN;
+}
+
+void ManualVelocitySmoothingZ::update(float dt, float velocity_target)
+{
+	// Update state
+	updateTrajectories(dt);
+
+	// Set max accel/vel/jerk
+	// Has to be done before _updateTrajDurations()
+	updateTrajConstraints(velocity_target);
+
+	// Lock or unlock position
+	// Has to be done before _updateTrajDurations()
+	checkPositionLock(velocity_target);
+
+	// Update durations
+	_smoothing.updateDurations(velocity_target);
+}
+
+void ManualVelocitySmoothingZ::updateTrajectories(float dt)
+{
+	_smoothing.updateTraj(dt);
+
+	_state.j = _smoothing.getCurrentJerk();
+	_state.a = _smoothing.getCurrentAcceleration();
+	_state.v = _smoothing.getCurrentVelocity();
+	_state.x = _smoothing.getCurrentPosition();
+}
+
+void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target)
+{
+	if (velocity_target < 0.f) { // up
+		_smoothing.setMaxAccel(_max_accel_up);
+		_smoothing.setMaxVel(_max_vel_up);
+
+	} else { // down
+		_smoothing.setMaxAccel(_max_accel_down);
+		_smoothing.setMaxVel(_max_vel_down);
+	}
+}
+
+void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target)
+{
+	/**
+	 * 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.
+	 */
+	if (fabsf(_state.v) < 0.1f &&
+	    fabsf(_state.a) < .2f &&
+	    fabsf(velocity_target) <= FLT_EPSILON) {
+		// Lock position
+		_position_lock_active = true;
+		_position_setpoint_locked = _state.x;
+
+	} else {
+		// Unlock position
+		if (_position_lock_active) {
+			 // Start the trajectory at the current velocity setpoint
+			_smoothing.setCurrentVelocity(_velocity_setpoint_feedback);
+			_position_setpoint_locked = NAN;
+		}
+
+		_position_lock_active = false;
+		_smoothing.setCurrentPosition(_position_estimate);
+	}
+}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp
new file mode 100644
index 000000000000..7d96803ee743
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ManualVelocitySmoothingZ.hpp
+ *
+ */
+
+#pragma once
+
+#include "VelocitySmoothing.hpp"
+
+class ManualVelocitySmoothingZ
+{
+public:
+	ManualVelocitySmoothingZ() = default;
+
+	virtual ~ManualVelocitySmoothingZ() = default;
+
+	void reset(float accel, float vel, float pos);
+	void update(float dt, float velocity_target);
+
+	void setVelSpFeedback(const float fb) { _velocity_setpoint_feedback = fb; }
+
+	void setMaxJerk(const float max_jerk) {
+		_smoothing.setMaxJerk(max_jerk);
+	}
+	void setMaxAccelUp(const float max_accel_up) {
+		_max_accel_up = max_accel_up;
+	}
+	void setMaxAccelDown(const float max_accel_down) {
+		_max_accel_down = max_accel_down;
+	}
+	void setMaxVelUp(const float max_vel_up) {
+		_max_vel_up = max_vel_up;
+	}
+	void setMaxVelDown(const float max_vel_down) {
+		_max_vel_down = max_vel_down;
+	}
+
+	float getCurrentJerk() const { return _state.j; }
+	float getCurrentAcceleration() const { return _state.a; }
+	void setCurrentVelocity(const float vel) {
+		_state.v = vel;
+		_smoothing.setCurrentVelocity(vel);
+	}
+	float getCurrentVelocity() const { return _state.v; }
+	void setCurrentPosition(const float pos) {
+		_state.x = pos;
+		_smoothing.setCurrentPosition(pos);
+	}
+	float getCurrentPosition() const { return _position_setpoint_locked; }
+	void setCurrentPositionEstimate(float pos) { _position_estimate = pos; }
+
+private:
+	void resetPositionLock();
+	void updateTrajectories(float dt);
+	void updateTrajConstraints(float vel_target);
+	void checkPositionLock(float velocity_target);
+	void updateTrajDurations(float velocity_target);
+
+	VelocitySmoothing _smoothing; ///< Smoothing in z direction
+
+	bool _position_lock_active{false};
+
+	float _position_setpoint_locked;
+
+	float _velocity_setpoint_feedback;
+	float _position_estimate;
+
+	struct {
+		float j;
+		float a;
+		float v;
+		float x;
+	} _state;
+
+	float _max_accel_up{0.f};
+	float _max_accel_down{0.f};
+	float _max_vel_up{0.f};
+	float _max_vel_down{0.f};
+};