Add FlightModeManager

to separate out setpoint generation from controllers

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -62,6 +62,7 @@ mc_att_control start
 # Start Multicopter Position Controller.
 #
 mc_hover_thrust_estimator start
+flight_mode_manager start
 mc_pos_control start
 
 #

ROMFS/px4fmu_common/init.d/rc.vtol_apps

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

boards/px4/sitl/default.cmake

@@ -32,6 +32,7 @@ px4_add_board(
 		dataman
 		ekf2
 		events
+		flight_mode_manager
 		fw_att_control
 		fw_pos_control_l1
 		land_detector

src/modules/flight_mode_manager/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(Takeoff)
+
+px4_add_module(
+	MODULE modules__flight_mode_manager
+	MAIN flight_mode_manager
+	COMPILE_FLAGS
+	SRCS
+		FlightModeManager.cpp
+		FlightModeManager.hpp
+	DEPENDS
+		FlightTasks
+		Takeoff
+		px4_work_queue
+	)

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -0,0 +1,555 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "FlightModeManager.hpp"
+
+#include <lib/mathlib/mathlib.h>
+
+using namespace time_literals;
+
+FlightModeManager::FlightModeManager() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+}
+
+FlightModeManager::~FlightModeManager()
+{
+	perf_free(_loop_perf);
+}
+
+bool FlightModeManager::init()
+{
+	if (!_vehicle_local_position_sub.registerCallback()) {
+		PX4_ERR("vehicle_local_position callback registration failed!");
+		return false;
+	}
+
+	// limit to every other vehicle_local_position update (~62.5 Hz)
+	_vehicle_local_position_sub.set_interval_us(16_ms);
+	_time_stamp_last_loop = hrt_absolute_time();
+	return true;
+}
+
+void FlightModeManager::Run()
+{
+	if (should_exit()) {
+		_vehicle_local_position_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		_flight_tasks.handleParameterUpdate();
+	}
+
+	// generate setpoints on local position changes
+	vehicle_local_position_s vehicle_local_position;
+
+	if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
+		_home_position_sub.update();
+		_vehicle_control_mode_sub.update();
+		_vehicle_land_detected_sub.update();
+		_vehicle_local_position_setpoint_sub.update();
+		_vehicle_status_sub.update();
+
+		const hrt_abstime time_stamp_now = hrt_absolute_time();
+		// Guard against too small (< 0.2ms) and too large (> 100ms) dt's.
+		const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f);
+		_time_stamp_last_loop = time_stamp_now;
+
+		start_flight_task();
+
+		if (_flight_tasks.isAnyTaskActive()) {
+			generateTrajectorySetpoint(dt, vehicle_local_position);
+		}
+
+	}
+
+	perf_end(_loop_perf);
+}
+
+void FlightModeManager::start_flight_task()
+{
+	bool task_failure = false;
+	bool should_disable_task = true;
+	int prev_failure_count = _task_failure_count;
+
+	// 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);
+		return;
+	}
+
+	if (_vehicle_status_sub.get().in_transition_mode) {
+
+		should_disable_task = false;
+		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition);
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			// we want to be in this mode, reset the failure count
+			_task_failure_count = 0;
+		}
+
+		return;
+	}
+
+	// offboard
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
+	    && (_vehicle_control_mode_sub.get().flag_control_altitude_enabled ||
+		_vehicle_control_mode_sub.get().flag_control_position_enabled ||
+		_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled ||
+		_vehicle_control_mode_sub.get().flag_control_velocity_enabled ||
+		_vehicle_control_mode_sub.get().flag_control_acceleration_enabled)) {
+
+		should_disable_task = false;
+		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Offboard);
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Offboard activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			// we want to be in this mode, reset the failure count
+			_task_failure_count = 0;
+		}
+	}
+
+	// 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);
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			// we want to be in this mode, reset the failure count
+			_task_failure_count = 0;
+		}
+
+	} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
+		// Auto related maneuvers
+		should_disable_task = false;
+		FlightTaskError error = FlightTaskError::NoError;
+
+		error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			// we want to be in this mode, reset the failure count
+			_task_failure_count = 0;
+		}
+
+	} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
+
+		// Emergency descend
+		should_disable_task = false;
+		FlightTaskError error = FlightTaskError::NoError;
+
+		error =  _flight_tasks.switchTask(FlightTaskIndex::Descend);
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			// we want to be in this mode, reset the failure count
+			_task_failure_count = 0;
+		}
+
+	}
+
+	// manual position control
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) {
+		should_disable_task = false;
+		FlightTaskError error = FlightTaskError::NoError;
+
+		switch (_param_mpc_pos_mode.get()) {
+		case 1:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth);
+			break;
+
+		case 3:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
+			break;
+
+		default:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
+			break;
+		}
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL);
+			task_failure = false;
+		}
+	}
+
+	// manual altitude control
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) {
+		should_disable_task = false;
+		FlightTaskError error = FlightTaskError::NoError;
+
+		switch (_param_mpc_pos_mode.get()) {
+		case 1:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth);
+			break;
+
+		case 3:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
+			break;
+
+		default:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
+			break;
+		}
+
+		if (error != FlightTaskError::NoError) {
+			if (prev_failure_count == 0) {
+				PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
+			}
+
+			task_failure = true;
+			_task_failure_count++;
+
+		} else {
+			check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL);
+			task_failure = false;
+		}
+	}
+
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
+		should_disable_task = false;
+	}
+
+	// check task failure
+	if (task_failure) {
+
+		// for some reason no flighttask was able to start.
+		// go into failsafe flighttask
+		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe);
+
+		if (error != FlightTaskError::NoError) {
+			// No task was activated.
+			_flight_tasks.switchTask(FlightTaskIndex::None);
+		}
+
+	} else if (should_disable_task) {
+		_flight_tasks.switchTask(FlightTaskIndex::None);
+	}
+}
+
+void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state)
+{
+	if (!task_failure) {
+		// we want to be in this mode, reset the failure count
+		_task_failure_count = 0;
+
+	} else if (_task_failure_count > NUM_FAILURE_TRIES) {
+		// tell commander to switch mode
+		PX4_WARN("Previous flight task failed, switching to mode %d", nav_state);
+		send_vehicle_cmd_do(nav_state);
+		_task_failure_count = 0; // avoid immediate resending of a vehicle command in the next iteration
+	}
+}
+
+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
+	command.target_system = 1;
+	command.target_component = 1;
+	command.source_system = 1;
+	command.source_component = 1;
+	command.confirmation = false;
+	command.from_external = false;
+
+	// set the main mode
+	switch (nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_STAB:
+		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_STABILIZED;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
+		command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		break;
+
+	default: //vehicle_status_s::NAVIGATION_STATE_POSCTL
+		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL;
+		break;
+	}
+
+	// publish the vehicle command
+	_vehicle_command_pub.publish(command);
+}
+
+void FlightModeManager::generateTrajectorySetpoint(const float dt,
+		const vehicle_local_position_s &vehicle_local_position)
+{
+	// 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));
+
+	// 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);
+
+	if (_flight_tasks.update()) {
+		setpoint = _flight_tasks.getPositionSetpoint();
+		constraints = _flight_tasks.getConstraints();
+
+		//_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); TODO
+
+	} else {
+		// FAILSAFE
+		// Task was not able to update correctly. Do Failsafe.
+		// by sending out an empty setpoint that is not executable to make the position controller failsafe
+		// TODO test failsafe
+	}
+
+	landing_gear_s landing_gear = _flight_tasks.getGear();
+
+	// limit altitude according to land detector
+	limitAltitude(setpoint, vehicle_local_position);
+
+	const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup;
+	const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight;
+	const bool flying_but_ground_contact = flying && _vehicle_land_detected_sub.get().ground_contact;
+
+	if (not_taken_off || flying_but_ground_contact) {
+		// we are not flying yet and need to avoid any corrections
+		reset_setpoint_to_nan(setpoint);
+		Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
+		// set yaw-sp to current yaw
+		setpoint.yawspeed = 0.f;
+		// prevent any integrator windup
+		// _control.resetIntegral(); TODO
+	}
+
+	_trajectory_setpoint_pub.publish(setpoint);
+
+	// if (flying) { TODO
+	// 	_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
+
+	// } else {
+	// 	// allow zero thrust when taking off and landing
+	// 	_control.setThrustLimits(0.f, _param_mpc_thr_max.get());
+	// }
+
+	// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
+	// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
+	if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
+		constraints.speed_up = _param_mpc_z_vel_max_up.get();
+	}
+
+	// limit tilt during takeoff ramupup
+	if (_takeoff.getTakeoffState() < TakeoffState::flight) {
+		constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
+	}
+
+	// handle smooth takeoff
+	_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed,
+				    constraints.want_takeoff, constraints.speed_up, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled,
+				    _time_stamp_last_loop);
+	constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up);
+
+	_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
+	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_pub.publish(landing_gear);
+	}
+
+	_old_landing_gear_position = landing_gear.landing_gear;
+}
+
+void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
+				      const vehicle_local_position_s &vehicle_local_position)
+{
+	if (_vehicle_land_detected_sub.get().alt_max < 0.0f || !_home_position_sub.get().valid_alt
+	    || !vehicle_local_position.z_valid || !vehicle_local_position.v_z_valid) {
+		// there is no altitude limitation present or the required information not available
+		return;
+	}
+
+	// maximum altitude == minimal z-value (NED)
+	const float min_z = _home_position_sub.get().z + (-_vehicle_land_detected_sub.get().alt_max);
+
+	if (vehicle_local_position.z < min_z) {
+		// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
+		setpoint.z = min_z;
+		setpoint.vz = math::max(setpoint.vz, 0.f);
+	}
+}
+
+void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
+{
+	setpoint.x = setpoint.y = setpoint.z = NAN;
+	setpoint.vx = setpoint.vy = setpoint.vz = NAN;
+	setpoint.yaw = setpoint.yawspeed = NAN;
+	setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
+	setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
+}
+
+int FlightModeManager::task_spawn(int argc, char *argv[])
+{
+	FlightModeManager *instance = new FlightModeManager();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int FlightModeManager::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FlightModeManager::print_status()
+{
+	if (_flight_tasks.isAnyTaskActive()) {
+		PX4_INFO("Running, active flight task: %i", _flight_tasks.getActiveTask());
+
+	} else {
+		PX4_INFO("Running, no flight task active");
+	}
+
+	perf_print_counter(_loop_perf);
+	return 0;
+}
+
+int FlightModeManager::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the multicopter rate controller. It takes rate setpoints (in acro mode
+via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
+
+The controller has a PID loop for angular rate error.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("flight_mode_manager", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int flight_mode_manager_main(int argc, char *argv[])
+{
+	return FlightModeManager::main(argc, argv);
+}

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 <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 <Takeoff.hpp>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+
+class FlightModeManager : public ModuleBase<FlightModeManager>, public ModuleParams, public px4::WorkItem
+{
+public:
+	FlightModeManager();
+	~FlightModeManager() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	bool init();
+
+private:
+	void Run() override;
+	void start_flight_task();
+	void check_failure(bool task_failure, uint8_t nav_state);
+	void send_vehicle_cmd_do(uint8_t nav_state);
+	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
+	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);
+
+	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
+	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};
+
+	perf_counter_t _loop_perf; ///< loop duration performance counter
+	hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration
+
+	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	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::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
+		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up
+	);
+};

src/modules/flight_mode_manager/Takeoff/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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(Takeoff
+	Takeoff.cpp
+)
+target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(Takeoff PUBLIC hysteresis)
+
+px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff)

src/modules/flight_mode_manager/Takeoff/Takeoff.cpp

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   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 Takeoff.cpp
+ */
+
+#include "Takeoff.hpp"
+#include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+
+void Takeoff::generateInitialRampValue(float velocity_p_gain)
+{
+	velocity_p_gain = math::max(velocity_p_gain, 0.01f);
+	_takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain;
+}
+
+void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
+				 const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
+{
+	_spoolup_time_hysteresis.set_state_and_update(armed, now_us);
+
+	switch (_takeoff_state) {
+	case TakeoffState::disarmed:
+		if (armed) {
+			_takeoff_state = TakeoffState::spoolup;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::spoolup:
+		if (_spoolup_time_hysteresis.get_state()) {
+			_takeoff_state = TakeoffState::ready_for_takeoff;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::ready_for_takeoff:
+		if (want_takeoff) {
+			_takeoff_state = TakeoffState::rampup;
+			_takeoff_ramp_vz = _takeoff_ramp_vz_init;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::rampup:
+		if (_takeoff_ramp_vz >= takeoff_desired_vz) {
+			_takeoff_state = TakeoffState::flight;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::flight:
+		if (landed) {
+			_takeoff_state = TakeoffState::ready_for_takeoff;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	if (armed && skip_takeoff) {
+		_takeoff_state = TakeoffState::flight;
+	}
+
+	// TODO: need to consider free fall here
+	if (!armed) {
+		_takeoff_state = TakeoffState::disarmed;
+	}
+}
+
+float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz)
+{
+	if (_takeoff_state < TakeoffState::rampup) {
+		return _takeoff_ramp_vz_init;
+	}
+
+	if (_takeoff_state == TakeoffState::rampup) {
+		if (_takeoff_ramp_time > dt) {
+			_takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time;
+
+		} else {
+			_takeoff_ramp_vz = takeoff_desired_vz;
+		}
+
+		if (_takeoff_ramp_vz < takeoff_desired_vz) {
+			return _takeoff_ramp_vz;
+		}
+	}
+
+	return takeoff_desired_vz;
+}

src/modules/flight_mode_manager/Takeoff/Takeoff.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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 Takeoff.hpp
+ *
+ * A class handling all takeoff states and a smooth ramp up of the motors.
+ */
+
+#pragma once
+
+#include <lib/hysteresis/hysteresis.h>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+enum class TakeoffState {
+	disarmed = 0,
+	spoolup,
+	ready_for_takeoff,
+	rampup,
+	flight
+};
+
+class Takeoff
+{
+public:
+	Takeoff() = default;
+	~Takeoff() = default;
+
+	// initialize parameters
+	void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); }
+	void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
+
+	/**
+	 * Calculate a vertical velocity to initialize the takeoff ramp
+	 * that when passed to the velocity controller results in a zero throttle setpoint.
+	 * @param hover_thrust normalized thrsut value with which the vehicle hovers
+	 * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust
+	 */
+	void generateInitialRampValue(const float velocity_p_gain);
+
+	/**
+	 * Update the state for the takeoff.
+	 * @param setpoint a vehicle_local_position_setpoint_s structure
+	 * @return true if setpoint has updated correctly
+	 */
+	void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
+				const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
+
+	/**
+	 * Update and return the velocity constraint ramp value during takeoff.
+	 * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved.
+	 * Returns zero on the ground and takeoff_desired_vz in flight.
+	 * @param dt time in seconds since the last call/loop iteration
+	 * @param takeoff_desired_vz end value for the velocity ramp
+	 * @return true if setpoint has updated correctly
+	 */
+	float updateRamp(const float dt, const float takeoff_desired_vz);
+
+	TakeoffState getTakeoffState() { return _takeoff_state; }
+
+private:
+	TakeoffState _takeoff_state = TakeoffState::disarmed;
+
+	systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
+
+	float _takeoff_ramp_time = 0.f;
+	float _takeoff_ramp_vz_init = 0.f;
+	float _takeoff_ramp_vz = 0.f;
+};

src/modules/flight_mode_manager/Takeoff/TakeoffTest.cpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <Takeoff.hpp>
+#include <drivers/drv_hrt.h>
+#include <lib/ecl/geo/geo.h>
+
+TEST(TakeoffTest, Initialization)
+{
+	Takeoff takeoff;
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
+}
+
+TEST(TakeoffTest, RegularTakeoffRamp)
+{
+	Takeoff takeoff;
+	takeoff.setSpoolupTime(1.f);
+	takeoff.setTakeoffRampTime(2.0);
+	takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
+
+	// disarmed, landed, don't want takeoff
+	takeoff.updateTakeoffState(false, true, false, 1.f, false, 0);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
+
+	// armed, not landed anymore, don't want takeoff
+	takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup);
+
+	// armed, not landed, don't want takeoff yet, spoolup time passed
+	takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff);
+
+	// armed, not landed, want takeoff
+	takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);
+
+	// armed, not landed, want takeoff, ramping up
+	takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
+
+	// armed, not landed, want takeoff, rampup time passed
+	takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight);
+}

src/modules/flight_mode_manager/flight_mode_manager_params.c

@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   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 flight_mode_manager_params.c
+ */