flight_mode_manager: merge with flight_tasks

.gitignore

@@ -101,8 +101,6 @@ msg/tmp/
 msg/topics_sources/
 platforms/posix/apps.cpp
 platforms/posix/apps.h
-src/lib/flight_tasks/FlightTasks_generated.cpp
-src/lib/flight_tasks/FlightTasks_generated.hpp
 src/lib/parameters/px4_parameters.c
 src/lib/parameters/px4_parameters.h
 src/lib/parameters/px4_parameters_public.h

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -21,7 +21,7 @@ vtol_att_control start
 
 mc_rate_control start vtol
 mc_att_control start vtol
-flight_mode_manager start vtol
+flight_mode_manager start
 mc_pos_control start vtol
 mc_hover_thrust_estimator start
 

posix-configs/SITL/init/test/test_shutdown

@@ -28,7 +28,7 @@ navigator start
 ekf2 start
 vtol_att_control start
 mc_hover_thrust_estimator start
-flight_mode_manager start vtol
+flight_mode_manager start
 mc_pos_control start vtol
 mc_att_control start vtol
 mc_rate_control start vtol

src/lib/CMakeLists.txt

@@ -46,7 +46,6 @@ add_subdirectory(controllib)
 add_subdirectory(conversion)
 add_subdirectory(drivers)
 add_subdirectory(ecl)
-add_subdirectory(flight_tasks)
 add_subdirectory(hysteresis)
 add_subdirectory(l1)
 add_subdirectory(landing_slope)

src/lib/flight_tasks/CMakeLists.txt

@@ -1,125 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2017 - 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.
-#
-############################################################################
-
-###########################################
-# 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
-)
-
-###########################################
-# Create Flight Tasks Library
-###########################################
-
-add_compile_options(
-	-Wno-cast-align
-	) # TODO: fix and enable
-
-px4_add_library(FlightTasks
-	FlightTasks.cpp
-	FlightTasks_generated.cpp
-)
-
-# add directories to target
-target_include_directories(FlightTasks PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
-
-# add all flight task dependencies
-foreach(task ${flight_tasks_all})
-	target_link_libraries(FlightTasks PUBLIC FlightTask${task})
-endforeach()
-
-# add subdirectory containing all tasks
-add_subdirectory(tasks)

src/lib/flight_tasks/FlightTasks.cpp

@@ -1,191 +0,0 @@
-#include "FlightTasks.hpp"
-
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_command_ack.h>
-
-FlightTasks::FlightTasks()
-{
-	// 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));
-	}
-
-	// disable all tasks
-	_initTask(FlightTaskIndex::None);
-}
-
-bool FlightTasks::update()
-{
-	_updateCommand();
-
-	if (isAnyTaskActive()) {
-		return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize();
-	}
-
-	return false;
-}
-
-const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
-{
-	if (isAnyTaskActive()) {
-		return _current_task.task->getPositionSetpoint();
-
-	} else {
-		return FlightTask::empty_setpoint;
-	}
-}
-
-const ekf_reset_counters_s FlightTasks::getResetCounters()
-{
-	if (isAnyTaskActive()) {
-		return _current_task.task->getResetCounters();
-
-	} else {
-		return FlightTask::zero_reset_counters;
-	}
-}
-
-const vehicle_constraints_s FlightTasks::getConstraints()
-{
-	if (isAnyTaskActive()) {
-		return _current_task.task->getConstraints();
-
-	} else {
-		return FlightTask::empty_constraints;
-	}
-}
-
-const landing_gear_s FlightTasks::getGear()
-{
-	if (isAnyTaskActive()) {
-		return _current_task.task->getGear();
-
-	} else {
-		return FlightTask::empty_landing_gear_default_keep;
-	}
-}
-
-FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
-{
-	// switch to the running task, nothing to do
-	if (new_task_index == _current_task.index) {
-		return FlightTaskError::NoError;
-	}
-
-	// Save current setpoints for the next FlightTask
-	const vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint();
-	const ekf_reset_counters_s last_reset_counters = 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 FlightTasks::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;
-}
-
-void FlightTasks::handleParameterUpdate()
-{
-	if (isAnyTaskActive()) {
-		_current_task.task->handleParameterUpdate();
-	}
-}
-
-const char *FlightTasks::errorToString(const FlightTaskError error)
-{
-	for (auto e : _taskError) {
-		if (static_cast<FlightTaskError>(e.error) == error) {
-			return e.msg;
-		}
-	}
-
-	return "This error is not mapped to a string or is unknown.";
-}
-
-void FlightTasks::reActivate()
-{
-	if (isAnyTaskActive()) {
-		_current_task.task->reActivate();
-	}
-}
-
-void FlightTasks::_updateCommand()
-{
-	// check if there's any new command
-	bool updated = _sub_vehicle_command.updated();
-
-	if (!updated) {
-		return;
-	}
-
-	// get command
-	vehicle_command_s command{};
-	_sub_vehicle_command.copy(&command);
-
-	// check what command it is
-	FlightTaskIndex desired_task = switchVehicleCommand(command.command);
-
-	if (desired_task == FlightTaskIndex::None) {
-		// ignore all unkown commands
-		return;
-	}
-
-	// 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.timestamp = hrt_absolute_time();
-	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;
-
-	_pub_vehicle_command_ack.publish(command_ack);
-}

src/lib/flight_tasks/FlightTasks.hpp

@@ -1,202 +0,0 @@
-/****************************************************************************
- *
- *   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 flight_taks.h
- *
- * Library class to hold and manage all implemented flight task instances
- *
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#pragma once
-
-#include "FlightTask.hpp"
-#include "FlightTasks_generated.hpp"
-#include <lib/weather_vane/WeatherVane.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/vehicle_command.h>
-
-#include <new>
-
-enum class FlightTaskError : int {
-	NoError = 0,
-	InvalidTask = -1,
-	ActivationFailed = -2
-};
-
-class FlightTasks
-{
-public:
-	FlightTasks();
-
-	~FlightTasks()
-	{
-		if (_current_task.task) {
-			_current_task.task->~FlightTask();
-		}
-	}
-
-	/**
-	 * Call regularly in the control loop cycle to execute the task
-	 * @return true on success, false on error
-	 */
-	bool update();
-
-	/**
-	 * Get the output data from the current task
-	 * @return output setpoint, to be executed by position control
-	 */
-	const vehicle_local_position_setpoint_s getPositionSetpoint();
-
-	/**
-	 * Get the local frame and attitude reset counters of the estimator from the current task
-	 * @return the reset counters
-	 */
-	const ekf_reset_counters_s getResetCounters();
-
-	/**
-	 * Get task dependent constraints
-	 * @return setpoint constraints that has to be respected by the position controller
-	 */
-	const vehicle_constraints_s getConstraints();
-
-	/**
-	 * Get landing gear position.
-	 * @return landing gear
-	 */
-	const landing_gear_s getGear();
-
-	/**
-	 * Get task avoidance desired waypoints
-	 * @return auto triplets in the mc_pos_control
-	 */
-	const vehicle_trajectory_waypoint_s getAvoidanceWaypoint();
-
-	/**
-	 * Get empty avoidance desired waypoints
-	 * @return empty triplets in the mc_pos_control
-	 */
-	const vehicle_trajectory_waypoint_s &getEmptyAvoidanceWaypoint();
-
-	/**
-	 * Switch to the next task in the available list (for testing)
-	 * @return 0 on success, <0 on error
-	 */
-	FlightTaskError switchTask() { return switchTask(static_cast<int>(_current_task.index) + 1); }
-
-	/**
-	 * 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);
-
-	/**
-	 * Get the number of the active task
-	 * @return number of active task, -1 if there is none
-	 */
-	int getActiveTask() const { return static_cast<int>(_current_task.index); }
-
-	/**
-	 * Check if any task is active
-	 * @return true if a task is active, false if not
-	 */
-	bool isAnyTaskActive() const { return _current_task.task; }
-
-	/**
-	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
-	 */
-	void handleParameterUpdate();
-
-	/**
-	 * Call this method to get the description of a task error.
-	 */
-	const char *errorToString(const FlightTaskError error);
-
-	/**
-	 * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy.
-	 */
-	void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);}
-
-	/**
-	 *   This method will re-activate current task.
-	 */
-	void reActivate();
-
-	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); }
-
-private:
-
-	/**
-	 * 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;
-		FlightTaskIndex index;
-	};
-	flight_task_t _current_task = {nullptr, FlightTaskIndex::None};
-
-	struct task_error_t {
-		int error;
-		const char *msg;
-	};
-
-	static constexpr int _numError = 3;
-	/**
-	 * Map from Error int to user friendly string.
-	 */
-	task_error_t _taskError[_numError] = {
-		{static_cast<int>(FlightTaskError::NoError), "No Error"},
-		{static_cast<int>(FlightTaskError::InvalidTask), "Invalid Task "},
-		{static_cast<int>(FlightTaskError::ActivationFailed), "Activation Failed"}
-	};
-
-	/**
-	 * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
-	 */
-	void _updateCommand();
-	FlightTaskIndex switchVehicleCommand(const int command);
-
-	uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */
-
-	uORB::Publication<vehicle_command_ack_s>	_pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
-
-	int _initTask(FlightTaskIndex task_index);
-};

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

@@ -39,10 +39,10 @@
  * @@author Christoph Tobler <christoph@@px4.io>
  */
 
-#include "FlightTasks.hpp"
+#include "FlightModeManager.hpp"
 #include "FlightTasks_generated.hpp"
 
-int FlightTasks::_initTask(FlightTaskIndex task_index)
+int FlightModeManager::_initTask(FlightTaskIndex task_index)
 {
 
 	// disable the old task if there is any
@@ -79,7 +79,7 @@ firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
 	return 0;
 }
 
-FlightTaskIndex FlightTasks::switchVehicleCommand(const int command)
+FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command)
 {
     switch (command) {
 @# loop through all additional tasks
@@ -97,4 +97,4 @@ upperCase = lambda s: s[:].upper() if s else ''
 	// ignore all unkown commands
 	default : return FlightTaskIndex::None;
 	}
-}
+}

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

@@ -42,7 +42,7 @@
  #pragma once
 
 // include all required headers
-#include "FlightTasks.hpp"
+#include "FlightModeManager.hpp"
 @# loop through all requested tasks
 @[if tasks]@
 @[for task in tasks]@

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

@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
-#include <flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp>
+
+#include "TrajectoryConstraints.hpp"
 
 using namespace matrix;
 using namespace math::trajectory;

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

@@ -33,7 +33,14 @@
 
 px4_add_library(FlightTaskManualPosition
 	FlightTaskManualPosition.cpp
+	FlightTaskManualPosition.hpp
+)
+
+target_link_libraries(FlightTaskManualPosition
+	PRIVATE
+		CollisionPrevention
+	PUBLIC
+		FlightTaskManualAltitude
 )
 
-target_link_libraries(FlightTaskManualPosition PUBLIC FlightTaskManualAltitude)
 target_include_directories(FlightTaskManualPosition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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

@@ -105,7 +105,6 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 bool FlightTaskOrbit::sendTelemetry()
 {
 	orbit_status_s orbit_status{};
-	orbit_status.timestamp = hrt_absolute_time();
 	orbit_status.radius = math::signNoZero(_v) * _r;
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
 	orbit_status.yaw_behaviour = _yaw_behaviour;
@@ -115,6 +114,7 @@ bool FlightTaskOrbit::sendTelemetry()
 		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;

src/modules/mc_pos_control/CMakeLists.txt

@@ -37,16 +37,12 @@ px4_add_module(
 	MODULE modules__mc_pos_control
 	MAIN mc_pos_control
 	COMPILE_FLAGS
-		-Wno-implicit-fallthrough # TODO: fix and remove
 	SRCS
 		MulticopterPositionControl.cpp
 		MulticopterPositionControl.hpp
 	DEPENDS
 		PositionControl
 		controllib
-		FlightTasks
 		git_ecl
 		ecl_geo
-		WeatherVane
-		CollisionPrevention
 	)

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -37,7 +37,6 @@
 #include <lib/mathlib/mathlib.h>
 #include <lib/matrix/matrix/math.hpp>
 
-using namespace time_literals;
 using namespace matrix;
 
 MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
@@ -271,7 +270,7 @@ void MulticopterPositionControl::Run()
 
 				failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe);
 				_control.setInputSetpoint(setpoint);
-				constraints = FlightTask::empty_constraints;
+				constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
 				_control.update(dt);
 			}
 

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -40,7 +40,6 @@
 #include <commander/px4_custom_mode.h>
 #include <drivers/drv_hrt.h>
 #include <lib/controllib/blocks.hpp>
-#include <lib/flight_tasks/FlightTasks.hpp>
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/systemlib/mavlink_log.h>
@@ -64,6 +63,8 @@
 
 #include "PositionControl/PositionControl.hpp"
 
+using namespace time_literals;
+
 class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
 	public ModuleParams, public px4::WorkItem
 {