lib: name folders consistently

src/lib/flight_tasks/tasks/Utility/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   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(FlightTaskUtility
+	ManualSmoothingZ.cpp
+	ManualSmoothingXY.cpp
+	ObstacleAvoidance.cpp
+	StraightLine.cpp
+	VelocitySmoothing.cpp
+	ManualVelocitySmoothingXY.cpp
+	ManualVelocitySmoothingZ.cpp
+)
+
+target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis)
+target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility)
+px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS FlightTaskUtility)
+px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS FlightTaskUtility)

src/lib/flight_tasks/tasks/Utility/Makefile

@@ -0,0 +1,12 @@
+
+.PHONY: all tests clean
+all: test_velocity_smoothing
+
+test_velocity_smoothing: test_velocity_smoothing.cpp VelocitySmoothing.cpp
+	@g++ $^ -std=c++11 -I ../../../ -o $@
+
+tests: test_velocity_smoothing
+	@echo "Test velocity smoothing"
+
+clean:
+	@rm test_velocity_smoothing

src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.cpp

@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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 ManualSmoothingXY.cpp
+ */
+
+#include "ManualSmoothingXY.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel) :
+	ModuleParams(parent), 	_vel_sp_prev(vel)
+
+{
+	_acc_state_dependent = _param_mpc_acc_hor.get();
+	_jerk_state_dependent = _param_mpc_jerk_max.get();
+}
+
+void
+ManualSmoothingXY::smoothVelocity(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
+				  const float &yawrate_sp, const float dt)
+{
+	_updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
+	_velocitySlewRate(vel_sp, dt);
+	_vel_sp_prev = vel_sp;
+}
+
+void
+ManualSmoothingXY::_updateAcceleration(Vector2f &vel_sp, const Vector2f &vel,  const float &yaw,
+				       const float &yawrate_sp, const float dt)
+{
+	Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp);
+
+	// Adapt acceleration and jerk based on current state and
+	// intention. Jerk is only used for braking.
+	_setStateAcceleration(vel_sp, vel, intention, dt);
+}
+
+ManualSmoothingXY::Intention
+ManualSmoothingXY::_getIntention(const Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
+				 const float &yawrate_sp)
+{
+
+	if (vel_sp.length() > FLT_EPSILON) {
+		// Distinguish between acceleration, deceleration and direction change
+
+		// Check if stick direction and current velocity are within 135.
+		// If current setpoint and velocity are more than 135 apart, we assume
+		// that the user demanded a direction change.
+		// The detection is done in body frame.
+		// Rotate velocity setpoint into body frame
+		Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
+		Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
+
+		if (vel_sp_heading.length() > FLT_EPSILON) {
+			vel_sp_heading.normalize();
+		}
+
+		if (vel_heading.length() > FLT_EPSILON) {
+			vel_heading.normalize();
+		}
+
+		const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f;
+
+		// In almost all cases we want to use acceleration.
+		// Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
+		// Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand.
+		bool yawspeed_demand =  fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
+		bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_max) && !yawspeed_demand
+					&& (vel.length() > 2.0f);
+		bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
+
+		if (direction_change) {
+			return Intention::direction_change;
+
+		} else if (deceleration) {
+			return Intention::deceleration;
+
+		} else {
+			return Intention::acceleration;
+		}
+	}
+
+	return Intention::brake; //default is brake
+}
+
+void
+ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f &vel,
+		const Intention &intention, const float dt)
+{
+	switch (intention) {
+	case Intention::brake: {
+
+			if (_intention == Intention::direction_change) {
+				// Vehicle switched from direction change to brake.
+				// Don't do any slwerate and just stop.
+				_jerk_state_dependent = 1e6f;
+				_vel_sp_prev = vel;
+
+			} else if (intention != _intention) {
+				// start the brake with lowest acceleration which
+				// makes stopping smoother
+				_acc_state_dependent = _param_mpc_dec_hor_slow.get();
+
+				// Adjust jerk based on current velocity. This ensures
+				// that the vehicle will stop much quicker at large speed but
+				// very slow at low speed.
+				_jerk_state_dependent = 1e6f; // default
+
+				if (_param_mpc_jerk_max.get() > _param_mpc_jerk_min.get() && _param_mpc_jerk_min.get() > FLT_EPSILON) {
+
+					_jerk_state_dependent = math::min((_param_mpc_jerk_max.get() - _param_mpc_jerk_min.get())
+									  / _vel_max * vel.length() + _param_mpc_jerk_min.get(), _param_mpc_jerk_max.get());
+				}
+
+				// User wants to brake smoothly but does NOT want the vehicle to
+				// continue to fly in the opposite direction. slewrate has to be reset
+				// by setting previous velocity setpoint to current velocity. */
+				_vel_sp_prev = vel;
+			}
+
+			/* limit jerk when braking to zero */
+			float jerk = (_param_mpc_acc_hor_max.get() - _acc_state_dependent) / dt;
+
+			if (jerk > _jerk_state_dependent) {
+				_acc_state_dependent = _jerk_state_dependent * dt
+						       + _acc_state_dependent;
+
+			} else {
+				_acc_state_dependent = _param_mpc_acc_hor_max.get();
+			}
+
+			break;
+		}
+
+	case Intention::direction_change: {
+			// We allow for fast change by setting previous setpoint to current  setpoint.
+
+
+			// Because previous setpoint is equal to current setpoint,
+			// slewrate will have no effect. Nonetheless, just set
+			// acceleration to maximum.
+			_acc_state_dependent = _param_mpc_acc_hor_max.get();
+			break;
+		}
+
+	case Intention::acceleration: {
+			// Limit acceleration linearly based on velocity setpoint.
+			_acc_state_dependent = (_param_mpc_acc_hor.get() - _param_mpc_dec_hor_slow.get())
+					       / _vel_max * vel_sp.length() + _param_mpc_dec_hor_slow.get();
+			break;
+		}
+
+	case Intention::deceleration: {
+			_acc_state_dependent = _param_mpc_dec_hor_slow.get();
+			break;
+		}
+	}
+
+	// Update intention for next iteration.
+	_intention = intention;
+}
+
+void
+ManualSmoothingXY::_velocitySlewRate(Vector2f &vel_sp, const float dt)
+{
+	// Adjust velocity setpoint if demand exceeds acceleration. /
+	Vector2f acc{};
+
+	if (dt > FLT_EPSILON) {
+		acc = (vel_sp - _vel_sp_prev) / dt;
+	}
+
+	if (acc.length() > _acc_state_dependent) {
+		vel_sp = acc.normalized() * _acc_state_dependent  * dt + _vel_sp_prev;
+	}
+}
+
+Vector2f
+ManualSmoothingXY::_getWorldToHeadingFrame(const Vector2f &vec, const float &yaw)
+{
+	Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
+	Vector3f vec_heading = q_yaw.conjugate_inversed(Vector3f(vec(0), vec(1), 0.0f));
+	return Vector2f(vec_heading);
+}
+
+Vector2f
+ManualSmoothingXY::_getHeadingToWorldFrame(const Vector2f &vec, const float &yaw)
+{
+	Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
+	Vector3f vec_world = q_yaw.conjugate(Vector3f(vec(0), vec(1), 0.0f));
+	return Vector2f(vec_world);
+}

src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.hpp

@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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 SmoothingXY.hpp
+ *
+ * This Class is used for smoothing the velocity setpoint in XY-direction.
+ * The velocity setpoint is smoothed by applying a velocity change limit, which
+ * we call acceleration. Depending on the user intention, the acceleration limit
+ * will differ.
+ * In manual mode we consider four states with different acceleration handling:
+ * 1. user wants to stop
+ * 2. user wants to quickly change direction
+ * 3. user wants to accelerate
+ * 4. user wants to decelerate
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <matrix/matrix/math.hpp>
+
+class ManualSmoothingXY : public ModuleParams
+{
+public:
+	ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel);
+	~ManualSmoothingXY() = default;
+
+	/**
+	 * Maximum velocity is required to detect user intention.
+	 * Maximum velocity depends on flight-task.
+	 * In order to deduce user intention from velocity, the maximum
+	 * allowed velocity has to be updated.
+	 * @param vel_max corresponds to vehicle constraint
+	 */
+	void updateMaxVelocity(const float &vel_max) {_vel_max = vel_max;};
+
+	/**
+	 * Smoothing of velocity setpoint horizontally based
+	 * on flight direction.
+	 * @param vel_sp: velocity setpoint in xy
+	 * @param dt: time delta in seconds
+	 */
+	void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel,  const float &yaw,
+			    const float &yawrate_sp, const float dt);
+
+	/*
+	 * User intention.
+	 * - brake when user demands a brake
+	 * - acceleration when vehicle keeps speed or accelerates in the same direction
+	 * - deceleration when vehicle slows down in the same direction
+	 * - direction_change whne vehcile demands an abrupt direction change
+	 */
+	enum class Intention {
+		brake,
+		acceleration,
+		deceleration,
+		direction_change
+	};
+	/**
+	 * Get user intention.
+	 * @see Intention
+	 */
+	Intention getIntention() { return _intention; }
+
+	/**
+	 *  Overwrite methods:
+	 *  Needed if different parameter values than default required.
+	 */
+	void overwriteHoverAcceleration(float acc) { _param_mpc_acc_hor_max.set(acc); }
+	void overwriteMaxAcceleration(float acc) { _param_mpc_acc_hor.set(acc); }
+	void overwriteDecelerationMin(float dec) { _param_mpc_dec_hor_slow.set(dec); }
+	void overwriteJerkMax(float jerk) { _param_mpc_jerk_max.set(jerk); }
+	void overwriteJerkMin(float jerk) { _param_mpc_jerk_min.set(jerk); }
+
+private:
+	/**
+	 * Sets velocity change limits (=acceleration).
+	 * Depending on the user intention, the acceleration differs.
+	 * @param vel_sp is desired velocity setpoint before slewrate.
+	 * @param vel is current velocity in horizontal direction
+	 * @param yaw is vehicle yaw
+	 * @param yawrate_sp is desired yawspeed
+	 * @param dt is delta-time
+	 */
+	void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+				 const float &yawrate_sp, const float dt);
+
+	/**
+	 * Gets user intention.
+	 * The intention is deduced from desired velocity setpoint.
+	 * @param vel_sp is desired velocity setpoint before slewrate.
+	 * @param vel is vehicle velocity in xy-direction.
+	 * @param yaw is vehicle yaw
+	 * @param yawrate_sp is the desired yaw-speed
+	 */
+	Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+				const float &yawrate_sp);
+
+	/**
+	 * Set acceleration depending on Intention.
+	 * @param vel_sp is desired velocity septoint in xy-direction
+	 * @param vel is vehicle velociy in xy-direction
+	 * @param intention is the user intention during flight
+	 * @param dt is delta-time
+	 */
+	void _setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
+				   const float dt);
+
+	/**
+	 * Limits the velocity setpoint change.
+	 * @param vel_sp that gets limited based on acceleration and previous velocity setpoint
+	 * @param dt is delta-time
+	 */
+	void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
+
+	/**
+	 * Rotate vector from local frame into heading frame.
+	 * @param vec is an arbitrary vector in local frame
+	 * @param yaw is the vehicle heading
+	 */
+	matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw);
+
+	/**
+	 * Rotate vector from heading frame to local frame.
+	 * @param vec is an arbitrary vector in heading frame
+	 * @param yaw is the vehicle yaw
+	 */
+	matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
+
+	Intention _intention{Intention::acceleration}; /**< user intention */
+	float _acc_state_dependent; /**< velocity change limit that depends on Intention */
+	float _jerk_state_dependent; /**< acceleration change limit during brake */
+	float _vel_max{10.0f}; /**< maximum horizontal speed allowed */
+	matrix::Vector2f _vel_sp_prev; /**< previous velocity setpoint */
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max, /**< acceleration in hover */
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< acceleration in flight */
+		(ParamFloat<px4::params::MPC_DEC_HOR_SLOW>) _param_mpc_dec_hor_slow, /**< deceleration in flight */
+		(ParamFloat<px4::params::MPC_JERK_MIN>) _param_mpc_jerk_min, /**< jerk min during brake */
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< jerk max during brake */
+		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< maximum velocity in manual controlled mode */
+	)
+};

src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.cpp

@@ -0,0 +1,159 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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 spec{fic 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 ManualSmoothingZ.cpp
+ */
+
+#include "ManualSmoothingZ.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick) :
+	ModuleParams(parent),
+	_vel(vel), _stick(stick), _vel_sp_prev(vel)
+{
+	_acc_state_dependent = _param_mpc_acc_up_max.get();
+	_max_acceleration = _param_mpc_acc_up_max.get();
+}
+
+void
+ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt)
+{
+	updateAcceleration(vel_sp, dt);
+	velocitySlewRate(vel_sp, dt);
+	_vel_sp_prev = vel_sp;
+}
+
+void
+ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
+{
+	// check for max acceleration
+	setMaxAcceleration();
+
+	// check if zero input stick
+	const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
+
+	// default is acceleration
+	ManualIntentionZ intention = ManualIntentionZ::acceleration;
+
+	// check zero input stick
+	if (is_current_zero) {
+		intention = ManualIntentionZ::brake;
+	}
+
+
+	// update intention
+	if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
+
+		// we start with lowest acceleration
+		_acc_state_dependent = _param_mpc_acc_down_max.get();
+
+		// reset slew-rate: this ensures that there
+		// is no delay present when user demands to brake
+		_vel_sp_prev = _vel;
+
+	}
+
+	switch (intention) {
+	case ManualIntentionZ::brake: {
+
+			// limit jerk when braking to zero
+			float jerk = (_param_mpc_acc_up_max.get() - _acc_state_dependent) / dt;
+
+			if (jerk > _param_mpc_jerk_max.get()) {
+				_acc_state_dependent = _param_mpc_jerk_max.get() * dt + _acc_state_dependent;
+
+			} else {
+				_acc_state_dependent = _param_mpc_acc_up_max.get();
+			}
+
+			break;
+		}
+
+	case ManualIntentionZ::acceleration: {
+
+			_acc_state_dependent = (getMaxAcceleration() - _param_mpc_acc_down_max.get())
+					       * fabsf(_stick) + _param_mpc_acc_down_max.get();
+			break;
+		}
+	}
+
+	_intention = intention;
+}
+
+void
+ManualSmoothingZ::setMaxAcceleration()
+{
+	if (_stick < -FLT_EPSILON) {
+		// accelerating upward
+		_max_acceleration =  _param_mpc_acc_up_max.get();
+
+	} else if (_stick > FLT_EPSILON) {
+		// accelerating downward
+		_max_acceleration = _param_mpc_acc_down_max.get();
+
+	} else {
+
+		// want to brake
+		if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
+			// at rest
+			_max_acceleration = _param_mpc_acc_up_max.get();
+
+		} else if (_vel_sp_prev < 0.0f) {
+			// braking downward
+			_max_acceleration = _param_mpc_acc_down_max.get();
+
+		} else {
+			// braking upward
+			_max_acceleration = _param_mpc_acc_up_max.get();
+		}
+	}
+}
+
+void
+ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt)
+{
+	// limit vertical acceleration
+	float acc = 0.f;
+
+	if (dt > FLT_EPSILON) {
+		acc = (vel_sp - _vel_sp_prev) / dt;
+	}
+
+	float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent;
+
+	if (fabsf(acc) > fabsf(max_acc)) {
+		vel_sp = max_acc * dt + _vel_sp_prev;
+	}
+}

src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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 SmoothingZ.hpp
+ *
+ * This Class is used for smoothing the velocity setpoints in Z-direction.
+ * In manual altitude control apply acceleration limit based on stick input.
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+
+/**
+ * User-intention.
+ * - brake: vehicle should stop
+ * - acceleration: vehicle should move up or down
+ */
+enum class ManualIntentionZ {
+	brake,
+	acceleration,
+};
+
+class ManualSmoothingZ : public ModuleParams
+{
+public:
+	ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick);
+	~ManualSmoothingZ() = default;
+
+	/**
+	 * Smooth velocity setpoint based on flight direction.
+	 * @param vel_sp[2] array: vel_sp[0] = current velocity set-point;
+	 * 					 	   vel_sp[1] = previous velocity set-point
+	 * 		  vel_sp will contain smoothed current / previous set-point.
+	 * @param dt: time delta in seconds
+	 */
+	void smoothVelFromSticks(float &vel_sp, const float dt);
+
+	/**
+	 * Get max accleration.
+	 */
+	float getMaxAcceleration() { return _max_acceleration; }
+
+	/**
+	 * Get user intention.
+	 * @see ManualIntentionZ
+	 */
+	ManualIntentionZ getIntention() { return _intention; }
+
+	/*
+	 * Overwrite methods:
+	 * Needed if different parameter values than default required.
+	 */
+	void overwriteAccelerationUp(float acc_max_up) { _param_mpc_acc_up_max.set(acc_max_up); }
+	void overwriteAccelerationDown(float acc_max_down)  {_param_mpc_acc_down_max.set(acc_max_down); }
+	void overwriteJerkMax(float jerk_max)  {_param_mpc_jerk_max.set(jerk_max); }
+
+private:
+	/**
+	 * Add delay to velocity setpoint change.
+	 * This method is used to smooth the velocity setpoint change.
+	 * @param vel_sp current velocity setpoint
+	 * @param dt delta-time
+	 */
+	void velocitySlewRate(float &vel_sp, const float dt);
+
+	/**
+	 * Computes the velocity setpoint change limit.
+	 * This method computes the limit with which the velocity setpoint change
+	 * is limited.
+	 * @see velocitySlewRate
+	 * @param vel_sp current velocity setpoint
+	 * @param dt delta-time
+	 */
+	void updateAcceleration(float &vel_sp, const float dt);
+
+	/**
+	 * Set maximum acceleration.
+	 * The maximum acceleration depends on the desired direction (up vs down).
+	 * @see _max_acceleration
+	 */
+	void setMaxAcceleration();
+
+	/**
+	 * User intention
+	 * @see ManualIntentionZ
+	 */
+	ManualIntentionZ _intention{ManualIntentionZ::acceleration};
+
+	const float &_vel; /**< vehicle velocity (dependency injection) */
+	const float &_stick; /**< stick input (dependency injection) */
+
+	float _acc_state_dependent; /**< acceleration that depends on _intention */
+	float _max_acceleration; /**< can be up or down maximum acceleration */
+	float _vel_sp_prev; /**< previous velocity setpoint */
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max
+	)
+};

src/lib/flight_tasks/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(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos)
+{
+	for (int i = 0; i < 2; i++) {
+		_trajectory[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, const 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) {
+		_trajectory[i].updateTraj(dt);
+
+		_state.j(i) = _trajectory[i].getCurrentJerk();
+		_state.a(i) = _trajectory[i].getCurrentAcceleration();
+		_state.v(i) = _trajectory[i].getCurrentVelocity();
+		_state.x(i) = _trajectory[i].getCurrentPosition();
+	}
+}
+
+void ManualVelocitySmoothingXY::updateTrajDurations(const Vector2f &velocity_target)
+{
+	for (int i = 0; i < 2; ++i) {
+		_trajectory[i].updateDurations(velocity_target(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectory, 2);
+}
+
+void ManualVelocitySmoothingXY::checkPositionLock(const 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
+			_trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
+			_trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
+			_state.v = _velocity_setpoint_feedback;
+			resetPositionLock();
+		}
+
+		_trajectory[0].setCurrentPosition(_position_estimate(0));
+		_trajectory[1].setCurrentPosition(_position_estimate(1));
+	}
+}

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp

@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   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 final
+{
+public:
+	ManualVelocitySmoothingXY() = default;
+	~ManualVelocitySmoothingXY() = default;
+
+	void reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos);
+	void update(float dt, const Vector2f &velocity_target);
+
+	void setVelSpFeedback(const Vector2f &fb) { _velocity_setpoint_feedback = fb; }
+
+	void setMaxJerk(const float max_jerk)
+	{
+		_trajectory[0].setMaxJerk(max_jerk);
+		_trajectory[1].setMaxJerk(max_jerk);
+	}
+	float getMaxJerk() const { return _trajectory[0].getMaxJerk(); }
+
+	void setMaxAccel(const float max_accel)
+	{
+		_trajectory[0].setMaxAccel(max_accel);
+		_trajectory[1].setMaxAccel(max_accel);
+	}
+	float getMaxAccel() const { return _trajectory[0].getMaxAccel(); }
+
+	void setMaxVel(const float max_vel)
+	{
+		_trajectory[0].setMaxVel(max_vel);
+		_trajectory[1].setMaxVel(max_vel);
+	}
+	float getMaxVel() const { return _trajectory[0].getMaxVel(); }
+
+	Vector2f getCurrentJerk() const { return _state.j; }
+	Vector2f getCurrentAcceleration() const { return _state.a; }
+
+	void setCurrentVelocity(const Vector2f &vel)
+	{
+		_state.v = vel;
+		_trajectory[0].setCurrentVelocity(vel(0));
+		_trajectory[1].setCurrentVelocity(vel(1));
+	}
+	Vector2f getCurrentVelocity() const { return _state.v; }
+
+	void setCurrentPosition(const Vector2f &pos)
+	{
+		_state.x = pos;
+		_trajectory[0].setCurrentPosition(pos(0));
+		_trajectory[1].setCurrentPosition(pos(1));
+		// TODO: check lock/unlock in case of EKF reset
+	}
+	Vector2f getCurrentPosition() const { return _position_setpoint_locked; }
+
+	void setCurrentPositionEstimate(const Vector2f &pos) { _position_estimate = pos; }
+
+private:
+	void resetPositionLock();
+	void updateTrajectories(float dt);
+	void checkPositionLock(const Vector2f &velocity_target);
+	void updateTrajDurations(const Vector2f &velocity_target);
+
+	VelocitySmoothing _trajectory[2]; ///< Trajectory 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;
+};

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Manual Velocity Smoothing library
+ * Run this test only using make tests TESTFILTER=ManualVelocitySmoothing
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+
+#include "ManualVelocitySmoothingXY.hpp"
+
+using namespace matrix;
+
+class ManualVelocitySmoothingXYTest : public ::testing::Test
+{
+public:
+	ManualVelocitySmoothingXY _smoothing;
+};
+
+
+TEST_F(ManualVelocitySmoothingXYTest, setGet)
+{
+	// GIVEN: Some max values
+	_smoothing.setMaxJerk(11.f);
+	_smoothing.setMaxAccel(7.f);
+	_smoothing.setMaxVel(5.f);
+
+	// THEN: We should be able to get them back
+	EXPECT_EQ(_smoothing.getMaxJerk(), 11.f);
+	EXPECT_EQ(_smoothing.getMaxAccel(), 7.f);
+	EXPECT_EQ(_smoothing.getMaxVel(), 5.f);
+}
+
+TEST_F(ManualVelocitySmoothingXYTest, getCurrentState)
+{
+	// GIVEN: the initial conditions
+	Vector2f v0(11.f, 13.f);
+	_smoothing.setCurrentVelocity(v0);
+
+	// WHEN: we get the current state
+	Vector3f j_end;
+	Vector3f a_end;
+	Vector3f v_end;
+	Vector3f x_end;
+	j_end.xy() = _smoothing.getCurrentJerk();
+	a_end.xy() = _smoothing.getCurrentAcceleration();
+	v_end.xy() = _smoothing.getCurrentVelocity();
+	x_end.xy() = _smoothing.getCurrentPosition();
+
+	// THEN: the returned values should match the input
+	EXPECT_EQ(j_end, Vector3f(0.f, 0.f, 0.f));
+	EXPECT_EQ(a_end, Vector3f(0.f, 0.f, 0.f));
+	EXPECT_EQ(v_end, Vector3f(11.f, 13.f, 0.f));
+	EXPECT_EQ(x_end, Vector3f(0.f, 0.f, 0.f));
+}

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.cpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   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)
+{
+	_trajectory.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
+	_trajectory.updateDurations(velocity_target);
+}
+
+void ManualVelocitySmoothingZ::updateTrajectories(float dt)
+{
+	_trajectory.updateTraj(dt);
+
+	_state.j = _trajectory.getCurrentJerk();
+	_state.a = _trajectory.getCurrentAcceleration();
+	_state.v = _trajectory.getCurrentVelocity();
+	_state.x = _trajectory.getCurrentPosition();
+}
+
+void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target)
+{
+	if (velocity_target < 0.f) { // up
+		_trajectory.setMaxAccel(_max_accel_up);
+		_trajectory.setMaxVel(_max_vel_up);
+
+	} else { // down
+		_trajectory.setMaxAccel(_max_accel_down);
+		_trajectory.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
+			_trajectory.setCurrentVelocity(_velocity_setpoint_feedback);
+			_state.v = _velocity_setpoint_feedback;
+			resetPositionLock();
+		}
+
+		_trajectory.setCurrentPosition(_position_estimate);
+	}
+}

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp

@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ManualVelocitySmoothingZ.hpp
+ *
+ */
+
+#pragma once
+
+#include "VelocitySmoothing.hpp"
+
+class ManualVelocitySmoothingZ final
+{
+public:
+	ManualVelocitySmoothingZ() = default;
+	~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)
+	{
+		_trajectory.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;
+		_trajectory.setCurrentVelocity(vel);
+	}
+	float getCurrentVelocity() const { return _state.v; }
+	void setCurrentPosition(const float pos)
+	{
+		_state.x = pos;
+		_trajectory.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 _trajectory; ///< Trajectory 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};
+};

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp

@@ -0,0 +1,208 @@
+/****************************************************************************
+ *
+ *   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 ObstacleAvoidance.cpp
+ */
+
+#include "ObstacleAvoidance.hpp"
+
+using namespace matrix;
+using namespace time_literals;
+
+/** Timeout in us for trajectory data to get considered invalid */
+static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
+/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */
+static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms;
+static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s;
+
+ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
+	ModuleParams(parent)
+{
+	_desired_waypoint = empty_trajectory_waypoint;
+	_failsafe_position.setNaN();
+	_avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE);
+	_no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US);
+
+}
+
+void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp,
+		float &yaw_speed_sp)
+{
+	_sub_vehicle_status.update();
+	_sub_vehicle_trajectory_waypoint.update();
+
+	if (_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
+		// if in failsafe nav_state LOITER, don't inject setpoints from avoidance system
+		return;
+	}
+
+	const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp)
+					    > TRAJECTORY_STREAM_TIMEOUT_US;
+	const bool avoidance_point_valid =
+		_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
+
+	_avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid, hrt_absolute_time());
+
+	if (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()) {
+		PX4_WARN("Obstacle Avoidance system failed, loitering");
+		_publishVehicleCmdDoLoiter();
+
+		if (!PX4_ISFINITE(_failsafe_position(0)) || !PX4_ISFINITE(_failsafe_position(1))
+		    || !PX4_ISFINITE(_failsafe_position(2))) {
+			// save vehicle position when entering failsafe
+			_failsafe_position = _position;
+		}
+
+		pos_sp = _failsafe_position;
+		vel_sp.setNaN();
+		yaw_sp = NAN;
+		yaw_speed_sp = NAN;
+		return;
+
+	} else {
+		_failsafe_position.setNaN();
+	}
+
+	if (avoidance_point_valid) {
+		pos_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
+		vel_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+
+		if (!_ext_yaw_active) {
+			// inject yaw setpoints only if weathervane isn't active
+			yaw_sp =  _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
+			yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
+		}
+	}
+
+}
+
+void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw,
+		const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed,
+		const bool ext_yaw_active, const int wp_type)
+{
+	_desired_waypoint.timestamp = hrt_absolute_time();
+	_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
+	_curr_wp = curr_wp;
+	_ext_yaw_active = ext_yaw_active;
+
+	curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
+
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type;
+
+	next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
+
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
+}
+
+void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type)
+{
+	pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
+	vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
+
+	_pub_traj_wp_avoidance_desired.publish(_desired_waypoint);
+
+	_desired_waypoint = empty_trajectory_waypoint;
+}
+
+void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
+		float target_acceptance_radius, const Vector2f &closest_pt)
+{
+	_position = pos;
+	position_controller_status_s pos_control_status = {};
+	pos_control_status.timestamp = hrt_absolute_time();
+
+	// vector from previous triplet to current target
+	Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp);
+	// vector from previous triplet to the vehicle projected position on the line previous-target triplet
+	Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp);
+	// fraction of the previous-tagerget line that has been flown
+	const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
+
+	Vector2f pos_to_target = Vector2f(_curr_wp - _position);
+
+	if (prev_curr_travelled > 1.0f) {
+		// if the vehicle projected position on the line previous-target is past the target waypoint,
+		// increase the target acceptance radius such that navigator will update the triplets
+		pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
+	}
+
+	const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
+
+	bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z);
+	_no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time());
+
+	if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()
+	    && _no_progress_z_hysteresis.get_state()) {
+		// vehicle above or below the target waypoint
+		pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
+	}
+
+	_prev_pos_to_target_z = pos_to_target_z;
+
+	// do not check for waypoints yaw acceptance in navigator
+	pos_control_status.yaw_acceptance = NAN;
+
+	_pub_pos_control_status.publish(pos_control_status);
+}
+
+void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
+{
+	vehicle_command_s command{};
+	command.timestamp = hrt_absolute_time();
+	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
+	command.param1 = (float)1; // base mode
+	command.param3 = (float)0; // sub mode
+	command.target_system = 1;
+	command.target_component = 1;
+	command.source_system = 1;
+	command.source_component = 1;
+	command.confirmation = false;
+	command.from_external = false;
+	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
+	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+
+	// publish the vehicle command
+	_pub_vehicle_command.publish(command);
+}

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp

@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   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 ObstacleAvoidance.hpp
+ * This class is used to inject the setpoints of an obstacle avoidance system
+ * into the FlightTasks
+ *
+ * @author Martina Rivizzigno
+ */
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module_params.h>
+#include <commander/px4_custom_mode.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/position_setpoint.h>
+
+#include <lib/hysteresis/hysteresis.h>
+
+#include <matrix/matrix/math.hpp>
+
+const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
+	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}
+	}
+};
+
+class ObstacleAvoidance : public ModuleParams
+{
+public:
+	ObstacleAvoidance(ModuleParams *parent);
+	~ObstacleAvoidance() = default;
+
+	/**
+	 * Inject setpoints from obstacle avoidance system into FlightTasks.
+	 * @param pos_sp, position setpoint
+	 * @param vel_sp, velocity setpoint
+	 * @param yaw_sp, yaw setpoint
+	 * @param yaw_speed_sp, yaw speed setpoint
+	 */
+	void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
+
+	/**
+	 * Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight.
+	 * @param curr_wp, current position triplet
+	 * @param curr_yaw, current yaw triplet
+	 * @param curr_yawspeed, current yaw speed triplet
+	 * @param next_wp, next position triplet
+	 * @param next_yaw, next yaw triplet
+	 * @param next_yawspeed, next yaw speed triplet
+	 * @param wp_type, current triplet type
+	 */
+	void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed,
+					     const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active,
+					     const int wp_type);
+	/**
+	 * Updates the desired setpoints to send to the obstacle avoidance system.
+	 * @param pos_sp, desired position setpoint computed by the active FlightTask
+	 * @param vel_sp, desired velocity setpoint computed by the active FlightTask
+	 * @param type, current triplet type
+	 */
+	void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type);
+
+	/**
+	 * Checks the vehicle progress between previous and current position waypoint of the triplet.
+	 * @param pos, vehicle position
+	 * @param prev_wp, previous position triplet
+	 * @param target_acceptance_radius, current position triplet xy acceptance radius
+	 * @param closest_pt, closest point to the vehicle on the line previous-current position triplet
+	 */
+	void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
+				    float target_acceptance_radius, const matrix::Vector2f &closest_pt);
+
+protected:
+
+	uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */
+	uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
+
+	vehicle_trajectory_waypoint_s _desired_waypoint{};  /**< desired vehicle trajectory waypoint to be sent to OA */
+
+	uORB::Publication<vehicle_trajectory_waypoint_s> _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};	/**< trajectory waypoint desired publication */
+	uORB::Publication<position_controller_status_s> _pub_pos_control_status{ORB_ID(position_controller_status)};	/**< position controller status publication */
+	uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};	/**< vehicle command do publication */
+
+	matrix::Vector3f _curr_wp = {}; /**< current position triplet */
+	matrix::Vector3f _position = {}; /**< current vehicle position */
+	matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
+
+	systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
+	systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
+
+	float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */
+
+	bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
+
+	/**
+	 * Publishes vehicle command.
+	 */
+	void _publishVehicleCmdDoLoiter();
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad    /**< Acceptance radius for multicopter altitude */
+	);
+
+};

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include <ObstacleAvoidance.hpp>
+#include <uORB/Subscription.hpp>
+
+
+using namespace matrix;
+// to run: make tests TESTFILTER=CollisionPrevention
+
+class ObstacleAvoidanceTest : public ::testing::Test
+{
+public:
+	Vector3f pos_sp;
+	Vector3f vel_sp;
+	float yaw_sp = 0.123f;
+	float yaw_speed_sp = NAN;
+	void SetUp() override
+
+	{
+		param_reset_all();
+		pos_sp = Vector3f(1.f, 1.2f, 0.1f);
+		vel_sp = Vector3f(0.3f, 0.4f, 0.1f);
+	}
+};
+
+class TestObstacleAvoidance : public ::ObstacleAvoidance
+{
+public:
+	TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {}
+	void paramsChanged() {ObstacleAvoidance::updateParamsImpl();}
+	void test_setPosition(Vector3f &pos) {_position = pos;}
+};
+
+TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); }
+
+TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
+{
+	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming
+	// from offboard
+	TestObstacleAvoidance oa;
+
+	vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
+	message.timestamp = hrt_absolute_time();
+	message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
+	message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f;
+	message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f;
+	message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f;
+	message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f;
+	message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
+
+	// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message
+	uORB::Publication<vehicle_trajectory_waypoint_s> vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
+	vehicle_trajectory_waypoint_pub.publish(message);
+
+	vehicle_status_s vehicle_status{};
+	vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+	uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
+	vehicle_status_pub.publish(vehicle_status);
+
+	// WHEN: we inject the vehicle_trajectory_waypoint in the interface
+	oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
+
+	// THEN: the setpoints should be injected
+	EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0));
+	EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1));
+	EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2));
+	EXPECT_TRUE(vel_sp.isAllNan());
+	EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp);
+	EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
+}
+
+TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
+{
+	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message
+	TestObstacleAvoidance oa;
+
+	vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
+	Vector3f pos(3.1f, 4.7f, 5.2f);
+	oa.test_setPosition(pos);
+
+	// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status
+	uORB::Publication<vehicle_trajectory_waypoint_s> vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
+	vehicle_trajectory_waypoint_pub.publish(message);
+
+	vehicle_status_s vehicle_status{};
+	vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+	uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
+	vehicle_status_pub.publish(vehicle_status);
+
+	// WHEN: we inject the vehicle_trajectory_waypoint in the interface
+	oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
+
+	// THEN: the setpoints shouldn't be injected
+	EXPECT_FLOAT_EQ(pos(0), pos_sp(0));
+	EXPECT_FLOAT_EQ(pos(1), pos_sp(1));
+	EXPECT_FLOAT_EQ(pos(2), pos_sp(2));
+	EXPECT_TRUE(vel_sp.isAllNan());
+	EXPECT_FALSE(PX4_ISFINITE(yaw_sp));
+	EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
+}
+
+TEST_F(ObstacleAvoidanceTest, oa_desired)
+{
+	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto
+	TestObstacleAvoidance oa;
+
+	pos_sp = Vector3f(1.f, 1.2f, NAN);
+	vel_sp = Vector3f(NAN, NAN, 0.7f);
+	int type = 4;
+	Vector3f curr_wp(1.f, 1.2f, 5.0f);
+	float curr_yaw = 1.02f;
+	float curr_yawspeed = NAN;
+	Vector3f next_wp(11.f, 1.2f, 5.0f);
+	float next_yaw = 0.82f;
+	float next_yawspeed = NAN;
+	bool ext_yaw_active = false;
+
+	// WHEN: we inject the setpoints and waypoints in the interface
+	oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active,
+					   type);
+	oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type);
+
+	// WHEN: we subscribe to the uORB message out of the interface
+	uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};
+	_sub_traj_wp_avoidance_desired.update();
+
+	// THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2
+	EXPECT_FLOAT_EQ(pos_sp(0),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]);
+	EXPECT_FLOAT_EQ(pos_sp(1),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]);
+	EXPECT_FALSE(PX4_ISFINITE(
+			     _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2]));
+	EXPECT_FALSE(PX4_ISFINITE(
+			     _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0]));
+	EXPECT_FALSE(PX4_ISFINITE(
+			     _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1]));
+	EXPECT_FLOAT_EQ(vel_sp(2),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]);
+	EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type);
+	EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid);
+
+	EXPECT_FLOAT_EQ(curr_wp(0),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]);
+	EXPECT_FLOAT_EQ(curr_wp(1),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]);
+	EXPECT_FLOAT_EQ(curr_wp(2),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]);
+	EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw);
+	EXPECT_FALSE(PX4_ISFINITE(
+			     _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed));
+	EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type);
+	EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid);
+
+	EXPECT_FLOAT_EQ(next_wp(0),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]);
+	EXPECT_FLOAT_EQ(next_wp(1),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]);
+	EXPECT_FLOAT_EQ(next_wp(2),
+			_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]);
+	EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw);
+	EXPECT_FALSE(PX4_ISFINITE(
+			     _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed));
+	EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type);
+	EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid);
+
+}

src/lib/flight_tasks/tasks/Utility/StraightLine.cpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 StraightLine.cpp
+ */
+
+#include "StraightLine.hpp"
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint)
+{
+	if (isEndReached()) {
+		// Vehicle has reached target, lock position
+		position_setpoint = _end;
+		velocity_setpoint.setNaN();
+		return;
+	}
+
+	Vector3f start_to_end = _end - _start;
+	float distance_start_to_end = start_to_end.norm();
+
+	// capture progress as ratio between 0 and 1 of entire distance
+	Vector3f vehicle_to_end = _end - _position;
+	float distance_vehicle_to_end = vehicle_to_end.norm();
+	float distance_from_start = Vector3f(_start - _position).norm();
+	float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end);
+
+	float distance_from_boundary = 0.f;
+
+	// calculate the distance to the closer boundary
+	if (progress < 0.5f) {
+		distance_from_boundary = (2 * progress) * distance_start_to_end;
+
+	} else {
+		distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end;
+	}
+
+	// ramp velocity based on the distance to the boundary
+	float velocity = 0.5f + (distance_from_boundary / 4.f);
+	velocity = math::min(velocity, _speed);
+	velocity_setpoint = vehicle_to_end.unit_or_zero() * velocity;
+
+	// check if we plan to go against the line direction which indicates we reached the goal
+	if (start_to_end * vehicle_to_end < 0) {
+		end_reached = true;
+	}
+}
+
+void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end)
+{
+	if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) {
+		_start = start;
+		_end = end;
+		end_reached = false;
+	}
+}

src/lib/flight_tasks/tasks/Utility/StraightLine.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 StraightLine.hpp
+ *
+ * lib to return setpoints on a straight line
+ *
+ * @author Christoph Tobler <christoph@px4.io>
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+class StraightLine
+{
+public:
+	StraightLine(const matrix::Vector3f &pos) : _position(pos) {};
+	~StraightLine() = default;
+
+	// setter functions
+	void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end);
+	void setSpeed(const float &speed) { _speed = speed; };
+
+	/**
+	 * Generate setpoints on a straight line according to parameters
+	 *
+	 * @param position_setpoint: reference to the 3D vector with the position setpoint to update
+	 * @param velocity_setpoint: reference to the 3D vector with the velocity setpoint to update
+	 */
+	void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint);
+
+	/**
+	 * Check if the end was reached
+	 *
+	 * @return false when on the way from start to end, true when end was reached
+	 */
+	bool isEndReached() { return end_reached; }
+
+private:
+	const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */
+
+	matrix::Vector3f _start; /**< Start point of the straight line */
+	matrix::Vector3f _end; /**< End point of the straight line */
+	float _speed = 1.f; /**< desired speed between accelerating and decelerating */
+
+	bool end_reached = true; /**< Flag to lock further movement when end is reached */
+};

src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.cpp

@@ -0,0 +1,302 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 <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)
+{
+	_state.j = 0.f;
+	_state.a = accel;
+	_state.v = vel;
+	_state.x = pos;
+
+	_state_init = _state;
+}
+
+float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max)
+{
+	/* Check maximum acceleration, saturate and recompute T1 if needed */
+	float accel_T1 = a0 + j_max * T1;
+	float T1_new = T1;
+
+	if (accel_T1 > a_max) {
+		T1_new = (a_max - a0) / j_max;
+
+	} else if (accel_T1 < -a_max) {
+		T1_new = (-a_max - a0) / j_max;
+	}
+
+	return T1_new;
+}
+
+float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
+{
+	float delta = 2.f * a0 * a0 + 4.f * j_max * v3;
+
+	if (delta < 0.f) {
+		// Solution is not real
+		return 0.f;
+	}
+
+	float sqrt_delta = sqrtf(delta);
+	float T1_plus = (-a0 + 0.5f * sqrt_delta) / j_max;
+	float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max;
+
+	float T3_plus = a0 / j_max + T1_plus;
+	float T3_minus = a0 / j_max + T1_minus;
+
+	float T1 = 0.f;
+
+	if (T1_plus >= 0.f && T3_plus >= 0.f) {
+		T1 = T1_plus;
+
+	} else if (T1_minus >= 0.f && T3_minus >= 0.f) {
+		T1 = T1_minus;
+	}
+
+	T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
+
+	return math::max(T1, 0.f);
+}
+
+float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max)
+{
+	float a = -j_max;
+	float b = j_max * T123 - a0;
+	float delta = T123 * T123 * j_max * j_max + 2.f * T123 * a0 * j_max - a0 * a0 - 4.f * j_max * v3;
+
+	if (delta < 0.f) {
+		// Solution is not real
+		return 0.f;
+	}
+
+	float sqrt_delta = sqrtf(delta);
+	float denominator_inv = 1.f / (2.f * a);
+	float T1_plus = math::max((-b + sqrt_delta) * denominator_inv, 0.f);
+	float T1_minus = math::max((-b - sqrt_delta) * denominator_inv, 0.f);
+
+	float T3_plus = a0 / j_max + T1_plus;
+	float T3_minus = a0 / j_max + T1_minus;
+
+	float T13_plus = T1_plus + T3_plus;
+	float T13_minus = T1_minus + T3_minus;
+
+	float T1 = 0.f;
+
+	if (T13_plus > T123) {
+		T1 = T1_minus;
+
+	} else if (T13_minus > T123) {
+		T1 = T1_plus;
+	}
+
+	T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
+
+	return T1;
+}
+
+
+float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max)
+{
+	float T2 = 0.f;
+
+	float den = a0 + j_max * T1;
+
+	if (math::abs_t(den) > FLT_EPSILON) {
+		T2 = (-0.5f * T1 * T1 * j_max - T1 * T3 * j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den;
+	}
+
+	return math::max(T2, 0.f);
+}
+
+float VelocitySmoothing::computeT2(float T123, float T1, float T3)
+{
+	float T2 = T123 - T1 - T3;
+	return math::max(T2, 0.f);
+}
+
+float VelocitySmoothing::computeT3(float T1, float a0, float j_max)
+{
+	float T3 = a0 / j_max + T1;
+	return math::max(T3, 0.f);
+}
+
+void VelocitySmoothing::updateDurations(float vel_setpoint)
+{
+	_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
+	_local_time = 0.f;
+	_state_init = _state;
+
+	_direction = computeDirection();
+
+	if (_direction != 0) {
+		updateDurationsMinimizeTotalTime();
+
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
+}
+
+int VelocitySmoothing::computeDirection()
+{
+	// Compute the velocity at which the trajectory will be
+	// when the acceleration will be zero
+	float vel_zero_acc = computeVelAtZeroAcc();
+
+	/* Depending of the direction, start accelerating positively or negatively */
+	int direction = math::sign(_vel_sp - vel_zero_acc);
+
+	if (direction == 0) {
+		// If by braking immediately the velocity is exactly
+		// the require one with zero acceleration, then brake
+		direction = math::sign(_state.a);
+	}
+
+	return direction;
+}
+
+float VelocitySmoothing::computeVelAtZeroAcc()
+{
+	float vel_zero_acc = _state.v;
+
+	if (fabsf(_state.a) > FLT_EPSILON) {
+		float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration
+		float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration
+		vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc;
+	}
+
+	return vel_zero_acc;
+}
+
+void VelocitySmoothing::updateDurationsMinimizeTotalTime()
+{
+	float jerk_max_T1 = _direction * _max_jerk;
+	float delta_v = _vel_sp - _state.v;
+
+	// compute increasing acceleration time
+	_T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
+
+	// compute decreasing acceleration time
+	_T3 = computeT3(_T1, _state.a, jerk_max_T1);
+
+	// compute constant acceleration time
+	_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
+}
+
+Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d)
+{
+	Trajectory traj;
+	float jt = d * j;
+	float t2 = t * t;
+	float t3 = t2 * t;
+
+	traj.j = jt;
+	traj.a = a0 + jt * t;
+	traj.v = v0 + a0 * t + 0.5f * jt * t2;
+	traj.x = x0 + v0 * t + 0.5f * a0 * t2 + 1.f / 6.f * jt * t3;
+
+	return traj;
+}
+
+void VelocitySmoothing::updateTraj(float dt, float time_stretch)
+{
+	_local_time += dt * time_stretch;
+	float t_remain = _local_time;
+
+	float t1 = math::min(t_remain, _T1);
+	_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
+	t_remain -= t1;
+
+	if (t_remain > 0.f) {
+		float t2 = math::min(t_remain, _T2);
+		_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
+		t_remain -= t2;
+	}
+
+	if (t_remain > 0.f) {
+		float t3 = math::min(t_remain, _T3);
+		_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
+		t_remain -= t3;
+	}
+
+	if (t_remain > 0.f) {
+		_state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t_remain, 0.f);
+	}
+}
+
+void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)
+{
+	float desired_time = 0.f;
+	int longest_traj_index = 0;
+
+	for (int i = 0; i < n_traj; i++) {
+		const float T123 = traj[i].getTotalTime();
+
+		if (T123 > desired_time) {
+			desired_time = T123;
+			longest_traj_index = i;
+		}
+	}
+
+	if (desired_time > FLT_EPSILON) {
+		for (int i = 0; i < n_traj; i++) {
+			if (i != longest_traj_index) {
+				traj[i].updateDurationsGivenTotalTime(desired_time);
+			}
+		}
+	}
+}
+
+void VelocitySmoothing::updateDurationsGivenTotalTime(float T123)
+{
+	float jerk_max_T1 = _direction * _max_jerk;
+	float delta_v = _vel_sp - _state.v;
+
+	// compute increasing acceleration time
+	_T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel);
+
+	// compute decreasing acceleration time
+	_T3 = computeT3(_T1, _state.a, jerk_max_T1);
+
+	// compute constant acceleration time
+	_T2 = computeT2(T123, _T1, _T3);
+}

src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.hpp

@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   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
+ * 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
+
+struct Trajectory {
+	float j; //< jerk
+	float a; //< acceleration
+	float v; //< velocity
+	float x; //< position
+};
+
+/**
+ * @class VelocitySmoothing
+ *
+ * TODO: document the algorithm
+ *    |T1| T2 |T3|
+ *     ___
+ *   __| |____   __ Jerk
+ *            |_|
+ *        ___
+ *       /   \	 Acceleration
+ *   ___/     \___
+ *             ___
+ *           ;"
+ *          /
+ *         / 	 Velocity
+ *        ;
+ *   ----"
+ */
+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);
+
+	/**
+	 * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle
+	 * and before updateTraj().
+	 * @param vel_setpoint velocity setpoint input
+	 */
+	void updateDurations(float vel_setpoint);
+
+	/**
+	 * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
+	 * @param dt integration period
+	 * @param time_stretch (optional) used to scale the integration period. This can be used to slow down
+	 * or fast-forward the trajectory
+	 */
+	void updateTraj(float dt, float time_stretch = 1.f);
+
+	/**
+	 * Getters and setters
+	 */
+	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; }
+
+	float getCurrentJerk() const { return _state.j; }
+	void setCurrentAcceleration(const float accel) { _state.a = _state_init.a = accel; }
+	float getCurrentAcceleration() const { return _state.a; }
+	void setCurrentVelocity(const float vel) { _state.v = _state_init.v = vel; }
+	float getCurrentVelocity() const { return _state.v; }
+	void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; }
+	float getCurrentPosition() const { return _state.x; }
+
+	float getVelSp() const { return _vel_sp; }
+
+	float getT1() const { return _T1; }
+	float getT2() const { return _T2; }
+	float getT3() const { return _T3; }
+	float getTotalTime() const { return _T1 + _T2 + _T3; }
+
+	/**
+	 * Synchronize several trajectories to have the same total time. This is required to generate
+	 * straight lines.
+	 * The resulting total time is the one of the longest trajectory.
+	 * @param traj an array of VelocitySmoothing objects
+	 * @param n_traj the number of trajectories to be synchronized
+	 */
+	static void timeSynchronization(VelocitySmoothing *traj, int n_traj);
+
+private:
+
+	/**
+	 * Compute T1, T2, T3 depending on the current state and velocity setpoint.
+	 * Minimize the total time of the trajectory
+	 */
+	void updateDurationsMinimizeTotalTime();
+
+	/**
+	 * Compute T1, T2, T3 depending on the current state and velocity setpoint.
+	 * @param T123 desired total time of the trajectory
+	 */
+	void updateDurationsGivenTotalTime(float T123);
+
+	/**
+	 * Compute the direction of the jerk to be applied in order to drive the current state
+	 * to the desired one
+	 */
+	int computeDirection();
+
+	/**
+	 * Compute the velocity at which the trajectory will be if the maximum jerk is applied
+	 * during the time required to cancel the current acceleration
+	 */
+	float computeVelAtZeroAcc();
+
+	/**
+	 * Compute increasing acceleration time
+	 */
+	inline float computeT1(float a0, float v3, float j_max, float a_max);
+
+	/**
+	 * Compute increasing acceleration time using total time constraint
+	 */
+	inline float computeT1(float T123, float a0, float v3, float j_max, float a_max);
+
+	/**
+	 * Saturate T1 in order to respect the maximum acceleration constraint
+	 */
+	inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max);
+
+	/**
+	 * Compute constant acceleration time
+	 */
+	inline float computeT2(float T1, float T3, float a0, float v3, float j_max);
+
+	/**
+	 * Compute constant acceleration time using total time constraint
+	 */
+	inline float computeT2(float T123, float T1, float T3);
+
+	/**
+	 * Compute decreasing acceleration time
+	 */
+	inline float computeT3(float T1, float a0, float j_max);
+
+	/**
+	 * Compute the jerk, acceleration, velocity and position
+	 * of a jerk-driven polynomial trajectory at a given time t
+	 * @param j jerk
+	 * @param a0 initial acceleration at t = 0
+	 * @param v0 initial velocity
+	 * @param x0 initial postion
+	 * @param t current time
+	 * @param d direction
+	 */
+	inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d);
+
+	/* Input */
+	float _vel_sp{0.0f};
+
+	/* Constraints */
+	float _max_jerk = 22.f;
+	float _max_accel = 8.f;
+	float _max_vel = 6.f;
+
+	/* State (previous setpoints) */
+	Trajectory _state{};
+	int _direction{0};
+
+	/* Initial conditions */
+	Trajectory _state_init{};
+
+	/* Duration of each phase */
+	float _T1 = 0.f; ///< Increasing acceleration [s]
+	float _T2 = 0.f; ///< Constant acceleration [s]
+	float _T3 = 0.f; ///< Decreasing acceleration [s]
+
+	float _local_time = 0.f; ///< Current local time
+};

src/lib/flight_tasks/tasks/Utility/VelocitySmoothingTest.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Velocity Smoothing library
+ * Run this test only using make tests TESTFILTER=VelocitySmoothing
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+
+#include "VelocitySmoothing.hpp"
+
+using namespace matrix;
+
+class VelocitySmoothingTest : public ::testing::Test
+{
+public:
+	void setConstraints(float j_max, float a_max, float v_max);
+	void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos);
+	void updateTrajectories(float dt, Vector3f velocity_setpoints);
+
+	VelocitySmoothing _trajectories[3];
+};
+
+void VelocitySmoothingTest::setConstraints(float j_max, float a_max, float v_max)
+{
+	for (int i = 0; i < 3; i++) {
+		_trajectories[i].setMaxJerk(j_max);
+		_trajectories[i].setMaxAccel(a_max);
+		_trajectories[i].setMaxVel(v_max);
+	}
+}
+
+void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vector3f x0)
+{
+	for (int i = 0; i < 3; i++) {
+		_trajectories[i].setCurrentAcceleration(a0(i));
+		_trajectories[i].setCurrentVelocity(v0(i));
+		_trajectories[i].setCurrentPosition(x0(i));
+	}
+}
+
+void VelocitySmoothingTest::updateTrajectories(float dt, Vector3f velocity_setpoints)
+{
+	for (int i = 0; i < 3; i++) {
+		_trajectories[i].updateTraj(dt);
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentJerk()), _trajectories[i].getMaxJerk());
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), _trajectories[i].getMaxAccel());
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity()), _trajectories[i].getMaxVel());
+	}
+
+	for (int i = 0; i < 3; i++) {
+		_trajectories[i].updateDurations(velocity_setpoints(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectories, 2);
+}
+
+TEST_F(VelocitySmoothingTest, testTimeSynchronization)
+{
+	// GIVEN: A set of constraints
+	const float j_max = 55.2f;
+	const float a_max = 6.f;
+	const float v_max = 6.f;
+
+	setConstraints(j_max, a_max, v_max);
+
+	// AND: A set of initial conditions
+	Vector3f a0(0.22f, 0.f, 0.22f);
+	Vector3f v0(2.47f, -5.59e-6f, 2.47f);
+	Vector3f x0(0.f, 0.f, 0.f);
+
+	setInitialConditions(a0, v0, x0);
+
+	// WHEN: We generate trajectories (time synchronized in XY) with constant setpoints and dt
+	Vector3f velocity_setpoints(-3.f, 1.f, 0.f);
+	updateTrajectories(0.f, velocity_setpoints);
+
+
+	// THEN: The X and Y trajectories should have the same total time (= time sunchronized)
+	EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
+}
+
+TEST_F(VelocitySmoothingTest, testConstantSetpoint)
+{
+	// GIVEN: A set of constraints
+	const float j_max = 55.2f;
+	const float a_max = 6.f;
+	const float v_max = 6.f;
+
+	setConstraints(j_max, a_max, v_max);
+
+	// AND: A set of initial conditions
+	Vector3f a0(0.f, 0.f, 0.f);
+	Vector3f v0(0.f, 0.f, 0.f);
+	Vector3f x0(0.f, 0.f, 0.f);
+
+	setInitialConditions(a0, v0, x0);
+
+	// WHEN: We generate trajectories with constant setpoints and dt
+	Vector3f velocity_setpoints(-3.f, 0.f, -1.f);
+
+	// Compute the number of steps required to reach desired value
+	// The updateTrajectories is first called once to compute the total time
+	const float dt = 0.01;
+	updateTrajectories(0.f, velocity_setpoints);
+	float t123 = _trajectories[0].getTotalTime();
+	int nb_steps = ceil(t123 / dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		updateTrajectories(dt, velocity_setpoints);
+	}
+
+	// THEN: All the trajectories should have reach their
+	// final state: desired velocity target and zero acceleration
+	for (int i = 0; i < 3; i++) {
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f);
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), 0.0001f);
+	}
+}
+
+TEST_F(VelocitySmoothingTest, testZeroSetpoint)
+{
+	// GIVEN: A set of null initial conditions
+	Vector3f a0(0.f, 0.f, 0.f);
+	Vector3f v0(0.f, 0.f, 0.f);
+	Vector3f x0(0.f, 0.f, 0.f);
+
+	setInitialConditions(a0, v0, x0);
+
+	// AND: Zero setpoints
+	Vector3f velocity_setpoints(0.f, 0.f, 0.f);
+	float t = 0.f;
+	const float dt = 0.01f;
+
+	// WHEN: We run a few times the algorithm
+	for (int i = 0; i < 60; i++) {
+		updateTrajectories(dt, velocity_setpoints);
+		t += dt;
+	}
+
+	// THEN: All the trajectories should still be zero
+	for (int i = 0; i < 3; i++) {
+		EXPECT_EQ(_trajectories[i].getCurrentJerk(), 0.f);
+		EXPECT_EQ(_trajectories[i].getCurrentAcceleration(), 0.f);
+		EXPECT_EQ(_trajectories[i].getCurrentVelocity(), 0.f);
+		EXPECT_EQ(_trajectories[i].getCurrentPosition(), 0.f);
+	}
+}

src/lib/flight_tasks/tasks/Utility/test_velocity_smoothing.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Velocity Smoothing library
+ * Build and run using: make && ./test_velocity_smoothing
+ */
+
+#include "VelocitySmoothing.hpp"
+#include <cstdio>
+#include <matrix/matrix/math.hpp>
+
+int main(int argc, char *argv[])
+{
+	VelocitySmoothing trajectory[3];
+
+	float a0[3] = {0.f, 0.f, 0.f};
+	float v0[3] = {0.f, 0.f, 0.f};
+	float x0[3] = {0.f, 0.f, 0.f};
+
+	float j_max = 55.2f;
+	float a_max = 6.f;
+	float v_max = 6.f;
+
+	for (int i = 0; i < 3; i++) {
+		trajectory[i].setMaxJerk(j_max);
+		trajectory[i].setMaxAccel(a_max);
+		trajectory[i].setMaxVel(v_max);
+		trajectory[i].setCurrentAcceleration(a0[i]);
+		trajectory[i].setCurrentVelocity(v0[i]);
+	}
+
+	const float dt = 0.01f;
+
+	float velocity_setpoint[3] = {1.f, 0.f, -1.f};
+
+	for (int i = 0; i < 3; i++) {
+		trajectory[i].updateDurations(velocity_setpoint[i]);
+	}
+
+	float t123 = trajectory[0].getTotalTime();
+	int nb_steps = ceil(t123 / dt);
+	printf("Nb steps = %d\n", nb_steps);
+
+	for (int i = 0; i < nb_steps; i++) {
+		for (int i = 0; i < 3; i++) {
+			trajectory[i].updateTraj(dt);
+		}
+
+		for (int i = 0; i < 3; i++) {
+			trajectory[i].updateDurations(velocity_setpoint[i]);
+		}
+
+		VelocitySmoothing::timeSynchronization(trajectory, 2);
+
+		for (int i = 0; i < 1; i++) {
+			printf("Traj[%d]\n", i);
+			printf("jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n",
+			       trajectory[i].getCurrentJerk(),
+			       trajectory[i].getCurrentAcceleration(),
+			       trajectory[i].getCurrentVelocity(),
+			       trajectory[i].getCurrentPosition());
+			printf("T1 = %.3f\tT2 = %.3f\tT3 = %.3f\n", trajectory[i].getT1(), trajectory[i].getT2(), trajectory[i].getT3());
+			printf("\n");
+		}
+	}
+
+	return 0;
+}