lib: name folders consistently

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskAuto
+	FlightTaskAuto.cpp
+)
+
+target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility)
+target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp

@@ -0,0 +1,488 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file FlightTaskAuto.cpp
+ */
+
+#include "FlightTaskAuto.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+static constexpr float SIGMA_NORM	= 0.001f;
+
+FlightTaskAuto::FlightTaskAuto() :
+	_obstacle_avoidance(this)
+{
+
+}
+
+bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_position_setpoint = _position;
+	_velocity_setpoint = _velocity;
+	_yaw_setpoint = _yaw_sp_prev = _yaw;
+	_yawspeed_setpoint = 0.0f;
+	_setDefaultConstraints();
+	return ret;
+}
+
+bool FlightTaskAuto::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+
+	_sub_home_position.update();
+	_sub_manual_control_setpoint.update();
+	_sub_vehicle_status.update();
+	_sub_triplet_setpoint.update();
+
+	// require valid reference and valid target
+	ret = ret && _evaluateGlobalReference() && _evaluateTriplets();
+	// require valid position
+	ret = ret && PX4_ISFINITE(_position(0))
+	      && PX4_ISFINITE(_position(1))
+	      && PX4_ISFINITE(_position(2))
+	      && PX4_ISFINITE(_velocity(0))
+	      && PX4_ISFINITE(_velocity(1))
+	      && PX4_ISFINITE(_velocity(2));
+
+	return ret;
+}
+
+bool FlightTaskAuto::updateFinalize()
+{
+	// All the auto FlightTasks have to comply with defined maximum yaw rate
+	// If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value
+	// it will see its setpoint constrained here
+	_limitYawRate();
+	_constraints.want_takeoff = _checkTakeoff();
+	return true;
+}
+
+void FlightTaskAuto::_limitYawRate()
+{
+	const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get());
+
+	_yaw_sp_aligned = true;
+
+	if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) {
+		// Limit the rate of change of the yaw setpoint
+		const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
+		const float dyaw_max = yawrate_max * _deltatime;
+		const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max);
+		float yaw_setpoint_sat = _yaw_sp_prev + dyaw;
+		yaw_setpoint_sat = matrix::wrap_pi(yaw_setpoint_sat);
+
+		// The yaw setpoint is aligned when it is within tolerance
+		_yaw_sp_aligned = fabsf(_yaw_setpoint - yaw_setpoint_sat) < math::radians(_param_mis_yaw_err.get());
+
+		_yaw_setpoint = yaw_setpoint_sat;
+		_yaw_sp_prev = _yaw_setpoint;
+	}
+
+	if (PX4_ISFINITE(_yawspeed_setpoint)) {
+		_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
+
+		// The yaw setpoint is aligned when its rate is not saturated
+		_yaw_sp_aligned = fabsf(_yawspeed_setpoint) < yawrate_max;
+	}
+}
+
+bool FlightTaskAuto::_evaluateTriplets()
+{
+	// TODO: fix the issues mentioned below
+	// We add here some conditions that are only required because:
+	// 1. navigator continuously sends triplet during mission due to yaw setpoint. This
+	// should be removed in the navigator and only updates if the current setpoint actually has changed.
+	//
+	// 2. navigator should be responsible to send always three valid setpoints. If there is only one setpoint,
+	// then previous will be set to current vehicle position and next will be set equal to setpoint.
+	//
+	// 3. navigator originally only supports gps guided maneuvers. However, it now also supports some flow-specific features
+	// such as land and takeoff. The navigator should use for auto takeoff/land with flow the position in xy at the moment the
+	// takeoff/land was initiated. Until then we do this kind of logic here.
+
+	// Check if triplet is valid. There must be at least a valid altitude.
+
+	if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
+		// Best we can do is to just set all waypoints to current state and return false.
+		_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
+		_type = WaypointType::position;
+		return false;
+	}
+
+	_type = (WaypointType)_sub_triplet_setpoint.get().current.type;
+
+	// Always update cruise speed since that can change without waypoint changes.
+	_mc_cruise_speed = _sub_triplet_setpoint.get().current.cruising_speed;
+
+	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
+		// If no speed is planned use the default cruise speed as limit
+		_mc_cruise_speed = _constraints.speed_xy;
+	}
+
+	// Ensure planned cruise speed is below the maximum such that the smooth trajectory doesn't get capped
+	_mc_cruise_speed = math::min(_mc_cruise_speed, _param_mpc_xy_vel_max.get());
+
+	// Temporary target variable where we save the local reprojection of the latest navigator current triplet.
+	Vector3f tmp_target;
+
+	if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat)
+	    || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) {
+		// No position provided in xy. Lock position
+		if (!PX4_ISFINITE(_lock_position_xy(0))) {
+			tmp_target(0) = _lock_position_xy(0) = _position(0);
+			tmp_target(1) = _lock_position_xy(1) = _position(1);
+
+		} else {
+			tmp_target(0) = _lock_position_xy(0);
+			tmp_target(1) = _lock_position_xy(1);
+		}
+
+	} else {
+		// reset locked position if current lon and lat are valid
+		_lock_position_xy.setAll(NAN);
+
+		// Convert from global to local frame.
+		map_projection_project(&_reference_position,
+				       _sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1));
+	}
+
+	tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude);
+
+	// Check if anything has changed. We do that by comparing the temporary target
+	// to the internal _triplet_target.
+	// TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed.
+
+	bool triplet_update = true;
+
+	if (PX4_ISFINITE(_triplet_target(0))
+	    && PX4_ISFINITE(_triplet_target(1))
+	    && PX4_ISFINITE(_triplet_target(2))
+	    && fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f
+	    && fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f
+	    && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f) {
+		// Nothing has changed: just keep old waypoints.
+		triplet_update = false;
+
+	} else {
+		_triplet_target = tmp_target;
+		_target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius;
+
+		if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
+			// Horizontal target is not finite.
+			_triplet_target(0) = _position(0);
+			_triplet_target(1) = _position(1);
+		}
+
+		if (!PX4_ISFINITE(_triplet_target(2))) {
+			_triplet_target(2) = _position(2);
+		}
+
+		// If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp.
+		_prev_prev_wp = _triplet_prev_wp;
+
+		if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
+			map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat,
+					       _sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
+			_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
+
+		} else {
+			_triplet_prev_wp = _position;
+		}
+
+		if (_type == WaypointType::loiter) {
+			_triplet_next_wp = _triplet_target;
+
+		} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
+			map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat,
+					       _sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
+			_triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude);
+
+		} else {
+			_triplet_next_wp = _triplet_target;
+		}
+	}
+
+	if (_ext_yaw_handler != nullptr) {
+		// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
+		(_param_wv_en.get() && _sub_triplet_setpoint.get().current.allow_weather_vane) ?	_ext_yaw_handler->activate() :
+		_ext_yaw_handler->deactivate();
+	}
+
+	// set heading
+	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+		_yaw_setpoint = _yaw;
+		_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
+
+	} else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) {
+		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
+		_yaw_setpoint = NAN;
+
+	} else {
+		if (_sub_triplet_setpoint.get().current.yaw_valid) {
+			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
+
+		} else {
+			_set_heading_from_mode();
+		}
+
+		_yawspeed_setpoint = NAN;
+	}
+
+	// Calculate the current vehicle state and check if it has updated.
+	State previous_state = _current_state;
+	_current_state = _getCurrentState();
+
+	if (triplet_update || (_current_state != previous_state)) {
+		_updateInternalWaypoints();
+		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
+	}
+
+	if (_param_com_obs_avoid.get()
+	    && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
+				_triplet_next_wp,
+				_sub_triplet_setpoint.get().next.yaw,
+				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
+				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
+		_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
+	}
+
+	return true;
+}
+
+void FlightTaskAuto::_set_heading_from_mode()
+{
+
+	Vector2f v; // Vector that points towards desired location
+
+	switch (_param_mpc_yaw_mode.get()) {
+
+	case 0: // Heading points towards the current waypoint.
+	case 4: // Same as 0 but yaw fisrt and then go
+		v = Vector2f(_target) - Vector2f(_position);
+		break;
+
+	case 1: // Heading points towards home.
+		if (_sub_home_position.get().valid_hpos) {
+			v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position);
+		}
+
+		break;
+
+	case 2: // Heading point away from home.
+		if (_sub_home_position.get().valid_hpos) {
+			v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x);
+		}
+
+		break;
+
+	case 3: // Along trajectory.
+		// The heading depends on the kind of setpoint generation. This needs to be implemented
+		// in the subclasses where the velocity setpoints are generated.
+		v.setAll(NAN);
+		break;
+	}
+
+	if (PX4_ISFINITE(v.length())) {
+		// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
+		// radius, lock yaw to current yaw.
+		// This prevents excessive yawing.
+		if (v.length() > _target_acceptance_radius) {
+			_compute_heading_from_2D_vector(_yaw_setpoint, v);
+			_yaw_lock = false;
+
+		} else {
+			if (!_yaw_lock) {
+				_yaw_setpoint = _yaw;
+				_yaw_lock = true;
+			}
+		}
+
+	} else {
+		_yaw_lock = false;
+		_yaw_setpoint = NAN;
+	}
+}
+
+bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp)
+{
+	return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
+}
+
+bool FlightTaskAuto::_evaluateGlobalReference()
+{
+	// check if reference has changed and update.
+	// Only update if reference timestamp has changed AND no valid reference altitude
+	// is available.
+	// TODO: this needs to be revisited and needs a more clear implementation
+	if (_sub_vehicle_local_position.get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) {
+		// don't need to update anything
+		return true;
+	}
+
+	double ref_lat = _sub_vehicle_local_position.get().ref_lat;
+	double ref_lon = _sub_vehicle_local_position.get().ref_lon;
+	_reference_altitude = _sub_vehicle_local_position.get().ref_alt;
+
+	if (!_sub_vehicle_local_position.get().z_global) {
+		// we have no valid global altitude
+		// set global reference to local reference
+		_reference_altitude = 0.0f;
+	}
+
+	if (!_sub_vehicle_local_position.get().xy_global) {
+		// we have no valid global alt/lat
+		// set global reference to local reference
+		ref_lat = 0.0;
+		ref_lon = 0.0;
+	}
+
+	// init projection
+	map_projection_init(&_reference_position, ref_lat, ref_lon);
+
+	// check if everything is still finite
+	return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon);
+}
+
+void FlightTaskAuto::_setDefaultConstraints()
+{
+	FlightTask::_setDefaultConstraints();
+
+	// only adjust limits if the new limit is lower
+	if (_constraints.speed_xy >= _param_mpc_xy_cruise.get()) {
+		_constraints.speed_xy = _param_mpc_xy_cruise.get();
+	}
+}
+
+Vector2f FlightTaskAuto::_getTargetVelocityXY()
+{
+	// guard against any bad velocity values
+	const float vx = _sub_triplet_setpoint.get().current.vx;
+	const float vy = _sub_triplet_setpoint.get().current.vy;
+	bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) &&
+			      _sub_triplet_setpoint.get().current.velocity_valid;
+
+	if (velocity_valid) {
+		return Vector2f(vx, vy);
+
+	} else {
+		// just return zero speed
+		return Vector2f{};
+	}
+}
+
+State FlightTaskAuto::_getCurrentState()
+{
+	// Calculate the vehicle current state based on the Navigator triplets and the current position.
+	Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
+	Vector2f pos_to_target(_triplet_target - _position);
+	Vector2f prev_to_pos(_position - _triplet_prev_wp);
+	// Calculate the closest point to the vehicle position on the line prev_wp - target
+	_closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
+
+	State return_state = State::none;
+
+	if (u_prev_to_target * pos_to_target < 0.0f) {
+		// Target is behind.
+		return_state = State::target_behind;
+
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
+		// Current position is more than cruise speed in front of previous setpoint.
+		return_state = State::previous_infront;
+
+	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
+		// Vehicle is more than cruise speed off track.
+		return_state = State::offtrack;
+
+	}
+
+	return return_state;
+}
+
+void FlightTaskAuto::_updateInternalWaypoints()
+{
+	// The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp.
+	// The cases where it differs:
+	// 1. The vehicle already passed the target -> go straight to target
+	// 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
+	// 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
+	switch (_current_state) {
+	case State::target_behind:
+		_target = _triplet_target;
+		_prev_wp = _position;
+		_next_wp = _triplet_next_wp;
+		break;
+
+	case State::previous_infront:
+		_next_wp = _triplet_target;
+		_target = _triplet_prev_wp;
+		_prev_wp = _position;
+		break;
+
+	case State::offtrack:
+		_next_wp = _triplet_target;
+		_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
+		_prev_wp = _position;
+		break;
+
+	case State::none:
+		_target = _triplet_target;
+		_prev_wp = _triplet_prev_wp;
+		_next_wp = _triplet_next_wp;
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
+{
+	if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
+		v.normalize();
+		// To find yaw: take dot product of x = (1,0) and v
+		// and multiply by the sign given of cross product of x and v.
+		// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
+		// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
+		heading =  math::sign(v(1)) * wrap_pi(acosf(v(0)));
+		return true;
+	}
+
+	// heading unknown and therefore do not change heading
+	return false;
+}

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAuto.hpp
+ *
+ * Map from global triplet to local quadruple.
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/position_setpoint.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <lib/ecl/geo/geo.h>
+#include <ObstacleAvoidance.hpp>
+
+/**
+ * This enum has to agree with position_setpoint_s type definition
+ * The only reason for not using the struct position_setpoint is because
+ * of the size
+ */
+enum class WaypointType : int {
+	position = position_setpoint_s::SETPOINT_TYPE_POSITION,
+	velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
+	loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
+	takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
+	land = position_setpoint_s::SETPOINT_TYPE_LAND,
+	idle = position_setpoint_s::SETPOINT_TYPE_IDLE,
+	offboard = position_setpoint_s::SETPOINT_TYPE_OFFBOARD, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks
+	follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET,
+};
+
+enum class State {
+	offtrack, /**< Vehicle is more than cruise speed away from track */
+	target_behind, /**< Vehicle is in front of target. */
+	previous_infront, /**< Vehilce is behind previous waypoint.*/
+	none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */
+};
+
+class FlightTaskAuto : public FlightTask
+{
+public:
+	FlightTaskAuto();
+
+	virtual ~FlightTaskAuto() = default;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool updateInitialize() override;
+	bool updateFinalize() override;
+
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+
+protected:
+	void _setDefaultConstraints() override;
+	matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
+	void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
+	bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
+
+	matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
+	matrix::Vector3f _prev_wp{}; /**< Previous waypoint  (local frame). If no previous triplet is available, the prev_wp is set to current position. */
+	matrix::Vector3f _target{}; /**< Target waypoint  (local frame).*/
+	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
+	float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
+	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
+
+	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
+	uORB::SubscriptionData<manual_control_setpoint_s>	_sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
+	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
+
+	State _current_state{State::none};
+	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
+	int _mission_gear{landing_gear_s::GEAR_KEEP};
+
+	float _yaw_sp_prev{NAN};
+	bool _yaw_sp_aligned{false};
+
+	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
+					(ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
+					(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
+					_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
+					(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
+					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
+					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
+					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
+					(ParamBool<px4::params::WV_EN>) _param_wv_en // enable/disable weather vane (VTOL)
+				       );
+
+private:
+	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
+	bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
+
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
+
+	matrix::Vector3f
+	_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
+	matrix::Vector3f
+	_triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/
+	matrix::Vector3f
+	_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
+	matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
+
+	map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
+	float _reference_altitude{NAN};  /**< Altitude relative to ground. */
+	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
+
+	WeatherVane *_ext_yaw_handler{nullptr};	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+
+	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
+	bool _evaluateTriplets(); /**< Checks and sets triplets. */
+	bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
+	bool _evaluateGlobalReference(); /**< Check is global reference is available. */
+	State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
+	void _set_heading_from_mode(); /**< @see  MPC_YAW_MODE */
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskAutoFollowMe
+	FlightTaskAutoFollowMe.cpp
+)
+
+target_link_libraries(FlightTaskAutoFollowMe PUBLIC FlightTaskAuto)
+target_include_directories(FlightTaskAutoFollowMe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp

@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoFollowMe.cpp
+ */
+
+#include "FlightTaskAutoFollowMe.hpp"
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+bool FlightTaskAutoFollowMe::update()
+{
+	_position_setpoint = _target;
+	matrix::Vector2f vel_sp = _getTargetVelocityXY();
+	_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
+	return true;
+}

src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoFollowMe.hpp
+ *
+ * Flight task for autonomous, gps driven follow-me mode.
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+
+class FlightTaskAutoFollowMe : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoFollowMe() = default;
+	virtual ~FlightTaskAutoFollowMe() = default;
+	bool update() override;
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskAutoLine
+	FlightTaskAutoLine.cpp
+)
+
+target_link_libraries(FlightTaskAutoLine PUBLIC FlightTaskAutoMapper)
+target_include_directories(FlightTaskAutoLine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp

@@ -0,0 +1,275 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightAutoLine.cpp
+ */
+
+#include "FlightTaskAutoLine.hpp"
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+static constexpr float SIGMA_NORM		=	0.001f;
+
+void FlightTaskAutoLine::_generateSetpoints()
+{
+	if (!PX4_ISFINITE(_yaw_setpoint)) {
+		// no valid heading -> set heading along track
+		_generateHeadingAlongTrack();
+	}
+
+	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
+		// Wait for the yaw setpoint to be aligned
+		if (!_position_locked) {
+			_velocity_setpoint.setAll(0.f);
+			_position_setpoint = _position;
+			_position_locked = true;
+		}
+
+	} else {
+		_position_locked = false;
+		_generateAltitudeSetpoints();
+		_generateXYsetpoints();
+	}
+}
+
+void FlightTaskAutoLine::_setSpeedAtTarget()
+{
+	// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
+	float angle = 2.0f; // minimum angle
+	_speed_at_target = 0.0f;
+
+	if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
+	    (Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius)) {
+		// angle = cosf(x) + 1.0f
+		angle =
+			Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
+			* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+			+ 1.0f;
+		_speed_at_target = math::expontialFromLimits(angle, 0.0f, _param_mpc_cruise_90.get(), _mc_cruise_speed);
+	}
+}
+
+
+void FlightTaskAutoLine::_generateHeadingAlongTrack()
+{
+	Vector2f prev_to_dest(_target - _prev_wp);
+	_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
+
+}
+
+void FlightTaskAutoLine::_generateXYsetpoints()
+{
+	_setSpeedAtTarget();
+	Vector2f pos_sp_to_dest(_target - _position_setpoint);
+	const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get();
+
+	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
+	    (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
+
+		// Vehicle reached target in xy and no passing required. Lock position
+		_position_setpoint(0) = _target(0);
+		_position_setpoint(1) = _target(1);
+		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
+
+	} else {
+		// Vehicle needs to pass waypoint
+		// Ensure that vehicle never gets stuck because of too low target-speed
+		_speed_at_target = math::max(_speed_at_target, 0.5f);
+
+		// Get various path specific vectors. */
+		Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
+		Vector2f prev_to_pos(_position - _prev_wp);
+		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+		Vector2f closest_to_dest(_target - _position);
+		Vector2f prev_to_dest(_target - _prev_wp);
+		float speed_sp_track = _mc_cruise_speed;
+		float speed_sp_prev_track = math::max(Vector2f(_velocity_setpoint) * u_prev_to_dest, 0.0f);
+
+		// Distance to target when brake should occur. The assumption is made that
+		// 1.5 * cruising speed is enough to break.
+		float target_threshold = 1.5f * _mc_cruise_speed;
+		float speed_threshold = _mc_cruise_speed;
+		const float threshold_max = target_threshold;
+
+		if (target_threshold > 0.5f * prev_to_dest.length()) {
+			// Target threshold cannot be more than distance from previous to target
+			target_threshold = 0.5f * prev_to_dest.length();
+		}
+
+		// Compute maximum speed at target threshold */
+		if (threshold_max > _target_acceptance_radius) {
+			float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius);
+			speed_threshold = m * (target_threshold - _target_acceptance_radius) + _speed_at_target; // speed at transition
+		}
+
+		// Either accelerate or decelerate
+		if (closest_to_dest.length() < target_threshold) {
+
+			// Vehicle is close to destination. Start to decelerate
+
+			if (!has_reached_altitude) {
+				// Altitude is not reached yet. Vehicle has to stop first before proceeding
+				_speed_at_target = 0.0f;
+			}
+
+			float acceptance_radius = _target_acceptance_radius;
+
+			if (_speed_at_target < 0.01f) {
+				// If vehicle wants to stop at the target, then set acceptance radius to zero as well.
+				acceptance_radius = 0.0f;
+			}
+
+			if ((target_threshold - acceptance_radius) >= SIGMA_NORM) {
+
+				// Slow down depending on distance to target minus acceptance radius.
+				float m = (speed_threshold - _speed_at_target) / (target_threshold - acceptance_radius);
+				speed_sp_track = m * (closest_to_dest.length() - acceptance_radius) + _speed_at_target; // speed at transition
+
+			} else {
+				speed_sp_track = _speed_at_target;
+			}
+
+			// If we are close to target and the previous speed setpoint along track was smaller than
+			// current speed setpoint along track, then take over the previous one.
+			// This ensures smoothness since we anyway want to slow down.
+			if ((speed_sp_prev_track < speed_sp_track)
+			    && (speed_sp_track * speed_sp_prev_track > 0.0f)
+			    && (speed_sp_prev_track > _speed_at_target)) {
+				speed_sp_track = speed_sp_prev_track;
+			}
+
+		} else {
+
+			// Vehicle is still far from destination. Accelerate or keep maximum target speed.
+			float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;
+
+			// If yaw offset is large, only accelerate with 0.5 m/s^2.
+			float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f;
+
+			if (acc_track > acc_max) {
+				// accelerate towards target
+				speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
+			}
+
+			// ensure that desired speed does not exceed speed at threshold
+			speed_sp_track = math::min(speed_threshold, speed_sp_track);
+		}
+
+		speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
+
+		_position_setpoint(0) = closest_pt(0);
+		_position_setpoint(1) = closest_pt(1);
+		Vector2f velocity_sp_xy = u_prev_to_dest * speed_sp_track;
+		_velocity_setpoint(0) =  velocity_sp_xy(0);
+		_velocity_setpoint(1) =  velocity_sp_xy(1);
+	}
+}
+
+void FlightTaskAutoLine::_generateAltitudeSetpoints()
+{
+	// Total distance between previous and target set-point.
+	const float dist = fabsf(_target(2) - _prev_wp(2));
+
+	// If target has not been reached, then compute set-point depending on maximum velocity.
+	if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _target(2)) > 0.1f)) {
+
+		// get various distances */
+		const float dist_to_prev = fabsf(_position(2) - _prev_wp(2));
+		const float dist_to_target = fabsf(_target(2) - _position(2));
+
+		// check sign
+		const bool flying_upward = _target(2) < _position(2);
+
+		// limit vertical downwards speed (positive z) close to ground
+		// for now we use the altitude above home and assume that we want to land at same height as we took off
+		float vel_limit = math::gradual(_alt_above_ground,
+						_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+						_param_mpc_land_speed.get(), _constraints.speed_down);
+
+		// Speed at threshold is by default maximum speed. Threshold defines
+		// the point in z at which vehicle slows down to reach target altitude.
+		float speed_sp = (flying_upward) ? _constraints.speed_up : vel_limit;
+
+		// Target threshold defines the distance to target(2) at which
+		// the vehicle starts to slow down to approach the target smoothly.
+		float target_threshold = speed_sp * 1.5f;
+
+		// If the total distance in z is NOT 2x distance of target_threshold, we
+		// will need to adjust the final_velocity in z.
+		const bool is_2_target_threshold = dist >= 2.0f * target_threshold;
+		const float min_vel = 0.2f; // minimum velocity: this is needed since estimation is not perfect
+		const float slope = (speed_sp - min_vel) / (target_threshold); // defines the the acceleration when slowing down */
+
+		if (!is_2_target_threshold) {
+			// Adjust final_velocity since we are already are close to target and therefore it is not necessary to accelerate
+			// upwards with full speed.
+			target_threshold = dist * 0.5f;
+			// get the velocity at target_threshold
+			speed_sp = slope * (target_threshold) + min_vel;
+		}
+
+		// we want to slow down
+		if (dist_to_target < target_threshold) {
+
+			speed_sp = slope * dist_to_target + min_vel;
+
+		} else if (dist_to_prev < target_threshold) {
+			// we want to accelerate
+
+			const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
+			const float acc_max = (flying_upward) ? (_param_mpc_acc_up_max.get() * 0.5f) : (_param_mpc_acc_down_max.get() * 0.5f);
+
+			if (acc > acc_max) {
+				speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
+			}
+		}
+
+		// make sure vel_sp_z is always positive
+		if (speed_sp < 0.0f) {
+			PX4_WARN("speed cannot be smaller than 0");
+			speed_sp = 0.0f;
+		}
+
+		// get the sign of vel_sp_z
+		_velocity_setpoint(2) = (flying_upward) ? -speed_sp : speed_sp;
+		_position_setpoint(2) = NAN; // We don't care about position setpoint
+
+	} else {
+
+		// vehicle reached desired target altitude
+		_velocity_setpoint(2) = 0.0f;
+		_position_setpoint(2) = _target(2);
+	}
+}

src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoLine.hpp
+ *
+ * Flight task for autonomous, gps driven mode. The vehicle flies
+ * along a straight line in between waypoints.
+ */
+
+#pragma once
+
+#include "FlightTaskAutoMapper.hpp"
+
+class FlightTaskAutoLine : public FlightTaskAutoMapper
+{
+public:
+	FlightTaskAutoLine() = default;
+	virtual ~FlightTaskAutoLine() = default;
+
+protected:
+
+	void _generateSetpoints() override; /**< Generate setpoints along line. */
+
+	void _generateHeadingAlongTrack(); /**< Generates heading along track. */
+	void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
+	void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+				       );
+
+
+private:
+	void _setSpeedAtTarget(); /**< Sets desiered speed at target */
+	float _speed_at_target{0.0f};
+	bool _position_locked{false};
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskAutoLineSmoothVel
+	FlightTaskAutoLineSmoothVel.cpp
+)
+
+target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper2 FlightTaskUtility)
+target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -0,0 +1,370 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightAutoLine.cpp
+ */
+
+#include "FlightTaskAutoLineSmoothVel.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskAutoMapper2::activate(last_setpoint);
+
+	checkSetpoints(last_setpoint);
+	const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
+	const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].reset(last_setpoint.acceleration[i], vel_prev(i), pos_prev(i));
+	}
+
+	_yaw_sp_prev = last_setpoint.yaw;
+	_updateTrajConstraints();
+
+	return ret;
+}
+
+void FlightTaskAutoLineSmoothVel::reActivate()
+{
+	// On ground, reset acceleration and velocity to zero
+	for (int i = 0; i < 2; ++i) {
+		_trajectory[i].reset(0.f, 0.f, _position(i));
+	}
+
+	_trajectory[2].reset(0.f, 0.7f, _position(2));
+}
+
+void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
+
+	if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
+
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
+
+	if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
+
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	for (int i = 0; i < 3; i++) {
+		if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
+	}
+
+	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
+}
+
+/**
+ * EKF reset handling functions
+ * Those functions are called by the base FlightTask in
+ * case of an EKF reset event
+ */
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY()
+{
+	_trajectory[0].setCurrentPosition(_position(0));
+	_trajectory[1].setCurrentPosition(_position(1));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY()
+{
+	_trajectory[0].setCurrentVelocity(_velocity(0));
+	_trajectory[1].setCurrentVelocity(_velocity(1));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ()
+{
+	_trajectory[2].setCurrentPosition(_position(2));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ()
+{
+	_trajectory[2].setCurrentVelocity(_velocity(2));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
+{
+	_yaw_sp_prev += delta_psi;
+}
+
+void FlightTaskAutoLineSmoothVel::_generateSetpoints()
+{
+	_prepareSetpoints();
+	_generateTrajectory();
+
+	if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
+		// no valid heading -> generate heading in this flight task
+		_generateHeading();
+	}
+}
+
+void FlightTaskAutoLineSmoothVel::_generateHeading()
+{
+	// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
+	if (!_generateHeadingAlongTraj()) {
+		_yaw_setpoint = _yaw_sp_prev;
+	}
+}
+
+bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
+{
+	bool res = false;
+	Vector2f vel_sp_xy(_velocity_setpoint);
+	Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
+
+	if ((vel_sp_xy.length() > .1f) &&
+	    (traj_to_target.length() > _target_acceptance_radius)) {
+		// Generate heading from velocity vector, only if it is long enough
+		// and if the drone is far enough from the target
+		_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
+		res = true;
+	}
+
+	return res;
+}
+
+/* Constrain some value vith a constrain depending on the sign of the constraint
+ * Example: 	- if the constrain is -5, the value will be constrained between -5 and 0
+ * 		- if the constrain is 5, the value will be constrained between 0 and 5
+ */
+float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
+{
+	const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
+	const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
+
+	return math::constrain(val, min, max);
+}
+
+float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
+{
+	return math::sign(val) * math::min(fabsf(val), fabsf(max));
+}
+
+float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const
+{
+	// Compute the maximum allowed speed at the waypoint assuming that we want to
+	// connect the two lines (prev-current and current-next)
+	// with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius
+	// The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius.
+	// This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that
+	// the real acceptance radius is smaller.
+	// It can be that the next waypoint is the last one or that the drone will have to stop for some other reason
+	// so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint.
+	float speed_at_target = 0.0f;
+
+	const float distance_current_next = (_target - _next_wp).xy().norm();
+	const bool waypoint_overlap = (_target - _prev_wp).xy().norm() < _target_acceptance_radius;
+	const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
+
+
+	if (distance_current_next > 0.001f &&
+	    !waypoint_overlap &&
+	    yaw_align_check_pass) {
+		Vector3f pos_traj;
+		pos_traj(0) = _trajectory[0].getCurrentPosition();
+		pos_traj(1) = _trajectory[1].getCurrentPosition();
+		pos_traj(2) = _trajectory[2].getCurrentPosition();
+		// Max speed between current and next
+		const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next, next_target_speed);
+		const float alpha = acosf(Vector2f((_target - pos_traj).xy()).unit_or_zero().dot(
+						  Vector2f((_target - _next_wp).xy()).unit_or_zero()));
+		// We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account
+		// that there is a jerk limit (a direct transition from line to circle is not possible)
+		// MPC_XY_TRAJ_P should be between 0 and 1.
+		float accel_tmp = _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get();
+		float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(alpha,
+					  accel_tmp,
+					  _target_acceptance_radius);
+		speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed);
+	}
+
+	return speed_at_target;
+}
+
+float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const
+{
+	float max_speed = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_auto.get(),
+			  _param_mpc_acc_hor.get(),
+			  braking_distance,
+			  final_speed);
+
+	return max_speed;
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
+{
+	// Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified
+	// that one is used as a velocity limit.
+	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
+
+	_want_takeoff = false;
+
+	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
+		// Wait for the yaw setpoint to be aligned
+		_velocity_setpoint.setAll(0.f);
+
+	} else {
+		if (PX4_ISFINITE(_position_setpoint(0)) &&
+		    PX4_ISFINITE(_position_setpoint(1))) {
+			// Use position setpoints to generate velocity setpoints
+
+			// Get various path specific vectors
+			Vector3f pos_traj;
+			pos_traj(0) = _trajectory[0].getCurrentPosition();
+			pos_traj(1) = _trajectory[1].getCurrentPosition();
+			pos_traj(2) = _trajectory[2].getCurrentPosition();
+			Vector2f pos_traj_to_dest_xy = (_position_setpoint - pos_traj).xy();
+			Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+
+			const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
+
+			// If the drone has to change altitude, stop at the waypoint, otherwise fly through
+			const float arrival_speed = has_reached_altitude ? _getSpeedAtTarget(0.f) : 0.f;
+			const Vector2f max_arrival_vel = u_pos_traj_to_dest_xy * arrival_speed;
+
+			Vector2f vel_abs_max_xy(_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)), max_arrival_vel(0)),
+						_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)), max_arrival_vel(1)));
+
+
+			const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed;
+
+			// Constrain the norm of each component using min and max values
+			Vector2f vel_sp_constrained_xy(_constrainAbs(vel_sp_xy(0), vel_abs_max_xy(0)),
+						       _constrainAbs(vel_sp_xy(1), vel_abs_max_xy(1)));
+
+			for (int i = 0; i < 2; i++) {
+				// If available, constrain the velocity using _velocity_setpoint(.)
+				if (PX4_ISFINITE(_velocity_setpoint(i))) {
+					_velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained_xy(i), _velocity_setpoint(i));
+
+				} else {
+					_velocity_setpoint(i) = vel_sp_constrained_xy(i);
+				}
+			}
+		}
+
+		if (PX4_ISFINITE(_position_setpoint(2))) {
+			const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
+					       _param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
+
+			// If available, constrain the velocity using _velocity_setpoint(.)
+			if (PX4_ISFINITE(_velocity_setpoint(2))) {
+				_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
+
+			} else {
+				_velocity_setpoint(2) = vel_sp_z;
+			}
+
+			_want_takeoff = _velocity_setpoint(2) < -0.3f;
+		}
+	}
+}
+
+void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
+{
+	// Update the constraints of the trajectories
+	_trajectory[0].setMaxAccel(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading
+	_trajectory[1].setMaxAccel(_param_mpc_acc_hor.get());
+	_trajectory[0].setMaxVel(_param_mpc_xy_vel_max.get());
+	_trajectory[1].setMaxVel(_param_mpc_xy_vel_max.get());
+	_trajectory[0].setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
+	_trajectory[1].setMaxJerk(_param_mpc_jerk_auto.get());
+	_trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get());
+
+	if (_velocity_setpoint(2) < 0.f) { // up
+		_trajectory[2].setMaxAccel(_param_mpc_acc_up_max.get());
+		_trajectory[2].setMaxVel(_param_mpc_z_vel_max_up.get());
+
+	} else { // down
+		_trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get());
+		_trajectory[2].setMaxVel(_param_mpc_z_vel_max_dn.get());
+	}
+}
+
+void FlightTaskAutoLineSmoothVel::_generateTrajectory()
+{
+	if (!PX4_ISFINITE(_velocity_setpoint(0)) || !PX4_ISFINITE(_velocity_setpoint(1))
+	    || !PX4_ISFINITE(_velocity_setpoint(2))) {
+		return;
+	}
+
+	/* Slow down the trajectory by decreasing the integration time based on the position error.
+	 * This is only performed when the drone is behind the trajectory
+	 */
+	Vector2f position_trajectory_xy(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
+	Vector2f position_xy(_position);
+	Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity());
+	Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
+	float position_error = drone_to_trajectory_xy.length();
+
+	float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
+
+	// Don't stretch time if the drone is ahead of the position setpoint
+	if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
+		time_stretch = 1.f;
+	}
+
+	Vector3f jerk_sp_smooth;
+	Vector3f accel_sp_smooth;
+	Vector3f vel_sp_smooth;
+	Vector3f pos_sp_smooth;
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].updateTraj(_deltatime, time_stretch);
+		jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
+		accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration();
+		vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity();
+		pos_sp_smooth(i) = _trajectory[i].getCurrentPosition();
+	}
+
+	_updateTrajConstraints();
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].updateDurations(_velocity_setpoint(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
+
+	_jerk_setpoint = jerk_sp_smooth;
+	_acceleration_setpoint = accel_sp_smooth;
+	_velocity_setpoint = vel_sp_smooth;
+	_position_setpoint = pos_sp_smooth;
+}

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoLineSmoothVel.hpp
+ *
+ * Flight task for autonomous, gps driven mode. The vehicle flies
+ * along a straight line in between waypoints.
+ */
+
+#pragma once
+
+#include "FlightTaskAutoMapper2.hpp"
+#include "VelocitySmoothing.hpp"
+
+class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
+{
+public:
+	FlightTaskAutoLineSmoothVel() = default;
+	virtual ~FlightTaskAutoLineSmoothVel() = default;
+
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	void reActivate() override;
+
+protected:
+
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
+
+	/** Reset position or velocity setpoints in case of EKF reset event */
+	void _ekfResetHandlerPositionXY() override;
+	void _ekfResetHandlerVelocityXY() override;
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+	void _ekfResetHandlerHeading(float delta_psi) override;
+
+	void _generateSetpoints() override; /**< Generate setpoints along line. */
+	void _generateHeading();
+	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
+
+	static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
+
+	static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */
+
+	/** Give 0 if next is the last target **/
+	float _getSpeedAtTarget(float next_target_speed) const;
+	float _getMaxSpeedFromDistance(float braking_distance, float final_speed) const;
+
+	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
+	void _updateTrajConstraints();
+	void _generateTrajectory();
+
+	/** determines when to trigger a takeoff (ignored in flight) */
+	bool _checkTakeoff() override { return _want_takeoff; };
+	bool _want_takeoff{false};
+
+	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper2,
+					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
+					(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_AUTO>) _param_mpc_jerk_auto,
+					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
+					(ParamFloat<px4::params::MPC_Z_TRAJ_P>) _param_mpc_z_traj_p
+				       );
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskAutoMapper
+	FlightTaskAutoMapper.cpp
+)
+
+target_link_libraries(FlightTaskAutoMapper PUBLIC FlightTaskAuto)
+target_include_directories(FlightTaskAutoMapper PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightAutoMapper.cpp
+ */
+
+#include "FlightTaskAutoMapper.hpp"
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskAuto::activate(last_setpoint);
+	_reset();
+	return ret;
+}
+
+bool FlightTaskAutoMapper::update()
+{
+	// always reset constraints because they might change depending on the type
+	_setDefaultConstraints();
+
+	_updateAltitudeAboveGround();
+
+	bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
+	bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
+
+	// 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state.
+	if (follow_line && !follow_line_prev) {
+		_reset();
+	}
+
+	// The only time a thrust set-point is sent out is during
+	// idle. Hence, reset thrust set-point to NAN in case the
+	// vehicle exits idle.
+
+	if (_type_previous == WaypointType::idle) {
+		_thrust_setpoint = Vector3f(NAN, NAN, NAN);
+	}
+
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_gear.landing_gear = _mission_gear;
+	}
+
+	if (_type == WaypointType::idle) {
+		_generateIdleSetpoints();
+
+	} else if (_type == WaypointType::land) {
+		_generateLandSetpoints();
+
+	} else if (follow_line) {
+		_generateSetpoints();
+
+	} else if (_type == WaypointType::takeoff) {
+		_generateTakeoffSetpoints();
+
+	} else if (_type == WaypointType::velocity) {
+		_generateVelocitySetpoints();
+	}
+
+	if (_param_com_obs_avoid.get()) {
+		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
+		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
+				_yawspeed_setpoint);
+	}
+
+	// update previous type
+	_type_previous = _type;
+
+	return true;
+}
+
+void FlightTaskAutoMapper::_reset()
+{
+	// Set setpoints equal current state.
+	_velocity_setpoint = _velocity;
+	_position_setpoint = _position;
+}
+
+void FlightTaskAutoMapper::_generateIdleSetpoints()
+{
+	// Send zero thrust setpoint
+	_position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
+	_thrust_setpoint.zero();
+}
+
+void FlightTaskAutoMapper::_generateLandSetpoints()
+{
+	// Keep xy-position and go down with landspeed
+	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
+	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
+	// set constraints
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
+	_constraints.speed_down = _param_mpc_land_speed.get();
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper::_generateTakeoffSetpoints()
+{
+	// Takeoff is completely defined by target position
+	_position_setpoint = _target;
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
+
+	// limit vertical speed during takeoff
+	_constraints.speed_up = math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(),
+					      _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
+
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper::_generateVelocitySetpoints()
+{
+	// TODO: Remove velocity force logic from navigator, since
+	// navigator should only send out waypoints.
+	_position_setpoint = Vector3f(NAN, NAN, _position(2));
+	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
+	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
+}
+
+void FlightTaskAutoMapper::_updateAltitudeAboveGround()
+{
+	// Altitude above ground is by default just the negation of the current local position in D-direction.
+	_alt_above_ground = -_position(2);
+
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		// We have a valid distance to ground measurement
+		_alt_above_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position.get().valid_alt) {
+		// if home position is set, then altitude above ground is relative to the home position
+		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
+	}
+}
+
+void FlightTaskAutoMapper::updateParams()
+{
+	FlightTaskAuto::updateParams();
+
+	// make sure that alt1 is above alt2
+	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
+}
+
+bool FlightTaskAutoMapper::_highEnoughForLandingGear()
+{
+	// return true if altitude is above two meters
+	return _alt_above_ground > 2.0f;
+}

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoMapper.hpp
+ *
+ * Abstract Flight task which generates local setpoints
+ * based on the triplet type.
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+
+class FlightTaskAutoMapper : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoMapper() = default;
+	virtual ~FlightTaskAutoMapper() = default;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool update() override;
+
+protected:
+
+	virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
+
+	void _generateIdleSetpoints();
+	void _generateLandSetpoints();
+	void _generateVelocitySetpoints();
+	void _generateTakeoffSetpoints();
+
+	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
+	void updateParams() override; /**< See ModuleParam class */
+
+	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>)
+					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
+					(ParamFloat<px4::params::MPC_LAND_ALT2>)
+					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed
+				       );
+
+private:
+
+	void _reset(); /**< Resets member variables to current vehicle state */
+	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
+	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskAutoMapper2
+	FlightTaskAutoMapper2.cpp
+)
+
+target_link_libraries(FlightTaskAutoMapper2 PUBLIC FlightTaskAuto)
+target_include_directories(FlightTaskAutoMapper2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightAutoMapper2.cpp
+ */
+
+#include "FlightTaskAutoMapper2.hpp"
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+bool FlightTaskAutoMapper2::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskAuto::activate(last_setpoint);
+	_reset();
+	return ret;
+}
+
+bool FlightTaskAutoMapper2::update()
+{
+	// always reset constraints because they might change depending on the type
+	_setDefaultConstraints();
+
+	_updateAltitudeAboveGround();
+
+	// The only time a thrust set-point is sent out is during
+	// idle. Hence, reset thrust set-point to NAN in case the
+	// vehicle exits idle.
+
+	if (_type_previous == WaypointType::idle) {
+		_thrust_setpoint = Vector3f(NAN, NAN, NAN);
+	}
+
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_gear.landing_gear = landing_gear_s::GEAR_UP;
+	}
+
+	switch (_type) {
+	case WaypointType::idle:
+		_prepareIdleSetpoints();
+		break;
+
+	case WaypointType::land:
+		_prepareLandSetpoints();
+		break;
+
+	case WaypointType::loiter:
+
+	/* fallthrought */
+	case WaypointType::position:
+		_preparePositionSetpoints();
+		break;
+
+	case WaypointType::takeoff:
+		_prepareTakeoffSetpoints();
+		break;
+
+	case WaypointType::velocity:
+		_prepareVelocitySetpoints();
+		break;
+
+	default:
+		_preparePositionSetpoints();
+		break;
+	}
+
+	if (_param_com_obs_avoid.get()) {
+		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
+		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
+				_yawspeed_setpoint);
+	}
+
+
+	_generateSetpoints();
+
+	// update previous type
+	_type_previous = _type;
+
+	return true;
+}
+
+void FlightTaskAutoMapper2::_reset()
+{
+	// Set setpoints equal current state.
+	_velocity_setpoint = _velocity;
+	_position_setpoint = _position;
+}
+
+void FlightTaskAutoMapper2::_prepareIdleSetpoints()
+{
+	// Send zero thrust setpoint
+	_position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
+	_thrust_setpoint.zero();
+}
+
+void FlightTaskAutoMapper2::_prepareLandSetpoints()
+{
+	float land_speed = _getLandSpeed();
+
+	// Keep xy-position and go down with landspeed
+	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
+	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
+
+	// set constraints
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
+{
+	// Takeoff is completely defined by target position
+	_position_setpoint = _target;
+	const float speed_tko = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
+				_param_mpc_tko_speed.get();
+	_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
+
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper2::_prepareVelocitySetpoints()
+{
+	// XY Velocity waypoint
+	// TODO : Rewiew that. What is the expected behavior?
+	_position_setpoint = Vector3f(NAN, NAN, _position(2));
+	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
+	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
+}
+
+void FlightTaskAutoMapper2::_preparePositionSetpoints()
+{
+	// Simple waypoint navigation: go to xyz target, with standard limitations
+	_position_setpoint = _target;
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN); // No special velocity limitations
+}
+
+void FlightTaskAutoMapper2::_updateAltitudeAboveGround()
+{
+	// Altitude above ground is by default just the negation of the current local position in D-direction.
+	_alt_above_ground = -_position(2);
+
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		// We have a valid distance to ground measurement
+		_alt_above_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position.get().valid_alt) {
+		// if home position is set, then altitude above ground is relative to the home position
+		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
+	}
+}
+
+void FlightTaskAutoMapper2::updateParams()
+{
+	FlightTaskAuto::updateParams();
+
+	// make sure that alt1 is above alt2
+	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
+}
+
+bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
+{
+	// return true if altitude is above two meters
+	return _alt_above_ground > 2.0f;
+}
+
+float FlightTaskAutoMapper2::_getLandSpeed()
+{
+	bool rc_assist_enabled = _param_mpc_land_rc_help.get();
+	bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;
+
+	float throttle = 0.5f;
+
+	if (rc_is_valid && rc_assist_enabled) {
+		throttle = _sub_manual_control_setpoint.get().z;
+	}
+
+	float speed = 0;
+
+	if (_alt_above_ground > _param_mpc_land_alt1.get()) {
+		speed = _constraints.speed_down;
+
+	} else {
+		float land_speed = _param_mpc_land_speed.get();
+		float head_room = _constraints.speed_down - land_speed;
+
+		speed = land_speed + 2 * (0.5f - throttle) * head_room;
+
+		// Allow minimum assisted land speed to be half of parameter
+		if (speed < land_speed * 0.5f) {
+			speed = land_speed * 0.5f;
+		}
+	}
+
+	return speed;
+}

src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoMapper2.hpp
+ *
+ * Abstract Flight task which generates local setpoints
+ * based on the triplet type.
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+
+class FlightTaskAutoMapper2 : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoMapper2() = default;
+	virtual ~FlightTaskAutoMapper2() = default;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool update() override;
+
+protected:
+
+	virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
+
+	void _prepareIdleSetpoints();
+	void _prepareLandSetpoints();
+	void _prepareVelocitySetpoints();
+	void _prepareTakeoffSetpoints();
+	void _preparePositionSetpoints();
+
+	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
+	void updateParams() override; /**< See ModuleParam class */
+
+	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>)
+					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
+					(ParamFloat<px4::params::MPC_LAND_ALT2>)
+					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed
+				       );
+
+private:
+
+	void _reset(); /**< Resets member variables to current vehicle state */
+	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
+	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
+	float _getLandSpeed(); /**< Returns landing descent speed. */
+};

src/lib/flight_tasks/tasks/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+# add the core Flight tasks that the additional flight tasks depend on
+add_subdirectory(FlightTask)
+add_subdirectory(Utility)
+add_subdirectory(Manual)
+add_subdirectory(Auto)
+add_subdirectory(AutoMapper)
+add_subdirectory(AutoMapper2)
+
+# add all additional flight tasks
+foreach(task ${flight_tasks_all})
+    add_subdirectory(${task})
+endforeach()

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(FlightTaskDescend
+	FlightTaskDescend.cpp
+)
+
+target_link_libraries(FlightTaskDescend PUBLIC FlightTask)
+target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskDescend.cpp
+ */
+
+#include "FlightTaskDescend.hpp"
+
+bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	// stay level to minimize horizontal drift
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN);
+	// keep heading
+	_yaw_setpoint = _yaw;
+	return ret;
+}
+
+bool FlightTaskDescend::update()
+{
+	if (PX4_ISFINITE(_velocity(2))) {
+		// land with landspeed
+		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		_thrust_setpoint(2) = NAN;
+
+	} else {
+		// descend with constant thrust (crash landing)
+		_velocity_setpoint(2) = NAN;
+		_thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f;
+	}
+
+	return true;
+}

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskDescend.hpp
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+
+class FlightTaskDescend : public FlightTask
+{
+public:
+	FlightTaskDescend() = default;
+	virtual ~FlightTaskDescend() = default;
+
+	bool update() override;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, ///< thrust at hover equilibrium
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed ///< velocity for controlled descend
+				       )
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskFailsafe
+	FlightTaskFailsafe.cpp
+)
+
+target_link_libraries(FlightTaskFailsafe PUBLIC FlightTask)
+target_include_directories(FlightTaskFailsafe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file FlightTaskFailsafe.cpp
+ */
+
+#include "FlightTaskFailsafe.hpp"
+
+bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_position_setpoint = _position;
+	_velocity_setpoint.zero();
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
+	_yaw_setpoint = _yaw;
+	_yawspeed_setpoint = 0.0f;
+	return ret;
+}
+
+bool FlightTaskFailsafe::update()
+{
+	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
+		// stay at current position setpoint
+		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
+		_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
+
+	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
+		// don't move along xy
+		_position_setpoint(0) = _position_setpoint(1) = NAN;
+		_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
+	}
+
+	if (PX4_ISFINITE(_position(2))) {
+		// stay at current altitude setpoint
+		_velocity_setpoint(2) = 0.0f;
+		_thrust_setpoint(2) = NAN;
+
+	} else if (PX4_ISFINITE(_velocity(2))) {
+		// land with landspeed
+		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		_position_setpoint(2) = NAN;
+		_thrust_setpoint(2) = NAN;
+	}
+
+	return true;
+
+}

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskFailsafe.hpp
+ *
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+
+class FlightTaskFailsafe : public FlightTask
+{
+public:
+	FlightTaskFailsafe() = default;
+
+	virtual ~FlightTaskFailsafe() = default;
+	bool update() override;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamFloat<px4::params::MPC_THR_HOVER>)
+					_param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
+				       )
+};

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

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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(FlightTask
+	FlightTask.cpp
+)
+
+target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -0,0 +1,199 @@
+#include "FlightTask.hpp"
+#include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+
+constexpr uint64_t FlightTask::_timeout;
+// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
+const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
+
+const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
+const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
+
+bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	_resetSetpoints();
+	_setDefaultConstraints();
+	_initEkfResetCounters();
+	_time_stamp_activate = hrt_absolute_time();
+	_gear = empty_landing_gear_default_keep;
+	return true;
+}
+
+void FlightTask::reActivate()
+{
+	activate(getPositionSetpoint());
+}
+
+bool FlightTask::updateInitialize()
+{
+	_time_stamp_current = hrt_absolute_time();
+	_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
+	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
+	_time_stamp_last = _time_stamp_current;
+
+	_sub_vehicle_local_position.update();
+	_sub_attitude.update();
+
+	_evaluateVehicleLocalPosition();
+	_checkEkfResetCounters();
+	return true;
+}
+
+void FlightTask::_initEkfResetCounters()
+{
+	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
+	_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
+	_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
+}
+
+void FlightTask::_checkEkfResetCounters()
+{
+	// Check if a reset event has happened
+	if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
+		_ekfResetHandlerPositionXY();
+		_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
+		_ekfResetHandlerVelocityXY();
+		_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
+		_ekfResetHandlerPositionZ();
+		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
+		_ekfResetHandlerVelocityZ();
+		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
+	}
+
+	if (_sub_attitude.get().quat_reset_counter != _reset_counters.quat) {
+		float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().delta_q_reset)).psi();
+		_ekfResetHandlerHeading(delta_psi);
+		_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
+	}
+}
+
+const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
+{
+	/* fill position setpoint message */
+	vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
+
+	vehicle_local_position_setpoint.x = _position_setpoint(0);
+	vehicle_local_position_setpoint.y = _position_setpoint(1);
+	vehicle_local_position_setpoint.z = _position_setpoint(2);
+
+	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
+	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
+	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
+
+	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
+	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
+	_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
+	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
+	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+
+	return vehicle_local_position_setpoint;
+}
+
+void FlightTask::_resetSetpoints()
+{
+	_position_setpoint.setAll(NAN);
+	_velocity_setpoint.setAll(NAN);
+	_acceleration_setpoint.setAll(NAN);
+	_jerk_setpoint.setAll(NAN);
+	_thrust_setpoint.setAll(NAN);
+	_yaw_setpoint = _yawspeed_setpoint = NAN;
+}
+
+void FlightTask::_evaluateVehicleLocalPosition()
+{
+	_position.setAll(NAN);
+	_velocity.setAll(NAN);
+	_yaw = NAN;
+	_dist_to_bottom = NAN;
+
+	if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) {
+		// yaw
+		_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().q)).psi();
+	}
+
+	// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
+	if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
+
+		// position
+		if (_sub_vehicle_local_position.get().xy_valid) {
+			_position(0) = _sub_vehicle_local_position.get().x;
+			_position(1) = _sub_vehicle_local_position.get().y;
+		}
+
+		if (_sub_vehicle_local_position.get().z_valid) {
+			_position(2) = _sub_vehicle_local_position.get().z;
+		}
+
+		// velocity
+		if (_sub_vehicle_local_position.get().v_xy_valid) {
+			_velocity(0) = _sub_vehicle_local_position.get().vx;
+			_velocity(1) = _sub_vehicle_local_position.get().vy;
+		}
+
+		if (_sub_vehicle_local_position.get().v_z_valid) {
+			_velocity(2) = _sub_vehicle_local_position.get().vz;
+		}
+
+		// distance to bottom
+		if (_sub_vehicle_local_position.get().dist_bottom_valid
+		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
+			_dist_to_bottom =  _sub_vehicle_local_position.get().dist_bottom;
+		}
+
+		// global frame reference coordinates to enable conversions
+		if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
+			globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
+						  _sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp);
+		}
+	}
+}
+
+void FlightTask::_setDefaultConstraints()
+{
+	_constraints.speed_xy = _param_mpc_xy_vel_max.get();
+	_constraints.speed_up = _param_mpc_z_vel_max_up.get();
+	_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
+	_constraints.min_distance_to_ground = NAN;
+	_constraints.max_distance_to_ground = NAN;
+	_constraints.want_takeoff = false;
+}
+
+bool FlightTask::_checkTakeoff()
+{
+	// position setpoint above the minimum altitude
+	bool position_triggered_takeoff = false;
+
+	if (PX4_ISFINITE(_position_setpoint(2))) {
+		// minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
+		float min_altitude = 0.2f;
+		const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
+
+		if (PX4_ISFINITE(min_distance_to_ground)) {
+			min_altitude = min_distance_to_ground + 0.05f;
+		}
+
+		position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
+	}
+
+	// upwards velocity setpoint
+	bool velocity_triggered_takeoff = false;
+
+	if (PX4_ISFINITE(_velocity_setpoint(2))) {
+		velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
+	}
+
+	return position_triggered_takeoff || velocity_triggered_takeoff;
+}

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ *   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 FlightTask.hpp
+ *
+ * Abstract base class for different advanced flight tasks like orbit, follow me, ...
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/landing_gear.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_constraints.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <lib/weather_vane/WeatherVane.hpp>
+
+class FlightTask : public ModuleParams
+{
+public:
+	FlightTask() :
+		ModuleParams(nullptr)
+	{
+		_resetSetpoints();
+		_constraints = empty_constraints;
+	}
+
+	virtual ~FlightTask() = default;
+
+	/**
+	 * Call once on the event where you switch to the task
+	 * @param state of the previous task
+	 * @return true on success, false on error
+	 */
+	virtual bool activate(vehicle_local_position_setpoint_s last_setpoint);
+
+	/**
+	 * Call this to reset an active Flight Task
+	 */
+	virtual void reActivate();
+
+	/**
+	 * To be called to adopt parameters from an arrived vehicle command
+	 * @param command received command message containing the parameters
+	 * @return true if accepted, false if declined
+	 */
+	virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; }
+
+	/**
+	 * Call before activate() or update()
+	 * to initialize time and input data
+	 * @return true on success, false on error
+	 */
+	virtual bool updateInitialize();
+
+	/**
+	 * To be called regularly in the control loop cycle to execute the task
+	 * @return true on success, false on error
+	 */
+	virtual bool update() = 0;
+
+	/**
+	 * Call after update()
+	 * to constrain the generated setpoints in order to comply
+	 * with the constraints of the current mode
+	 * @return true on success, false on error
+	 */
+	virtual bool updateFinalize() { return true; };
+
+	/**
+	 * Get the output data
+	 * @return task output setpoints that get executed by the positon controller
+	 */
+	const vehicle_local_position_setpoint_s getPositionSetpoint();
+
+	/**
+	 * Get vehicle constraints.
+	 * The constraints can vary with task.
+	 * @return constraints
+	 */
+	const vehicle_constraints_s &getConstraints() { return _constraints; }
+
+	/**
+	 * Get landing gear position.
+	 * The constraints can vary with task.
+	 * @return landing gear
+	 */
+	const landing_gear_s &getGear() { return _gear; }
+
+	/**
+	 * Get avoidance desired waypoint
+	 * @return desired waypoints
+	 */
+	const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
+
+	/**
+	 * Empty setpoint.
+	 * All setpoints are set to NAN.
+	 */
+	static const vehicle_local_position_setpoint_s empty_setpoint;
+
+	/**
+	 * Empty constraints.
+	 * All constraints are set to NAN.
+	 */
+	static const vehicle_constraints_s empty_constraints;
+
+	/**
+	 * default landing gear state
+	 */
+	static const landing_gear_s empty_landing_gear_default_keep;
+
+	/**
+	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
+	 */
+	void handleParameterUpdate()
+	{
+		updateParams();
+	}
+
+	/**
+	 * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
+	 * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
+	 */
+	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
+
+	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
+					const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
+
+protected:
+
+	uORB::SubscriptionData<vehicle_local_position_s>	_sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<vehicle_attitude_s>		_sub_attitude{ORB_ID(vehicle_attitude)};
+
+	/** Reset all setpoints to NAN */
+	void _resetSetpoints();
+
+	/** Check and update local position */
+	void _evaluateVehicleLocalPosition();
+
+	/** Set constraints to default values */
+	virtual void _setDefaultConstraints();
+
+	/** Determine when to trigger a takeoff (ignored in flight) */
+	virtual bool _checkTakeoff();
+
+	/**
+	 * Monitor the EKF reset counters and
+	 * call the appropriate handling functions in case of a reset event
+	 */
+	void _initEkfResetCounters();
+	void _checkEkfResetCounters();
+	virtual void _ekfResetHandlerPositionXY() {};
+	virtual void _ekfResetHandlerVelocityXY() {};
+	virtual void _ekfResetHandlerPositionZ() {};
+	virtual void _ekfResetHandlerVelocityZ() {};
+	virtual void _ekfResetHandlerHeading(float delta_psi) {};
+
+	/* Time abstraction */
+	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
+	float _time = 0; /**< passed time in seconds since the task was activated */
+	float _deltatime = 0; /**< passed time in seconds since the task was last updated */
+	hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */
+	hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
+	hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
+
+	/* Current vehicle state */
+	matrix::Vector3f _position; /**< current vehicle position */
+	matrix::Vector3f _velocity; /**< current vehicle velocity */
+	float _yaw = 0.f; /**< current vehicle yaw heading */
+	float _dist_to_bottom = 0.0f; /**< current height above ground level */
+
+	/**
+	 * Setpoints which the position controller has to execute.
+	 * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.
+	 * If more than one type of setpoint is set, then order of control is a as follow: position, velocity,
+	 * acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the
+	 * _velocity_setpoint is used as feedforward.
+	 * _acceleration_setpoint and _jerk_setpoint are currently not supported.
+	 */
+	matrix::Vector3f _position_setpoint;
+	matrix::Vector3f _velocity_setpoint;
+	matrix::Vector3f _acceleration_setpoint;
+	matrix::Vector3f _jerk_setpoint;
+	matrix::Vector3f _thrust_setpoint;
+	float _yaw_setpoint;
+	float _yawspeed_setpoint;
+
+	matrix::Vector3f _velocity_setpoint_feedback;
+	matrix::Vector3f _thrust_setpoint_feedback;
+
+	/* Counters for estimator local position resets */
+	struct {
+		uint8_t xy = 0;
+		uint8_t vxy = 0;
+		uint8_t z = 0;
+		uint8_t vz = 0;
+		uint8_t quat = 0;
+	} _reset_counters;
+
+	/**
+	 * Vehicle constraints.
+	 * The constraints can vary with tasks.
+	 */
+	vehicle_constraints_s _constraints{};
+
+	landing_gear_s _gear{};
+
+	/**
+	 * Desired waypoints.
+	 * Goals set by the FCU to be sent to the obstacle avoidance system.
+	 */
+	vehicle_trajectory_waypoint_s _desired_waypoint{};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
+					(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
+				       )
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskManual
+	FlightTaskManual.cpp
+)
+
+target_link_libraries(FlightTaskManual PUBLIC FlightTask)
+target_include_directories(FlightTaskManual PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file FlightTaskManual.cpp
+ */
+
+#include "FlightTaskManual.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+#include <drivers/drv_hrt.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+bool FlightTaskManual::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+
+	_sub_manual_control_setpoint.update();
+
+	const bool sticks_available = _evaluateSticks();
+
+	if (_sticks_data_required) {
+		ret = ret && sticks_available;
+	}
+
+	return ret;
+}
+
+bool FlightTaskManual::_evaluateSticks()
+{
+	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
+
+	/* Sticks are rescaled linearly and exponentially to [-1,1] */
+	if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
+
+		/* Linear scale  */
+		_sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */
+		_sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */
+		_sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
+		_sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */
+
+		/* Exponential scale */
+		_sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+		_sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+		_sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
+		_sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
+
+		// Only switch the landing gear up if the user switched from gear down to gear up.
+		// If the user had the switch in the gear up position and took off ignore it
+		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+		int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
+
+		if (_gear_switch_old != gear_switch) {
+			_applyGearSwitch(gear_switch);
+		}
+
+		_gear_switch_old = gear_switch;
+
+		// valid stick inputs are required
+		const bool valid_sticks =  PX4_ISFINITE(_sticks(0))
+					   && PX4_ISFINITE(_sticks(1))
+					   && PX4_ISFINITE(_sticks(2))
+					   && PX4_ISFINITE(_sticks(3));
+
+		return valid_sticks;
+
+	} else {
+		/* Timeout: set all sticks to zero */
+		_sticks.zero();
+		_sticks_expo.zero();
+		_gear.landing_gear = landing_gear_s::GEAR_KEEP;
+		return false;
+	}
+}
+
+void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
+{
+	if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+		_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+	}
+
+	if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		_gear.landing_gear = landing_gear_s::GEAR_UP;
+	}
+}

src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskManual.hpp
+ *
+ * Linear and exponential map from stick inputs to range -1 and 1.
+ *
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+#include <uORB/topics/manual_control_setpoint.h>
+
+class FlightTaskManual : public FlightTask
+{
+public:
+	FlightTaskManual() = default;
+
+	virtual ~FlightTaskManual() = default;
+
+	bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); };
+	bool updateInitialize() override;
+
+protected:
+
+	bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */
+	matrix::Vector<float, 4> _sticks; /**< unmodified manual stick inputs */
+	matrix::Vector<float, 4> _sticks_expo; /**< modified manual sticks using expo function*/
+	int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/
+
+	float stickDeadzone() const { return _param_mpc_hold_dz.get(); }
+
+private:
+
+	bool _evaluateSticks(); /**< checks and sets stick inputs */
+	void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
+
+	uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */
+					(ParamFloat<px4::params::MPC_XY_MAN_EXPO>)
+					_param_mpc_xy_man_expo, /**< ratio of exponential curve for stick input in xy direction */
+					(ParamFloat<px4::params::MPC_Z_MAN_EXPO>)
+					_param_mpc_z_man_expo, /**< ratio of exponential curve for stick input in z direction */
+					(ParamFloat<px4::params::MPC_YAW_EXPO>)
+					_param_mpc_yaw_expo, /**< ratio of exponential curve for stick input in yaw for modes except acro */
+					(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t /**< time at which commander considers RC lost */
+				       )
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualAltitude
+	FlightTaskManualAltitude.cpp
+)
+
+target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual)
+target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightManualAltitude.cpp
+ */
+
+#include "FlightTaskManualAltitude.hpp"
+#include <float.h>
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+bool FlightTaskManualAltitude::updateInitialize()
+{
+	bool ret = FlightTaskManual::updateInitialize();
+
+	_sub_home_position.update();
+
+	// in addition to manual require valid position and velocity in D-direction and valid yaw
+	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
+}
+
+bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskManual::activate(last_setpoint);
+	_yaw_setpoint = NAN;
+	_yawspeed_setpoint = 0.0f;
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
+	_position_setpoint(2) = _position(2);
+	_velocity_setpoint(2) = 0.0f;
+	_setDefaultConstraints();
+
+	_constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
+
+	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
+		_constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
+
+	} else {
+		_constraints.min_distance_to_ground = -INFINITY;
+	}
+
+	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
+		_constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
+
+	} else {
+		_constraints.max_distance_to_ground = INFINITY;
+	}
+
+	_max_speed_up = _constraints.speed_up;
+	_min_speed_down = _constraints.speed_down;
+
+	return ret;
+}
+
+void FlightTaskManualAltitude::_scaleSticks()
+{
+	// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed
+	_yawspeed_setpoint = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
+
+	const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
+	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
+}
+
+void FlightTaskManualAltitude::_updateAltitudeLock()
+{
+	// Depending on stick inputs and velocity, position is locked.
+	// If not locked, altitude setpoint is set to NAN.
+
+	// Check if user wants to break
+	const bool apply_brake = fabsf(_sticks_expo(2)) <= FLT_EPSILON;
+
+	// Check if vehicle has stopped
+	const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
+
+	// Manage transition between use of distance to ground and distance to local origin
+	// when terrain hold behaviour has been selected.
+	if (_param_mpc_alt_mode.get() == 2) {
+		// Use horizontal speed as a transition criteria
+		float spd_xy = Vector2f(_velocity).length();
+
+		// Use presence of horizontal stick inputs as a transition criteria
+		float stick_xy = Vector2f(&_sticks_expo(0)).length();
+		bool stick_input = stick_xy > 0.001f;
+
+		if (_terrain_hold) {
+			bool too_fast = spd_xy > _param_mpc_hold_max_xy.get();
+
+			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
+				// Stop using distance to ground
+				_terrain_hold = false;
+				_terrain_follow = false;
+
+				// Adjust the setpoint to maintain the same height error to reduce control transients
+				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
+					_position_setpoint(2) = _position(2) + (_dist_to_ground_lock - _dist_to_bottom);
+
+				} else {
+					_position_setpoint(2) = _position(2);
+				}
+			}
+
+		} else {
+			bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get();
+
+			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
+				// Start using distance to ground
+				_terrain_hold = true;
+				_terrain_follow = true;
+
+				// Adjust the setpoint to maintain the same height error to reduce control transients
+				if (PX4_ISFINITE(_position_setpoint(2))) {
+					_dist_to_ground_lock = _dist_to_bottom + (_position_setpoint(2) - _position(2));
+				}
+			}
+		}
+
+	}
+
+	if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
+		// terrain following
+		_terrainFollowing(apply_brake, stopped);
+		// respect maximum altitude
+		_respectMaxAltitude();
+
+	} else {
+		// normal mode where height is dependent on local frame
+
+		if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) {
+			// lock position
+			_position_setpoint(2) = _position(2);
+
+			// Ensure that minimum altitude is respected if
+			// there is a distance sensor and distance to bottom is below minimum.
+			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
+				_terrainFollowing(apply_brake, stopped);
+
+			} else {
+				_dist_to_ground_lock = NAN;
+			}
+
+		} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
+			// Position is locked but check if a reset event has happened.
+			// We will shift the setpoints.
+			if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) {
+				_position_setpoint(2) = _position(2);
+				_reset_counter = _sub_vehicle_local_position.get().z_reset_counter;
+			}
+
+		} else  {
+			// user demands velocity change
+			_position_setpoint(2) = NAN;
+			// ensure that maximum altitude is respected
+			_respectMaxAltitude();
+		}
+	}
+}
+
+void FlightTaskManualAltitude::_respectMinAltitude()
+{
+	const bool respectAlt = PX4_ISFINITE(_dist_to_bottom)
+				&& _dist_to_bottom < _constraints.min_distance_to_ground;
+
+	// Height above ground needs to be limited (flow / range-finder)
+	if (respectAlt) {
+		// increase altitude to minimum flow distance
+		_position_setpoint(2) = _position(2)
+					- (_constraints.min_distance_to_ground - _dist_to_bottom);
+	}
+}
+
+void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
+{
+	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
+		// User wants to break and vehicle reached zero velocity. Lock height to ground.
+
+		// lock position
+		_position_setpoint(2) = _position(2);
+		// ensure that minimum altitude is respected
+		_respectMinAltitude();
+		// lock distance to ground but adjust first for minimum altitude
+		_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+
+	} else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) {
+		// vehicle needs to follow terrain
+
+		// difference between the current distance to ground and the desired distance to ground
+		const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom;
+		// adjust position setpoint for the delta (note: NED frame)
+		_position_setpoint(2) = _position(2) - delta_distance_to_ground;
+
+	} else {
+		// user demands velocity change in D-direction
+		_dist_to_ground_lock = _position_setpoint(2) = NAN;
+	}
+}
+
+void FlightTaskManualAltitude::_respectMaxAltitude()
+{
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+
+		// if there is a valid maximum distance to ground, linearly increase speed limit with distance
+		// below the maximum, preserving control loop vertical position error gain.
+		if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
+			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
+								-_min_speed_down, _max_speed_up);
+
+		} else {
+			_constraints.speed_up = _max_speed_up;
+		}
+
+		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
+		if (_dist_to_bottom >  _constraints.max_distance_to_ground) {
+			// difference between current distance to ground and maximum distance to ground
+			const float delta_distance_to_max = _dist_to_bottom - _constraints.max_distance_to_ground;
+			// set position setpoint to maximum distance to ground
+			_position_setpoint(2) = _position(2) +  delta_distance_to_max;
+			// limit speed downwards to 0.7m/s
+			_constraints.speed_down = math::min(_min_speed_down, 0.7f);
+
+		} else {
+			_constraints.speed_down = _min_speed_down;
+
+		}
+	}
+}
+
+void FlightTaskManualAltitude::_respectGroundSlowdown()
+{
+	float dist_to_ground = NAN;
+
+	// if there is a valid distance to bottom or vertical distance to home
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		dist_to_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position.get().valid_alt) {
+		dist_to_ground = -(_position(2) - _sub_home_position.get().z);
+	}
+
+	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	if (PX4_ISFINITE(dist_to_ground)) {
+		const float limit_down = math::gradual(dist_to_ground,
+						       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+						       _param_mpc_land_speed.get(), _constraints.speed_down);
+		const float limit_up = math::gradual(dist_to_ground,
+						     _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+						     _param_mpc_tko_speed.get(), _constraints.speed_up);
+		_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
+	}
+}
+
+void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
+{
+	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
+	Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
+	v(0) = v_r(0);
+	v(1) = v_r(1);
+}
+
+void FlightTaskManualAltitude::_updateHeadingSetpoints()
+{
+	/* Yaw-lock depends on stick input. If not locked,
+	 * yaw_sp is set to NAN.
+	 * TODO: add yawspeed to get threshold.*/
+	if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
+		// no fixed heading when rotating around yaw by stick
+		_yaw_setpoint = NAN;
+
+	} else {
+		// hold the current heading when no more rotation commanded
+		if (!PX4_ISFINITE(_yaw_setpoint)) {
+			_yaw_setpoint = _yaw;
+		}
+	}
+}
+
+void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
+{
+	// Only reset the yaw setpoint when the heading is locked
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		_yaw_setpoint += delta_psi;
+	}
+}
+
+void FlightTaskManualAltitude::_updateSetpoints()
+{
+	_updateHeadingSetpoints(); // get yaw setpoint
+
+	// Thrust in xy are extracted directly from stick inputs. A magnitude of
+	// 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no
+	// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
+	// setpoint along z-direction, which is computed in PositionControl.cpp.
+
+	Vector2f sp(&_sticks(0));
+	_rotateIntoHeadingFrame(sp);
+
+	if (sp.length() > 1.0f) {
+		sp.normalize();
+	}
+
+	_thrust_setpoint(0) = sp(0);
+	_thrust_setpoint(1) = sp(1);
+	_thrust_setpoint(2) = NAN;
+
+	_updateAltitudeLock();
+	_respectGroundSlowdown();
+}
+
+bool FlightTaskManualAltitude::_checkTakeoff()
+{
+	// stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
+	return _sticks(2) < -0.3f;
+}
+
+bool FlightTaskManualAltitude::update()
+{
+	_scaleSticks();
+	_updateSetpoints();
+	_constraints.want_takeoff = _checkTakeoff();
+
+	return true;
+}

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightManualAltitude.hpp
+ *
+ * Flight task for manual controlled altitude.
+ */
+
+#pragma once
+
+#include "FlightTaskManual.hpp"
+#include <uORB/topics/home_position.h>
+
+class FlightTaskManualAltitude : public FlightTaskManual
+{
+public:
+	FlightTaskManualAltitude() = default;
+	virtual ~FlightTaskManualAltitude() = default;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool updateInitialize() override;
+	bool update() override;
+
+protected:
+	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
+	void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
+	virtual void _updateSetpoints(); /**< updates all setpoints */
+	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
+	bool _checkTakeoff() override;
+
+	/**
+	 * rotates vector into local frame
+	 */
+	void _rotateIntoHeadingFrame(matrix::Vector2f &vec);
+
+	/**
+	 *  Check and sets for position lock.
+	 *  If sticks are at center position, the vehicle
+	 *  will exit velocity control and enter position control.
+	 */
+	void _updateAltitudeLock();
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
+					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
+					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
+					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
+					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below wich to land with land speed */
+					(ParamFloat<px4::params::MPC_LAND_SPEED>)
+					_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
+					(ParamFloat<px4::params::MPC_TKO_SPEED>)
+					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
+				       )
+private:
+	/**
+	 * Terrain following.
+	 * During terrain following, the position setpoint is adjusted
+	 * such that the distance to ground is kept constant.
+	 * @param apply_brake is true if user wants to break
+	 * @param stopped is true if vehicle has stopped moving in D-direction
+	 */
+	void _terrainFollowing(bool apply_brake, bool stopped);
+
+	/**
+	 * Minimum Altitude during range sensor operation.
+	 * If a range sensor is used for altitude estimates, for
+	 * best operation a minimum altitude is required. The minimum
+	 * altitude is only enforced during altitude lock.
+	 */
+	void _respectMinAltitude();
+
+	void _respectMaxAltitude();
+
+	/**
+	 * Sets downwards velocity constraint based on the distance to ground.
+	 * To ensure a slowdown to land speed before hitting the ground.
+	 */
+	void _respectGroundSlowdown();
+
+	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
+
+	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
+	float _max_speed_up = 10.0f;
+	float _min_speed_down = 1.0f;
+	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
+	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
+
+	/**
+	 * Distance to ground during terrain following.
+	 * If user does not demand velocity change in D-direction and the vehcile
+	 * is in terrain-following mode, then height to ground will be locked to
+	 * _dist_to_ground_lock.
+	 */
+	float _dist_to_ground_lock = NAN;
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualAltitudeSmooth
+	FlightTaskManualAltitudeSmooth.cpp
+)
+
+target_link_libraries(FlightTaskManualAltitudeSmooth PUBLIC FlightTaskManualAltitude FlightTaskUtility)
+target_include_directories(FlightTaskManualAltitudeSmooth PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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 FlightManualAltitude.cpp
+ */
+
+#include "FlightTaskManualAltitudeSmooth.hpp"
+
+using namespace matrix;
+
+FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
+	_smoothing(this, _velocity(2), _sticks(2))
+{}
+
+void FlightTaskManualAltitudeSmooth::_updateSetpoints()
+{
+	/* Get yaw, thrust */
+	FlightTaskManualAltitude::_updateSetpoints();
+
+	/* Smooth velocity in z*/
+	_smoothing.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
+
+	/* Check for altitude lock*/
+	_updateAltitudeLock();
+}

src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.hpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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 FlightManualAltitude.hpp
+ *
+ * Flight task for manual controlled altitude.
+ */
+
+#pragma once
+
+#include "FlightTaskManualAltitude.hpp"
+#include "ManualSmoothingZ.hpp"
+
+class FlightTaskManualAltitudeSmooth : public FlightTaskManualAltitude
+{
+public:
+	FlightTaskManualAltitudeSmooth();
+	virtual ~FlightTaskManualAltitudeSmooth() = default;
+
+protected:
+	virtual void _updateSetpoints() override;
+
+private:
+	ManualSmoothingZ _smoothing; /**< smoothing for velocity setpoints */
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualAltitudeSmoothVel
+	FlightTaskManualAltitudeSmoothVel.cpp
+)
+
+target_link_libraries(FlightTaskManualAltitudeSmoothVel PUBLIC FlightTaskManualAltitude FlightTaskUtility)
+target_include_directories(FlightTaskManualAltitudeSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightManualAltitude.cpp
+ */
+
+#include "FlightTaskManualAltitudeSmoothVel.hpp"
+
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
+
+	// Check if the previous FlightTask provided setpoints
+	checkSetpoints(last_setpoint);
+
+	_smoothing.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z);
+
+	return ret;
+}
+
+void FlightTaskManualAltitudeSmoothVel::reActivate()
+{
+	// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
+	// using the generated jerk, reset the z derivatives to zero
+	_smoothing.reset(0.f, 0.f, _position(2));
+}
+
+void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	if (!PX4_ISFINITE(setpoints.acceleration[2])) { setpoints.acceleration[2] = 0.f; }
+}
+
+void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
+{
+	_smoothing.setCurrentPosition(_position(2));
+}
+
+void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ()
+{
+	_smoothing.setCurrentVelocity(_velocity(2));
+}
+
+void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
+{
+	// Set max accel/vel/jerk
+	// Has to be done before _updateTrajectories()
+	_updateTrajConstraints();
+
+	_smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2));
+	_smoothing.setCurrentPositionEstimate(_position(2));
+
+	// Get yaw setpoint, un-smoothed position setpoints
+	FlightTaskManualAltitude::_updateSetpoints();
+
+	_smoothing.update(_deltatime, _velocity_setpoint(2));
+
+	// Fill the jerk, acceleration, velocity and position setpoint vectors
+	_setOutputState();
+}
+
+void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
+{
+	_smoothing.setMaxJerk(_param_mpc_jerk_max.get());
+
+	_smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get());
+	_smoothing.setMaxVelUp(_constraints.speed_up);
+
+	_smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get());
+	_smoothing.setMaxVelDown(_constraints.speed_down);
+}
+
+
+void FlightTaskManualAltitudeSmoothVel::_setOutputState()
+{
+	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
+	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
+	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
+	_position_setpoint(2) = _smoothing.getCurrentPosition();
+}

src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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 FlightManualAltitudeSmoothVel.hpp
+ *
+ * Flight task for manual controlled altitude using the velocity smoothing library
+ */
+
+#pragma once
+
+#include "FlightTaskManualAltitude.hpp"
+#include "ManualVelocitySmoothingZ.hpp"
+
+class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
+{
+public:
+	FlightTaskManualAltitudeSmoothVel() = default;
+	virtual ~FlightTaskManualAltitudeSmoothVel() = default;
+
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	void reActivate() override;
+
+protected:
+
+	virtual void _updateSetpoints() override;
+
+	/** Reset position or velocity setpoints in case of EKF reset event */
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+	)
+
+private:
+
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
+
+	void _updateTrajConstraints();
+	void _setOutputState();
+
+	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualPosition
+	FlightTaskManualPosition.cpp
+)
+
+target_link_libraries(FlightTaskManualPosition PUBLIC FlightTaskManualAltitude)
+target_include_directories(FlightTaskManualPosition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskManualPosition.cpp
+ */
+
+#include "FlightTaskManualPosition.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
+{
+
+}
+
+bool FlightTaskManualPosition::updateInitialize()
+{
+	bool ret = FlightTaskManualAltitude::updateInitialize();
+	// require valid position / velocity in xy
+	return ret && PX4_ISFINITE(_position(0))
+	       && PX4_ISFINITE(_position(1))
+	       && PX4_ISFINITE(_velocity(0))
+	       && PX4_ISFINITE(_velocity(1));
+}
+
+bool FlightTaskManualPosition::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	// all requirements from altitude-mode still have to hold
+	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
+
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
+
+	// set task specific constraint
+	if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) {
+		_constraints.speed_xy = _param_mpc_vel_manual.get();
+	}
+
+	_position_setpoint(0) = _position(0);
+	_position_setpoint(1) = _position(1);
+	_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
+	_velocity_scale = _constraints.speed_xy;
+
+	// for position-controlled mode, we need a valid position and velocity state
+	// in NE-direction
+	return ret;
+}
+
+void FlightTaskManualPosition::_scaleSticks()
+{
+	/* Use same scaling as for FlightTaskManualAltitude */
+	FlightTaskManualAltitude::_scaleSticks();
+
+	/* Constrain length of stick inputs to 1 for xy*/
+	Vector2f stick_xy(&_sticks_expo(0));
+
+	float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
+
+	if (mag > FLT_EPSILON) {
+		stick_xy = stick_xy.normalized() * mag;
+	}
+
+	// scale the stick inputs
+	if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) {
+		// estimator provides vehicle specific max
+
+		// use the minimum of the estimator and user specified limit
+		_velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position.get().vxy_max);
+		// Allow for a minimum of 0.3 m/s for repositioning
+		_velocity_scale = fmaxf(_velocity_scale, 0.3f);
+
+	} else if (stick_xy.length() > 0.5f) {
+		// raise the limit at a constant rate up to the user specified value
+
+		if (_velocity_scale < _constraints.speed_xy) {
+			_velocity_scale += _deltatime * _param_mpc_acc_hor_estm.get();
+
+		} else {
+			_velocity_scale = _constraints.speed_xy;
+
+		}
+	}
+
+	// scale velocity to its maximum limits
+	Vector2f vel_sp_xy = stick_xy * _velocity_scale;
+
+	/* Rotate setpoint into local frame. */
+	_rotateIntoHeadingFrame(vel_sp_xy);
+
+	// collision prevention
+	if (_collision_prevention.is_active()) {
+		_collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position),
+						     Vector2f(_velocity));
+	}
+
+	_velocity_setpoint(0) = vel_sp_xy(0);
+	_velocity_setpoint(1) = vel_sp_xy(1);
+}
+
+void FlightTaskManualPosition::_updateXYlock()
+{
+	/* If position lock is not active, position setpoint is set to NAN.*/
+	const float vel_xy_norm = Vector2f(_velocity).length();
+	const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
+	const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get());
+
+	if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
+		_position_setpoint(0) = _position(0);
+		_position_setpoint(1) = _position(1);
+
+	} else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) {
+		// Position is locked but check if a reset event has happened.
+		// We will shift the setpoints.
+		if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) {
+			_position_setpoint(0) = _position(0);
+			_position_setpoint(1) = _position(1);
+			_reset_counter = _sub_vehicle_local_position.get().xy_reset_counter;
+		}
+
+	} else {
+		/* don't lock*/
+		_position_setpoint(0) = NAN;
+		_position_setpoint(1) = NAN;
+	}
+}
+
+void FlightTaskManualPosition::_updateSetpoints()
+{
+	FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
+
+	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
+	if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
+		_yaw_setpoint = NAN;
+		_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+	}
+
+	_thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
+	_updateXYlock(); // check for position lock
+}

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskManualPosition.hpp
+ *
+ * Flight task for manual position controlled mode.
+ *
+ */
+
+#pragma once
+
+#include <lib/collision_prevention/CollisionPrevention.hpp>
+#include "FlightTaskManualAltitude.hpp"
+
+class FlightTaskManualPosition : public FlightTaskManualAltitude
+{
+public:
+	FlightTaskManualPosition();
+
+	virtual ~FlightTaskManualPosition() = default;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool updateInitialize() override;
+
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
+
+
+protected:
+	void _updateXYlock(); /**< applies position lock based on stick and velocity */
+	void _updateSetpoints() override;
+	void _scaleSticks() override;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
+					(ParamFloat<px4::params::MPC_ACC_HOR_ESTM>) _param_mpc_acc_hor_estm
+				       )
+private:
+	float _velocity_scale{0.0f}; //scales the stick input to velocity
+	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+
+	WeatherVane *_weathervane_yaw_handler =
+		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualPositionSmooth
+	FlightTaskManualPositionSmooth.cpp
+)
+
+target_link_libraries(FlightTaskManualPositionSmooth PUBLIC FlightTaskManualPosition FlightTaskUtility)
+target_include_directories(FlightTaskManualPositionSmooth PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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 FlightManualPositionSmooth.cpp
+ */
+
+#include "FlightTaskManualPositionSmooth.hpp"
+
+using namespace matrix;
+
+FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
+	_smoothingXY(this, Vector2f(_velocity)),
+	_smoothingZ(this, _velocity(2), _sticks(2))
+{}
+
+void FlightTaskManualPositionSmooth::_updateSetpoints()
+{
+	/* Get yaw setpont, un-smoothed position setpoints.*/
+	FlightTaskManualPosition::_updateSetpoints();
+
+	/* Smooth velocity setpoint in xy.*/
+	Vector2f vel(_velocity);
+	Vector2f vel_sp_xy(_velocity_setpoint);
+	_smoothingXY.updateMaxVelocity(_constraints.speed_xy);
+	_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
+	_velocity_setpoint(0) = vel_sp_xy(0);
+	_velocity_setpoint(1) = vel_sp_xy(1);
+
+	/* Check for xy position lock.*/
+	_updateXYlock();
+
+	/* Smooth velocity in z.*/
+	_smoothingZ.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
+
+	/* Check for altitude lock*/
+	_updateAltitudeLock();
+
+}

src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.hpp

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 FlightManualPositionSmooth.hpp
+ *
+ * Flight task for smooth manual controlled position.
+ */
+
+#pragma once
+
+#include "FlightTaskManualPosition.hpp"
+#include "ManualSmoothingXY.hpp"
+#include "ManualSmoothingZ.hpp"
+
+class FlightTaskManualPositionSmooth : public FlightTaskManualPosition
+{
+public:
+	FlightTaskManualPositionSmooth();
+
+	virtual ~FlightTaskManualPositionSmooth() = default;
+
+protected:
+
+	virtual void _updateSetpoints() override;
+
+private:
+	ManualSmoothingXY _smoothingXY; /**< smoothing for velocity setpoints in xy */
+	ManualSmoothingZ _smoothingZ; /**< smoothing for velocity in z */
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskManualPositionSmoothVel
+	FlightTaskManualPositionSmoothVel.cpp
+)
+
+target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility)
+target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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

@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   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 "FlightTaskManualPositionSmoothVel.hpp"
+
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskManualPosition::activate(last_setpoint);
+
+	// Check if the previous FlightTask provided setpoints
+	checkSetpoints(last_setpoint);
+	const Vector2f accel_prev(last_setpoint.acceleration[0], last_setpoint.acceleration[1]);
+	const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy);
+	const Vector2f pos_prev(last_setpoint.x, last_setpoint.y);
+
+	_smoothing_xy.reset(accel_prev, vel_prev, pos_prev);
+	_smoothing_z.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z);
+
+	return ret;
+}
+
+void FlightTaskManualPositionSmoothVel::reActivate()
+{
+	// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
+	// using the generated jerk, reset the z derivatives to zero
+	_smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position));
+	_smoothing_z.reset(0.f, 0.f, _position(2));
+}
+
+void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
+
+	if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
+
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
+
+	if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
+
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	for (int i = 0; i < 3; i++) {
+		if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
+	}
+}
+
+void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY()
+{
+	_smoothing_xy.setCurrentPosition(_position.xy());
+}
+
+void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY()
+{
+	_smoothing_xy.setCurrentVelocity(_velocity.xy());
+}
+
+void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ()
+{
+	_smoothing_z.setCurrentPosition(_position(2));
+}
+
+void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ()
+{
+	_smoothing_z.setCurrentVelocity(_velocity(2));
+}
+
+void FlightTaskManualPositionSmoothVel::_updateSetpoints()
+{
+	// Set max accel/vel/jerk
+	// Has to be done before _updateTrajectories()
+	_updateTrajConstraints();
+	_updateTrajVelFeedback();
+	_updateTrajCurrentPositionEstimate();
+
+	// Get yaw setpoint, un-smoothed position setpoints
+	FlightTaskManualPosition::_updateSetpoints();
+
+	_updateTrajectories(_velocity_setpoint);
+
+	// Fill the jerk, acceleration, velocity and position setpoint vectors
+	_setOutputState();
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
+{
+	_updateTrajConstraintsXY();
+	_updateTrajConstraintsZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY()
+{
+	_smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get());
+	_smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get());
+	_smoothing_xy.setMaxVel(_constraints.speed_xy);
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ()
+{
+	_smoothing_z.setMaxJerk(_param_mpc_jerk_max.get());
+
+	_smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get());
+	_smoothing_z.setMaxVelUp(_constraints.speed_up);
+
+	_smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get());
+	_smoothing_z.setMaxVelDown(_constraints.speed_down);
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback()
+{
+	_smoothing_xy.setVelSpFeedback(Vector2f(_velocity_setpoint_feedback));
+	_smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2));
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate()
+{
+	_smoothing_xy.setCurrentPositionEstimate(Vector2f(_position));
+	_smoothing_z.setCurrentPositionEstimate(_position(2));
+}
+
+void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target)
+{
+	_smoothing_xy.update(_deltatime, Vector2f(vel_target));
+	_smoothing_z.update(_deltatime, vel_target(2));
+}
+
+void FlightTaskManualPositionSmoothVel::_setOutputState()
+{
+	_setOutputStateXY();
+	_setOutputStateZ();
+}
+
+void FlightTaskManualPositionSmoothVel::_setOutputStateXY()
+{
+	_jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk();
+	_acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration();
+	_velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity();
+	_position_setpoint.xy() = _smoothing_xy.getCurrentPosition();
+}
+
+void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
+{
+	_jerk_setpoint(2) = _smoothing_z.getCurrentJerk();
+	_acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration();
+	_velocity_setpoint(2) = _smoothing_z.getCurrentVelocity();
+	_position_setpoint(2) = _smoothing_z.getCurrentPosition();
+}

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

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskManualPositionSmoothVel.hpp
+ *
+ * Flight task for smooth manual controlled position.
+ */
+
+#pragma once
+
+#include "FlightTaskManualPosition.hpp"
+#include "ManualVelocitySmoothingXY.hpp"
+#include "ManualVelocitySmoothingZ.hpp"
+
+using matrix::Vector2f;
+using matrix::Vector3f;
+
+class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
+{
+public:
+	FlightTaskManualPositionSmoothVel() = default;
+
+	virtual ~FlightTaskManualPositionSmoothVel() = default;
+
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	void reActivate() override;
+
+protected:
+
+	virtual void _updateSetpoints() override;
+
+	/** Reset position or velocity setpoints in case of EKF reset event */
+	void _ekfResetHandlerPositionXY() override;
+	void _ekfResetHandlerVelocityXY() override;
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
+					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+				       )
+
+private:
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
+
+	void _updateTrajConstraints();
+	void _updateTrajConstraintsXY();
+	void _updateTrajConstraintsZ();
+
+	void _updateTrajVelFeedback();
+	void _updateTrajCurrentPositionEstimate();
+
+	void _updateTrajectories(Vector3f vel_target);
+
+	void _setOutputState();
+	void _setOutputStateXY();
+	void _setOutputStateZ();
+
+	ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
+	ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskOffboard
+	FlightTaskOffboard.cpp
+)
+
+target_link_libraries(FlightTaskOffboard PUBLIC FlightTask)
+target_include_directories(FlightTaskOffboard PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file FlightTaskAuto.cpp
+ */
+#include "FlightTaskOffboard.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskOffboard::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+
+	_sub_triplet_setpoint.update();
+
+	// require a valid triplet
+	ret = ret && _sub_triplet_setpoint.get().current.valid;
+
+	// require valid position / velocity in xy
+	return ret && PX4_ISFINITE(_position(0))
+	       && PX4_ISFINITE(_position(1))
+	       && PX4_ISFINITE(_velocity(0))
+	       && PX4_ISFINITE(_velocity(1));
+}
+
+bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_position_setpoint = _position;
+	_velocity_setpoint.setZero();
+	_position_lock.setAll(NAN);
+	return ret;
+}
+
+bool FlightTaskOffboard::update()
+{
+	// reset setpoint for every loop
+	_resetSetpoints();
+
+	if (!_sub_triplet_setpoint.get().current.valid) {
+		_setDefaultConstraints();
+		_position_setpoint = _position;
+		return false;
+	}
+
+	// Yaw / Yaw-speed
+	if (_sub_triplet_setpoint.get().current.yaw_valid) {
+		// yaw control required
+		_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
+
+		if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
+			// yawspeed is used as feedforward
+			_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
+		}
+
+	} else if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
+		// only yawspeed required
+		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
+		// set yaw setpoint to NAN since not used
+		_yaw_setpoint = NAN;
+
+	}
+
+	// Loiter
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+		// loiter just means that the vehicle should keep position
+		if (!PX4_ISFINITE(_position_lock(0))) {
+			_position_setpoint = _position_lock = _position;
+
+		} else {
+			_position_setpoint = _position_lock;
+		}
+
+		// don't have to continue
+		return true;
+
+	} else {
+		_position_lock.setAll(NAN);
+	}
+
+	// Takeoff
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+		// just do takeoff to default altitude
+		if (!PX4_ISFINITE(_position_lock(0))) {
+			_position_setpoint = _position_lock = _position;
+			_position_setpoint(2) = _position_lock(2) = _position(2) - _param_mis_takeoff_alt.get();
+
+		} else {
+			_position_setpoint = _position_lock;
+		}
+
+		// don't have to continue
+		return true;
+
+	} else {
+		_position_lock.setAll(NAN);
+	}
+
+	// Land
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		// land with landing speed, but keep position in xy
+		if (!PX4_ISFINITE(_position_lock(0))) {
+			_position_setpoint = _position_lock = _position;
+			_position_setpoint(2) = _position_lock(2) = NAN;
+			_velocity_setpoint(2) = _param_mpc_land_speed.get();
+
+		} else {
+			_position_setpoint = _position_lock;
+			_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		}
+
+		// don't have to continue
+		return true;
+
+	} else {
+		_position_lock.setAll(NAN);
+	}
+
+	// IDLE
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		_thrust_setpoint.zero();
+		return true;
+	}
+
+	// Possible inputs:
+	// 1. position setpoint
+	// 2. position setpoint + velocity setpoint (velocity used as feedforward)
+	// 3. velocity setpoint
+	// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported
+	const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid
+				      && _sub_vehicle_local_position.get().xy_valid;
+	const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid
+				     && _sub_vehicle_local_position.get().z_valid;
+	const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid
+				      && _sub_vehicle_local_position.get().v_xy_valid;
+	const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid
+				     && _sub_vehicle_local_position.get().v_z_valid;
+	const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy;
+	const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z;
+	const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid;
+
+	// if nothing is valid in xy, then exit offboard
+	if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
+		return false;
+	}
+
+	// if nothing is valid in z, then exit offboard
+	if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) {
+		return false;
+	}
+
+	// XY-direction
+	if (feedforward_ctrl_xy) {
+		_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
+		_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
+		_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
+		_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
+
+	} else if (position_ctrl_xy) {
+		_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
+		_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
+
+	} else if (velocity_ctrl_xy) {
+
+		if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
+			// in local frame: don't require any transformation
+			_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
+			_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
+
+		} else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
+			// in body frame: need to transorm first
+			// Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
+			_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf(
+							_yaw) * _sub_triplet_setpoint.get().current.vy;
+			_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf(
+							_yaw) * _sub_triplet_setpoint.get().current.vy;
+
+		} else {
+			// no valid frame
+			return false;
+		}
+	}
+
+	// Z-direction
+	if (feedforward_ctrl_z) {
+		_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
+		_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
+
+	} else if (position_ctrl_z) {
+		_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
+
+	} else if (velocity_ctrl_z) {
+		_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
+	}
+
+	// Acceleration
+	// Note: this is not supported yet and will be mapped to normalized thrust directly.
+	if (_sub_triplet_setpoint.get().current.acceleration_valid) {
+		_thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
+		_thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
+		_thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
+	}
+
+	// use default conditions of upwards position or velocity to take off
+	_constraints.want_takeoff = _checkTakeoff();
+
+	return true;
+}

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskOffboard.hpp
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/position_setpoint.h>
+
+class FlightTaskOffboard : public FlightTask
+{
+public:
+	FlightTaskOffboard() = default;
+
+	virtual ~FlightTaskOffboard() = default;
+	bool update() override;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool updateInitialize() override;
+
+protected:
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
+
+private:
+	matrix::Vector3f _position_lock{};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
+					(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt
+				       )
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskOrbit
+	FlightTaskOrbit.cpp
+)
+
+target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmooth)
+target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskOrbit.cpp
+ */
+
+#include "FlightTaskOrbit.hpp"
+
+#include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+
+using namespace matrix;
+
+FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position)
+{
+	_sticks_data_required = false;
+}
+
+bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
+{
+	bool ret = true;
+	// save previous velocity and roatation direction
+	float v = fabsf(_v);
+	bool clockwise = _v > 0;
+
+	// commanded radius
+	if (PX4_ISFINITE(command.param1)) {
+		clockwise = command.param1 > 0;
+		const float r = fabsf(command.param1);
+		ret = ret && setRadius(r);
+	}
+
+	// commanded velocity, take sign of radius as rotation direction
+	if (PX4_ISFINITE(command.param2)) {
+		v = command.param2;
+	}
+
+	ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f));
+
+	// TODO: apply x,y / z independently in geo library
+	// commanded center coordinates
+	// if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
+	// 	map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
+	// }
+
+	// commanded altitude
+	// if(PX4_ISFINITE(command.param7)) {
+	// 	_position_setpoint(2) = gl_ref.alt - command.param7;
+	// }
+
+	if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6) && PX4_ISFINITE(command.param7)) {
+		if (globallocalconverter_tolocal(command.param5, command.param6, command.param7, &_center(0), &_center(1),
+						 &_position_setpoint(2))) {
+			// global to local conversion failed
+			ret = false;
+		}
+	}
+
+	// perpendicularly approach the orbit circle again when new parameters get commanded
+	_in_circle_approach = true;
+
+	return ret;
+}
+
+bool FlightTaskOrbit::sendTelemetry()
+{
+	orbit_status_s orbit_status{};
+	orbit_status.timestamp = hrt_absolute_time();
+	orbit_status.radius = math::signNoZero(_v) * _r;
+	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
+
+	if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y,
+					  &orbit_status.z)) {
+		return false; // don't send the message if the transformation failed
+	}
+
+	_orbit_status_pub.publish(orbit_status);
+
+	return true;
+}
+
+bool FlightTaskOrbit::setRadius(float r)
+{
+	// clip the radius to be within range
+	r = math::constrain(r, _radius_min, _radius_max);
+
+	// small radius is more important than high velocity for safety
+	if (!checkAcceleration(r, _v, _acceleration_max)) {
+		_v = math::sign(_v) * sqrtf(_acceleration_max * r);
+	}
+
+	_r = r;
+	return true;
+}
+
+bool FlightTaskOrbit::setVelocity(const float v)
+{
+	if (fabs(v) < _velocity_max &&
+	    checkAcceleration(_r, v, _acceleration_max)) {
+		_v = v;
+		return true;
+	}
+
+	return false;
+}
+
+bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
+{
+	return v * v < a * r;
+}
+
+bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	bool ret = FlightTaskManualAltitudeSmooth::activate(last_setpoint);
+	_r = _radius_min;
+	_v =  1.f;
+	_center = Vector2f(_position);
+	_center(0) -= _r;
+
+	// need a valid position and velocity
+	ret = ret && PX4_ISFINITE(_position(0))
+	      && PX4_ISFINITE(_position(1))
+	      && PX4_ISFINITE(_position(2))
+	      && PX4_ISFINITE(_velocity(0))
+	      && PX4_ISFINITE(_velocity(1))
+	      && PX4_ISFINITE(_velocity(2));
+
+	return ret;
+}
+
+bool FlightTaskOrbit::update()
+{
+	// update altitude
+	FlightTaskManualAltitudeSmooth::update();
+
+	// stick input adjusts parameters within a fixed time frame
+	const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f);
+	const float v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f);
+
+	setRadius(r);
+	setVelocity(v);
+
+	Vector2f center_to_position = Vector2f(_position) - _center;
+
+	// make vehicle front always point towards the center
+	_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+
+	if (_in_circle_approach) {
+		generate_circle_approach_setpoints();
+
+	} else {
+		generate_circle_setpoints(center_to_position);
+	}
+
+	// publish information to UI
+	sendTelemetry();
+
+	return true;
+}
+
+void FlightTaskOrbit::generate_circle_approach_setpoints()
+{
+	if (_circle_approach_line.isEndReached()) {
+		// calculate target point on circle and plan a line trajectory
+		Vector2f start_to_center = _center - Vector2f(_position);
+		Vector2f start_to_circle = (start_to_center.norm() - _r) * start_to_center.unit_or_zero();
+		Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
+		Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2));
+		_circle_approach_line.setLineFromTo(_position, target);
+		_circle_approach_line.setSpeed(_param_mpc_xy_cruise.get());
+	}
+
+	// follow the planned line and switch to orbiting once the circle is reached
+	_circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint);
+	_in_circle_approach = !_circle_approach_line.isEndReached();
+
+	// yaw stays constant
+	_yawspeed_setpoint = NAN;
+}
+
+void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
+{
+	// xy velocity to go around in a circle
+	Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
+	velocity_xy = velocity_xy.unit_or_zero();
+	velocity_xy *= _v;
+
+	// xy velocity adjustment to stay on the radius distance
+	velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
+
+	_velocity_setpoint(0) = velocity_xy(0);
+	_velocity_setpoint(1) = velocity_xy(1);
+	_position_setpoint(0) = _position_setpoint(1) = NAN;
+
+	// yawspeed feed-forward because we know the necessary angular rate
+	_yawspeed_setpoint = _v / _r;
+}

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-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 FlightTaskOrbit.hpp
+ *
+ * Flight task for orbiting in a circle around a target position
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include "FlightTaskManualAltitudeSmooth.hpp"
+#include <uORB/Publication.hpp>
+#include <uORB/topics/orbit_status.h>
+#include <StraightLine.hpp>
+
+class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
+{
+public:
+	FlightTaskOrbit();
+	virtual ~FlightTaskOrbit() = default;
+
+	bool applyCommandParameters(const vehicle_command_s &command) override;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool update() override;
+
+	/**
+	 * Check the feasibility of orbit parameters with respect to
+	 * centripetal acceleration a = v^2 / r
+	 * @param r desired radius
+	 * @param v desired velocity
+	 * @param a maximal allowed acceleration
+	 * @return true on success, false if value not accepted
+	 */
+	bool checkAcceleration(float r, float v, float a);
+
+protected:
+	/**
+	 * Send out telemetry information for the log and MAVLink.
+	 * @return true on success, false on error
+	 */
+	bool sendTelemetry();
+
+	/**
+	 * Change the radius of the circle.
+	 * @param r desired new radius
+	 * @return true on success, false if value not accepted
+	 */
+	bool setRadius(const float r);
+
+	/**
+	 * Change the velocity of the vehicle on the circle.
+	 * @param v desired new velocity
+	 * @return true on success, false if value not accepted
+	 */
+	bool setVelocity(const float v);
+
+private:
+	void generate_circle_approach_setpoints(); /**< generates setpoints to smoothly reach the closest point on the circle when starting from far away */
+	void generate_circle_setpoints(matrix::Vector2f center_to_position); /**< generates xy setpoints to orbit the vehicle */
+
+	float _r = 0.f; /**< radius with which to orbit the target */
+	float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */
+	matrix::Vector2f _center; /**< local frame coordinates of the center point */
+
+	bool _in_circle_approach = false;
+	StraightLine _circle_approach_line;
+
+	// TODO: create/use parameters for limits
+	const float _radius_min = 1.f;
+	const float _radius_max = 100.f;
+	const float _velocity_max = 10.f;
+	const float _acceleration_max = 2.f;
+
+	uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise /**< cruise speed for circle approach */
+	)
+};

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

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+# create header only library
+add_library(FlightTaskSport INTERFACE)
+
+# include header directory
+target_include_directories(FlightTaskSport
+INTERFACE
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(FlightTaskSport INTERFACE FlightTaskManual)

src/lib/flight_tasks/tasks/Sport/FlightTaskSport.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskSport.hpp
+ *
+ * Manual controlled position with maximum speed and no smoothing.
+ */
+
+#pragma once
+
+#include "FlightTaskManualPosition.hpp"
+#include "mathlib/mathlib.h"
+#include <float.h>
+
+using namespace matrix;
+
+class FlightTaskSport : public FlightTaskManualPosition
+{
+public:
+	FlightTaskSport() = default;
+
+	virtual ~FlightTaskSport() = default;
+
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override
+	{
+		bool ret = FlightTaskManualPosition::activate(last_setpoint);
+
+		// default constraints already are the maximum allowed limits
+		_setDefaultConstraints();
+
+		return ret;
+	}
+};

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskTransition
+	FlightTaskTransition.cpp
+)
+
+target_link_libraries(FlightTaskTransition PUBLIC FlightTask)
+target_include_directories(FlightTaskTransition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskTranstion.cpp
+ */
+
+#include "FlightTaskTransition.hpp"
+
+bool FlightTaskTransition::updateInitialize()
+{
+	return FlightTask::updateInitialize();
+}
+
+bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s last_setpoint)
+{
+	checkSetpoints(last_setpoint);
+	_transition_altitude = last_setpoint.z;
+	_transition_yaw = last_setpoint.yaw;
+	_acceleration_setpoint.setAll(0.f);
+	_velocity_prev = _velocity;
+	return FlightTask::activate(last_setpoint);
+}
+
+void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the setpoint is unknown, set to the current estimate
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
+}
+
+void FlightTaskTransition::updateAccelerationEstimate()
+{
+	// Estimate the acceleration by filtering the raw derivative of the velocity estimate
+	// This is done to provide a good estimate of the current acceleration to the next flight task after back-transition
+	_acceleration_setpoint = 0.9f * _acceleration_setpoint + 0.1f * (_velocity - _velocity_prev) / _deltatime;
+
+	if (!PX4_ISFINITE(_acceleration_setpoint(0)) ||
+	    !PX4_ISFINITE(_acceleration_setpoint(1)) ||
+	    !PX4_ISFINITE(_acceleration_setpoint(2))) {
+		_acceleration_setpoint.setAll(0.f);
+	}
+
+	_velocity_prev = _velocity;
+}
+
+bool FlightTaskTransition::update()
+{
+	// level wings during the transition, altitude should be controlled
+	_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
+	_thrust_setpoint(2) = NAN;
+	_position_setpoint *= NAN;
+	_velocity_setpoint *= NAN;
+	_position_setpoint(2) = _transition_altitude;
+
+	updateAccelerationEstimate();
+
+	_yaw_setpoint = _transition_yaw;
+	return true;
+}

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskTransition.hpp
+ *
+ * Flight task for automatic VTOL transitions between hover and forward flight and vice versa.
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+
+class FlightTaskTransition : public FlightTask
+{
+public:
+	FlightTaskTransition() = default;
+
+	virtual ~FlightTaskTransition() = default;
+	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool updateInitialize() override;
+	bool update() override;
+
+private:
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
+	void updateAccelerationEstimate();
+
+	float _transition_altitude = 0.0f;
+	float _transition_yaw = 0.0f;
+	matrix::Vector3f _velocity_prev{};
+};

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;
+}