Replace rate controller with RateControlLibrary

This commit makes the fw attitude controller take share the rate controller as a library with the mc_rate_control module

src/lib/CMakeLists.txt

@@ -59,6 +59,7 @@ add_subdirectory(perf)
 add_subdirectory(pid)
 add_subdirectory(pid_design)
 add_subdirectory(pwm)
+add_subdirectory(rate_control)
 add_subdirectory(rc)
 add_subdirectory(sensor_calibration)
 add_subdirectory(slew_rate)

src/lib/rate_control/CMakeLists.txt

@@ -32,11 +32,11 @@
 ############################################################################
 
 px4_add_library(RateControl
-	RateControl.cpp
+	rate_control.cpp
-	RateControl.hpp
+	rate_control.hpp
 )
 target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(RateControl PRIVATE mathlib)
 
-px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl)
+px4_add_unit_gtest(SRC rate_control_test.cpp LINKLIBS RateControl)

src/lib/rate_control/rate_control.cpp

@@ -35,7 +35,7 @@
  * @file RateControl.cpp
  */
 
-#include <RateControl.hpp>
+#include "rate_control.hpp"
 #include <px4_platform_common/defines.h>
 
 using namespace matrix;

src/lib/rate_control/rate_control.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file RateControl.hpp
+ * @file rate_control.hpp
  *
  * PID 3 axis angular rate / angular velocity control.
  */

src/lib/rate_control/rate_control_test.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include <gtest/gtest.h>
-#include <RateControl.hpp>
+#include <lib/rate_control/rate_control.hpp>
 
 using namespace matrix;
 

src/modules/fw_att_control/CMakeLists.txt

@@ -30,6 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
 px4_add_module(
 	MODULE modules__fw_att_control
 	MAIN fw_att_control
@@ -44,5 +45,6 @@ px4_add_module(
 		ecl_yaw_controller.cpp
 	DEPENDS
 		px4_work_queue
+		RateControl
 		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::interpolate;
 using math::radians;
@@ -81,23 +83,9 @@ 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());
@@ -106,6 +94,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;
 }
 
@@ -312,6 +313,11 @@ void FixedwingAttitudeControl::Run()
 		float rollspeed = angular_velocity.xyz[0];
 		float pitchspeed = angular_velocity.xyz[1];
 		float yawspeed = angular_velocity.xyz[2];
+		const Vector3f rates(rollspeed, pitchspeed, yawspeed);
+
+		vehicle_angular_acceleration_s angular_acceleration{};
+		_vehicle_angular_acceleration_sub.copy(&angular_acceleration);
+		const Vector3f angular_accel{angular_acceleration.xyz};
 
 		if (_vehicle_status.is_vtol_tailsitter) {
 			/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
@@ -531,9 +537,8 @@ void FixedwingAttitudeControl::Run()
 					}
 
 					/* Update input data for rate controllers */
-					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
+					Vector3f rates_setpoint = Vector3f(_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(),
-					control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
+									   _yaw_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;
@@ -547,11 +552,13 @@ void FixedwingAttitudeControl::Run()
 						    && ((now - pid_autotune.timestamp) < 1_s)) {
 
 							bodyrate_ff = matrix::Vector3f(pid_autotune.rate_sp);
+							rates_setpoint = rates_setpoint + bodyrate_ff;
 						}
 					}
 
 					/* 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) * _airspeed_scaling * _airspeed_scaling;
 					_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
 						(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
 
@@ -559,7 +566,7 @@ void FixedwingAttitudeControl::Run()
 						_roll_ctrl.reset_integrator();
 					}
 
-					float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
+					float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling;
 					_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
 						(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
 
@@ -576,7 +583,7 @@ void FixedwingAttitudeControl::Run()
 						yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get();
 
 					} else {
-						yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
+						yaw_u = att_control(2) * _airspeed_scaling * _airspeed_scaling;
 					}
 
 					_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
@@ -587,7 +594,7 @@ void FixedwingAttitudeControl::Run()
 					}
 
 					if (!PX4_ISFINITE(yaw_u)) {
-						_yaw_ctrl.reset_integrator();
+						// _yaw_ctrl.reset_integrator();
 						_wheel_ctrl.reset_integrator();
 					}
 
@@ -626,19 +633,15 @@ void FixedwingAttitudeControl::Run()
 			} else {
 				vehicle_rates_setpoint_poll();
 
-				_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
+				const Vector3f rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
-				_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
+				const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
-				_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
 
-				float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
+				_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(att_control(0))) ? att_control(
-				_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
+							0) + trim_roll : trim_roll;
-
+				_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(att_control(1))) ? att_control(
-				float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
+							1) + trim_pitch : trim_pitch;
-				_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch :
+				_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(att_control(2))) ? att_control(
-						trim_pitch;
+							2) + trim_yaw : trim_yaw;
-
-				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_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
 						_rates_sp.thrust_body[0] : 0.0f;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -31,6 +31,10 @@
  *
  ****************************************************************************/
 
+#pragma once
+
+#include <lib/rate_control/rate_control.hpp>
+
 #include <drivers/drv_hrt.h>
 #include "ecl_pitch_controller.h"
 #include "ecl_roll_controller.h"
@@ -61,6 +65,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>
@@ -120,6 +125,7 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};		/**< vehicle land detected subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};				/**< vehicle status subscription */
 	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 
 	uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
@@ -235,6 +241,7 @@ private:
 	ECL_PitchController		_pitch_ctrl;
 	ECL_YawController		_yaw_ctrl;
 	ECL_WheelController		_wheel_ctrl;
+	RateControl _rate_control; ///< class for rate control calculations
 
 	/**
 	 * @brief Update flap control setting

src/modules/fw_att_control/ecl_controller.h

@@ -80,8 +80,6 @@ public:
 	virtual ~ECL_Controller() = default;
 
 	virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
-	virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) = 0;
-	virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0;
 
 	/* Setters */
 	void set_time_constant(float time_constant);

src/modules/fw_att_control/ecl_pitch_controller.cpp

@@ -58,66 +58,11 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
 	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;
+	float euler_rate_setpoint =  pitch_error / _tc;
+
+	/* Transform setpoint to body angular rates (jacobian) */
+	_rate_setpoint = cosf(ctl_data.roll) * euler_rate_setpoint +
+			 cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
 
 	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

@@ -61,8 +61,6 @@ public:
 	~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)

src/modules/fw_att_control/ecl_roll_controller.cpp

@@ -56,64 +56,10 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData
 	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;
+	float euler_rate_setpoint = roll_error / _tc;
+
+	/* Transform setpoint to body angular rates (jacobian) */
+	_rate_setpoint = euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
 	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_roll_controller.h

@@ -59,8 +59,6 @@ public:
 	~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

src/modules/fw_att_control/ecl_wheel_controller.h

@@ -60,9 +60,9 @@ public:
 
 	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
 
-	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data);
 
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override { (void)ctl_data; return 0; }
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) { (void)ctl_data; return 0; }
 };
 
 #endif // ECL_HEADING_CONTROLLER_H

src/modules/fw_att_control/ecl_yaw_controller.cpp

@@ -82,8 +82,12 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
 
 	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 <
+		float euler_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);
+					    ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
+
+		/* Transform setpoint to body angular rates (jacobian) */
+		_rate_setpoint = -sinf(ctl_data.roll) * euler_rate_setpoint +
+				 cosf(ctl_data.roll) * cosf(ctl_data.pitch) * euler_rate_setpoint;
 	}
 
 	if (!PX4_ISFINITE(_rate_setpoint)) {
@@ -93,62 +97,3 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
 
 	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

@@ -59,8 +59,6 @@ public:
 	~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};

src/modules/mc_rate_control/CMakeLists.txt

@@ -31,8 +31,6 @@
 #
 ############################################################################
 
-add_subdirectory(RateControl)
-
 px4_add_module(
 	MODULE modules__mc_rate_control
 	MAIN mc_rate_control

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -33,8 +33,7 @@
 
 #pragma once
 
-#include <RateControl.hpp>
+#include <lib/rate_control/rate_control.hpp>
-
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>