Auto Smooth Vel - Add AutoMapper2 and AutoLineSmoothVel flight tasks

src/lib/FlightTasks/CMakeLists.txt

@@ -59,6 +59,7 @@ list(APPEND flight_tasks_all
 	ManualPositionSmoothVel
 	Sport
 	AutoLine
+	AutoLineSmoothVel
 	AutoFollowMe
 	Offboard
 	Failsafe

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

+############################################################################
+#
+#   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/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

+/****************************************************************************
+ *
+ *   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()
+{
+	bool ret = FlightTaskAutoMapper2::activate();
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].reset(0.f, _velocity(i), _position(i));
+	}
+
+	return ret;
+}
+
+void FlightTaskAutoLineSmoothVel::_setDefaultConstraints()
+{
+	FlightTaskAuto::_setDefaultConstraints();
+
+	_constraints.speed_xy = MPC_XY_VEL_MAX.get(); // TODO : Should be computed using heading
+}
+
+void FlightTaskAutoLineSmoothVel::_generateSetpoints()
+{
+	if (!PX4_ISFINITE(_yaw_setpoint)) {
+		// no valid heading -> set heading along track
+		// TODO: Generate heading along trajectory velocity vector
+		_generateHeadingAlongTrack();
+	}
+
+	_prepareSetpoints();
+	_generateTrajectory();
+}
+
+void FlightTaskAutoLineSmoothVel::_generateHeadingAlongTrack()
+{
+	Vector2f prev_to_dest(_target - _prev_wp);
+	_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
+
+}
+
+/* Constrain some value vith a constrain depending on the sign of the constrain
+ * 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
+ */
+inline float FlightTaskAutoLineSmoothVel::constrain_one_side(float val, float constrain)
+{
+	const float min = (constrain < FLT_EPSILON) ? constrain : 0.f;
+	const float max = (constrain > FLT_EPSILON) ? constrain : 0.f;
+
+	return math::constrain(val, min, max);
+}
+
+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.
+
+	if (PX4_ISFINITE(_position_setpoint(0)) &&
+	    PX4_ISFINITE(_position_setpoint(1))) {
+		// Use position setpoints to generate velocity setpoints
+
+		// Get various path specific vectors. */
+		Vector2f pos_traj;
+		pos_traj(0) = _trajectory[0].getCurrentPosition();
+		pos_traj(1) = _trajectory[1].getCurrentPosition();
+		Vector2f pos_sp_xy(_position_setpoint);
+		Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
+		Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
+		Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
+		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+		Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
+
+		float speed_sp_track = _mc_cruise_speed;
+
+		speed_sp_track = Vector2f(pos_traj_to_dest).length() * 0.3f;
+		speed_sp_track = math::constrain(speed_sp_track, 0.0f, MPC_XY_CRUISE.get());
+		Vector2f velocity_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
+
+		for (int i = 0; i < 2; i++) {
+			// If available, constrain the velocity using _velocity_setpoint(.)
+			if (PX4_ISFINITE(_velocity_setpoint(i))) {
+				_velocity_setpoint(i) = constrain_one_side(velocity_sp_xy(i), _velocity_setpoint(i));
+
+			} else {
+				_velocity_setpoint(i) = velocity_sp_xy(i);
+			}
+
+			_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
+						 0.3f;  // Along-track setpoint + cross-track P controller
+		}
+
+	} else if (!PX4_ISFINITE(_velocity_setpoint(0)) &&
+		   !PX4_ISFINITE(_velocity_setpoint(1))) {
+		// No position nor velocity setpoints available, set the velocity targer to zero
+
+		_velocity_setpoint(0) = 0.f;
+		_velocity_setpoint(1) = 0.f;
+	}
+
+	if (PX4_ISFINITE(_position_setpoint(2))) {
+		const float velocity_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
+					    0.3f; // 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) = constrain_one_side(velocity_sp_z, _velocity_setpoint(2));
+
+		} else {
+			_velocity_setpoint(2) = velocity_sp_z;
+		}
+
+	} else if (!PX4_ISFINITE(_velocity_setpoint(2))) {
+		// No position nor velocity setpoints available, set the velocity targer to zero
+		_velocity_setpoint(2) = 0.f;
+	}
+}
+
+void FlightTaskAutoLineSmoothVel::_generateTrajectory()
+{
+	// Update the constraints of the trajectories
+	_trajectory[0].setMaxAccel(MPC_ACC_HOR_MAX.get()); // TODO : Should be computed using heading
+	_trajectory[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
+	_trajectory[0].setMaxVel(_constraints.speed_xy);
+	_trajectory[1].setMaxVel(_constraints.speed_xy);
+	_trajectory[0].setMaxJerk(MPC_JERK_MIN.get()); // TODO : Should be computed using heading
+	_trajectory[1].setMaxJerk(MPC_JERK_MIN.get());
+	_trajectory[2].setMaxJerk(MPC_JERK_MIN.get());
+
+	if (_velocity_setpoint(2) < 0.f) { // up
+		_trajectory[2].setMaxAccel(MPC_ACC_UP_MAX.get());
+		_trajectory[2].setMaxVel(MPC_Z_VEL_MAX_UP.get());
+
+	} else { // down
+		_trajectory[2].setMaxAccel(MPC_ACC_DOWN_MAX.get());
+		_trajectory[2].setMaxVel(MPC_Z_VEL_MAX_DN.get());
+	}
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
+
+	/* 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 accel_sp_smooth; // Dummy variable
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].integrate(_deltatime * time_stretch, accel_sp_smooth(i), _velocity_setpoint(i), _position_setpoint(i));
+	}
+}

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

+/****************************************************************************
+ *
+ *   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() override;
+
+protected:
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper2,
+					(ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
+					(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR, // acceleration in flight
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
+					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX,
+					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
+					(ParamFloat<px4::params::MPC_JERK_MIN>) MPC_JERK_MIN
+				       );
+
+	void _generateSetpoints() override; /**< Generate setpoints along line. */
+	void _setDefaultConstraints() override;
+
+	inline float constrain_one_side(float val, float constrain);
+	void _generateHeadingAlongTrack(); /**< Generates heading along track. */
+	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
+	void _generateTrajectory();
+	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
+};

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

+############################################################################
+#
+#   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/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

+/****************************************************************************
+ *
+ *   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()
+{
+	bool ret = FlightTaskAuto::activate();
+	_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);
+	}
+
+	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;
+	}
+
+	_generateSetpoints();
+
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
+	}
+
+	// update previous type
+	_type_previous = _type;
+
+	return true;
+}
+
+void FlightTaskAutoMapper2::_reset()
+{
+	// Set setpoints equal current state.
+	_velocity_setpoint = _velocity;
+	_position_setpoint = _position;
+	_speed_at_target = 0.0f;
+}
+
+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()
+{
+	// Keep xy-position and go down with landspeed
+	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
+	const float speed_lnd = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_down : MPC_LAND_SPEED.get();
+	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
+
+	// set constraints
+	_constraints.tilt = MPC_TILTMAX_LND.get();
+	_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
+{
+	// Takeoff is completely defined by target position
+	_position_setpoint = _target;
+	const float speed_tko = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_up : MPC_TKO_SPEED.get();
+	_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
+
+	_constraints.landing_gear = vehicle_constraints_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
+	MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get()));
+}
+
+bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
+{
+	// return true if altitude is above two meters
+	return _alt_above_ground > 2.0f;
+}

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

+/****************************************************************************
+ *
+ *   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() override;
+	bool update() override;
+
+protected:
+
+	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>) MPC_LAND_SPEED,
+					(ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED
+				       );
+
+	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 */
+
+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/FlightTasks/tasks/CMakeLists.txt

@@ -37,6 +37,7 @@ 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})

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -885,7 +885,22 @@ MulticopterPositionControl::start_flight_task()
 
 	} else if (_control_mode.flag_control_auto_enabled) {
 		// Auto relate maneuvers
-		int error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
+		int error = 0;
+		switch (MPC_POS_MODE.get()) {
+		case 0:
+		case 1:
+		case 2:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
+			break;
+
+		case 3:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
+			break;
+
+		default:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
+			break;
+		}
 
 		if (error != 0) {
 			if (prev_failure_count == 0) {