diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt
index 06a1a6dec65dce31da77f98031d2aa9cd7b99be5..f6ab6cac475174dbeb13b5234e6a7b2fe4b8fbcc 100644
--- a/src/lib/FlightTasks/CMakeLists.txt
+++ b/src/lib/FlightTasks/CMakeLists.txt
@@ -56,6 +56,7 @@ list(APPEND flight_tasks_all
 	ManualAltitudeSmooth
 	ManualPosition
 	ManualPositionSmooth
+	ManualPositionSmoothVel
 	Sport
 	AutoLine
 	AutoFollowMe
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8d4489fa3e55321a6e8844e02b61035f8f7d830d
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/CMakeLists.txt
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 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.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualPositionSmoothVel
+	FlightTaskManualPositionSmoothVel.cpp
+)
+
+target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility)
+target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06403a254a0d751601913997a2d863952cc32479
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 "FlightTaskManualPositionSmoothVel.hpp"
+
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskManualPositionSmoothVel::activate()
+{
+	bool ret = FlightTaskManualPosition::activate();
+
+	// TODO: get current accel
+	for (int i = 0; i < 3; ++i) {
+		_smoothing[i].reset(0.f, _velocity(i), _position(i));
+	}
+
+	return ret;
+}
+
+void FlightTaskManualPositionSmoothVel::_updateSetpoints()
+{
+	/* Get yaw setpont, un-smoothed position setpoints.*/
+	FlightTaskManualPosition::_updateSetpoints();
+
+	/* Update constraints */
+	_smoothing[0].setMaxAccel(MPC_ACC_HOR_MAX.get());
+	_smoothing[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
+	_smoothing[0].setMaxVel(_constraints.speed_xy);
+	_smoothing[1].setMaxVel(_constraints.speed_xy);
+
+	Vector2f vel_xy = Vector2f(&_velocity(0));
+	float jerk = _jerk_max.get();
+
+	if (_jerk_min.get() > _jerk_max.get()) {
+		_jerk_min.set(0.f);
+	}
+
+	if (_jerk_min.get() > FLT_EPSILON) {
+		// interpolate between min and max jerk
+		jerk = math::min(_jerk_min.get() + (_jerk_max.get() - _jerk_min.get()) * vel_xy.length() / _constraints.speed_xy,
+				 _jerk_max.get());
+	}
+
+	for (int i = 0; i < 2; ++i) {
+		_smoothing[i].setMaxJerk(jerk);
+
+		float smoothed_velocity_setpoint, smoothed_position_setpoint;
+		_smoothing[i].update(_deltatime, _position(i), _velocity_setpoint(i),
+				     smoothed_velocity_setpoint, smoothed_position_setpoint);
+		_position_setpoint(i) = smoothed_position_setpoint;
+		_velocity_setpoint(i) = smoothed_velocity_setpoint;
+	}
+
+	// TODO: z
+}
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fac63e9564317250de63f8d22f1d1c00287c84f0
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 FlightTaskManualPositionSmoothVel.hpp
+ *
+ * Flight task for smooth manual controlled position.
+ */
+
+#pragma once
+
+#include "FlightTaskManualPosition.hpp"
+#include "VelocitySmoothing.hpp"
+
+class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
+{
+public:
+	FlightTaskManualPositionSmoothVel() = default;
+
+	virtual ~FlightTaskManualPositionSmoothVel() = default;
+
+	bool activate() override;
+
+protected:
+
+	virtual void _updateSetpoints() override;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
+					(ParamFloat<px4::params::MPC_JERK_MIN>) _jerk_min, /**< Minimum jerk (velocity-based if > 0) */
+					(ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max
+				       )
+private:
+
+	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
+};
diff --git a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
index 9c7511309a4722039c0816a60037aa80ac48476d..4e8164182d813834cd9b6d7aa6584cdf2bce1327 100644
--- a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
+++ b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_library(FlightTaskUtility
 	ManualSmoothingZ.cpp
 	ManualSmoothingXY.cpp
 	StraightLine.cpp
+	VelocitySmoothing.cpp
 )
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask)
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..62b9154419cce938c3c2905801c7e4267196278a
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 "VelocitySmoothing.hpp"
+
+#include <cstdio>
+#include <float.h>
+#include <math.h>
+
+#include <mathlib/mathlib.h>
+
+VelocitySmoothing::VelocitySmoothing(float initial_accel, float initial_vel, float initial_pos)
+{
+	reset(initial_accel, initial_vel, initial_pos);
+}
+
+void VelocitySmoothing::reset(float accel, float vel, float pos)
+{
+	_jerk = 0.f;
+	_accel = accel;
+	_vel = vel;
+	_pos = pos;
+}
+
+float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk)
+{
+	float b = 2.f * accel_prev / max_jerk;
+	float c = vel_prev / max_jerk + accel_prev * accel_prev / (2.f * max_jerk * max_jerk) - vel_setpoint / max_jerk;
+	float delta = b * b - 4.f * c;
+
+	if (delta < 0.f) {
+		return 0.f;
+	}
+
+	float sqrt_delta = sqrtf(delta);
+	float T1_plus = (-b + sqrt_delta) * 0.5f;
+	float T1_minus = (-b - sqrt_delta) * 0.5f;
+
+	float T1 = math::max(math::max(T1_plus, T1_minus), 0.f);
+
+//	if (T1 < FLT_EPSILON) {
+//		// debug
+//		printf("No feasible solution found, set T1 = 0\n");
+//		printf("T1_plus = %.3f T1_minus = %.3f\n", (double) T1_plus, (double) T1_minus);
+//	}
+
+	/* Check maximum acceleration, saturate and recompute T1 if needed */
+	float a1 = accel_prev + max_jerk * T1;
+
+	if (a1 > _max_accel) {
+		T1 = (_max_accel - accel_prev) / max_jerk;
+
+	} else if (a1 < -_max_accel) {
+		T1 = (-_max_accel - accel_prev) / max_jerk;
+	}
+
+	return math::max(T1, 0.f);
+}
+
+
+float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint,
+				   float max_jerk)
+{
+	float f = accel_prev * T1 + max_jerk * T1 * T1 * 0.5f + vel_prev + accel_prev * T3 + max_jerk * T1 * T3
+		  - max_jerk * T3 * T3 * 0.5f;
+	float T2 = (vel_setpoint - f) / (accel_prev + max_jerk * T1);
+	return math::max(T2, 0.f);
+}
+
+float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
+{
+	float T3 = accel_prev / max_jerk + T1;
+	return math::max(T3, 0.f);
+}
+
+void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev, float dt,
+				   float &accel_out, float &vel_out, float &pos_out)
+{
+	accel_out = jerk * dt + accel_prev;
+
+	if (accel_out > _max_accel) {
+		accel_out = _max_accel;
+
+	} else if (accel_out < -_max_accel) {
+		accel_out = -_max_accel;
+	}
+
+	vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
+
+	if (vel_out > _max_vel) {
+		vel_out = _max_vel;
+
+	} else if (vel_out < -_max_vel) {
+		vel_out = -_max_vel;
+	}
+
+	pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos;
+}
+
+void VelocitySmoothing::update(float dt, float pos, float vel_setpoint, float &vel_setpoint_smooth,
+			       float &pos_setpoint_smooth)
+{
+	/* Depending of the direction, start accelerating positively or negatively */
+	const float max_jerk = (vel_setpoint - _vel > 0.f) ? _max_jerk : -_max_jerk;
+
+	// compute increasing acceleration time
+	float T1 = computeT1(_accel, _vel, vel_setpoint, max_jerk);
+
+	/* Force T1/2/3 to zero if smaller than an epoch to avoid chattering */
+	if (T1 < dt) {
+		T1 = 0.f;
+	}
+
+	// compute decreasing acceleration time
+	float T3 = computeT3(T1, _accel, max_jerk);
+
+	if (T3 < dt) {
+		T3 = 0.f;
+	}
+
+	// compute constant acceleration time
+	float T2 = computeT2(T1, T3, _accel, _vel, vel_setpoint, max_jerk);
+
+	if (T2 < dt) {
+		T2 = 0.f;
+	}
+
+	/* Integrate the trajectory */
+	float accel_new, vel_new, pos_new;
+	integrateT(_jerk, _accel, _vel, _pos, dt, accel_new, vel_new, pos_new);
+
+	/* Apply correct jerk (min, max or zero) */
+	if (T1 > 0.f) {
+		_jerk = max_jerk;
+
+	} else if (T2 > 0.f) {
+		_jerk = 0.f;
+
+	} else if (T3 > 0.f) {
+		_jerk = -max_jerk;
+
+	} else {
+		_jerk = 0.f;
+	}
+
+	_accel = accel_new;
+	_vel = vel_new;
+
+	/* Lock the position setpoint if the error is bigger than some value */
+	float x_err = pos_new - pos;
+
+	if (fabsf(x_err) <= max_pos_err) {
+		_pos = pos_new;
+	} // else: keep last position
+
+	/* set output variables */
+	vel_setpoint_smooth = _vel;
+	pos_setpoint_smooth = _pos;
+}
+
+
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e5fd7c6d4a5227806d361d0f750b716f8cb4b4f1
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+/**
+ * @class VelocitySmoothing
+ *
+ * TODO: document the algorithm
+ */
+class VelocitySmoothing
+{
+public:
+	VelocitySmoothing(float initial_accel = 0.f, float initial_vel = 0.f, float initial_pos = 0.f);
+	~VelocitySmoothing() = default;
+
+	/**
+	 * Reset the state.
+	 * @param accel Current acceleration
+	 * @param vel Current velocity
+	 * @param pos Current position
+	 */
+	void reset(float accel, float vel, float pos);
+
+	/**
+	 * Update the setpoint and get the smoothed setpoints. This should be called on every cycle.
+	 * @param dt delta time between last update() call and now [s]
+	 * @param pos Current vehicle's position
+	 * @param vel_setpoint velocity setpoint input
+	 * @param vel_setpoint_smooth returned smoothed velocity setpoint
+	 * @param pos_setpoint_smooth returned smoothed position setpoint
+	 */
+	void update(float dt, float pos, float vel_setpoint, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
+
+
+	/* Get / Set constraints (constraints can be updated at any time) */
+
+	float getMaxJerk() const { return _max_jerk; }
+	void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; }
+
+	float getMaxAccel() const { return _max_accel; }
+	void setMaxAccel(float max_accel) { _max_accel = max_accel; }
+
+	float getMaxVel() const { return _max_vel; }
+	void setMaxVel(float max_vel) { _max_vel = max_vel; }
+
+private:
+	/**
+	 * Compute increasing acceleration time
+	 */
+	inline float computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
+	/**
+	 * Compute constant acceleration time
+	 */
+	inline float computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
+	/**
+	 * Compute decreasing acceleration time
+	 */
+	inline float computeT3(float T1, float accel_prev, float max_jerk);
+
+	/**
+	 * Integrate the jerk, acceleration and velocity to get the new setpoints and states.
+	 */
+	inline void integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev, float dt,
+			       float &accel_out, float &vel_out, float &pos_out);
+
+	/* Constraints */
+	float _max_jerk = 22.f;
+	float _max_accel = 8.f;
+	float _max_vel = 6.f;
+
+	/* State (previous setpoints) */
+	float _jerk;
+	float _accel;
+	float _vel;
+	float _pos;
+
+	static constexpr float max_pos_err = 1.f; ///< maximum position error (if above, the position setpoint is locked)
+};
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 527e99728aae9098f314cb6b50470e0a1c2922e0..bc331fa25ecc316bf1c821edeb864c0edd7e26de 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -917,6 +917,10 @@ MulticopterPositionControl::start_flight_task()
 			error =  _flight_tasks.switchTask(FlightTaskIndex::Sport);
 			break;
 
+		case 3:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
+			break;
+
 		default:
 			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
 			break;
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 7ecdd234472fe62bbeb091fb9a90a2c7c03f90b0..4f23b1cfa26a2f39d221f4eba2420f29f200d35e 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -633,12 +633,13 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f);
  * 	 and jerk limits.
  * 2 Sport mode that is the same Default position control but with velocity limits set to
  * 	 the maximum allowed speeds (MPC_XY_VEL_MAX)
+ * 3 Smooth position control with maximum acceleration and jerk limits (different algorithm
+ *   than 1).
  *
- * @min 0
- * @max 2
  * @value 0 Default position control
  * @value 1 Smooth position control
  * @value 2 Sport position control
+ * @value 3 Smooth position control (Velocity)
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_INT32(MPC_POS_MODE, 1);