flight_mode_manager: merge with flight_tasks

src/modules/flight_mode_manager/CMakeLists.txt

@@ -31,17 +31,105 @@
 #
 ############################################################################
 
+
+###########################################
+# Prepare flight tasks
+###########################################
+
+# add upstream flight tasks (they are being handled differently from the core inside the python script)
+list(APPEND flight_tasks_to_add
+	Orbit
+)
+# remove possible duplicates
+list(REMOVE_DUPLICATES flight_tasks_to_add)
+
+# remove flight tasks depending on target
+if(flight_tasks_to_remove)
+	list(REMOVE_ITEM flight_tasks_to_add
+		${flight_tasks_to_remove}
+	)
+endif()
+
+# add core flight tasks to list
+list(APPEND flight_tasks_all
+	ManualAltitude
+	ManualAltitudeSmoothVel
+	ManualPosition
+	ManualPositionSmoothVel
+	AutoLineSmoothVel
+	AutoFollowMe
+	Offboard
+	Failsafe
+	Descend
+	Transition
+	ManualAcceleration
+	${flight_tasks_to_add}
+)
+
+# set the files to be generated
+set(files_to_generate
+	FlightTasks_generated.hpp
+	FlightTasks_generated.cpp
+)
+
+# generate files needed for Flight Tasks
+set(python_args
+	-t ${flight_tasks_all}
+	-i ${CMAKE_CURRENT_SOURCE_DIR}/Templates
+	-o ${CMAKE_CURRENT_BINARY_DIR}
+	-f ${files_to_generate}
+)
+
+# add the additional tasks for the python script (if there are any)
+if(flight_tasks_to_add)
+	list(APPEND python_args
+		-s ${flight_tasks_to_add}
+	)
+endif()
+
+# generate the files using the python script and template
+add_custom_command(
+	OUTPUT
+		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
+		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
+	COMMAND ${PYTHON_EXECUTABLE} generate_flight_tasks.py ${python_args}
+	COMMENT "Generating Flight Tasks"
+	DEPENDS
+		Templates/FlightTasks_generated.cpp.em
+		Templates/FlightTasks_generated.hpp.em
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	VERBATIM
+)
+
+add_compile_options(
+	-Wno-cast-align
+) # TODO: fix and enable
+
+# add subdirectory containing all tasks
+add_subdirectory(tasks)
+
 add_subdirectory(Takeoff)
 
 px4_add_module(
 	MODULE modules__flight_mode_manager
 	MAIN flight_mode_manager
 	COMPILE_FLAGS
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}
+		${CMAKE_CURRENT_BINARY_DIR}
 	SRCS
 		FlightModeManager.cpp
 		FlightModeManager.hpp
+
+		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
+		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
 	DEPENDS
-		FlightTasks
 		Takeoff
 		px4_work_queue
-	)
+		WeatherVane
+)
+
+# add all flight task dependencies
+foreach(task ${flight_tasks_all})
+	target_link_libraries(modules__flight_mode_manager PUBLIC FlightTask${task})
+endforeach()

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -38,21 +38,28 @@
 
 using namespace time_literals;
 
-FlightModeManager::FlightModeManager(bool vtol) :
+FlightModeManager::FlightModeManager() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
-	if (vtol) {
-		// if vehicle is a VTOL we want to enable weathervane capabilities
-		_wv_controller = new WeatherVane();
+	updateParams();
+
+	// initialize all flight-tasks
+	// currently this is required to get all parameters read
+	for (int i = 0; i < static_cast<int>(FlightTaskIndex::Count); i++) {
+		_initTask(static_cast<FlightTaskIndex>(i));
 	}
 
-	updateParams();
+	// disable all tasks
+	_initTask(FlightTaskIndex::None);
 }
 
 FlightModeManager::~FlightModeManager()
 {
+	if (_current_task.task) {
+		_current_task.task->~FlightTask();
+	}
+
 	delete _wv_controller;
 	perf_free(_loop_perf);
 }
@@ -100,8 +107,13 @@ void FlightModeManager::Run()
 		_home_position_sub.update();
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_sub.update();
-		_vehicle_local_position_setpoint_sub.update();
-		_vehicle_status_sub.update();
+
+		if (_vehicle_status_sub.update()) {
+			if (_vehicle_status_sub.get().is_vtol && (_wv_controller == nullptr)) {
+				// if vehicle is a VTOL we want to enable weathervane capabilities
+				_wv_controller = new WeatherVane();
+			}
+		}
 
 		// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
 		_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false,
@@ -129,7 +141,7 @@ void FlightModeManager::Run()
 
 		start_flight_task();
 
-		if (_flight_tasks.isAnyTaskActive()) {
+		if (isAnyTaskActive()) {
 			generateTrajectorySetpoint(dt, vehicle_local_position);
 		}
 
@@ -141,7 +153,10 @@ void FlightModeManager::Run()
 void FlightModeManager::updateParams()
 {
 	ModuleParams::updateParams();
-	_flight_tasks.handleParameterUpdate();
+
+	if (isAnyTaskActive()) {
+		_current_task.task->handleParameterUpdate();
+	}
 
 	_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
 	_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
@@ -160,7 +175,7 @@ void FlightModeManager::start_flight_task()
 
 	// Do not run any flight task for VTOLs in fixed-wing mode
 	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		_flight_tasks.switchTask(FlightTaskIndex::None);
+		switchTask(FlightTaskIndex::None);
 		return;
 	}
 
@@ -168,18 +183,18 @@ void FlightModeManager::start_flight_task()
 	// exclude Orbit mode since the task is initiated in FlightTasks through the vehicle_command and we should not switch out
 	if (_last_vehicle_nav_state != _vehicle_status_sub.get().nav_state
 	    && _vehicle_status_sub.get().nav_state != vehicle_status_s::NAVIGATION_STATE_ORBIT) {
-		_flight_tasks.switchTask(FlightTaskIndex::None);
+		switchTask(FlightTaskIndex::None);
 	}
 
 	// Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode)
 	if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) {
 
 		should_disable_task = false;
-		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition);
+		FlightTaskError error = switchTask(FlightTaskIndex::Transition);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Transition activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -202,11 +217,11 @@ void FlightModeManager::start_flight_task()
 		_vehicle_control_mode_sub.get().flag_control_acceleration_enabled)) {
 
 		should_disable_task = false;
-		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Offboard);
+		FlightTaskError error = switchTask(FlightTaskIndex::Offboard);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Offboard activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Offboard activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -221,11 +236,11 @@ void FlightModeManager::start_flight_task()
 	// Auto-follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
 		should_disable_task = false;
-		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe);
+		FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowMe);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Follow-Me activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -241,11 +256,11 @@ void FlightModeManager::start_flight_task()
 		should_disable_task = false;
 		FlightTaskError error = FlightTaskError::NoError;
 
-		error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
+		error = switchTask(FlightTaskIndex::AutoLineSmoothVel);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Auto activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -262,11 +277,11 @@ void FlightModeManager::start_flight_task()
 		should_disable_task = false;
 		FlightTaskError error = FlightTaskError::NoError;
 
-		error =  _flight_tasks.switchTask(FlightTaskIndex::Descend);
+		error = switchTask(FlightTaskIndex::Descend);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Descend activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -286,22 +301,22 @@ void FlightModeManager::start_flight_task()
 
 		switch (_param_mpc_pos_mode.get()) {
 		case 0:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
+			error = switchTask(FlightTaskIndex::ManualPosition);
 			break;
 
 		case 3:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
+			error = switchTask(FlightTaskIndex::ManualPositionSmoothVel);
 			break;
 
 		case 4:
 		default:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAcceleration);
+			error = switchTask(FlightTaskIndex::ManualAcceleration);
 			break;
 		}
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Position-Ctrl activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -320,18 +335,18 @@ void FlightModeManager::start_flight_task()
 
 		switch (_param_mpc_pos_mode.get()) {
 		case 0:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
+			error = switchTask(FlightTaskIndex::ManualAltitude);
 			break;
 
 		case 3:
 		default:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
+			error = switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
 			break;
 		}
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
-				PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
+				PX4_WARN("Altitude-Ctrl activation failed with error: %s", errorToString(error));
 			}
 
 			task_failure = true;
@@ -352,15 +367,15 @@ void FlightModeManager::start_flight_task()
 
 		// for some reason no flighttask was able to start.
 		// go into failsafe flighttask
-		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe);
+		FlightTaskError error = switchTask(FlightTaskIndex::Failsafe);
 
 		if (error != FlightTaskError::NoError) {
 			// No task was activated.
-			_flight_tasks.switchTask(FlightTaskIndex::None);
+			switchTask(FlightTaskIndex::None);
 		}
 
 	} else if (should_disable_task) {
-		_flight_tasks.switchTask(FlightTaskIndex::None);
+		switchTask(FlightTaskIndex::None);
 	}
 
 	_last_vehicle_nav_state = _vehicle_status_sub.get().nav_state;
@@ -383,7 +398,6 @@ void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state)
 void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
 {
 	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
@@ -415,29 +429,78 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
 	}
 
 	// publish the vehicle command
+	command.timestamp = hrt_absolute_time();
 	_vehicle_command_pub.publish(command);
 }
 
 void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		const vehicle_local_position_s &vehicle_local_position)
 {
+	// In case flight task was not able to update correctly we send the empty setpoint which makes the position controller failsafe.
+	if (_vehicle_command_sub.updated()) {
+		// get command
+		vehicle_command_s command{};
+		_vehicle_command_sub.copy(&command);
+
+		// check what command it is
+		FlightTaskIndex desired_task = switchVehicleCommand(command.command);
+
+		// ignore all unkown commands
+		if (desired_task != FlightTaskIndex::None) {
+			// switch to the commanded task
+			FlightTaskError switch_result = switchTask(desired_task);
+			uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+
+			// if we are in/switched to the desired task
+			if (switch_result >= FlightTaskError::NoError) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+				// if the task is running apply parameters to it and see if it rejects
+				if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
+					cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+
+					// if we just switched and parameters are not accepted, go to failsafe
+					if (switch_result >= FlightTaskError::NoError) {
+						switchTask(FlightTaskIndex::ManualPosition);
+						cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+					}
+				}
+			}
+
+			// send back acknowledgment
+			vehicle_command_ack_s command_ack{};
+			command_ack.command = command.command;
+			command_ack.result = cmd_result;
+			command_ack.result_param1 = static_cast<int>(switch_result);
+			command_ack.target_system = command.source_system;
+			command_ack.target_component = command.source_component;
+			command_ack.timestamp = hrt_absolute_time();
+			_vehicle_command_ack_pub.publish(command_ack);
+		}
+	}
+
+
+	_current_task.task->setYawHandler(_wv_controller);
+
 	// Inform FlightTask about the input and output of the velocity controller
 	// This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
-	_flight_tasks.updateVelocityControllerIO(Vector3f(_vehicle_local_position_setpoint_sub.get().vx,
-			_vehicle_local_position_setpoint_sub.get().vy, _vehicle_local_position_setpoint_sub.get().vz),
-			Vector3f(_vehicle_local_position_setpoint_sub.get().acceleration));
+	if (_vehicle_local_position_setpoint_sub.updated()) {
+		vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+
+		if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)) {
+			const Vector3f vel_sp{vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy, vehicle_local_position_setpoint.vz};
+			const Vector3f thrust_sp{vehicle_local_position_setpoint.acceleration};
+			_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp);
+		}
+	}
+
+	if (_current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize()) {
+		// updated
+	}
 
 	// setpoints and constraints for the position controller from flighttask or failsafe
-	vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
-	vehicle_constraints_s constraints = FlightTask::empty_constraints;
-
-	_flight_tasks.setYawHandler(_wv_controller);
-
-	// In case flight task was not able to update correctly we send the empty setpoint which makes the position controller failsafe.
-	if (_flight_tasks.update()) {
-		setpoint = _flight_tasks.getPositionSetpoint();
-		constraints = _flight_tasks.getConstraints();
-	}
+	vehicle_local_position_setpoint_s setpoint = _current_task.task->getPositionSetpoint();
+	vehicle_constraints_s constraints = _current_task.task->getConstraints();
 
 	// limit altitude according to land detector
 	limitAltitude(setpoint, vehicle_local_position);
@@ -456,8 +519,16 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		constraints.reset_integral = true;
 	}
 
+	if (not_taken_off) {
+		// reactivate the task which will reset the setpoint to current state
+		_current_task.task->reActivate();
+	}
+
+
+	setpoint.timestamp = hrt_absolute_time();
 	_trajectory_setpoint_pub.publish(setpoint);
 
+
 	// Allow ramping from zero thrust on takeoff
 	if (flying) {
 		constraints.minimum_thrust = _param_mpc_thr_min.get();
@@ -484,21 +555,18 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 				    _time_stamp_last_loop);
 	constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up);
 
-	constraints.flight_task = _flight_tasks.getActiveTask();
+	constraints.flight_task = static_cast<uint32_t>(_current_task.index);
+	constraints.timestamp = hrt_absolute_time();
 	_vehicle_constraints_pub.publish(constraints);
 
-	if (not_taken_off) {
-		// reactivate the task which will reset the setpoint to current state
-		_flight_tasks.reActivate();
-	}
 
 	// if there's any change in landing gear setpoint publish it
-	landing_gear_s landing_gear = _flight_tasks.getGear();
+	landing_gear_s landing_gear = _current_task.task->getGear();
 
 	if (landing_gear.landing_gear != _old_landing_gear_position
 	    && landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
 
-		landing_gear.timestamp = _time_stamp_last_loop;
+		landing_gear.timestamp = hrt_absolute_time();
 		_landing_gear_pub.publish(landing_gear);
 	}
 
@@ -533,17 +601,73 @@ void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s
 	setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
 }
 
-int FlightModeManager::task_spawn(int argc, char *argv[])
+FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 {
-	bool vtol = false;
-
-	if (argc > 1) {
-		if (strcmp(argv[1], "vtol") == 0) {
-			vtol = true;
-		}
+	// switch to the running task, nothing to do
+	if (new_task_index == _current_task.index) {
+		return FlightTaskError::NoError;
 	}
 
-	FlightModeManager *instance = new FlightModeManager(vtol);
+	// Save current setpoints for the next FlightTask
+	vehicle_local_position_setpoint_s last_setpoint{};
+	ekf_reset_counters_s last_reset_counters = FlightTask::zero_reset_counters;
+
+	if (isAnyTaskActive()) {
+		last_setpoint = _current_task.task->getPositionSetpoint();
+		last_reset_counters = _current_task.task->getResetCounters();
+	}
+
+	if (_initTask(new_task_index)) {
+		// invalid task
+		return FlightTaskError::InvalidTask;
+	}
+
+	if (!isAnyTaskActive()) {
+		// no task running
+		return FlightTaskError::NoError;
+	}
+
+	// activation failed
+	if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
+		_current_task.task->~FlightTask();
+		_current_task.task = nullptr;
+		_current_task.index = FlightTaskIndex::None;
+		return FlightTaskError::ActivationFailed;
+	}
+
+	_current_task.task->setResetCounters(last_reset_counters);
+
+	return FlightTaskError::NoError;
+}
+
+FlightTaskError FlightModeManager::switchTask(int new_task_index)
+{
+	// make sure we are in range of the enumeration before casting
+	if (static_cast<int>(FlightTaskIndex::None) <= new_task_index &&
+	    static_cast<int>(FlightTaskIndex::Count) > new_task_index) {
+		return switchTask(FlightTaskIndex(new_task_index));
+	}
+
+	switchTask(FlightTaskIndex::None);
+	return FlightTaskError::InvalidTask;
+}
+
+const char *FlightModeManager::errorToString(const FlightTaskError error)
+{
+	switch (error) {
+	case FlightTaskError::NoError: return "No Error";
+
+	case FlightTaskError::InvalidTask: return "Invalid Task ";
+
+	case FlightTaskError::ActivationFailed: return "Activation Failed";
+	}
+
+	return "This error is not mapped to a string or is unknown.";
+}
+
+int FlightModeManager::task_spawn(int argc, char *argv[])
+{
+	FlightModeManager *instance = new FlightModeManager();
 
 	if (instance) {
 		_object.store(instance);
@@ -571,8 +695,8 @@ int FlightModeManager::custom_command(int argc, char *argv[])
 
 int FlightModeManager::print_status()
 {
-	if (_flight_tasks.isAnyTaskActive()) {
-		PX4_INFO("Running, active flight task: %i", _flight_tasks.getActiveTask());
+	if (isAnyTaskActive()) {
+		PX4_INFO("Running, active flight task: %i", static_cast<uint32_t>(_current_task.index));
 
 	} else {
 		PX4_INFO("Running, no flight task active");
@@ -598,7 +722,6 @@ and outputs setpoints for controllers.
 
 	PRINT_MODULE_USAGE_NAME("flight_mode_manager", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -33,17 +33,21 @@
 
 #pragma once
 
+#include "FlightTask.hpp"
+#include "FlightTasks_generated.hpp"
+
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/flight_tasks/FlightTasks.hpp>
 
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -52,10 +56,18 @@
 
 #include "Takeoff/Takeoff.hpp"
 
+#include <new>
+
+enum class FlightTaskError : int {
+	NoError = 0,
+	InvalidTask = -1,
+	ActivationFailed = -2
+};
+
 class FlightModeManager : public ModuleBase<FlightModeManager>, public ModuleParams, public px4::WorkItem
 {
 public:
-	FlightModeManager(bool vtol = false);
+	FlightModeManager();
 	~FlightModeManager() override;
 
 	/** @see ModuleBase */
@@ -82,31 +94,67 @@ private:
 	void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
 	void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
 
+	/**
+	 * Switch to a specific task (for normal usage)
+	 * @param task index to switch to
+	 * @return 0 on success, <0 on error
+	 */
+	FlightTaskError switchTask(FlightTaskIndex new_task_index);
+	FlightTaskError switchTask(int new_task_index);
+
+	/**
+	 * Call this method to get the description of a task error.
+	 */
+	const char *errorToString(const FlightTaskError error);
+
+	/**
+	 * Check if any task is active
+	 * @return true if a task is active, false if not
+	 */
+	bool isAnyTaskActive() const { return _current_task.task; }
+
+	// generated
+	int _initTask(FlightTaskIndex task_index);
+	FlightTaskIndex switchVehicleCommand(const int command);
+
 	static constexpr int NUM_FAILURE_TRIES = 10; ///< number of tries before switching to a failsafe flight task
 
-	FlightTasks _flight_tasks; ///< class generating position control setpoints depending on vehicle task
+	/**
+	 * Union with all existing tasks: we use it to make sure that only the memory of the largest existing
+	 * task is needed, and to avoid using dynamic memory allocations.
+	 */
+	TaskUnion _task_union; /**< storage for the currently active task */
+
+	struct flight_task_t {
+		FlightTask *task{nullptr};
+		FlightTaskIndex index{FlightTaskIndex::None};
+	} _current_task{};
+
 	Takeoff _takeoff; ///< state machine and ramp to bring the vehicle off the ground without a jump
 	WeatherVane *_wv_controller{nullptr};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
 	int _task_failure_count{0};
 	uint8_t _last_vehicle_nav_state{0};
 
-	perf_counter_t _loop_perf; ///< loop duration performance counter
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter
 	hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionData<vehicle_local_position_setpoint_s> _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
 
 	DEFINE_PARAMETERS(

src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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 FlightTasks_generated.cpp
+ *
+ * Generated file to switch between all required flight tasks
+ *
+ * @@author Christoph Tobler <christoph@@px4.io>
+ */
+
+#include "FlightModeManager.hpp"
+#include "FlightTasks_generated.hpp"
+
+int FlightModeManager::_initTask(FlightTaskIndex task_index)
+{
+
+	// disable the old task if there is any
+	if (_current_task.task) {
+		_current_task.task->~FlightTask();
+		_current_task.task = nullptr;
+		_current_task.index = FlightTaskIndex::None;
+	}
+
+	switch (task_index) {
+	case FlightTaskIndex::None:
+		// already disabled task
+		break;
+
+@# loop through all requested tasks
+@[if tasks]@
+@[for task in tasks]@
+@{
+firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
+}@
+	case FlightTaskIndex::@(task):
+		_current_task.task = new (&_task_union.@(firstLowercase(task))) FlightTask@(task)();
+		break;
+
+@[end for]@
+@[end if]@
+	default:
+		// invalid task
+		return 1;
+	}
+
+	// task construction succeeded
+	_current_task.index = task_index;
+	return 0;
+}
+
+FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command)
+{
+    switch (command) {
+@# loop through all additional tasks
+@[if tasks_add]@
+@[for task in tasks_add]@
+@{
+upperCase = lambda s: s[:].upper() if s else ''
+}@
+	case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) :
+		return FlightTaskIndex::@(task);
+		break;
+
+@[end for]@
+@[end if]@
+	// ignore all unkown commands
+	default : return FlightTaskIndex::None;
+	}
+}

src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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 FlightTasks_generated.hpp
+ *
+ * Generated Header to list all required flight tasks
+ *
+ * @@author Christoph Tobler <christoph@@px4.io>
+ */
+
+ #pragma once
+
+// include all required headers
+#include "FlightModeManager.hpp"
+@# loop through all requested tasks
+@[if tasks]@
+@[for task in tasks]@
+#include "FlightTask@(task).hpp"
+@[end for]@
+@[end if]@
+
+enum class FlightTaskIndex : int {
+    None = -1,
+@# loop through all requested tasks
+@[if tasks]@
+@[for task in tasks]@
+    @(task),
+@[end for]@
+@[end if]@
+
+    Count // number of tasks
+};
+
+union TaskUnion {
+    TaskUnion() {}
+    ~TaskUnion() {}
+
+@# loop through all requested tasks
+@[if tasks]@
+@{
+firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
+}@
+@[for task in tasks]@
+    FlightTask@(task) @(firstLowercase(task));
+@[end for]@
+@[end if]@
+};

src/modules/flight_mode_manager/generate_flight_tasks.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+
+import em
+import os
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated")
+parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)")
+parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory")
+parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory")
+parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate")
+
+# Parse arguments
+args = parser.parse_args()
+
+for gen_file in args.gen_files:
+    ofile = args.directory_out + "/" + gen_file
+    output_file = open(ofile, 'w')
+    # need to specify the em_globals inside the loop -> em.Error: interpreter globals collision
+    em_globals = {
+        "tasks": args.tasks_all,
+        "tasks_add": args.tasks_add,
+    }
+    interpreter = em.Interpreter(output=output_file, globals=em_globals)
+    interpreter.file(open(args.directory_in + "/" + gen_file + ".em"))
+    interpreter.shutdown()

src/modules/flight_mode_manager/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 avoidance FlightTask FlightTaskUtility)
+target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   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>
+
+// TODO: make this switchable in the board config, like a module
+#if CONSTRAINED_FLASH
+#include <lib/avoidance/ObstacleAvoidance_dummy.hpp>
+#else
+#include <lib/avoidance/ObstacleAvoidance.hpp>
+#endif
+
+/**
+ * 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(const 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. */
+	bool _prev_was_valid{false};
+	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. */
+	bool _next_was_valid{false};
+	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<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::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/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp

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

src/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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 FlightTaskAutoMapper FlightTaskUtility)
+target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+
+px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp)

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -0,0 +1,490 @@
+/****************************************************************************
+ *
+ *   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 "TrajectoryConstraints.hpp"
+
+using namespace matrix;
+
+bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskAutoMapper::activate(last_setpoint);
+
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f accel_prev{last_setpoint.acceleration};
+
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current postion
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// No acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+	}
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
+	}
+
+	_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
+	_updateTrajConstraints();
+
+	return ret;
+}
+
+void FlightTaskAutoLineSmoothVel::reActivate()
+{
+	FlightTaskAutoMapper::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));
+}
+
+/**
+ * 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()
+{
+	_updateTurningCheck();
+	_prepareSetpoints();
+	_generateTrajectory();
+
+	if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
+		// no valid heading -> generate heading in this flight task
+		_generateHeading();
+	}
+}
+
+void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
+{
+	const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
+				_trajectory[1].getCurrentVelocity());
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f target_xy(_target);
+	const Vector2f u_vel_traj = vel_traj.unit_or_zero();
+	const Vector2f pos_to_target = Vector2f(target_xy - pos_traj);
+	const float cos_align = u_vel_traj * pos_to_target.unit_or_zero();
+
+	// The vehicle is turning if the angle between the velocity vector
+	// and the direction to the target is greater than 10 degrees, the
+	// velocity is large enough and the drone isn't in the acceptance
+	// radius of the last WP.
+	_is_turning = vel_traj.longerThan(0.2f)
+		      && cos_align < 0.98f
+		      && pos_to_target.longerThan(_target_acceptance_radius);
+}
+
+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() > 2.f)) {
+		// 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 sign(val) * math::min(fabsf(val), fabsf(max));
+}
+
+float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
+{
+	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+			  _trajectory[1].getCurrentPosition(),
+			  _trajectory[2].getCurrentPosition());
+
+	math::trajectory::VehicleDynamicLimits config;
+	config.z_accept_rad = _param_nav_mc_alt_rad.get();
+	config.xy_accept_rad = _target_acceptance_radius;
+	config.max_acc_xy = _trajectory[0].getMaxAccel();
+	config.max_jerk = _trajectory[0].getMaxJerk();
+	config.max_speed_xy = _mc_cruise_speed;
+	config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get();
+
+	// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
+	// (eg. Obstacle Avoidance)
+
+	Vector3f waypoints[3] = {pos_traj, _target, _next_wp};
+
+	if (isTargetModified()) {
+		waypoints[2] = waypoints[1] = _position_setpoint;
+	}
+
+	float max_xy_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	return max_xy_speed;
+}
+
+float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
+{
+	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+			  _trajectory[1].getCurrentPosition(),
+			  _trajectory[2].getCurrentPosition());
+	float z_setpoint = _target(2);
+
+	// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
+	// (eg. Obstacle Avoidance)
+	bool z_valid = PX4_ISFINITE(_position_setpoint(2));
+	bool z_modified =  z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON;
+
+	if (z_modified) {
+		z_setpoint = _position_setpoint(2);
+	}
+
+	const float distance_start_target = fabs(z_setpoint - pos_traj(2));
+	const float arrival_z_speed = 0.f;
+
+	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
+					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
+					    distance_start_target, arrival_z_speed));
+
+	return max_speed;
+}
+
+Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const
+{
+	Vector3f pos_crossing_point{};
+
+	if (isTargetModified()) {
+		// Strictly follow the modified setpoint
+		pos_crossing_point = _position_setpoint;
+
+	} else {
+		if (_is_turning) {
+			// Get the crossing point using L1-style guidance
+			pos_crossing_point.xy() = getL1Point();
+			pos_crossing_point(2) = _target(2);
+
+		} else {
+			pos_crossing_point = _target;
+		}
+	}
+
+	return pos_crossing_point;
+}
+
+bool FlightTaskAutoLineSmoothVel::isTargetModified() const
+{
+	const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
+	const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
+	const bool z_modified =  z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
+
+	return xy_modified || z_modified;
+}
+
+Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const
+{
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f target_xy(_target);
+	const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero();
+	const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
+	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+	const Vector2f closest_pt = Vector2f(_prev_wp) + prev_to_closest;
+
+	// Compute along-track error using L1 distance and cross-track error
+	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
+
+	const float l1 = math::max(_target_acceptance_radius, 5.f);
+	float alongtrack_error = 0.f;
+
+	// Protect against sqrt of a negative number
+	if (l1 > crosstrack_error) {
+		alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
+	}
+
+	// Position of the point on the line where L1 intersect the line between the two waypoints
+	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
+
+	return l1_point;
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
+{
+	// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
+	// If a velocity is specified, that is used as a feedforward to track the position setpoint
+	// (ie. it assumes the position setpoint is moving at the specified velocity)
+	// 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);
+		return;
+	}
+
+	const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
+	const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));
+
+	if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
+		// Use 3D position setpoint to generate a 3D velocity setpoint
+		Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+				  _trajectory[1].getCurrentPosition(),
+				  _trajectory[2].getCurrentPosition());
+
+		const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero());
+
+		float xy_speed = _getMaxXYSpeed();
+		const float z_speed = _getMaxZSpeed();
+
+		if (_is_turning) {
+			// Lock speed during turn
+			xy_speed = math::min(_max_speed_prev, xy_speed);
+
+		} else {
+			_max_speed_prev = xy_speed;
+		}
+
+		Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
+		math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
+		math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
+
+		for (int i = 0; i < 3; i++) {
+			// If available, use the existing velocity as a feedforward, otherwise replace it
+			if (PX4_ISFINITE(_velocity_setpoint(i))) {
+				_velocity_setpoint(i) += vel_sp_constrained(i);
+
+			} else {
+				_velocity_setpoint(i) = vel_sp_constrained(i);
+			}
+		}
+	}
+
+	else if (xy_pos_setpoint_valid) {
+		// Use 2D position setpoint to generate a 2D velocity setpoint
+
+		// Get various path specific vectors
+		Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
+		Vector2f pos_traj_to_dest_xy = Vector2f(getCrossingPoint()) - pos_traj;
+		Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+
+		float xy_speed = _getMaxXYSpeed();
+
+		if (_is_turning) {
+			// Lock speed during turn
+			xy_speed = math::min(_max_speed_prev, xy_speed);
+
+		} else {
+			_max_speed_prev = xy_speed;
+		}
+
+		Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed;
+
+		for (int i = 0; i < 2; i++) {
+			// If available, use the existing velocity as a feedforward, otherwise replace it
+			if (PX4_ISFINITE(_velocity_setpoint(i))) {
+				_velocity_setpoint(i) += vel_sp_constrained_xy(i);
+
+			} else {
+				_velocity_setpoint(i) = vel_sp_constrained_xy(i);
+			}
+		}
+	}
+
+	else if (z_pos_setpoint_valid) {
+		// Use Z position setpoint to generate a Z velocity setpoint
+
+		const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
+		const float vel_sp_z = z_dir * _getMaxZSpeed();
+
+		// If available, use the existing velocity as a feedforward, otherwise replace it
+		if (PX4_ISFINITE(_velocity_setpoint(2))) {
+			_velocity_setpoint(2) += vel_sp_z;
+
+		} 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
+		float z_accel_constraint = _param_mpc_acc_up_max.get();
+		float z_vel_constraint = _param_mpc_z_vel_max_up.get();
+
+		// The constraints are broken because they are used as hard limits by the position controller, so put this here
+		// until the constraints don't do things like cause controller integrators to saturate. Once the controller
+		// doesn't use z speed constraints, this can go in AutoMapper::_prepareTakeoffSetpoints(). Accel limit is to
+		// emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint.
+		if (_type == WaypointType::takeoff &&  _dist_to_ground < _param_mpc_land_alt1.get()) {
+			z_vel_constraint = _param_mpc_tko_speed.get();
+			z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get());
+
+			// Keep the altitude setpoint at the current altitude
+			// to avoid having it going down into the ground during
+			// the initial ramp as the velocity does not start at 0
+			_trajectory[2].setCurrentPosition(_position(2));
+		}
+
+		_trajectory[2].setMaxVel(z_vel_constraint);
+		_trajectory[2].setMaxAccel(z_accel_constraint);
+
+	} 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, 3);
+
+	_jerk_setpoint = jerk_sp_smooth;
+	_acceleration_setpoint = accel_sp_smooth;
+	_velocity_setpoint = vel_sp_smooth;
+	_position_setpoint = pos_sp_smooth;
+}

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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 "FlightTaskAutoMapper.hpp"
+#include <motion_planning/VelocitySmoothing.hpp>
+
+class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
+{
+public:
+	FlightTaskAutoLineSmoothVel() = default;
+	virtual ~FlightTaskAutoLineSmoothVel() = default;
+
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	void reActivate() override;
+
+protected:
+
+	/** 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();
+	void _updateTurningCheck();
+	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 */
+
+	float _getMaxXYSpeed() const;
+	float _getMaxZSpeed() const;
+
+	matrix::Vector3f getCrossingPoint() const;
+	bool isTargetModified() const;
+	matrix::Vector2f getL1Point() const;
+
+	float _max_speed_prev{};
+	bool _is_turning{false};
+
+	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(FlightTaskAutoMapper,
+					(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
+				       );
+};

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp

@@ -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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_defines.h>
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+namespace math
+{
+namespace trajectory
+{
+using matrix::Vector3f;
+using matrix::Vector2f;
+
+struct VehicleDynamicLimits {
+	float z_accept_rad;
+	float xy_accept_rad;
+
+	float max_acc_xy;
+	float max_jerk;
+
+	float max_speed_xy;
+
+	// TODO: remove this
+	float max_acc_xy_radius_scale;
+};
+
+/*
+ * Compute the maximum allowed speed at the waypoint assuming that we want to
+ * connect the two lines (current-target and target-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.
+ *
+ */
+inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target,
+		const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config)
+{
+	const float distance_target_next = (target - next_target).xy().norm();
+
+	const bool target_next_different = distance_target_next  > 0.001f;
+	const bool waypoint_overlap = distance_target_next < config.xy_accept_rad;
+	const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad;
+	const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad;
+
+	float speed_at_target = 0.0f;
+
+	if (target_next_different &&
+	    !waypoint_overlap &&
+	    has_reached_altitude &&
+	    altitude_stays_same
+	   ) {
+		const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
+						  Vector2f((target - next_target).xy()).unit_or_zero()));
+		const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
+		float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy;
+		float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad);
+		speed_at_target = min(min(max_speed_in_turn, exit_speed), config.max_speed_xy);
+	}
+
+	float start_to_target = (start_position - target).xy().norm();
+	float max_speed = computeMaxSpeedFromDistance(config.max_jerk, config.max_acc_xy, start_to_target, speed_at_target);
+
+	return min(config.max_speed_xy, max_speed);
+}
+
+/*
+ * This function computes the maximum speed XY that can be travelled, given a set of waypoints and vehicle dynamics
+ *
+ * The first waypoint should be the starting location, and the later waypoints the desired points to be followed.
+ *
+ * @param waypoints the list of waypoints to be followed, the first of which should be the starting location
+ * @param config the vehicle dynamic limits
+ *
+ * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
+ */
+template <size_t N>
+float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
+{
+	static_assert(N >= 2, "Need at least 2 points to compute speed");
+
+	float max_speed = 0.f;
+
+	for (size_t j = 0; j < N - 1; j++) {
+		size_t i = N - 2 - j;
+		max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
+				waypoints[i + 1],
+				waypoints[min(i + 2, N - 1)],
+				max_speed, config);
+	}
+
+	return max_speed;
+}
+
+/*
+ * Constrain the 3D vector given a maximum XY norm
+ * If the XY norm of the 3D vector is larger than the maximum norm, the whole vector
+ * is scaled down to respect the constraint.
+ * If the maximum norm is small (defined by the "accuracy" parameter),
+ * only the XY components are scaled down to avoid affecting
+ * Z in case of numerical issues
+ */
+inline void clampToXYNorm(Vector3f &target, float max_xy_norm, float accuracy = FLT_EPSILON)
+{
+	const float xynorm = target.xy().norm();
+	const float scale_factor = (xynorm > FLT_EPSILON)
+				   ? max_xy_norm / xynorm
+				   : 1.f;
+
+	if (scale_factor < 1.f) {
+		if (max_xy_norm < accuracy && xynorm < accuracy) {
+			target.xy() = Vector2f(target) * scale_factor;
+
+		} else {
+			target *= scale_factor;
+		}
+	}
+}
+
+/*
+ * Constrain the 3D vector given a maximum Z norm
+ * If the Z component of the 3D vector is larger than the maximum norm, the whole vector
+ * is scaled down to respect the constraint.
+ * If the maximum norm is small (defined by the "accuracy parameter),
+ * only the Z component is scaled down to avoid affecting
+ * XY in case of numerical issues
+ */
+inline void clampToZNorm(Vector3f &target, float max_z_norm, float accuracy = FLT_EPSILON)
+{
+	const float znorm = fabs(target(2));
+	const float scale_factor = (znorm > FLT_EPSILON)
+				   ? max_z_norm / znorm
+				   : 1.f;
+
+	if (scale_factor < 1.f) {
+		if (max_z_norm < accuracy && znorm < accuracy) {
+			target(2) *= scale_factor;
+
+		} else {
+			target *= scale_factor;
+		}
+	}
+}
+
+}
+}

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp

@@ -0,0 +1,364 @@
+#include <gtest/gtest.h>
+
+#include "TrajectoryConstraints.hpp"
+
+using namespace matrix;
+using namespace math::trajectory;
+
+class TrajectoryConstraintsTest : public ::testing::Test
+{
+public:
+	VehicleDynamicLimits config;
+
+	Vector3f vehicle_location;
+	Vector3f target;
+	Vector3f next_target;
+
+	float final_speed = 0;
+
+	void SetUp() override
+	{
+		config.z_accept_rad = 1.f;
+		config.xy_accept_rad = 0.99f;
+
+		config.max_acc_xy = 3.f;
+		config.max_jerk = 10.f;
+
+		config.max_speed_xy = 10.f;
+
+		config.max_acc_xy_radius_scale = 0.8f;
+
+		/*
+		 *             (20,20)
+		 *              Next target
+		 *
+		 *              ^
+		 *              |
+		 *
+		 * (10,10)      (20,10)
+		 * Vehicle  ->  Target
+		 *
+		 */
+		vehicle_location = Vector3f(10, 10, 5);
+		target = Vector3f(20, 10, 5);
+		next_target = Vector3f(20, 20, 5);
+	}
+};
+
+TEST_F(TrajectoryConstraintsTest, testStraight)
+{
+	// GIVEN: 3 waypoints in straight line
+	next_target = target + 2.f * (target - vehicle_location);
+	target = vehicle_location + 0.5f * (next_target - vehicle_location);
+
+	// WHEN: we get the speed for straight line travel
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be the same as speed directly to the end point
+	Vector3f direct_points[2] = {vehicle_location, next_target};
+	float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
+
+	EXPECT_FLOAT_EQ(through_speed, direct_speed);
+}
+
+TEST_F(TrajectoryConstraintsTest, testStraightNaN)
+{
+	// GIVEN: 3 waypoints in straight line
+	next_target = target + 2.f * (target - vehicle_location);
+	target = vehicle_location + 0.5f * (next_target - vehicle_location);
+	next_target(0) = NAN;
+	next_target(1) = NAN;
+
+	// WHEN: we get the speed for points which are NaN afterwards
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be the same as speed to the closer point
+	Vector3f direct_points[2] = {vehicle_location, target};
+	float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
+
+	EXPECT_FLOAT_EQ(through_speed, direct_speed);
+}
+
+TEST_F(TrajectoryConstraintsTest, testStraightLowJerkClose)
+{
+	// GIVEN: 3 waypoints in straight line
+	next_target = target + 2.f * (target - vehicle_location);
+	target = vehicle_location + 0.05f * (next_target - vehicle_location);
+	config.max_jerk = 8.f;
+
+	// WHEN: we get the speed for straight line travel
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be the same as speed directly to the end point
+	Vector3f direct_points[2] = {vehicle_location, next_target};
+	float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
+
+	EXPECT_FLOAT_EQ(through_speed, direct_speed);
+}
+
+TEST_F(TrajectoryConstraintsTest, testStraightMidClose)
+{
+	// GIVEN: 3 waypoints in straight line
+	next_target = target + 2.f * (target - vehicle_location);
+	target = vehicle_location + 0.05f * (next_target - vehicle_location);
+
+	// WHEN: we get the speed for straight line travel
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be the same as speed directly to the end point
+	Vector3f direct_points[2] = {vehicle_location, next_target};
+	float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
+
+	EXPECT_FLOAT_EQ(through_speed, direct_speed);
+}
+
+TEST_F(TrajectoryConstraintsTest, testStraightMidFar)
+{
+	// GIVEN: 3 waypoints in straight line
+	next_target = target + 2.f * (target - vehicle_location);
+	target = vehicle_location + 0.95f * (next_target - vehicle_location);
+
+	// WHEN: we get the speed for straight line travel
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be the same as speed directly to the end point
+	Vector3f direct_points[2] = {vehicle_location, next_target};
+	float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
+
+	EXPECT_FLOAT_EQ(through_speed, direct_speed);
+}
+
+
+TEST_F(TrajectoryConstraintsTest, test90Angle)
+{
+	// GIVEN: 3 waypoints in 90 degree angle
+	EXPECT_FLOAT_EQ(0.f, (vehicle_location - target).dot(target - next_target));
+
+	// WHEN: we get the speed for travel around the path
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be slightly faster than stopping at the intermediate point
+	Vector3f stop_points[2] = {vehicle_location, target};
+	float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
+
+	EXPECT_GT(through_speed, stop_speed); //faster
+	EXPECT_LT(through_speed, stop_speed * 1.03f); // but less than 3% faster
+}
+
+TEST_F(TrajectoryConstraintsTest, test45Angle)
+{
+	// GIVEN: 3 waypoints in 45 degree angle
+	next_target = Vector3f(25, 15, 5);
+
+	// WHEN: we get the speed for travel around the path
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be slightly faster than stopping at the intermediate point
+	Vector3f stop_points[2] = {vehicle_location, target};
+	float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
+
+	EXPECT_GT(through_speed, stop_speed * 1.03f); // more than 3% faster
+	EXPECT_LT(through_speed, stop_speed * 1.06f); // but less than 6% faster
+}
+
+TEST_F(TrajectoryConstraintsTest, test10Angle)
+{
+	// GIVEN: 3 waypoints in 10 degree angle
+	next_target = Vector3f(30, 11.7, 5);
+
+	// WHEN: we get the speed for travel around the path
+	Vector3f waypoints[3] = {vehicle_location, target, next_target};
+	float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	// THEN: it should be slightly faster than stopping at the intermediate point
+	Vector3f stop_points[2] = {vehicle_location, target};
+	float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
+
+	EXPECT_GT(through_speed, stop_speed * 1.25f); // more than 25% faster
+	EXPECT_LT(through_speed, stop_speed * 1.3f); // but less than 30% faster
+}
+
+TEST_F(TrajectoryConstraintsTest, test10AngleFarNext)
+{
+	// GIVEN: 3 waypoints in 10 degree angle, but next waypoint is far
+	next_target = 2.f * (Vector3f(30, 11.7, 5) - target) + target;
+
+	// WHEN: we get the speed for travel around the path
+	Vector3f far_waypoints[3] = {vehicle_location, target, next_target};
+	float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config);
+
+	// THEN: it should be the same speed as a closer next waypoint at the same angle, since the bottleneck is the turn
+	next_target = Vector3f(30, 11.7, 5);
+	Vector3f close_waypoints[3] = {vehicle_location, target, next_target};
+	float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config);
+
+	EXPECT_FLOAT_EQ(far_speed, close_speed);
+}
+
+TEST_F(TrajectoryConstraintsTest, test10AngleCloseNext)
+{
+	// GIVEN: 3 waypoints in right angle, but next waypoint is far
+	next_target = .2f * (Vector3f(30, 11.7, 5) - target) + target;
+
+	// WHEN: we get the speed for travel around the path
+	Vector3f close_waypoints[3] = {vehicle_location, target, next_target};
+	float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config);
+
+	// THEN: it should be slower than a further next waypoint at the same angle, since the bottleneck is the distance
+	next_target = Vector3f(30, 11.7, 5);
+	Vector3f normal_waypoints[3] = {vehicle_location, target, next_target};
+	float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config);
+
+	EXPECT_LT(close_speed, normal_speed);
+}
+
+TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectLarge)
+{
+	// GIVEN: a short vector
+	Vector3f vec(1, 2, 3);
+
+	// WHEN: we clamp it on XY with a long cutoff
+	clampToXYNorm(vec, 1000.f);
+
+	// THEN: it shouldn't change
+	EXPECT_EQ(vec, Vector3f(1, 2, 3));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToZNormNoEffect)
+{
+	// GIVEN: a short vector
+	Vector3f vec(1, 2, 3);
+
+	// WHEN: we clamp it on XY with a long cutoff
+	clampToZNorm(vec, 1000.f);
+
+	// THEN: it shouldn't change
+	EXPECT_EQ(vec, Vector3f(1, 2, 3));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectExact)
+{
+	// GIVEN: a vector
+	Vector3f vec(3, 4, 1);
+
+	// WHEN: we clamp it on XY with exact cutoff
+	clampToXYNorm(vec, 5.f);
+
+	// THEN: it shouldn't change
+	EXPECT_EQ(vec, Vector3f(3, 4, 1));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToZNormNoEffectExact)
+{
+	// GIVEN: a vector
+	Vector3f vec(3, 4, -1);
+
+	// WHEN: we clamp it on Z with exact cutoff
+	clampToZNorm(vec, 1.f);
+
+	// THEN: it shouldn't change
+	EXPECT_EQ(vec, Vector3f(3, 4, -1));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToXYNormHalf)
+{
+	// GIVEN: a vector
+	Vector3f vec(3, 4, 1);
+
+	// WHEN: we clamp it on XY with half hypot length
+	clampToXYNorm(vec, 2.5f);
+
+	// THEN: it should be half length
+	EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 0.5f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToZNormHalf)
+{
+	// GIVEN: a vector
+	Vector3f vec(3, 4, 10);
+
+	// WHEN: we clamp it on Z with half length
+	clampToZNorm(vec, 5.f);
+
+	// THEN: it should be half length
+	EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 5.f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToXYNormZero)
+{
+	// GIVEN: a vector
+	Vector3f vec(3, 4, 1);
+
+	// WHEN: we clamp it on XY with half hypot length
+	clampToXYNorm(vec, 0.f);
+
+	// THEN: it should be 0
+	EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToZNormZero)
+{
+	// GIVEN: a vector
+	Vector3f vec(3, 4, 1);
+
+	// WHEN: we clamp it on Z with half hypot length
+	clampToZNorm(vec, 0.f);
+
+	// THEN: it should be 0
+	EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToXYNormVecZero)
+{
+	// GIVEN: a vector
+	Vector3f vec(0, 0, 0);
+
+	// WHEN: we clamp it on XY
+	clampToXYNorm(vec, 1.f);
+
+	// THEN: it should be 0 still
+	EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToZNormVecZero)
+{
+	// GIVEN: a vector
+	Vector3f vec(0, 0, 0);
+
+	// WHEN: we clamp it on Z
+	clampToZNorm(vec, 1.f);
+
+	// THEN: it should be 0 still
+	EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToXYNormVecZeroToZero)
+{
+	// GIVEN: a vector
+	Vector3f vec(0, 0, 0);
+
+	// WHEN: we clamp it on XY
+	clampToXYNorm(vec, 0.f);
+
+	// THEN: it should be 0 still
+	EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
+}
+
+TEST(TrajectoryConstraintsClamp, clampToZNormVecZeroToZero)
+{
+	// GIVEN: a vector
+	Vector3f vec(0, 0, 0);
+
+	// WHEN: we clamp it on XY
+	clampToZNorm(vec, 0.f);
+
+	// THEN: it should be 0 still
+	EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
+}

src/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   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;
+
+FlightTaskAutoMapper::FlightTaskAutoMapper() :
+	_sticks(this)
+{}
+
+bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskAuto::activate(last_setpoint);
+	_reset();
+	return ret;
+}
+
+bool FlightTaskAutoMapper::update()
+{
+	bool ret = FlightTaskAuto::update();
+	// always reset constraints because they might change depending on the type
+	_setDefaultConstraints();
+
+	// 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) {
+		_acceleration_setpoint.setNaN();
+	}
+
+	// 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 ret;
+}
+
+void FlightTaskAutoMapper::_reset()
+{
+	// Set setpoints equal current state.
+	_velocity_setpoint = _velocity;
+	_position_setpoint = _position;
+}
+
+void FlightTaskAutoMapper::_prepareIdleSetpoints()
+{
+	// Send zero thrust setpoint
+	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
+	_velocity_setpoint.setNaN();
+	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
+}
+
+void FlightTaskAutoMapper::_prepareLandSetpoints()
+{
+	// Keep xy-position and go down with landspeed
+	float land_speed = _getLandSpeed();
+	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
+	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
+{
+	// Takeoff is completely defined by target position
+	_position_setpoint = _target;
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
+
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoMapper::_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 FlightTaskAutoMapper::_preparePositionSetpoints()
+{
+	// Simple waypoint navigation: go to xyz target, with standard limitations
+	_position_setpoint = _target;
+	_velocity_setpoint.setNaN(); // No special velocity limitations
+}
+
+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 _dist_to_ground > 2.0f;
+}
+
+float FlightTaskAutoMapper::_getLandSpeed()
+{
+	float land_speed = math::gradual(_dist_to_ground,
+					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					 _param_mpc_land_speed.get(), _constraints.speed_down);
+
+	// user input assisted land speed
+	if (_param_mpc_land_rc_help.get()
+	    && (_dist_to_ground < _param_mpc_land_alt1.get())
+	    && _sticks.checkAndSetStickInputs()) {
+		// stick full up -1 -> stop, stick full down 1 -> double the speed
+		land_speed *= (1 + _sticks.getPositionExpo()(2));
+	}
+
+	return land_speed;
+}

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.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 FlightTaskAutoMapper.hpp
+ *
+ * Abstract Flight task which generates local setpoints
+ * based on the triplet type.
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+#include "Sticks.hpp"
+
+class FlightTaskAutoMapper : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoMapper();
+	virtual ~FlightTaskAutoMapper() = default;
+	bool activate(const 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 updateParams() override; /**< See ModuleParam class */
+
+	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,
+					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
+					_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
+				       );
+
+private:
+	Sticks _sticks;
+	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/modules/flight_mode_manager/tasks/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.
+#
+############################################################################
+
+# add the core Flight tasks that the additional flight tasks depend on
+add_subdirectory(FlightTask)
+add_subdirectory(Utility)
+add_subdirectory(Auto)
+add_subdirectory(AutoMapper)
+
+# add all additional flight tasks
+foreach(task ${flight_tasks_all})
+    add_subdirectory(${task})
+endforeach()

src/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	// stay level to minimize horizontal drift
+	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
+	// keep heading
+	_yaw_setpoint = _yaw;
+	return ret;
+}
+
+bool FlightTaskDescend::update()
+{
+	bool ret = FlightTask::update();
+
+	if (PX4_ISFINITE(_velocity(2))) {
+		// land with landspeed
+		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		_acceleration_setpoint(2) = NAN;
+
+	} else {
+		// descend with constant acceleration (crash landing)
+		_velocity_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = .3f;
+	}
+
+	return ret;
+}

src/modules/flight_mode_manager/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(const 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/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_position_setpoint = _position;
+	_velocity_setpoint.zero();
+	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, .3f);
+	_yaw_setpoint = _yaw;
+	_yawspeed_setpoint = 0.f;
+	return ret;
+}
+
+bool FlightTaskFailsafe::update()
+{
+	bool ret = FlightTask::update();
+
+	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
+		// stay at current position setpoint
+		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
+		_acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f;
+
+	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
+		// don't move along xy
+		_position_setpoint(0) = _position_setpoint(1) = NAN;
+		_acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
+	}
+
+	if (PX4_ISFINITE(_position(2))) {
+		// stay at current altitude setpoint
+		_velocity_setpoint(2) = 0.f;
+		_acceleration_setpoint(2) = NAN;
+
+	} else if (PX4_ISFINITE(_velocity(2))) {
+		// land with landspeed
+		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		_position_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = NAN;
+	}
+
+	return ret;
+}

src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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(const vehicle_local_position_setpoint_s &last_setpoint) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed
+				       )
+};

src/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -0,0 +1,207 @@
+#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 ekf_reset_counters_s FlightTask::zero_reset_counters = {};
+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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	_resetSetpoints();
+	_setDefaultConstraints();
+	_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_home_position.update();
+
+	_evaluateVehicleLocalPosition();
+	_evaluateDistanceToGround();
+	return true;
+}
+
+bool FlightTask::update()
+{
+	_checkEkfResetCounters();
+	return true;
+}
+
+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_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
+		_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
+		_reset_counters.heading = _sub_vehicle_local_position.get().heading_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);
+	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
+	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+
+	// deprecated, only kept for output logging
+	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
+
+	return vehicle_local_position_setpoint;
+}
+
+void FlightTask::_resetSetpoints()
+{
+	_position_setpoint.setNaN();
+	_velocity_setpoint.setNaN();
+	_acceleration_setpoint.setNaN();
+	_jerk_setpoint.setNaN();
+	_yaw_setpoint = _yawspeed_setpoint = NAN;
+}
+
+void FlightTask::_evaluateVehicleLocalPosition()
+{
+	_position.setAll(NAN);
+	_velocity.setAll(NAN);
+	_yaw = NAN;
+	_dist_to_bottom = NAN;
+
+	// 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) {
+
+		// yaw
+		_yaw = _sub_vehicle_local_position.get().heading;
+
+		// 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::_evaluateDistanceToGround()
+{
+	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
+	_dist_to_ground = -_position(2);
+
+	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);
+	}
+}
+
+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 = NAN;
+	_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/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ *   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 <uORB/topics/home_position.h>
+#include <lib/weather_vane/WeatherVane.hpp>
+
+struct ekf_reset_counters_s {
+	uint8_t xy;
+	uint8_t vxy;
+	uint8_t z;
+	uint8_t vz;
+	uint8_t heading;
+};
+
+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 last_setpoint last output of the previous task
+	 * @return true on success, false on error
+	 */
+	virtual bool activate(const 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();
+
+	/**
+	 * 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();
+
+	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
+	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
+
+	/**
+	 * 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;
+
+	/**.
+	 * All counters are set to 0.
+	 */
+	static const ekf_reset_counters_s zero_reset_counters;
+
+	/**
+	 * 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 &acc_sp)
+	{
+		_velocity_setpoint_feedback = vel_sp;
+		_acceleration_setpoint_feedback = acc_sp;
+	}
+
+protected:
+	uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
+
+	/** Reset all setpoints to NAN */
+	void _resetSetpoints();
+
+	/** Check and update local position */
+	void _evaluateVehicleLocalPosition();
+
+	void _evaluateDistanceToGround();
+
+	/** 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
+	 * TODO: add the delta values to all the handlers
+	 */
+	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{}; /**< passed time in seconds since the task was activated */
+	float _deltatime{}; /**< passed time in seconds since the task was last updated */
+	hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */
+	hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */
+	hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */
+
+	/* Current vehicle state */
+	matrix::Vector3f _position; /**< current vehicle position */
+	matrix::Vector3f _velocity; /**< current vehicle velocity */
+	float _yaw{}; /**< current vehicle yaw heading */
+	float _dist_to_bottom{}; /**< current height above ground level */
+	float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
+
+	/**
+	 * 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;
+	float _yaw_setpoint{};
+	float _yawspeed_setpoint{};
+	matrix::Vector3f _velocity_setpoint_feedback;
+	matrix::Vector3f _acceleration_setpoint_feedback;
+
+	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
+
+	/**
+	 * 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
+				       )
+};

src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt

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

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 FlightTaskManualAcceleration.cpp
+ */
+
+#include "FlightTaskManualAcceleration.hpp"
+
+using namespace matrix;
+
+FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
+	_stick_acceleration_xy(this)
+{};
+
+bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
+
+	if (PX4_ISFINITE(last_setpoint.vx)) {
+		_velocity_setpoint.xy() = Vector2f(last_setpoint.vx, last_setpoint.vy);
+
+	} else {
+		_velocity_setpoint.xy() = Vector2f(_velocity);
+	}
+
+	_stick_acceleration_xy.resetPosition();
+
+	if (PX4_ISFINITE(last_setpoint.acceleration[0])) {
+		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
+	}
+
+	return ret;
+}
+
+bool FlightTaskManualAcceleration::update()
+{
+	bool ret = FlightTaskManualAltitudeSmoothVel::update();
+
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
+				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
+			_deltatime);
+	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
+
+	_constraints.want_takeoff = _checkTakeoff();
+	return ret;
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY()
+{
+	if (PX4_ISFINITE(_position_setpoint(0))) {
+		_position_setpoint(0) = _position(0);
+		_position_setpoint(1) = _position(1);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY()
+{
+	if (PX4_ISFINITE(_velocity_setpoint(0))) {
+		_velocity_setpoint(0) = _velocity(0);
+		_velocity_setpoint(1) = _velocity(1);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerPositionZ()
+{
+	if (PX4_ISFINITE(_position_setpoint(2))) {
+		_position_setpoint(2) = _position(2);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerVelocityZ()
+{
+	if (PX4_ISFINITE(_velocity_setpoint(2))) {
+		_velocity_setpoint(2) = _velocity(2);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi)
+{
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		_yaw_setpoint += delta_psi;
+	}
+}

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "FlightTaskManualAltitudeSmoothVel.hpp"
+#include "StickAccelerationXY.hpp"
+#include "StickYaw.hpp"
+
+class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
+{
+public:
+	FlightTaskManualAcceleration();
+	virtual ~FlightTaskManualAcceleration() = default;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool update() override;
+
+private:
+	StickAccelerationXY _stick_acceleration_xy;
+	StickYaw _stick_yaw;
+
+	void _ekfResetHandlerPositionXY() override;
+	void _ekfResetHandlerVelocityXY() override;
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+	void _ekfResetHandlerHeading(float delta_psi) override;
+};

src/modules/flight_mode_manager/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 FlightTask FlightTaskUtility)
+target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -0,0 +1,385 @@
+/****************************************************************************
+ *
+ *   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>
+#include <ecl/geo/geo.h>
+
+using namespace matrix;
+
+FlightTaskManualAltitude::FlightTaskManualAltitude() :
+	_sticks(this)
+{}
+
+bool FlightTaskManualAltitude::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+
+	_sticks.checkAndSetStickInputs();
+
+	if (_sticks_data_required) {
+		ret = ret && _sticks.isAvailable();
+	}
+
+	// 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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_yaw_setpoint = NAN;
+	_yawspeed_setpoint = 0.f;
+	_acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
+	_position_setpoint(2) = _position(2);
+	_velocity_setpoint(2) = 0.f;
+	_setDefaultConstraints();
+
+	_updateConstraintsFromEstimator();
+
+	_max_speed_up = _constraints.speed_up;
+	_max_speed_down = _constraints.speed_down;
+
+	return ret;
+}
+
+void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
+{
+	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;
+	}
+}
+
+void FlightTaskManualAltitude::_scaleSticks()
+{
+	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
+	const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
+	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
+
+	// Use sticks input with deadzone and exponential curve for vertical velocity
+	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
+	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
+}
+
+float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
+{
+	const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
+	const float alpha = _deltatime / den;
+	_yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target;
+	return _yawspeed_filter_state;
+}
+
+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.getPositionExpo()(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.getPositionExpo().slice<2, 1>(0, 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),
+								-_max_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(_max_speed_down, 0.7f);
+
+		} else {
+			_constraints.speed_down = _max_speed_down;
+
+		}
+	}
+}
+
+void FlightTaskManualAltitude::_respectGroundSlowdown()
+{
+	// 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()
+{
+	if (_isYawInput()) {
+		_unlockYaw();
+
+	} else {
+		_lockYaw();
+	}
+}
+
+bool FlightTaskManualAltitude::_isYawInput()
+{
+	/*
+	 * A threshold larger than FLT_EPSILON is required because the
+	 * _yawspeed_setpoint comes from an IIR filter and takes too much
+	 * time to reach zero.
+	 */
+	return fabsf(_yawspeed_setpoint) > 0.001f;
+}
+
+void FlightTaskManualAltitude::_unlockYaw()
+{
+	// no fixed heading when rotating around yaw by stick
+	_yaw_setpoint = NAN;
+}
+
+void FlightTaskManualAltitude::_lockYaw()
+{
+	// 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.getPosition().slice<2, 1>(0, 0));
+
+	_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
+	_man_input_filter.update(sp);
+	sp = _man_input_filter.getState();
+	_rotateIntoHeadingFrame(sp);
+
+	if (sp.length() > 1.0f) {
+		sp.normalize();
+	}
+
+	_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
+
+	_updateAltitudeLock();
+	_respectGroundSlowdown();
+}
+
+bool FlightTaskManualAltitude::_checkTakeoff()
+{
+	// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])
+	return _sticks.getPosition()(2) < -0.3f;
+}
+
+bool FlightTaskManualAltitude::update()
+{
+	bool ret = FlightTask::update();
+	_updateConstraintsFromEstimator();
+	_scaleSticks();
+	_updateSetpoints();
+	_constraints.want_takeoff = _checkTakeoff();
+
+	return ret;
+}

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   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 "FlightTask.hpp"
+#include "Sticks.hpp"
+#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
+#include <uORB/Subscription.hpp>
+
+class FlightTaskManualAltitude : public FlightTask
+{
+public:
+	FlightTaskManualAltitude();
+	virtual ~FlightTaskManualAltitude() = default;
+	bool activate(const 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;
+	void _updateConstraintsFromEstimator();
+
+	/**
+	 * 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();
+
+	Sticks _sticks;
+	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(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_Y_TAU>) _param_mpc_man_y_tau,
+					(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 */
+					(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
+				       )
+private:
+	bool _isYawInput();
+	void _unlockYaw();
+	void _lockYaw();
+
+	/**
+	 * Filter between stick input and yaw setpoint
+	 *
+	 * @param yawspeed_target yaw setpoint desired by the stick
+	 * @return filtered value from independent filter state
+	 */
+	float _applyYawspeedFilter(float yawspeed_target);
+
+	/**
+	 * 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();
+
+	void setGearAccordingToSwitch();
+
+	float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
+	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
+	float _max_speed_up = 10.0f;
+	float _max_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;
+
+	AlphaFilter<matrix::Vector2f> _man_input_filter;
+};

src/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -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 FlightManualAltitude.cpp
+ */
+
+#include "FlightTaskManualAltitudeSmoothVel.hpp"
+
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
+
+	// Check if the previous FlightTask provided setpoints
+
+	// If the position setpoint is unknown, set to the current postion
+	float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;
+
+	_smoothing.reset(az_sp_last, vz_sp_last, z_sp_last);
+
+	return ret;
+}
+
+void FlightTaskManualAltitudeSmoothVel::reActivate()
+{
+	FlightTaskManualAltitude::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::_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/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   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(const 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_CUSTOM_PARENT(FlightTaskManualAltitude,
+					(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 _updateTrajConstraints();
+	void _setOutputState();
+
+	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
+};

src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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
+	FlightTaskManualPosition.hpp
+)
+
+target_link_libraries(FlightTaskManualPosition
+	PRIVATE
+		CollisionPrevention
+	PUBLIC
+		FlightTaskManualAltitude
+)
+
+target_include_directories(FlightTaskManualPosition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	// all requirements from altitude-mode still have to hold
+	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
+
+	// 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.getPositionExpo().slice<2, 1>(0, 0);
+
+	const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
+
+	if (mag > FLT_EPSILON) {
+		stick_xy = stick_xy.normalized() * mag;
+	}
+
+	const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max;
+
+	if (PX4_ISFINITE(max_speed_from_estimator)) {
+		// use the minimum of the estimator and user specified limit
+		_velocity_scale = fminf(_constraints.speed_xy, max_speed_from_estimator);
+		// Allow for a minimum of 0.3 m/s for repositioning
+		_velocity_scale = fmaxf(_velocity_scale, 0.3f);
+
+	} else {
+		_velocity_scale = _constraints.speed_xy;
+	}
+
+	_velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale);
+
+	// 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.xy() = vel_sp_xy;
+}
+
+float FlightTaskManualPosition::_computeVelXYGroundDist()
+{
+	float max_vel_xy = _constraints.speed_xy;
+
+	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	if (PX4_ISFINITE(_dist_to_ground)) {
+		max_vel_xy = math::gradual(_dist_to_ground,
+					   _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					   _param_mpc_land_vel_xy.get(), _constraints.speed_xy);
+	}
+
+	return max_vel_xy;
+}
+
+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
+	_acceleration_setpoint.setNaN(); // don't use the horizontal setpoints from FlightTaskAltitude
+
+	_updateXYlock(); // check for position lock
+
+	// 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;
+
+		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
+		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
+			// vehicle is steady
+			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+		}
+	}
+}

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   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(const 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_LAND_VEL_XY>) _param_mpc_land_vel_xy,
+					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy
+				       )
+private:
+	float _computeVelXYGroundDist();
+	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/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ *   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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskManualPosition::activate(last_setpoint);
+
+	// Check if the previous FlightTask provided setpoints
+	Vector3f accel_prev{last_setpoint.acceleration};
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current postion
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// No acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+	}
+
+	_smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev});
+	_smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2));
+
+	return ret;
+}
+
+void FlightTaskManualPositionSmoothVel::reActivate()
+{
+	FlightTaskManualPosition::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::_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/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   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(const 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 _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/modules/flight_mode_manager/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/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp

@@ -0,0 +1,243 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ */
+
+#include "FlightTaskOffboard.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskOffboard::updateInitialize()
+{
+	bool ret = FlightTask::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 FlightTaskOffboard::activate(const 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()
+{
+	bool ret = FlightTask::update();
+
+	// reset setpoint for every loop
+	_resetSetpoints();
+
+	_sub_triplet_setpoint.update();
+
+	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 ret;
+
+	} 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 ret;
+
+	} 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 ret;
+
+	} else {
+		_position_lock.setAll(NAN);
+	}
+
+	// IDLE
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
+		_velocity_setpoint.setNaN();
+		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
+		return ret;
+	}
+
+	// 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) {
+		_acceleration_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
+		_acceleration_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
+		_acceleration_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 ret;
+}

src/modules/flight_mode_manager/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(const 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/modules/flight_mode_manager/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 FlightTaskManualAltitudeSmoothVel SlewRate)
+target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -0,0 +1,275 @@
+/****************************************************************************
+ *
+ *   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));
+
+	// commanded heading behaviour
+	if (PX4_ISFINITE(command.param3)) {
+		_yaw_behaviour = command.param3;
+	}
+
+	// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING
+	_initial_heading = _yaw;
+
+	// 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;
+	_circle_approach_line.reset();
+
+	return ret;
+}
+
+bool FlightTaskOrbit::sendTelemetry()
+{
+	orbit_status_s orbit_status{};
+	orbit_status.radius = math::signNoZero(_v) * _r;
+	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
+	orbit_status.yaw_behaviour = _yaw_behaviour;
+
+	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.timestamp = hrt_absolute_time();
+	_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 = sign(_v) * sqrtf(_acceleration_max * r);
+	}
+
+	if (fabs(_r - r) > FLT_EPSILON) {
+		_circle_approach_line.reset();
+	}
+
+	_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(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
+	_r = _radius_min;
+	_v =  1.f;
+	_center = Vector2f(_position);
+	_center(0) -= _r;
+	_initial_heading = _yaw;
+	_slew_rate_yaw.setForcedValue(_yaw);
+	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
+
+	// 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
+	bool ret = FlightTaskManualAltitudeSmoothVel::update();
+
+	// stick input adjusts parameters within a fixed time frame
+	const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
+	const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
+
+	setRadius(r);
+	setVelocity(v);
+
+	const Vector2f center_to_position = Vector2f(_position) - _center;
+
+	if (_in_circle_approach) {
+		generate_circle_approach_setpoints(center_to_position);
+
+	} else {
+		generate_circle_setpoints(center_to_position);
+		generate_circle_yaw_setpoints(center_to_position);
+	}
+
+	// Apply yaw smoothing
+	_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
+
+	// publish information to UI
+	sendTelemetry();
+
+	return ret;
+}
+
+void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f &center_to_position)
+{
+	const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
+
+	if (_circle_approach_line.isEndReached()) {
+		// calculate target point on circle and plan a line trajectory
+		const Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
+		const 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());
+	}
+
+	_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
+
+	// 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();
+}
+
+void FlightTaskOrbit::generate_circle_setpoints(const 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();
+
+	_position_setpoint(0) = _position_setpoint(1) = NAN;
+	_velocity_setpoint.xy() = velocity_xy;
+	_acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r;
+}
+
+void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_position)
+{
+	switch (_yaw_behaviour) {
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
+		// make vehicle keep the same heading as when the orbit was commanded
+		_yaw_setpoint = _initial_heading;
+		_yawspeed_setpoint = NAN;
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
+		// no yaw setpoint
+		_yaw_setpoint = NAN;
+		_yawspeed_setpoint = NAN;
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
+		_yaw_setpoint = atan2f(sign(_v) * center_to_position(0), -sign(_v) * center_to_position(1));
+		_yawspeed_setpoint = _v / _r;
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
+		// inherit setpoint from altitude flight task
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
+	default:
+		_yaw_setpoint = atan2f(-center_to_position(1), -center_to_position(0));
+		// yawspeed feed-forward because we know the necessary angular rate
+		_yawspeed_setpoint = _v / _r;
+		break;
+	}
+}