WIP add attitude setpoints

Add attitude control
Add attitude controls
Replace ecl controls

Fix
Fix

src/modules/fw_att_control/AttitudeControl/AttitudeControl.cpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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 AttitudeControl.cpp
+ */
+
+#include <AttitudeControl.hpp>
+
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight)
+{
+	_proportional_gain = proportional_gain;
+	_yaw_w = math::constrain(yaw_weight, 0.f, 1.f);
+
+	// compensate for the effect of the yaw weight rescaling the output
+	if (_yaw_w > 1e-4f) {
+		_proportional_gain(2) /= _yaw_w;
+	}
+}
+
+matrix::Vector3f AttitudeControl::update(const Quatf &q) const
+{
+	Quatf qd = _attitude_setpoint_q;
+
+	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
+	const Vector3f e_z = q.dcm_z();
+	const Vector3f e_z_d = qd.dcm_z();
+	Quatf qd_red(e_z, e_z_d);
+
+	if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
+		// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
+		// full attitude control anyways generates no yaw input and directly takes the combination of
+		// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
+		qd_red = qd;
+
+	} else {
+		// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
+		qd_red *= q;
+	}
+
+	// mix full and reduced desired attitude
+	Quatf q_mix = qd_red.inversed() * qd;
+	q_mix.canonicalize();
+	// catch numerical problems with the domain of acosf and asinf
+	q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
+	q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
+	qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));
+
+	// quaternion attitude control law, qe is rotation from q to qd
+	const Quatf qe = q.inversed() * qd;
+
+	// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
+	// also taking care of the antipodal unit quaternion ambiguity
+	const Vector3f eq = 2.f * qe.canonical().imag();
+
+	// calculate angular rates setpoint
+	matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);
+
+	// Feed forward the yaw setpoint rate.
+	// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
+	// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
+	// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
+	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
+	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
+	// such that it can be added to the rates setpoint.
+	if (is_finite(_yawspeed_setpoint)) {
+		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
+	}
+
+	// limit rates
+	for (int i = 0; i < 3; i++) {
+		rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
+	}
+
+	return rate_setpoint;
+}

src/modules/fw_att_control/AttitudeControl/AttitudeControl.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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 AttitudeControl.hpp
+ *
+ * A quaternion based attitude controller.
+ *
+ * @author Matthias Grob	<maetugr@gmail.com>
+ *
+ * Publication documenting the implemented Quaternion Attitude Control:
+ * Nonlinear Quadrocopter Attitude Control (2013)
+ * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea
+ * Institute for Dynamic Systems and Control (IDSC), ETH Zurich
+ *
+ * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/math/Limits.hpp>
+
+class AttitudeControl
+{
+public:
+	AttitudeControl() = default;
+	~AttitudeControl() = default;
+
+	/**
+	 * Set proportional attitude control gain
+	 * @param proportional_gain 3D vector containing gains for roll, pitch, yaw
+	 * @param yaw_weight A fraction [0,1] deprioritizing yaw compared to roll and pitch
+	 */
+	void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight);
+
+	/**
+	 * Set hard limit for output rate setpoints
+	 * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw
+	 */
+	void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; }
+
+	/**
+	 * Set a new attitude setpoint replacing the one tracked before
+	 * @param qd desired vehicle attitude setpoint
+	 * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame
+	 */
+	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint)
+	{
+		_attitude_setpoint_q = qd;
+		_attitude_setpoint_q.normalize();
+		_yawspeed_setpoint = yawspeed_setpoint;
+	}
+
+	/**
+	 * Adjust last known attitude setpoint by a delta rotation
+	 * Optional use to avoid glitches when attitude estimate reference e.g. heading changes.
+	 * @param q_delta delta rotation to apply
+	 */
+	void adaptAttitudeSetpoint(const matrix::Quatf &q_delta)
+	{
+		_attitude_setpoint_q = q_delta * _attitude_setpoint_q;
+		_attitude_setpoint_q.normalize();
+	}
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param q estimation of the current vehicle attitude unit quaternion
+	 * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller
+	 */
+	matrix::Vector3f update(const matrix::Quatf &q) const;
+
+private:
+	matrix::Vector3f _proportional_gain;
+	matrix::Vector3f _rate_limit;
+	float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize caompared to roll and pitch
+
+	matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control
+	float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint
+};

src/modules/fw_att_control/AttitudeControl/AttitudeControlTest.cpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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 <AttitudeControl.hpp>
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+TEST(AttitudeControlTest, AllZeroCase)
+{
+	AttitudeControl attitude_control;
+	Vector3f rate_setpoint = attitude_control.update(Quatf());
+	EXPECT_EQ(rate_setpoint, Vector3f());
+}
+
+class AttitudeControlConvergenceTest : public ::testing::Test
+{
+public:
+	AttitudeControlConvergenceTest()
+	{
+		_attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f), .4f);
+		_attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
+	}
+
+	void checkConvergence()
+	{
+		int i; // need function scope to check how many steps
+		Vector3f rate_setpoint(1000.f, 1000.f, 1000.f);
+
+		_attitude_control.setAttitudeSetpoint(_quat_goal, 0.f);
+
+		for (i = 100; i > 0; i--) {
+			// run attitude control to get rate setpoints
+			const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state);
+			// rotate the simulated state quaternion according to the rate setpoint
+			_quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new));
+			_quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem
+
+			// expect the error and hence also the output to get smaller with each iteration
+			if (rate_setpoint_new.norm() >= rate_setpoint.norm()) {
+				break;
+			}
+
+			rate_setpoint = rate_setpoint_new;
+		}
+
+		EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical());
+		// it shouldn't have taken longer than an iteration timeout to converge
+		EXPECT_GT(i, 0);
+	}
+
+	AttitudeControl _attitude_control;
+	Quatf _quat_state;
+	Quatf _quat_goal;
+};
+
+TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
+{
+	const int inputs = 8;
+
+	const Quatf QArray[inputs] = {
+		Quatf(),
+		Quatf(0, 1, 0, 0),
+		Quatf(0, 0, 1, 0),
+		Quatf(0, 0, 0, 1),
+		Quatf(0.698f, 0.024f, -0.681f, -0.220f),
+		Quatf(-0.820f, -0.313f, 0.225f, -0.423f),
+		Quatf(0.599f, -0.172f, 0.755f, -0.204f),
+		Quatf(0.216f, -0.662f, 0.290f, -0.656f)
+	};
+
+	for (int i = 0; i < inputs; i++) {
+		for (int j = 0; j < inputs; j++) {
+			printf("--- Input combination: %d %d\n", i, j);
+			_quat_state = QArray[i];
+			_quat_goal = QArray[j];
+			_quat_state.normalize();
+			_quat_goal.normalize();
+			checkConvergence();
+		}
+	}
+}
+
+TEST(AttitudeControlTest, YawWeightScaling)
+{
+	// GIVEN: default tuning and pure yaw turn command
+	AttitudeControl attitude_control;
+	const float yaw_gain = 2.8f;
+	const float yaw_sp = .1f;
+	Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f));
+	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f);
+	attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f));
+	attitude_control.setAttitudeSetpoint(pure_yaw_attitude, 0.f);
+
+	// WHEN: we run one iteration of the controller
+	Vector3f rate_setpoint = attitude_control.update(Quatf());
+
+	// THEN: no actuation in roll, pitch
+	EXPECT_EQ(Vector2f(rate_setpoint), Vector2f());
+	// THEN: actuation error * gain in yaw
+	EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f);
+
+	// GIVEN: additional corner case of zero yaw weight
+	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f);
+	// WHEN: we run one iteration of the controller
+	rate_setpoint = attitude_control.update(Quatf());
+	// THEN: no actuation (also no NAN)
+	EXPECT_EQ(rate_setpoint, Vector3f());
+}

src/modules/fw_att_control/AttitudeControl/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(FWAttitudeControl
+	AttitudeControl.cpp
+	AttitudeControl.hpp
+)
+target_compile_options(FWAttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(FWAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS FWAttitudeControl)

src/modules/fw_att_control/CMakeLists.txt

@@ -30,6 +30,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
+add_subdirectory(RateControl)
+add_subdirectory(AttitudeControl)
+
 px4_add_module(
 	MODULE modules__fw_att_control
 	MAIN fw_att_control
@@ -38,11 +42,10 @@ px4_add_module(
 		FixedwingAttitudeControl.hpp
 
 		ecl_controller.cpp
-		ecl_pitch_controller.cpp
-		ecl_roll_controller.cpp
 		ecl_wheel_controller.cpp
-		ecl_yaw_controller.cpp
 	DEPENDS
 		px4_work_queue
+		FWRateControl
+		FWAttitudeControl
 		SlewRate
 	)

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -34,6 +34,8 @@
 #include "FixedwingAttitudeControl.hpp"
 
 using namespace time_literals;
+using namespace matrix;
+
 using math::constrain;
 using math::gradual;
 using math::radians;
@@ -50,10 +52,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	parameters_update();
 
 	// set initial maximum body rate setpoints
-	_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
-	_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
-	_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
-	_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
+	// _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
+	// _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
+	// _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
+	// _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
 
 	_rate_ctrl_status_pub.advertise();
 	_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
@@ -79,26 +81,6 @@ FixedwingAttitudeControl::init()
 int
 FixedwingAttitudeControl::parameters_update()
 {
-	/* pitch control parameters */
-	_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
-	_pitch_ctrl.set_k_p(_param_fw_pr_p.get());
-	_pitch_ctrl.set_k_i(_param_fw_pr_i.get());
-	_pitch_ctrl.set_k_ff(_param_fw_pr_ff.get());
-	_pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get());
-
-	/* roll control parameters */
-	_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
-	_roll_ctrl.set_k_p(_param_fw_rr_p.get());
-	_roll_ctrl.set_k_i(_param_fw_rr_i.get());
-	_roll_ctrl.set_k_ff(_param_fw_rr_ff.get());
-	_roll_ctrl.set_integrator_max(_param_fw_rr_imax.get());
-
-	/* yaw control parameters */
-	_yaw_ctrl.set_k_p(_param_fw_yr_p.get());
-	_yaw_ctrl.set_k_i(_param_fw_yr_i.get());
-	_yaw_ctrl.set_k_ff(_param_fw_yr_ff.get());
-	_yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get());
-
 	/* wheel control parameters */
 	_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
 	_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
@@ -106,6 +88,19 @@ FixedwingAttitudeControl::parameters_update()
 	_wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get());
 	_wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get()));
 
+	const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get());
+	const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get());
+	const Vector3f rate_d = Vector3f(0.0f, 0.0f, 0.0f);
+
+	_rate_control.setGains(rate_p, rate_i, rate_d);
+
+	_rate_control.setIntegratorLimit(
+		Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));
+
+	_rate_control.setFeedForwardGain(
+		Vector3f(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()));
+
+
 	return PX4_OK;
 }
 
@@ -343,6 +338,9 @@ void FixedwingAttitudeControl::Run()
 			yawspeed = helper;
 		}
 
+
+		const Vector3f rates(rollspeed, pitchspeed, yawspeed);
+
 		const matrix::Eulerf euler_angles(R);
 
 		vehicle_attitude_setpoint_poll();
@@ -388,16 +386,16 @@ void FixedwingAttitudeControl::Run()
 			const float airspeed = get_airspeed_and_update_scaling();
 
 			/* reset integrals where needed */
-			if (_att_sp.roll_reset_integral) {
-				_roll_ctrl.reset_integrator();
-			}
+			// if (_att_sp.roll_reset_integral) {
+			// 	_roll_ctrl.reset_integrator();
+			// }
 
-			if (_att_sp.pitch_reset_integral) {
-				_pitch_ctrl.reset_integrator();
-			}
+			// if (_att_sp.pitch_reset_integral) {
+			// 	_pitch_ctrl.reset_integrator();
+			// }
 
 			if (_att_sp.yaw_reset_integral) {
-				_yaw_ctrl.reset_integrator();
+				// _yaw_ctrl.reset_integrator();
 				_wheel_ctrl.reset_integrator();
 			}
 
@@ -408,9 +406,9 @@ void FixedwingAttitudeControl::Run()
 			    || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
 
-				_roll_ctrl.reset_integrator();
-				_pitch_ctrl.reset_integrator();
-				_yaw_ctrl.reset_integrator();
+				// _roll_ctrl.reset_integrator();
+				// _pitch_ctrl.reset_integrator();
+				// _yaw_ctrl.reset_integrator();
 				_wheel_ctrl.reset_integrator();
 			}
 
@@ -454,16 +452,17 @@ void FixedwingAttitudeControl::Run()
 			if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
 				if (_vcontrol_mode.flag_control_attitude_enabled
 				    || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-					_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
-					_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
-					_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
-					_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
+					using math::radians;
+					// _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
+					// _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
+					// _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
+					// _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
 
 				} else {
-					_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
-					_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
-					_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
-					_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
+					// _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
+					// _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
+					// _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
+					// _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
 				}
 			}
 
@@ -499,26 +498,22 @@ void FixedwingAttitudeControl::Run()
 			trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get();
 
 			/* Run attitude controllers */
+			Vector3f rates_setpoint;
+
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
-					_roll_ctrl.control_attitude(dt, control_input);
-					_pitch_ctrl.control_attitude(dt, control_input);
+					/* Run ATTITUDE controller */
+					_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
+					rates_setpoint = _attitude_control.update(Quatf(att.q));
 
 					if (wheel_control) {
 						_wheel_ctrl.control_attitude(dt, control_input);
-						_yaw_ctrl.reset_integrator();
 
 					} else {
 						// runs last, because is depending on output of roll and pitch attitude
-						_yaw_ctrl.control_attitude(dt, control_input);
 						_wheel_ctrl.reset_integrator();
 					}
 
-					/* Update input data for rate controllers */
-					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
-					control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
-					control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
-
 					const hrt_abstime now = hrt_absolute_time();
 					autotune_attitude_control_status_s pid_autotune;
 					matrix::Vector3f bodyrate_ff;
@@ -534,22 +529,28 @@ void FixedwingAttitudeControl::Run()
 						}
 					}
 
+					vehicle_angular_acceleration_s v_angular_acceleration{};
+					// _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration);
+					const Vector3f angular_accel{v_angular_acceleration.xyz};
+
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
-					float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0));
+					const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
+
+					float roll_u = att_control(0);
 					_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
 						(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
 
-					if (!PX4_ISFINITE(roll_u)) {
-						_roll_ctrl.reset_integrator();
-					}
+					// if (!PX4_ISFINITE(roll_u)) {
+					// 	_roll_ctrl.reset_integrator();
+					// }
 
-					float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
+					float pitch_u = att_control(1);
 					_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
 						(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
 
-					if (!PX4_ISFINITE(pitch_u)) {
-						_pitch_ctrl.reset_integrator();
-					}
+					// if (!PX4_ISFINITE(pitch_u)) {
+					// 	_pitch_ctrl.reset_integrator();
+					// }
 
 					float yaw_u = 0.0f;
 
@@ -557,7 +558,7 @@ void FixedwingAttitudeControl::Run()
 						yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
 
 					} else {
-						yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
+						yaw_u = att_control(2);
 					}
 
 					_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
@@ -568,7 +569,7 @@ void FixedwingAttitudeControl::Run()
 					}
 
 					if (!PX4_ISFINITE(yaw_u)) {
-						_yaw_ctrl.reset_integrator();
+						// _yaw_ctrl.reset_integrator();
 						_wheel_ctrl.reset_integrator();
 					}
 
@@ -596,9 +597,9 @@ void FixedwingAttitudeControl::Run()
 				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
 				 * only once available
 				 */
-				_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
-				_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
-				_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
+				_rates_sp.roll = rates_setpoint(0);
+				_rates_sp.pitch = rates_setpoint(1);
+				_rates_sp.yaw = rates_setpoint(2);
 
 				_rates_sp.timestamp = hrt_absolute_time();
 
@@ -607,19 +608,20 @@ void FixedwingAttitudeControl::Run()
 			} else {
 				vehicle_rates_setpoint_poll();
 
-				_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
-				_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
-				_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
+				///TODO: Link angular acceleration to
+				vehicle_angular_acceleration_s v_angular_acceleration{};
+				// _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration);
+				const Vector3f angular_accel{v_angular_acceleration.xyz};
 
-				float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
-				_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
+				rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
+				const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
 
-				float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
-				_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch :
-						trim_pitch;
-
-				float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
-				_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
+				_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(att_control(0))) ? att_control(
+							0) + trim_roll : trim_roll;
+				_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(att_control(1))) ? att_control(
+							1) + trim_pitch : trim_pitch;
+				_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(att_control(2))) ? att_control(
+							2) + trim_yaw : trim_yaw;
 
 				_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
 						_rates_sp.thrust_body[0] : 0.0f;
@@ -627,14 +629,14 @@ void FixedwingAttitudeControl::Run()
 
 			rate_ctrl_status_s rate_ctrl_status{};
 			rate_ctrl_status.timestamp = hrt_absolute_time();
-			rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
-			rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
+			// rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
+			// rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
 
 			if (wheel_control) {
 				rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
 
 			} else {
-				rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
+				// rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
 			}
 
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -31,11 +31,13 @@
  *
  ****************************************************************************/
 
+#pragma once
+
+#include <RateControl.hpp>
+#include <AttitudeControl.hpp>
+
 #include <drivers/drv_hrt.h>
-#include "ecl_pitch_controller.h"
-#include "ecl_roll_controller.h"
 #include "ecl_wheel_controller.h"
-#include "ecl_yaw_controller.h"
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
@@ -61,6 +63,7 @@
 #include <uORB/topics/manual_control_setpoint.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_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -229,10 +232,9 @@ private:
 		(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw
 	)
 
-	ECL_RollController		_roll_ctrl;
-	ECL_PitchController		_pitch_ctrl;
-	ECL_YawController		_yaw_ctrl;
 	ECL_WheelController		_wheel_ctrl;
+	RateControl _rate_control; ///< class for rate control calculations
+	AttitudeControl _attitude_control; /**< class for attitude control calculations */
 
 	/**
 	 * @brief Update flap control setting

src/modules/fw_att_control/RateControl/CMakeLists.txt

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

src/modules/fw_att_control/RateControl/RateControl.cpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
+				      const Vector<bool, 3> &saturation_negative)
+{
+	_control_allocator_saturation_positive = saturation_positive;
+	_control_allocator_saturation_negative = saturation_negative;
+}
+
+Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
+			     const float dt, const bool landed)
+{
+	// angular rates error
+	Vector3f rate_error = rate_sp - rate;
+
+	// PID control with feed forward
+	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
+
+	// 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 (_control_allocator_saturation_positive(i)) {
+			rate_error(i) = math::min(rate_error(i), 0.f);
+		}
+
+		// prevent further negative control saturation
+		if (_control_allocator_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/fw_att_control/RateControl/RateControl.hpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   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/mathlib.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 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 control saturation vector from control allocator
+	 */
+	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+				 const matrix::Vector<bool, 3> &saturation_negative);
+
+	/**
+	 * 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 matrix::Vector3f &angular_accel, 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_int; ///< integral term of the rate controller
+
+	// Feedback from control allocation
+	matrix::Vector<bool, 3> _control_allocator_saturation_negative;
+	matrix::Vector<bool, 3> _control_allocator_saturation_positive;
+};

src/modules/fw_att_control/RateControl/RateControlTest.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   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
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 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.
  *
@@ -31,36 +31,14 @@
  *
  ****************************************************************************/
 
-/**
- * @file ecl_roll_controller.h
- * Definition of a simple orthogonal roll PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
- */
+#include <gtest/gtest.h>
+#include <RateControl.hpp>
 
-#ifndef ECL_ROLL_CONTROLLER_H
-#define ECL_ROLL_CONTROLLER_H
+using namespace matrix;
 
-#include "ecl_controller.h"
-
-class ECL_RollController :
-	public ECL_Controller
+TEST(RateControlTest, AllZeroCase)
 {
-public:
-	ECL_RollController() = default;
-	~ECL_RollController() = default;
-
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
-	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
-};
-
-#endif // ECL_ROLL_CONTROLLER_H
+	RateControl rate_control;
+	Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
+	EXPECT_EQ(torque, Vector3f());
+}

src/modules/fw_att_control/ecl_pitch_controller.cpp

@@ -1,123 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp
- * Implementation of a simple orthogonal pitch PID controller.
- *
- * Authors and acknowledgements in header.
- */
-
-#include "ecl_pitch_controller.h"
-#include <float.h>
-#include <lib/geo/geo.h>
-#include <mathlib/mathlib.h>
-
-float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
-	      PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.airspeed))) {
-
-		return _rate_setpoint;
-	}
-
-	/* Calculate the error */
-	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
-
-	/*  Apply P controller: rate setpoint from current error and time constant */
-	_rate_setpoint =  pitch_error / _tc;
-
-	return _rate_setpoint;
-}
-
-float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.body_y_rate) &&
-	      PX4_ISFINITE(ctl_data.body_z_rate) &&
-	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.airspeed_min) &&
-	      PX4_ISFINITE(ctl_data.airspeed_max) &&
-	      PX4_ISFINITE(ctl_data.scaler))) {
-
-		return math::constrain(_last_output, -1.0f, 1.0f);
-	}
-
-	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
-
-	if (!ctl_data.lock_integrator && _k_i > 0.0f) {
-
-		/* Integral term scales with 1/IAS^2 */
-		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
-
-		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
-		if (_last_output < -1.0f) {
-			/* only allow motion to center: increase value */
-			id = math::max(id, 0.0f);
-
-		} else if (_last_output > 1.0f) {
-			/* only allow motion to center: decrease value */
-			id = math::min(id, 0.0f);
-		}
-
-		/* add and constrain */
-		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
-	}
-
-	/* Apply PI rate controller and store non-limited output */
-	/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
-	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
-		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
-		       + _integrator;
-
-	return math::constrain(_last_output, -1.0f, 1.0f);
-}
-
-float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
-{
-	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
-			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
-
-	set_bodyrate_setpoint(_bodyrate_setpoint);
-
-	return control_bodyrate(dt, ctl_data);
-}

src/modules/fw_att_control/ecl_pitch_controller.h

@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h
- * Definition of a simple orthogonal pitch PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
- */
-
-#ifndef ECL_PITCH_CONTROLLER_H
-#define ECL_PITCH_CONTROLLER_H
-
-#include <mathlib/mathlib.h>
-
-#include "ecl_controller.h"
-
-class ECL_PitchController :
-	public ECL_Controller
-{
-public:
-	ECL_PitchController() = default;
-	~ECL_PitchController() = default;
-
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
-	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
-
-	/* Additional Setters */
-	void set_max_rate_pos(float max_rate_pos)
-	{
-		_max_rate = max_rate_pos;
-	}
-
-	void set_max_rate_neg(float max_rate_neg)
-	{
-		_max_rate_neg = max_rate_neg;
-	}
-
-	void set_bodyrate_setpoint(float rate)
-	{
-		_bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
-	}
-
-protected:
-	float _max_rate_neg{0.0f};
-};
-
-#endif // ECL_PITCH_CONTROLLER_H

src/modules/fw_att_control/ecl_roll_controller.cpp

@@ -1,119 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp
- * Implementation of a simple orthogonal roll PID controller.
- *
- * Authors and acknowledgements in header.
- */
-
-#include "ecl_roll_controller.h"
-#include <float.h>
-#include <lib/geo/geo.h>
-#include <mathlib/mathlib.h>
-
-float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
-	      PX4_ISFINITE(ctl_data.roll))) {
-
-		return _rate_setpoint;
-	}
-
-	/* Calculate the error */
-	float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
-
-	/*  Apply P controller: rate setpoint from current error and time constant */
-	_rate_setpoint = roll_error / _tc;
-
-	return _rate_setpoint;
-}
-
-float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.body_x_rate) &&
-	      PX4_ISFINITE(ctl_data.body_z_rate) &&
-	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.airspeed_min) &&
-	      PX4_ISFINITE(ctl_data.airspeed_max) &&
-	      PX4_ISFINITE(ctl_data.scaler))) {
-
-		return math::constrain(_last_output, -1.0f, 1.0f);
-	}
-
-	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;
-
-	if (!ctl_data.lock_integrator && _k_i > 0.0f) {
-
-		/* Integral term scales with 1/IAS^2 */
-		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
-
-		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
-		if (_last_output < -1.0f) {
-			/* only allow motion to center: increase value */
-			id = math::max(id, 0.0f);
-
-		} else if (_last_output > 1.0f) {
-			/* only allow motion to center: decrease value */
-			id = math::min(id, 0.0f);
-		}
-
-		/* add and constrain */
-		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
-	}
-
-	/* Apply PI rate controller and store non-limited output */
-	/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
-	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
-		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
-		       + _integrator;
-
-	return math::constrain(_last_output, -1.0f, 1.0f);
-}
-
-float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
-{
-	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
-
-	set_bodyrate_setpoint(_bodyrate_setpoint);
-
-	return control_bodyrate(dt, ctl_data);
-}

src/modules/fw_att_control/ecl_yaw_controller.cpp

@@ -1,154 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp
- * Implementation of a simple orthogonal coordinated turn yaw PID controller.
- *
- * Authors and acknowledgements in header.
- */
-
-#include "ecl_yaw_controller.h"
-#include <float.h>
-#include <lib/geo/geo.h>
-#include <mathlib/mathlib.h>
-
-float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
-
-		return _rate_setpoint;
-	}
-
-	float constrained_roll;
-	bool inverted = false;
-
-	/* roll is used as feedforward term and inverted flight needs to be considered */
-	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
-		/* not inverted, but numerically still potentially close to infinity */
-		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
-
-	} else {
-		inverted = true;
-
-		// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
-		//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
-		if (ctl_data.roll > 0.0f) {
-			/* right hemisphere */
-			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
-
-		} else {
-			/* left hemisphere */
-			constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f));
-		}
-	}
-
-	constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint));
-
-
-	if (!inverted) {
-		/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
-		_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
-				 ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
-	}
-
-	if (!PX4_ISFINITE(_rate_setpoint)) {
-		PX4_WARN("yaw rate sepoint not finite");
-		_rate_setpoint = 0.0f;
-	}
-
-	return _rate_setpoint;
-}
-
-float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.body_y_rate) &&
-	      PX4_ISFINITE(ctl_data.body_z_rate) &&
-	      PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.airspeed_min) &&
-	      PX4_ISFINITE(ctl_data.airspeed_max) &&
-	      PX4_ISFINITE(ctl_data.scaler))) {
-
-		return math::constrain(_last_output, -1.0f, 1.0f);
-	}
-
-	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;
-
-	if (!ctl_data.lock_integrator && _k_i > 0.0f) {
-
-		/* Integral term scales with 1/IAS^2 */
-		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
-
-		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
-		if (_last_output < -1.0f) {
-			/* only allow motion to center: increase value */
-			id = math::max(id, 0.0f);
-
-		} else if (_last_output > 1.0f) {
-			/* only allow motion to center: decrease value */
-			id = math::min(id, 0.0f);
-		}
-
-		/* add and constrain */
-		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
-	}
-
-	/* Apply PI rate controller and store non-limited output */
-	/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
-	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
-		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
-		       + _integrator;
-
-	return math::constrain(_last_output, -1.0f, 1.0f);
-}
-
-float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
-{
-	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
-			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff;
-
-	set_bodyrate_setpoint(_bodyrate_setpoint);
-
-	return control_bodyrate(dt, ctl_data);
-}

src/modules/fw_att_control/ecl_yaw_controller.h

@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h
- * Definition of a simple orthogonal coordinated turn yaw PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
- */
-
-#ifndef ECL_YAW_CONTROLLER_H
-#define ECL_YAW_CONTROLLER_H
-
-#include "ecl_controller.h"
-
-class ECL_YawController :
-	public ECL_Controller
-{
-public:
-	ECL_YawController() = default;
-	~ECL_YawController() = default;
-
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
-	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
-
-protected:
-	float _max_rate{0.0f};
-
-};
-
-#endif // ECL_YAW_CONTROLLER_H