initial control allocation support

- control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See https://github.com/PX4/PX4-Autopilot/pull/13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>

src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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 AngularVelocityControl.cpp
+ */
+
+#include <AngularVelocityControl.hpp>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_p = P;
+	_gain_i = I;
+	_gain_d = D;
+}
+
+void AngularVelocityControl::setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+		const matrix::Vector<bool, 3> &saturation_negative)
+{
+	_saturation_positive = saturation_positive;
+	_saturation_negative = saturation_negative;
+}
+
+void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp,
+				    const Vector3f &angular_acceleration, const float dt, const bool landed)
+{
+	// angular rates error
+	Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity;
+
+	// P + D control
+	_angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration);
+
+	// I + FF control
+	Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp);
+
+	// compute torque setpoint
+	_torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity);
+
+	// update integral only if we are not landed
+	if (!landed) {
+		updateIntegral(angular_velocity_error, dt);
+	}
+}
+
+void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt)
+{
+	for (int i = 0; i < 3; i++) {
+		// prevent further positive control saturation
+		if (_saturation_positive(i)) {
+			angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f);
+		}
+
+		// prevent further negative control saturation
+		if (_saturation_negative(i)) {
+			angular_velocity_error(i) = math::max(angular_velocity_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 = angular_velocity_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 angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt;
+
+		// do not propagate the result if out of range or invalid
+		if (PX4_ISFINITE(angular_velocity_i)) {
+			_angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i));
+		}
+	}
+}
+
+void AngularVelocityControl::reset()
+{
+	_angular_velocity_int.zero();
+	_torque_sp.zero();
+	_angular_accel_sp.zero();
+}

src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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 AngularVelocityControl.hpp
+ *
+ * PID 3 axis angular velocity control.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
+
+class AngularVelocityControl
+{
+public:
+	AngularVelocityControl() = default;
+	~AngularVelocityControl() = default;
+
+	/**
+	 * Set the 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 direct angular velocity setpoint 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 inertia matrix
+	 * @see _inertia
+	 * @param inertia inertia matrix
+	 */
+	void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
+
+	/**
+	 * Set saturation status
+	 * @see _saturation_positive
+	 * @see _saturation_negative
+	 * @param saturation_positive positive saturation
+	 * @param saturation_negative negative saturation
+	 */
+	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+				 const matrix::Vector<bool, 3> &saturation_negative);
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param angular_velocity estimation of the current vehicle angular velocity
+	 * @param angular_velocity_sp desired vehicle angular velocity setpoint
+	 * @param angular_acceleration estimation of the current vehicle angular acceleration
+	 * @param dt elapsed time since last update
+	 * @param landed whether the vehicle is on the ground
+	 */
+	void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp,
+		    const matrix::Vector3f &angular_acceleration, const float dt, const bool landed);
+
+	/**
+	 * Get the desired angular acceleration
+	 * @see _angular_accel_sp
+	 */
+	const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
+
+	/**
+	 * Get the torque vector to apply to the vehicle
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
+
+	/**
+	 * Get the integral term
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getIntegral() {return _angular_velocity_int;};
+
+	/**
+	 * Set the integral term to 0 to prevent windup
+	 * @see _angular_velocity_int
+	 */
+	void resetIntegral() { _angular_velocity_int.zero(); }
+
+	/**
+	 * ReSet the controller state
+	 */
+	void reset();
+
+private:
+	void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt);
+
+	// Gains
+	matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z
+	matrix::Vector3f _gain_i; ///< integral gain
+	matrix::Vector3f _gain_d; ///< derivative gain
+	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
+	matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain
+	matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
+
+	// States
+	matrix::Vector3f _angular_velocity_int;
+	matrix::Vector<bool, 3> _saturation_positive;
+	matrix::Vector<bool, 3> _saturation_negative;
+
+	// Output
+	matrix::Vector3f _angular_accel_sp; 	//< Angular acceleration setpoint computed using P and D gains
+	matrix::Vector3f _torque_sp;		//< Torque setpoint to apply to the vehicle
+};

src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   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 <AngularVelocityControl.hpp>
+
+using namespace matrix;
+
+TEST(AngularVelocityControlTest, AllZeroCase)
+{
+	AngularVelocityControl control;
+	control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
+	Vector3f torque = control.getTorqueSetpoint();
+	EXPECT_EQ(torque, Vector3f());
+}

src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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(AngularVelocityControl
+	AngularVelocityControl.cpp
+)
+target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(AngularVelocityControl PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)

src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -0,0 +1,344 @@
+/****************************************************************************
+ *
+ *   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 "AngularVelocityController.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+#include <ecl/geo/geo.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+AngularVelocityController::AngularVelocityController() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+	parameters_updated();
+}
+
+AngularVelocityController::~AngularVelocityController()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+AngularVelocityController::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_velocity callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+AngularVelocityController::parameters_updated()
+{
+	// 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 k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get());
+
+	_control.setGains(
+		k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())),
+		k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())),
+		k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get())));
+
+	_control.setIntegratorLimit(
+		Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get()));
+
+	_control.setFeedForwardGain(
+		Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get()));
+
+	// inertia matrix
+	const float inertia[3][3] = {
+		{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
+		{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
+		{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
+	};
+	_control.setInertiaMatrix(matrix::Matrix3f(inertia));
+
+	// Hover thrust
+	if (!_param_mpc_use_hte.get()) {
+		_hover_thrust = _param_mpc_thr_hover.get();
+	}
+}
+
+void
+AngularVelocityController::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 vehicle_angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
+		const hrt_abstime now = hrt_absolute_time();
+		_timestamp_sample = vehicle_angular_velocity.timestamp_sample;
+
+		// 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 angular_velocity{vehicle_angular_velocity.xyz};
+
+		/* check for updates in other topics */
+		_vehicle_status_sub.update(&_vehicle_status);
+		_vehicle_control_mode_sub.update(&_vehicle_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;
+			}
+		}
+
+		// Check for updates of hover thrust
+		if (_param_mpc_use_hte.get()) {
+			hover_thrust_estimate_s hte;
+
+			if (_hover_thrust_estimate_sub.update(&hte)) {
+				_hover_thrust = hte.hover_thrust;
+			}
+		}
+
+		// check angular acceleration topic
+		vehicle_angular_acceleration_s vehicle_angular_acceleration;
+
+		if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
+			_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
+		}
+
+		// check rates setpoint topic
+		vehicle_rates_setpoint_s vehicle_rates_setpoint;
+
+		if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
+			_angular_velocity_sp(0) = vehicle_rates_setpoint.roll;
+			_angular_velocity_sp(1) = vehicle_rates_setpoint.pitch;
+			_angular_velocity_sp(2) = vehicle_rates_setpoint.yaw;
+			_thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body);
+
+			// Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G
+			_thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust);
+		}
+
+		// run the controller
+		if (_vehicle_control_mode.flag_control_rates_enabled) {
+			// reset integral if disarmed
+			if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				_control.resetIntegral();
+			}
+
+			// update saturation status from mixer feedback
+			if (_control_allocator_status_sub.updated()) {
+				control_allocator_status_s control_allocator_status;
+
+				if (_control_allocator_status_sub.copy(&control_allocator_status)) {
+					Vector<bool, 3> saturation_positive;
+					Vector<bool, 3> saturation_negative;
+
+					if (not control_allocator_status.torque_setpoint_achieved) {
+						for (size_t i = 0; i < 3; i++) {
+							if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
+								saturation_positive(i) = true;
+
+							} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
+								saturation_negative(i) = true;
+							}
+						}
+					}
+
+					_control.setSaturationStatus(saturation_positive, saturation_negative);
+				}
+			}
+
+			// run rate controller
+			_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);
+
+			// publish rate controller status
+			rate_ctrl_status_s rate_ctrl_status{};
+			Vector3f integral = _control.getIntegral();
+			rate_ctrl_status.timestamp = hrt_absolute_time();
+			rate_ctrl_status.rollspeed_integ = integral(0);
+			rate_ctrl_status.pitchspeed_integ = integral(1);
+			rate_ctrl_status.yawspeed_integ = integral(2);
+			_rate_ctrl_status_pub.publish(rate_ctrl_status);
+
+			// publish controller output
+			publish_angular_acceleration_setpoint();
+			publish_torque_setpoint();
+			publish_thrust_setpoint();
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+void
+AngularVelocityController::publish_angular_acceleration_setpoint()
+{
+	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
+
+	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
+	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
+	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
+	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
+	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
+
+	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
+}
+
+void
+AngularVelocityController::publish_torque_setpoint()
+{
+	Vector3f torque_sp = _control.getTorqueSetpoint();
+
+	vehicle_torque_setpoint_s v_torque_sp = {};
+	v_torque_sp.timestamp = hrt_absolute_time();
+	v_torque_sp.timestamp_sample = _timestamp_sample;
+	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
+	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
+	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
+
+	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+}
+
+void
+AngularVelocityController::publish_thrust_setpoint()
+{
+	vehicle_thrust_setpoint_s v_thrust_sp = {};
+	v_thrust_sp.timestamp = hrt_absolute_time();
+	v_thrust_sp.timestamp_sample = _timestamp_sample;
+	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
+	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
+	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
+
+	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+}
+
+int AngularVelocityController::task_spawn(int argc, char *argv[])
+{
+	AngularVelocityController *instance = new AngularVelocityController();
+
+	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 AngularVelocityController::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int AngularVelocityController::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AngularVelocityController::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the angular velocity controller.
+It takes angular velocity setpoints and measured angular
+velocity as inputs and outputs actuator setpoints.
+
+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;
+}
+
+/**
+ * Angular velocity controller app start / stop handling function
+ */
+extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]);
+
+int angular_velocity_controller_main(int argc, char *argv[])
+{
+	return AngularVelocityController::main(argc, argv);
+}

src/modules/angular_velocity_controller/AngularVelocityController.hpp

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   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 <AngularVelocityControl.hpp>
+
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#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/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/hover_thrust_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.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>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+
+
+class AngularVelocityController : public ModuleBase<AngularVelocityController>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	AngularVelocityController();
+
+	virtual ~AngularVelocityController();
+
+	/** @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();
+
+	void		publish_angular_acceleration_setpoint();
+	void		publish_torque_setpoint();
+	void		publish_thrust_setpoint();
+	void		publish_actuator_controls();
+
+	AngularVelocityControl _control; 	///< class for control calculations
+
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};				/**< motor limits subscription */
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};				/**< parameter updates subscription */
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; 	/**< vehicle angular acceleration subscription */
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};			/**< vehicle control mode subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};			/**< vehicle land detected subscription */
+	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};			/**< vehicle rates setpoint subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};					/**< vehicle status subscription */
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::Publication<rate_ctrl_status_s>			   _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};						/**< controller status publication */
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};	/**< angular acceleration setpoint publication */
+	uORB::Publication<vehicle_thrust_setpoint_s>		   _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};				/**< thrust setpoint publication */
+	uORB::Publication<vehicle_torque_setpoint_s>		   _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};				/**< torque setpoint publication */
+
+	vehicle_control_mode_s		_vehicle_control_mode{};
+	vehicle_status_s		_vehicle_status{};
+
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	float _battery_status_scale{0.0f};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	matrix::Vector3f _angular_velocity_sp;		/**< angular velocity setpoint */
+	matrix::Vector3f _angular_acceleration;		/**< angular acceleration (estimated) */
+	matrix::Vector3f _thrust_sp;			/**< thrust setpoint */
+
+	float _hover_thrust{0.5f};			/**< Normalized hover thrust **/
+
+	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};
+	hrt_abstime _timestamp_sample{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::AVC_X_P>) _param_avc_x_p,
+		(ParamFloat<px4::params::AVC_X_I>) _param_avc_x_i,
+		(ParamFloat<px4::params::AVC_X_I_LIM>) _param_avc_x_i_lim,
+		(ParamFloat<px4::params::AVC_X_D>) _param_avc_x_d,
+		(ParamFloat<px4::params::AVC_X_FF>) _param_avc_x_ff,
+		(ParamFloat<px4::params::AVC_X_K>) _param_avc_x_k,
+
+		(ParamFloat<px4::params::AVC_Y_P>) _param_avc_y_p,
+		(ParamFloat<px4::params::AVC_Y_I>) _param_avc_y_i,
+		(ParamFloat<px4::params::AVC_Y_I_LIM>) _param_avc_y_i_lim,
+		(ParamFloat<px4::params::AVC_Y_D>) _param_avc_y_d,
+		(ParamFloat<px4::params::AVC_Y_FF>) _param_avc_y_ff,
+		(ParamFloat<px4::params::AVC_Y_K>) _param_avc_y_k,
+
+		(ParamFloat<px4::params::AVC_Z_P>) _param_avc_z_p,
+		(ParamFloat<px4::params::AVC_Z_I>) _param_avc_z_i,
+		(ParamFloat<px4::params::AVC_Z_I_LIM>) _param_avc_z_i_lim,
+		(ParamFloat<px4::params::AVC_Z_D>) _param_avc_z_d,
+		(ParamFloat<px4::params::AVC_Z_FF>) _param_avc_z_ff,
+		(ParamFloat<px4::params::AVC_Z_K>) _param_avc_z_k,
+
+		(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,
+		(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,
+		(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
+		(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
+		(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
+		(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
+		(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
+
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
+		(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte
+	)
+
+};

src/modules/angular_velocity_controller/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   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(AngularVelocityControl)
+
+px4_add_module(
+	MODULE modules__angular_velocity_control
+	MAIN angular_velocity_controller
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		AngularVelocityController.cpp
+		AngularVelocityController.hpp
+	DEPENDS
+		circuit_breaker
+		mathlib
+		AngularVelocityControl
+		px4_work_queue
+	)

src/modules/angular_velocity_controller/angular_velocity_controller_params.c

@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   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 angular_velocity_controller_params.c
+ * Parameters for angular velocity controller.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Body X axis angular velocity P gain
+ *
+ * Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_P, 18.f);
+
+/**
+ * Body X axis angular velocity I gain
+ *
+ * Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f);
+
+/**
+ * Body X axis angular velocity integrator limit
+ *
+ * Body X axis angular velocity 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.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f);
+
+/**
+ * Body X axis angular velocity D gain
+ *
+ * Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f);
+
+/**
+ * Body X axis angular velocity feedforward gain
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f);
+
+/**
+ * Body X axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_X_K * (AVC_X_P * error
+ * 			+ AVC_X_I * error_integral
+ * 			+ AVC_X_D * error_derivative)
+ * Set AVC_X_P=1 to implement a PID in the ideal form.
+ * Set AVC_X_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f);
+
+/**
+ * Body Y axis angular velocity P gain
+ *
+ * Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f);
+
+/**
+ * Body Y axis angular velocity I gain
+ *
+ * Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f);
+
+/**
+ * Body Y axis angular velocity integrator limit
+ *
+ * Body Y axis angular velocity 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.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f);
+
+/**
+ * Body Y axis angular velocity D gain
+ *
+ * Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f);
+
+/**
+ * Body Y axis angular velocity feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f);
+
+/**
+ * Body Y axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_Y_K * (AVC_Y_P * error
+ * 			     + AVC_Y_I * error_integral
+ * 			     + AVC_Y_D * error_derivative)
+ * Set AVC_Y_P=1 to implement a PID in the ideal form.
+ * Set AVC_Y_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 20.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f);
+
+/**
+ * Body Z axis angular velocity P gain
+ *
+ * Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f);
+
+/**
+ * Body Z axis angular velocity I gain
+ *
+ * Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f);
+
+/**
+ * Body Z axis angular velocity integrator limit
+ *
+ * Body Z axis angular velocity 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.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f);
+
+/**
+ * Body Z axis angular velocity D gain
+ *
+ * Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f);
+
+/**
+ * Body Z axis angular velocity feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f);
+
+/**
+ * Body Z axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_Z_K * (AVC_Z_P * error
+ * 			     + AVC_Z_I * error_integral
+ * 			     + AVC_Z_D * error_derivative)
+ * Set AVC_Z_P=1 to implement a PID in the ideal form.
+ * Set AVC_Z_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f);

src/modules/angular_velocity_controller/vehicle_model_params.c

@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_model_params.c
+ * Parameters for vehicle model.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Mass
+ *
+ * @unit kg
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
+
+/**
+ * Inertia matrix, XX component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f);
+
+/**
+ * Inertia matrix, YY component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f);
+
+/**
+ * Inertia matrix, ZZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f);
+
+/**
+ * Inertia matrix, XY component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
+
+/**
+ * Inertia matrix, XZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
+
+/**
+ * Inertia matrix, YZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);