FlightTaskAuto: abstract class for mapping triplets to quadruple

FlightTaskAuto: add type that corresponds to triplet type

FligthTaskAuto: set all setpoints if invalid in xy

FlightTaskAuto: cast triplet type to WaypointType

FlightTaskAutoLine: class for px4 legacy auto

FlightTaskAutoLine: methods prototype

FlightTaskAuto: change sp to wp (=Waypoint)
add params

FlightTaskAutoLine: follow waypoints along line based on flight state

src/lib/FlightTasks/CMakeLists.txt

@@ -32,16 +32,18 @@
 ############################################################################
 
 px4_add_library(flight_tasks
-	tasks/FlightTask.cpp
-	tasks/FlightTaskManual.cpp
-	tasks/FlightTaskManualStabilized.cpp
-	tasks/FlightTaskOrbit.cpp
-	tasks/FlightTaskManualAltitude.cpp
-	tasks/FlightTaskManualAltitudeSmooth.cpp
-	tasks/FlightTaskManualPosition.cpp
-	tasks/FlightTaskManualPositionSmooth.cpp
-	tasks/Utility/ManualSmoothingZ.cpp
-	tasks/Utility/ManualSmoothingXY.cpp
-	SubscriptionArray.cpp
-	FlightTasks.cpp
+tasks/FlightTask.cpp
+		tasks/FlightTaskManual.cpp
+		tasks/FlightTaskManualStabilized.cpp
+		tasks/FlightTaskOrbit.cpp
+		tasks/FlightTaskManualAltitude.cpp
+		tasks/FlightTaskManualAltitudeSmooth.cpp
+		tasks/FlightTaskManualPosition.cpp
+		tasks/FlightTaskManualPositionSmooth.cpp
+		tasks/FlightTaskAuto.cpp
+		tasks/FlightTaskAutoLine.cpp
+		tasks/Utility/ManualSmoothingZ.cpp
+		tasks/Utility/ManualSmoothingXY.cpp
+		SubscriptionArray.cpp
+		FlightTasks.cpp
 )

src/lib/FlightTasks/FlightTasks.cpp

@@ -72,6 +72,10 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		_current_task = new (&_task_union.sport) FlightTaskSport();
 		break;
 
+	case 8:
+		_current_task = new (&_task_union.autoLine) FlightTaskAutoLine(this, "ALN");
+		break;
+
 	default:
 		/* invalid task */
 		return -1;

src/lib/FlightTasks/FlightTasks.hpp

@@ -47,6 +47,7 @@
 #include "tasks/FlightTaskManualPosition.hpp"
 #include "tasks/FlightTaskManualPositionSmooth.hpp"
 #include "tasks/FlightTaskManualStabilized.hpp"
+#include "tasks/FlightTaskAutoLine.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 #include "tasks/FlightTaskSport.hpp"
 
@@ -139,6 +140,7 @@ private:
 		FlightTaskManualPositionSmooth position_smooth;
 		FlightTaskOrbit orbit;
 		FlightTaskSport sport;
+		FlightTaskAutoLine autoLine;
 	} _task_union; /**< storage for the currently active task */
 
 	FlightTask *_current_task = nullptr;

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -2,6 +2,7 @@
 #include <mathlib/mathlib.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}};
 
@@ -59,6 +60,15 @@ bool FlightTask::_evaluateVehiclePosition()
 		_position = matrix::Vector3f(&_sub_vehicle_local_position->get().x);
 		_velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx);
 		_yaw = _sub_vehicle_local_position->get().yaw;
+
+		/* Check if reference has changed and update. */
+		if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference) {
+			map_projection_init(&_reference_position, _sub_vehicle_local_position->get().ref_lat,
+					    _sub_vehicle_local_position->get().ref_lon);
+			_reference_altitude = _sub_vehicle_local_position->get().ref_alt;
+			_time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp;
+		}
+
 		return true;
 
 	} else {

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -47,6 +47,8 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
+#include <lib/geo/geo.h>
+
 
 #include "../SubscriptionArray.hpp"
 
@@ -114,6 +116,7 @@ protected:
 	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 */
+	hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update */
 
 	/* Current vehicle state */
 	matrix::Vector3f _position; /**< current vehicle position */
@@ -129,6 +132,10 @@ protected:
 	float _yaw_setpoint;
 	float _yawspeed_setpoint;
 
+	/* Current reference position */
+	map_projection_reference_s _reference_position{}; /**< structure used to project lat/lon setpoint into local frame */
+	float _reference_altitude = 0.0f;  /**< altitude relative to ground */
+
 	/**
 	 * Get the output data
 	 */

src/lib/FlightTasks/tasks/FlightTaskAuto.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 FlightTaskAuto.cpp
+ */
+
+#include "FlightTaskAuto.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+#include <lib/geo/geo.h>
+
+using namespace matrix;
+
+FlightTaskAuto::FlightTaskAuto(control::SuperBlock *parent, const char *name) :
+	FlightTask(parent, name),
+	_mc_cruise_default(this, "MPC_XY_CRUISE", false)
+{}
+
+bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!FlightTask::initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) {
+		return false;
+	}
+
+	return true;
+}
+
+bool FlightTaskAuto::activate()
+{
+	_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
+	_position_lock = matrix::Vector3f(NAN, NAN, NAN);
+	_yaw_wp = _yaw;
+	return FlightTask::activate();
+}
+
+bool FlightTaskAuto::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+	return (ret && _evaluateTriplets());
+}
+
+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 update once 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.
+	 */
+
+	_mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed;
+
+	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
+		/* Use default */
+		_mc_cruise_speed = _mc_cruise_default.get();
+	}
+
+	_yaw_wp = _sub_triplet_setpoint->get().current.yaw;
+	_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
+
+	/* We need to have a valid current triplet */
+	if (_isFinite(_sub_triplet_setpoint->get().current)) {
+
+		/* Reset position lock */
+		_position_lock = matrix::Vector3f(NAN, NAN, NAN);
+
+		/* 1. only consider updated if current target has has changed.
+		 * Note how bad this implementation is. In mission, in every iteration we do double operations */
+		matrix::Vector3f target;
+		map_projection_project(&_reference_position,
+				       _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1));
+		target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
+
+		/* Dont't do any updates if the current target has not changed */
+		if (fabsf(target.length() - _target.length()) < FLT_EPSILON) {
+			return true;
+		}
+
+		/* We have a new target setpoint: update all previous setpoints */
+		_target = target;
+
+		/* Set previous to previous - 1 */
+		_prev_prev_wp = _prev_wp;
+
+		/* Check if previous is valid and update accordingly */
+		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, &_prev_wp(0), &_prev_wp(1));
+			_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude);
+
+
+		} else {
+			_prev_wp = _position;
+		}
+
+		/* Check if previous is valid and update accordingly */
+		if (_type == WaypointType::loiter) {
+			_next_wp = _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, &_next_wp(0), &_next_wp(1));
+			_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude);
+
+		} else {
+			_next_wp = _target;
+		}
+
+		return true;
+
+	} else if (PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) {
+		/* We only have a valid altitude. Hold position in xy. */
+
+		_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
+
+		if (!PX4_ISFINITE(_position_lock.length())) {
+			_position_lock = _position;
+		}
+
+		_prev_prev_wp = _prev_wp;
+		_prev_wp = _position_lock;
+		_prev_wp(2) = _target(2);
+		_target = _position_lock;
+		_target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
+		_next_wp = _target;
+
+		return true;
+
+	} else {
+		/* Reset everything */
+		_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
+		_position_lock = matrix::Vector3f(NAN, NAN, NAN);
+		_yaw_wp = _yaw;
+
+		return false;
+	}
+}
+
+bool FlightTaskAuto::_isFinite(const position_setpoint_s sp)
+{
+	return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
+}

src/lib/FlightTasks/tasks/FlightTaskAuto.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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 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>
+
+/* This enum has to agree with position_setpoint_s type definition */
+enum class WaypointType : int {
+	position = 0,
+	velocity,
+	loiter,
+	takeoff,
+	land,
+	idle
+};
+
+class FlightTaskAuto : public FlightTask
+{
+public:
+	FlightTaskAuto(control::SuperBlock *parent, const char *name);
+
+	virtual ~FlightTaskAuto() = default;
+
+	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
+
+	bool activate() override;
+
+	bool updateInitialize() override;
+
+protected:
+
+	matrix::Vector3f _prev_prev_wp{}; /**< Triplet previous-previous triplet. This will be used for smoothing trajectories -> not used yet. */
+	matrix::Vector3f _prev_wp{}; /**< Triplet previous setpoint in local frame. If not previous triplet is available, the prev_wp is set to current position. */
+	matrix::Vector3f _target{}; /**< Triplet target setpoint in local frame. */
+	matrix::Vector3f _next_wp{}; /**< Triplet setpoint in local frame. If no next setpoint is available, next is set to target. */
+	float _yaw_wp =
+		0.0f; /**< Triplet yaw waypoint. Unfortunately navigator sends yaw setpoint continuously. It would be better if a yaw setpoint is attached
+	to triplet waypoint. This way it would be easy for multicopter to implement features where yaw does not matter. */
+	float _mc_cruise_speed =
+		0.0f; /**< Cruise speed with which multicopter flies and gets set by triplet. If no valid, default cruise speed is used. */
+	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
+
+private:
+	control::BlockParamFloat _mc_cruise_default; /**< Default mc cruise speed*/
+
+	matrix::Vector3f _position_lock{}; /**< Position lock is NAN except when target lat/lon are not finite. */
+	map_projection_reference_s _reference; /**< Reference frame from global to local */
+
+	uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
+
+	bool _evaluateTriplets(); /**< Checks and sets triplets */
+	bool _isFinite(const position_setpoint_s sp);
+	void _updateReference();
+};

src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp

@@ -0,0 +1,489 @@
+/****************************************************************************
+ *
+ *   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;
+
+#define SIGMA_SINGLE_OP			0.000001f
+#define SIGMA_NORM			0.001f
+
+FlightTaskAutoLine::FlightTaskAutoLine(control::SuperBlock *parent, const char *name) :
+	FlightTaskAuto(parent, name),
+	_land_speed(parent, "MPC_LAND_SPEED", false),
+	_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false),
+	_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false),
+	_acc_max_xy(parent, "MPC_ACC_HOR_MAX", false),
+	_acc_xy(parent, "MPC_ACC_HOR", false),
+	_acc_max_up(parent, "MPC_ACC_UP_MAX", false),
+	_acc_max_down(parent, "MPC_ACC_DOWN_MAX", false),
+	_cruise_speed_90(parent, "MPC_CRUISE_90", false),
+	_nav_rad(parent, "NAV_ACC_RAD", false),
+	_mis_yaw_error(parent, "MIS_YAW_ERR", false)
+{}
+
+bool FlightTaskAutoLine::activate()
+{
+	_vel_sp_xy = matrix::Vector2f(&_velocity(0));
+	_pos_sp_xy = matrix::Vector2f(&_position(0));
+	_vel_sp_z = _velocity(2);
+	_pos_sp_z = _position(2);
+	_destination = _target;
+	_origin = _prev_wp;
+	_speed_at_target = 0.0f;
+
+	return FlightTaskAuto::activate();
+}
+
+bool FlightTaskAutoLine::update()
+{
+	if (_type == WaypointType::idle) {
+
+		_generateIdleSetpoints();
+
+	} else if (_type == WaypointType::land) {
+
+		_generateLandSetpoints();
+
+	} else if (_type == WaypointType::loiter || _type == WaypointType::position) {
+
+		_generateSetpoints();
+
+	} else if (_type == WaypointType::takeoff) {
+
+		_generateTakeoffSetpoints();
+
+	} else if (_type == WaypointType::velocity) {
+
+		_generateVelocitySetpoints();
+	}
+
+	/* Send setpoints */
+	_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
+	_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
+	_setYawSetpoint(_yaw_wp);
+
+	return true;
+}
+
+void FlightTaskAutoLine::_generateIdleSetpoints()
+{
+	/* Send zero thrust setpoint */
+	_vel_sp_xy = matrix::Vector2f(NAN, NAN);
+	_vel_sp_z = NAN;
+	_pos_sp_xy = matrix::Vector2f(NAN, NAN);
+	_pos_sp_z = NAN;
+	_setThrustSetpoint(Vector3f(0.0f, 0.0f, 0.0f));
+}
+
+void FlightTaskAutoLine::_generateLandSetpoints()
+{
+	_pos_sp_xy = Vector2f(&_target(0));
+	_pos_sp_z = NAN;
+	_vel_sp_xy *= NAN;
+	_vel_sp_z = _land_speed.get();
+}
+
+void FlightTaskAutoLine::_generateTakeoffSetpoints()
+{
+	_pos_sp_xy = Vector2f(&_target(0));
+	_pos_sp_z = _target(2);
+	_vel_sp_xy.zero();
+	_vel_sp_z = 0.0f;
+}
+
+void FlightTaskAutoLine::_generateSetpoints()
+{
+	_updateInternalWaypoints();
+	_generateAltitudeSetpoints();
+	_generateXYsetpoints();
+}
+
+void FlightTaskAutoLine::_updateInternalWaypoints()
+{
+
+	if (_type == WaypointType::loiter) {
+		/* If loiter, then we want to stop at loiter anyway. Hence, just
+		 * set next waypoint to target.
+		 */
+		_next_wp = _target;
+	}
+
+	/* Adjust destination and origin based on current vehicle state */
+	Vector2f u_prev_to_target = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero();
+	Vector2f pos_to_target = Vector2f(&(_target - _position)(0));
+	Vector2f prev_to_pos = Vector2f(&(_position - _prev_wp)(0));
+	Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
+
+	if (u_prev_to_target * pos_to_target < 0.0f) {
+
+		/* Target is behind */
+		if (_current_state != State::target_behind) {
+
+			_destination = _target;
+			_origin = _position;
+			_current_state = State::target_behind;
+
+			float angle = 2.0f;
+			_speed_at_target = 0.0f;
+
+			/* angle = cos(x) + 1.0
+			 * angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0 */
+
+			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
+			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+
+				angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
+					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+					+ 1.0f;
+				_speed_at_target = _getVelcoityFromAngle(angle);
+			}
+		}
+
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
+
+		/* current position more than cruise speed in front of previous setpoint. */
+		if (_current_state != State::previous_infront) {
+			_destination = _prev_wp;
+			_origin = _position;
+			_current_state = State::previous_infront;
+
+			float angle = 2.0f;
+			_speed_at_target = 0.0f;
+
+			/* angle = cos(x) + 1.0
+			 * angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0 */
+			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
+			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+
+				angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
+					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+					+ 1.0f;
+				_speed_at_target = _getVelcoityFromAngle(angle);
+			}
+
+		}
+
+	} else if (Vector2f(Vector2f(&_position(0)) - closest_pt).length() > _mc_cruise_speed) {
+
+		/* Vehicle is more than cruise speed off track */
+		if (_current_state != State::offtrack) {
+			_destination = matrix::Vector3f(closest_pt(0), closest_pt(1), _target(2));
+			_origin = _position;
+			_current_state = State::offtrack;
+
+			float angle = 2.0f;
+			_speed_at_target = 0.0f;
+
+			/* angle = cos(x) + 1.0
+			 * angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0 */
+			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
+			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+
+				angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
+					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+					+ 1.0f;
+				_speed_at_target = _getVelcoityFromAngle(angle);
+			}
+
+		}
+
+	} else {
+
+		if ((_target - _destination).length() > 0.01f) {
+			_destination = _target;
+			_origin = _prev_wp;
+			_current_state = State::none;
+
+			float angle = 2.0f;
+			_speed_at_target = 0.0f;
+
+
+			/* angle = cos(x) + 1.0
+			 * angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0 */
+			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
+			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+
+				angle =
+					Vector2f(&(_destination - _origin)(0)).unit_or_zero()
+					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+					+ 1.0f;
+				_speed_at_target = _getVelcoityFromAngle(angle);
+			}
+		}
+	}
+}
+
+void FlightTaskAutoLine::_generateXYsetpoints()
+{
+
+	Vector2f pos_sp_to_dest = Vector2f(&_target(0)) - _pos_sp_xy;
+	const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < _nav_rad.get();
+
+	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _nav_rad.get()) ||
+	    (!has_reached_altitude && pos_sp_to_dest.length() < _nav_rad.get())) {
+
+		/* Vehicle reached target in xy. Lock position */
+		_pos_sp_xy = Vector2f(&_destination(0));
+		_vel_sp_xy.zero();
+
+	} else {
+
+		Vector2f u_prev_to_dest = Vector2f(&(_destination - _origin)(0)).unit_or_zero();
+		Vector2f prev_to_pos(&(_position - _origin)(0));
+		Vector2f closest_pt = Vector2f(&_origin(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+		Vector2f closest_to_dest = Vector2f(&(_destination - _position)(0));
+		Vector2f prev_to_dest = Vector2f(&(_destination - _origin)(0));
+		float speed_sp_track = _mc_cruise_speed;
+		float speed_sp_prev_track = math::max(_vel_sp_xy * 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 = 0.5f * prev_to_dest.length();
+		}
+
+		/* Compute maximum speed at target threshold */
+		if (threshold_max > _nav_rad.get()) {
+			float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _nav_rad.get());
+			speed_threshold = m * (target_threshold - _nav_rad.get()) + _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 = _nav_rad.get();
+
+			if (_speed_at_target < 0.01f) {
+				acceptance_radius = 0.0f;
+			}
+
+			if ((target_threshold - acceptance_radius) >= SIGMA_NORM) {
+
+				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;
+			float yaw_diff = _wrap_pi(_yaw_wp - _yaw);
+
+			/* If yaw offset is large, only accelerate with 0.5 m/s^2 */
+			float acc_max = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acc_xy.get();
+
+			if (acc_track > acc_max) {
+				speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
+			}
+		}
+
+		speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
+
+		_pos_sp_xy = closest_pt;
+		_vel_sp_xy = u_prev_to_dest * speed_sp_track;
+	}
+}
+
+void FlightTaskAutoLine::_generateAltitudeSetpoints()
+{
+
+	/* Total distance between previous and target setpoint */
+	const float dist = fabsf(_destination(2) - _origin(2));
+
+	/* If target has not been reached, then compute setpoint depending on maximum velocity */
+	if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _destination(2)) > 0.1f)) {
+
+		/* get various distances */
+		const float dist_to_prev = fabsf(_position(2) - _origin(2));
+		const float dist_to_target = fabsf(_destination(2) - _position(2));
+
+		/* check sign */
+		const bool flying_upward = _destination(2) < _position(2);
+
+		/* 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) ? _vel_max_up.get() : _vel_max_down.get();
+
+		/* 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
+			 * up to 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(_vel_sp_z)) / _deltatime;
+			const float acc_max = (flying_upward) ? (_acc_max_up.get() * 0.5f) : (_acc_max_down.get() * 0.5f);
+
+			if (acc > acc_max) {
+				speed_sp = acc_max * _deltatime + fabsf(_vel_sp_z);
+			}
+		}
+
+		/* 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 */
+		_vel_sp_z = (flying_upward) ? -speed_sp : speed_sp;
+		_pos_sp_z = NAN; // We don't care about position setpoint */
+
+	} else {
+
+		/* Vehicle reached desired target altitude */
+		_vel_sp_z = 0.0f;
+		_pos_sp_z = _target(2);
+	}
+}
+void FlightTaskAutoLine::_generateVelocitySetpoints()
+{
+	/* TODO: Remove velocity force logic from navigator, since
+	 * navigator should only send out waypoints. */
+	_vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed;
+	_vel_sp_z = 0.0f;
+	_pos_sp_z = _position(2);
+	_pos_sp_xy = Vector2f(NAN, NAN);
+}
+
+float FlightTaskAutoLine::_getVelcoityFromAngle(const float angle)
+{
+	/* Minimum cruise speed when passing waypoint */
+	float min_cruise_speed = 0.0f;
+
+	/* Make sure that cruise speed is larger than minimum*/
+	if ((_mc_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
+		return _mc_cruise_speed;
+	}
+
+	/* Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees.
+	 * It needs to be always larger than minimum cruise speed. */
+	float middle_cruise_speed = _cruise_speed_90.get();
+
+	if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
+		middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
+	}
+
+	if ((_mc_cruise_speed - middle_cruise_speed) < SIGMA_NORM) {
+		middle_cruise_speed = (_mc_cruise_speed + min_cruise_speed) * 0.5f;
+	}
+
+	/* If middle cruise speed is exactly in the middle, then compute
+	 * speed linearly
+	 */
+	bool use_linear_approach = false;
+
+	if (((_mc_cruise_speed + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) {
+		use_linear_approach = true;
+	}
+
+	/* Compute speed sp at target */
+	float speed_close;
+
+	if (use_linear_approach) {
+
+		/* velocity close to target adjusted to angle
+		 * vel_close =  m*x+q
+		 */
+		float slope = -(_mc_cruise_speed - min_cruise_speed) / 2.0f;
+		speed_close = slope * angle + _mc_cruise_speed;
+
+	} else {
+
+		/* Speed close to target adjusted to angle x.
+		 * speed_close = a *b ^x + c; where at angle x = 0 -> speed_close = cruise; angle x = 1 -> speed_close = middle_cruise_speed (this means that at 90degrees
+		 * the velocity at target is middle_cruise_speed);
+		 * angle x = 2 -> speed_close = min_cruising_speed */
+
+		/* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */
+		float a = -((middle_cruise_speed - _mc_cruise_speed) * (middle_cruise_speed - _mc_cruise_speed))
+			  / (2.0f * middle_cruise_speed - _mc_cruise_speed - min_cruise_speed);
+		float c = _mc_cruise_speed - a;
+		float b = (middle_cruise_speed - c) / a;
+		speed_close = a * powf(b, angle) + c;
+	}
+
+	/* speed_close needs to be in between max and min */
+	return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed);
+}

src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskAutoLine.hpp
+ *
+ * Flight task for autonomous, gps driven mode. The vehicle flies
+ * along a straight line in between waypoints.
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+
+class FlightTaskAutoLine : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoLine(control::SuperBlock *parent, const char *name);
+
+	virtual ~FlightTaskAutoLine() = default;
+
+	bool activate() override;
+
+	bool update() override;
+
+protected:
+
+	matrix::Vector2f _vel_sp_xy{};
+	matrix::Vector2f _pos_sp_xy{};
+	float _vel_sp_z = 0.0f;
+	float _pos_sp_z = 0.0f;
+
+	matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */
+	matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */
+	float _speed_at_target = 0.0f; /**< Desired velocity at 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 */
+	};
+	State _current_state{State::none};
+
+	control::BlockParamFloat _land_speed; /**< Downward speed during landing. */
+	control::BlockParamFloat _vel_max_up; /**< Maximum upward velocity. */
+	control::BlockParamFloat _vel_max_down; /**< Maximum downward velocity. */
+	control::BlockParamFloat _acc_max_xy; /**< Maximum acceleration in hover. */
+	control::BlockParamFloat _acc_xy; /**< Maximum acceleration from hover to fast forward flight. */
+	control::BlockParamFloat _acc_max_up; /**< Max acceleration up. */
+	control::BlockParamFloat _acc_max_down; /**< Max acceleration down. */
+	control::BlockParamFloat _cruise_speed_90; /**< Speed when angle is 90 degrees between prev-current/current-next. */
+
+	/* None-position control params */
+	control::BlockParamFloat _nav_rad; /**< Radius that is used by navigator that defines when to update triplets */
+	control::BlockParamFloat _mis_yaw_error; /**< Yaw threshold during mission to consider yaw as accepted */
+
+	void _generateIdleSetpoints();
+	void _generateLandSetpoints();
+	void _generateVelocitySetpoints();
+	void _generateTakeoffSetpoints();
+	void _updateInternalWaypoints();
+	void _generateSetpoints(); /**< Generate setpoints during auto tracking */
+	void _generateAltitudeSetpoints();
+	void _generateXYsetpoints();
+	float _getVelcoityFromAngle(const float angle); /** Computes the speed at target depending on angle */
+};

src/modules/mc_pos_control/PositionControl.cpp

@@ -108,7 +108,23 @@ void PositionControl::generateThrustYawSetpoint(const float &dt)
 	 */
 	if (!_skipController) {
 		_positionController();
+
+		if (_smooth_takeoff) {
+			_setTakeoffVelocity(dt);
+
+		}  else {
+			_takeoff_vel_sp = 0.5f; // Reset to pointing downwards.
+		}
+
 		_velocityController(dt);
+
+	} else {
+		if (_smooth_takeoff) {
+			_setTakeoffThrust(dt);
+
+		} else {
+			_takeoff_vel_sp = 0.5f; // Reset to pointing downwards.
+		}
 	}
 }
 
@@ -119,6 +135,13 @@ void PositionControl::_interfaceMapping()
 	 * do not require control.
 	 */
 
+	if (PX4_ISFINITE(_pos_sp(2)) && _smooth_takeoff) {
+		_takeoff_max_speed = 0.6f;
+
+	} else {
+		_takeoff_max_speed = _VelMaxZ[0];
+	}
+
 	/* Loop through x,y and z components of all setpoints. */
 	for (int i = 0; i <= 2; i++) {
 
@@ -176,11 +199,11 @@ void PositionControl::_positionController()
 	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;
 
 	/* Make sure velocity setpoint is constrained in all directions (xyz). */
-	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
+	Vector2f vel_sp_xy(&_vel_sp(0));
 
-	if (vel_norm_xy > _VelMaxXY) {
-		_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy;
-		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy;
+	if (vel_sp_xy.length() >= _VelMaxXY) {
+		_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_sp_xy.length();
+		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_sp_xy.length();
 	}
 
 	/* Saturate velocity in D-direction */
@@ -311,6 +334,33 @@ void PositionControl::_updateParams()
 	}
 }
 
+void PositionControl::_setTakeoffVelocity(const float dt)
+{
+
+	/* Limit velocity setpoint to maximum takeoff velocity */
+	_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_max_speed);
+	/* Smooth takeoff is achieved once target velocity is reached. (NED frame). */
+	_smooth_takeoff = _takeoff_vel_sp >= _vel_sp(2);
+	/* ramp vertical velocity limit up to takeoff speed */
+	_takeoff_vel_sp += _vel_sp(2) * dt / _TakeoffRampTime;
+	/* limit vertical velocity to the current ramp value */
+	_vel_sp(2) = math::max(_vel_sp(2), _takeoff_vel_sp);
+
+}
+
+void PositionControl::_setTakeoffThrust(const float dt)
+{
+
+	/* Smooth takeoff is achieved once target velocity is reached. (NED frame). */
+	_smooth_takeoff = _takeoff_vel_sp >= _thr_sp(2);
+	/* ramp vertical velocity limit up to takeoff speed */
+	_takeoff_vel_sp += _thr_sp(2) * dt / _TakeoffRampTime;
+	/* limit vertical velocity to the current ramp value */
+	_thr_sp(2) = math::max(_thr_sp(2), _takeoff_vel_sp);
+
+
+}
+
 void PositionControl::_setParams()
 {
 	param_get(_Pxy_h, &Pp(0));

src/modules/mc_pos_control/PositionControl.hpp

@@ -64,7 +64,8 @@ public:
 
 	~PositionControl() {};
 
-	void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot);
+	void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot,
+			 const bool smooth_takeoff);
 	void updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
 	void updateConstraints(const Controller::Constraints &constraints);
 	void generateThrustYawSetpoint(const float &dt);
@@ -74,6 +75,7 @@ public:
 	float getYawspeedSetpoint() {return _yawspeed_sp;}
 	matrix::Vector3f getVelSp() {return _vel_sp;}
 	matrix::Vector3f getPosSp() {return _pos_sp;}
+	bool getSmoothTakeoff() {return _smooth_takeoff;};
 
 private:
 
@@ -91,6 +93,9 @@ private:
 	matrix::Vector3f _thr_sp{};
 	float _yaw_sp{};
 	float _yawspeed_sp{};
+	float _takeoff_vel_sp = 0.5f; // starts positive (NED frame)
+	float _takeoff_max_speed = 1.0f;
+	bool _smooth_takeoff{false};
 
 	/* Other variables */
 	matrix::Vector3f _thr_int{};
@@ -127,6 +132,8 @@ private:
 	};
 	Limits _ThrustLimit;
 	float _ThrHover{0.5f};
+
+	float _ThrLimit[2]; //index 0: max, index 1: min
 	bool _skipController{false};
 
 	/* Helper methods */
@@ -135,4 +142,6 @@ private:
 	void _velocityController(const float &dt);
 	void _updateParams();
 	void _setParams();
+	void _setTakeoffVelocity(const float dt);
+	void _setTakeoffThrust(const float dt);
 };