create new multicopter rate controller module (mc_rate_control) split out of mc_att_control

src/modules/mc_rate_control/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(RateControl)
+
+px4_add_module(
+	MODULE modules__mc_rate_control
+	MAIN mc_rate_control
+	COMPILE_FLAGS
+	SRCS
+		MulticopterRateControl.cpp
+		MulticopterRateControl.hpp
+	DEPENDS
+		circuit_breaker
+		mathlib
+		RateControl
+		px4_work_queue
+	)

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -0,0 +1,412 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MulticopterRateControl.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+MulticopterRateControl::MulticopterRateControl() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+	parameters_updated();
+}
+
+MulticopterRateControl::~MulticopterRateControl()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+MulticopterRateControl::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_velocity callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+MulticopterRateControl::parameters_updated()
+{
+	// rate control parameters
+	// The controller gain K is used to convert the parallel (P + I/s + sD) form
+	// to the ideal (K * [1 + 1/sTi + sTd]) form
+	const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
+
+	_rate_control.setGains(
+		rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())),
+		rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())),
+		rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));
+
+	_rate_control.setIntegratorLimit(
+		Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()));
+
+	_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false);
+
+	_rate_control.setFeedForwardGain(
+		Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
+
+
+	// manual rate control acro mode rate limits
+	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
+				  radians(_param_mc_acro_y_max.get()));
+
+	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
+}
+
+void
+MulticopterRateControl::vehicle_status_poll()
+{
+	/* check if there is new status information */
+	if (_vehicle_status_sub.update(&_vehicle_status)) {
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (_actuators_id == nullptr) {
+			if (_vehicle_status.is_vtol) {
+				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+
+			} else {
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
+		}
+	}
+}
+
+float
+MulticopterRateControl::get_landing_gear_state()
+{
+	// Only switch the landing gear up if we are not landed and if
+	// the user switched from gear down to gear up.
+	// If the user had the switch in the gear up position and took off ignore it
+	// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+	if (_landed) {
+		_gear_state_initialized = false;
+	}
+
+	float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
+
+	if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
+		landing_gear = landing_gear_s::GEAR_UP;
+
+	} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+		// Switching the gear off does put it into a safe defined state
+		_gear_state_initialized = true;
+	}
+
+	return landing_gear;
+}
+
+void
+MulticopterRateControl::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_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();
+		parameters_updated();
+	}
+
+	/* run controller on gyro changes */
+	vehicle_angular_velocity_s angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+		const hrt_abstime now = hrt_absolute_time();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+		_last_run = now;
+
+		const Vector3f rates{angular_velocity.xyz};
+
+		/* check for updates in other topics */
+		_v_control_mode_sub.update(&_v_control_mode);
+
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+
+			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed;
+				_maybe_landed = vehicle_land_detected.maybe_landed;
+			}
+		}
+
+		vehicle_status_poll();
+
+		const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
+
+		// generate the rate setpoint from sticks?
+		bool manual_rate_sp = false;
+
+		if (_v_control_mode.flag_control_manual_enabled &&
+		    !_v_control_mode.flag_control_altitude_enabled &&
+		    !_v_control_mode.flag_control_velocity_enabled &&
+		    !_v_control_mode.flag_control_position_enabled) {
+
+			// landing gear controlled from stick inputs if we are in Manual/Stabilized mode
+			//  limit landing gear update rate to 50 Hz
+			if (hrt_elapsed_time(&_landing_gear.timestamp) > 20_ms) {
+				_landing_gear.landing_gear = get_landing_gear_state();
+				_landing_gear.timestamp = hrt_absolute_time();
+				_landing_gear_pub.publish(_landing_gear);
+			}
+
+			if (!_v_control_mode.flag_control_attitude_enabled) {
+				manual_rate_sp = true;
+			}
+
+			// Check if we are in rattitude mode and the pilot is within the center threshold on pitch and roll
+			//  if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro)
+			if (_v_control_mode.flag_control_rattitude_enabled) {
+				manual_rate_sp =
+					(fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) ||
+					(fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get());
+			}
+
+		} else {
+			_landing_gear_sub.update(&_landing_gear);
+		}
+
+		if (manual_rate_sp) {
+			if (manual_control_updated) {
+
+				// manual rates control - ACRO mode
+				const Vector3f man_rate_sp{
+					math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
+					math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
+					math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
+
+				_rates_sp = man_rate_sp.emult(_acro_rate_max);
+				_thrust_sp = _manual_control_sp.z;
+
+				// publish rate setpoint
+				vehicle_rates_setpoint_s v_rates_sp{};
+				v_rates_sp.roll = _rates_sp(0);
+				v_rates_sp.pitch = _rates_sp(1);
+				v_rates_sp.yaw = _rates_sp(2);
+				v_rates_sp.thrust_body[0] = 0.0f;
+				v_rates_sp.thrust_body[1] = 0.0f;
+				v_rates_sp.thrust_body[2] = -_thrust_sp;
+				v_rates_sp.timestamp = hrt_absolute_time();
+
+				_v_rates_sp_pub.publish(v_rates_sp);
+			}
+
+		} else {
+			// use rates setpoint topic
+			vehicle_rates_setpoint_s v_rates_sp;
+
+			if (_v_rates_sp_sub.update(&v_rates_sp)) {
+				_rates_sp(0) = v_rates_sp.roll;
+				_rates_sp(1) = v_rates_sp.pitch;
+				_rates_sp(2) = v_rates_sp.yaw;
+				_thrust_sp = -v_rates_sp.thrust_body[2];
+			}
+		}
+
+		// calculate loop update rate while disarmed or at least a few times (updating the filter is expensive)
+		if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator += dt;
+			++_loop_counter;
+
+			if (_dt_accumulator > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
+				_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator = 0;
+				_loop_counter = 0;
+				_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true);
+			}
+		}
+
+		// run the rate controller
+		if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
+
+			// reset integral if disarmed
+			if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				_rate_control.resetIntegral();
+			}
+
+			// update saturation status from mixer feedback
+			if (_motor_limits_sub.updated()) {
+				multirotor_motor_limits_s motor_limits;
+
+				if (_motor_limits_sub.copy(&motor_limits)) {
+					MultirotorMixer::saturation_status saturation_status;
+					saturation_status.value = motor_limits.saturation_status;
+
+					_rate_control.setSaturationStatus(saturation_status);
+				}
+			}
+
+			// run rate controller
+			const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
+
+			// publish rate controller status
+			rate_ctrl_status_s rate_ctrl_status{};
+			_rate_control.getRateControlStatus(rate_ctrl_status);
+			rate_ctrl_status.timestamp = hrt_absolute_time();
+			_controller_status_pub.publish(rate_ctrl_status);
+
+			// publish actuator controls
+			actuator_controls_s actuators{};
+			actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
+			actuators.timestamp_sample = angular_velocity.timestamp_sample;
+
+			// scale effort by battery status if enabled
+			if (_param_mc_bat_scale_en.get()) {
+				if (_battery_status_sub.updated()) {
+					battery_status_s battery_status;
+
+					if (_battery_status_sub.copy(&battery_status)) {
+						_battery_status_scale = battery_status.scale;
+					}
+				}
+
+				if (_battery_status_scale > 0.0f) {
+					for (int i = 0; i < 4; i++) {
+						actuators.control[i] *= _battery_status_scale;
+					}
+				}
+			}
+
+			actuators.timestamp = hrt_absolute_time();
+			orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
+
+		} else if (_v_control_mode.flag_control_termination_enabled) {
+			if (!_vehicle_status.is_vtol) {
+				// publish actuator controls
+				actuator_controls_s actuators{};
+				actuators.timestamp = hrt_absolute_time();
+				orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
+			}
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+int MulticopterRateControl::task_spawn(int argc, char *argv[])
+{
+	MulticopterRateControl *instance = new MulticopterRateControl();
+
+	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 MulticopterRateControl::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int MulticopterRateControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int MulticopterRateControl::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(MODULE_NAME, "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Multicopter rate control app start / stop handling function
+ */
+extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]);
+
+int mc_rate_control_main(int argc, char *argv[])
+{
+	return MulticopterRateControl::main(argc, argv);
+}

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -0,0 +1,194 @@
+/****************************************************************************
+ *
+ *   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 <RateControl.hpp>
+
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/mixer/mixer.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/landing_gear.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/multirotor_motor_limits.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+
+class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
+{
+public:
+	MulticopterRateControl();
+
+	virtual ~MulticopterRateControl();
+
+	/** @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;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void		parameters_updated();
+
+	void		vehicle_status_poll();
+
+	/**
+	 * Get the landing gear state based on the manual control switch position
+	 * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
+	 */
+	float		get_landing_gear_state();
+
+	RateControl _rate_control; ///< class for rate control calculations
+
+	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};		/**< vehicle rates setpoint subscription */
+	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
+	uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};		/**< motor limits subscription */
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};			/**< battery status subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
+	uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT};	/**< controller status publication */
+	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
+	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
+
+	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
+	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
+
+	landing_gear_s 			_landing_gear{};
+	manual_control_setpoint_s	_manual_control_sp{};
+	vehicle_control_mode_s		_v_control_mode{};
+	vehicle_status_s		_vehicle_status{};
+
+	bool _actuators_0_circuit_breaker_enabled{false};	/**< circuit breaker to suppress output */
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	float _battery_status_scale{0.0f};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
+	float _loop_update_rate_hz{initial_update_rate_hz};          /**< current rate-controller loop update rate in [Hz] */
+
+	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */
+
+	float		_thrust_sp{0.0f};		/**< thrust setpoint */
+
+	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
+
+	hrt_abstime _task_start{hrt_absolute_time()};
+	hrt_abstime _last_run{0};
+	float _dt_accumulator{0.0f};
+	int _loop_counter{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
+		(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
+		(ParamFloat<px4::params::MC_RR_INT_LIM>) _param_mc_rr_int_lim,
+		(ParamFloat<px4::params::MC_ROLLRATE_D>) _param_mc_rollrate_d,
+		(ParamFloat<px4::params::MC_ROLLRATE_FF>) _param_mc_rollrate_ff,
+		(ParamFloat<px4::params::MC_ROLLRATE_K>) _param_mc_rollrate_k,
+
+		(ParamFloat<px4::params::MC_PITCHRATE_P>) _param_mc_pitchrate_p,
+		(ParamFloat<px4::params::MC_PITCHRATE_I>) _param_mc_pitchrate_i,
+		(ParamFloat<px4::params::MC_PR_INT_LIM>) _param_mc_pr_int_lim,
+		(ParamFloat<px4::params::MC_PITCHRATE_D>) _param_mc_pitchrate_d,
+		(ParamFloat<px4::params::MC_PITCHRATE_FF>) _param_mc_pitchrate_ff,
+		(ParamFloat<px4::params::MC_PITCHRATE_K>) _param_mc_pitchrate_k,
+
+		(ParamFloat<px4::params::MC_YAWRATE_P>) _param_mc_yawrate_p,
+		(ParamFloat<px4::params::MC_YAWRATE_I>) _param_mc_yawrate_i,
+		(ParamFloat<px4::params::MC_YR_INT_LIM>) _param_mc_yr_int_lim,
+		(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
+		(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
+		(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
+
+		(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff,			/**< Cutoff frequency for the D-term filter */
+
+		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,			/**< scaling factor from stick to yaw rate */
+
+		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
+		(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
+		(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
+		(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo,				/**< expo stick curve shape (roll & pitch) */
+		(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y,				/**< expo stick curve shape (yaw) */
+		(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo,			/**< superexpo stick curve shape (roll & pitch) */
+		(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy,			/**< superexpo stick curve shape (yaw) */
+
+		(ParamFloat<px4::params::MC_RATT_TH>) _param_mc_ratt_th,
+
+		(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
+
+		(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
+	)
+
+	matrix::Vector3f _acro_rate_max;	/**< max attitude rates in acro mode */
+
+};
+

src/modules/mc_rate_control/RateControl/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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(RateControl
+	RateControl.cpp
+)
+target_include_directories(RateControl
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(RateControl PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl)

src/modules/mc_rate_control/RateControl/RateControl.cpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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 RateControl.cpp
+ */
+
+#include <RateControl.hpp>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_p = P;
+	_gain_i = I;
+	_gain_d = D;
+}
+
+void RateControl::setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
+{
+	// only do expensive filter update if the cutoff changed
+	if (force || fabsf(_lp_filters_d.get_cutoff_freq() - cutoff) > 0.01f) {
+		_lp_filters_d.set_cutoff_frequency(loop_rate, cutoff);
+		_lp_filters_d.reset(_rate_prev);
+	}
+}
+
+void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status)
+{
+	_mixer_saturation_positive[0] = status.flags.roll_pos;
+	_mixer_saturation_positive[1] = status.flags.pitch_pos;
+	_mixer_saturation_positive[2] = status.flags.yaw_pos;
+	_mixer_saturation_negative[0] = status.flags.roll_neg;
+	_mixer_saturation_negative[1] = status.flags.pitch_neg;
+	_mixer_saturation_negative[2] = status.flags.yaw_neg;
+}
+
+Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed)
+{
+	// angular rates error
+	Vector3f rate_error = rate_sp - rate;
+
+	// prepare D-term based on low-pass filtered rates
+	const Vector3f rate_filtered(_lp_filters_d.apply(rate));
+	Vector3f rate_d;
+
+	if (dt > FLT_EPSILON) {
+		rate_d = (rate_filtered - _rate_prev_filtered) / dt;
+	}
+
+	// PID control with feed forward
+	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
+
+	_rate_prev = rate;
+	_rate_prev_filtered = rate_filtered;
+
+	// update integral only if we are not landed
+	if (!landed) {
+		updateIntegral(rate_error, dt);
+	}
+
+	return torque;
+}
+
+void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
+{
+	for (int i = 0; i < 3; i++) {
+		// prevent further positive control saturation
+		if (_mixer_saturation_positive[i]) {
+			rate_error(i) = math::min(rate_error(i), 0.f);
+		}
+
+		// prevent further negative control saturation
+		if (_mixer_saturation_negative[i]) {
+			rate_error(i) = math::max(rate_error(i), 0.f);
+		}
+
+		// I term factor: reduce the I gain with increasing rate error.
+		// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
+		// change (noticeable in a bounce-back effect after a flip).
+		// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
+		// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
+		// and up to 200 deg error leads to <25% reduction of I.
+		float i_factor = rate_error(i) / math::radians(400.f);
+		i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
+
+		// Perform the integration using a first order method
+		float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;
+
+		// do not propagate the result if out of range or invalid
+		if (PX4_ISFINITE(rate_i)) {
+			_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
+		}
+	}
+}
+
+void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
+{
+	rate_ctrl_status.rollspeed_integ = _rate_int(0);
+	rate_ctrl_status.pitchspeed_integ = _rate_int(1);
+	rate_ctrl_status.yawspeed_integ = _rate_int(2);
+}

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   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 RateControl.hpp
+ *
+ * PID 3 axis angular rate / angular velocity control.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+
+#include <lib/mixer/mixer.h>
+#include <uORB/topics/rate_ctrl_status.h>
+
+class RateControl
+{
+public:
+	RateControl() = default;
+	~RateControl() = default;
+
+	/**
+	 * Set the rate control gains
+	 * @param P 3D vector of proportional gains for body x,y,z axis
+	 * @param I 3D vector of integral gains
+	 * @param D 3D vector of derivative gains
+	 */
+	void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+
+	/**
+	 * Set the mximum absolute value of the integrator for all axes
+	 * @param integrator_limit limit value for all axes x, y, z
+	 */
+	void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
+
+	/**
+	 * Set update frequency and low-pass filter cutoff that is applied to the derivative term
+	 * @param loop_rate [Hz] rate with which update function is called
+	 * @param cutoff [Hz] cutoff frequency for the low-pass filter on the dervative term
+	 * @param force flag to force an expensive update even if the cutoff didn't change
+	 */
+	void setDTermCutoff(const float loop_rate, const float cutoff, const bool force);
+
+	/**
+	 * Set direct rate to torque feed forward gain
+	 * @see _gain_ff
+	 * @param FF 3D vector of feed forward gains for body x,y,z axis
+	 */
+	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
+
+	/**
+	 * Set saturation status
+	 * @param status message from mixer reporting about saturation
+	 */
+	void setSaturationStatus(const MultirotorMixer::saturation_status &status);
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param rate estimation of the current vehicle angular rate
+	 * @param rate_sp desired vehicle angular rate setpoint
+	 * @param dt desired vehicle angular rate setpoint
+	 * @return [-1,1] normalized torque vector to apply to the vehicle
+	 */
+	matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt,
+				const bool landed);
+
+	/**
+	 * Set the integral term to 0 to prevent windup
+	 * @see _rate_int
+	 */
+	void resetIntegral() { _rate_int.zero(); }
+
+	/**
+	 * Get status message of controller for logging/debugging
+	 * @param rate_ctrl_status status message to fill with internal states
+	 */
+	void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
+
+private:
+	void updateIntegral(matrix::Vector3f &rate_error, const float dt);
+
+	// Gains
+	matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z
+	matrix::Vector3f _gain_i; ///< rate control integral gain
+	matrix::Vector3f _gain_d; ///< rate control derivative gain
+	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
+	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
+
+	// States
+	matrix::Vector3f _rate_prev; ///< angular rates of previous update
+	matrix::Vector3f _rate_prev_filtered; ///< low-pass filtered angular rates of previous update
+	matrix::Vector3f _rate_int; ///< integral term of the rate controller
+	math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw)
+	bool _mixer_saturation_positive[3] {};
+	bool _mixer_saturation_negative[3] {};
+};

src/modules/mc_rate_control/RateControl/RateControlTest.cpp

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   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 <RateControl.hpp>
+
+using namespace matrix;
+
+TEST(RateControlTest, AllZeroCase)
+{
+	RateControl rate_control;
+	Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false);
+	EXPECT_EQ(torque, Vector3f());
+}

src/modules/mc_rate_control/mc_rate_control_params.c

@@ -0,0 +1,415 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_rate_control_params.c
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ */
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @max 0.5
+ * @decimal 3
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f);
+
+/**
+ * Roll rate integrator limit
+ *
+ * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 0.01
+ * @decimal 4
+ * @increment 0.0005
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f);
+
+/**
+ * Roll rate feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
+
+/**
+ * Roll rate controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error
+ * 			     + MC_ROLLRATE_I * error_integral
+ * 			     + MC_ROLLRATE_D * error_derivative)
+ * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form.
+ * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @max 0.6
+ * @decimal 3
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f);
+
+/**
+ * Pitch rate integrator limit
+ *
+ * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f);
+
+/**
+ * Pitch rate feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f);
+
+/**
+ * Pitch rate controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error
+ * 			     + MC_PITCHRATE_I * error_integral
+ * 			     + MC_PITCHRATE_D * error_derivative)
+ * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form.
+ * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @max 0.6
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f);
+
+/**
+ * Yaw rate integrator limit
+ *
+ * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+
+/**
+ * Yaw rate feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f);
+
+/**
+ * Yaw rate controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = MC_YAWRATE_K * (MC_YAWRATE_P * error
+ * 			     + MC_YAWRATE_I * error_integral
+ * 			     + MC_YAWRATE_D * error_derivative)
+ * Set MC_YAWRATE_P=1 to implement a PID in the ideal form.
+ * Set MC_YAWRATE_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
+
+/**
+ * Max acro roll rate
+ * default: 2 turns per second
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f);
+
+/**
+ * Max acro pitch rate
+ * default: 2 turns per second
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f);
+
+/**
+ * Max acro yaw rate
+ * default 1.5 turns per second
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f);
+
+/**
+ * Acro mode Expo factor for Roll and Pitch.
+ *
+ * Exponential factor for tuning the input curve shape.
+ *
+ * 0 Purely linear input curve
+ * 1 Purely cubic input curve
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f);
+
+/**
+ * Acro mode Expo factor for Yaw.
+ *
+ * Exponential factor for tuning the input curve shape.
+ *
+ * 0 Purely linear input curve
+ * 1 Purely cubic input curve
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f);
+
+/**
+ * Acro mode SuperExpo factor for Roll and Pitch.
+ *
+ * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO.
+ *
+ * 0 Pure Expo function
+ * 0.7 resonable shape enhancement for intuitive stick feel
+ * 0.95 very strong bent input curve only near maxima have effect
+ *
+ * @min 0
+ * @max 0.95
+ * @decimal 2
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f);
+
+/**
+ * Acro mode SuperExpo factor for Yaw.
+ *
+ * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y.
+ *
+ * 0 Pure Expo function
+ * 0.7 resonable shape enhancement for intuitive stick feel
+ * 0.95 very strong bent input curve only near maxima have effect
+ *
+ * @min 0
+ * @max 0.95
+ * @decimal 2
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f);
+
+/**
+ * Battery power level scaler
+ *
+ * This compensates for voltage drop of the battery over time by attempting to
+ * normalize performance across the operating range of the battery. The copter
+ * should constantly behave as if it was fully charged with reduced max acceleration
+ * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
+ * it will still be 0.5 at 60% battery.
+ *
+ * @boolean
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
+
+/**
+ * Cutoff frequency for the low pass filter on the D-term in the rate controller
+ *
+ * The D-term uses the derivative of the rate and thus is the most susceptible to noise.
+ * Therefore, using a D-term filter allows to decrease the driver-level filtering, which
+ * leads to reduced control latency and permits to increase the P gains.
+ * A value of 0 disables the filter.
+ *
+ * @unit Hz
+ * @min 0
+ * @max 1000
+ * @decimal 0
+ * @increment 10
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);
+