fw rate: add gain compression algorithm

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -291,6 +291,7 @@ void FixedwingRateControl::Run()
 			// Reset integrators if the aircraft is on ground or not in a state where the fw attitude controller is run
 			if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) {
 
+				_gain_compression.reset();
 				_rate_control.resetIntegral();
 			}
 
@@ -370,10 +371,11 @@ void FixedwingRateControl::Run()
 				_rate_control.setFeedForwardGain(scaled_gain_ff);
 
 				// Run attitude RATE controllers which need the desired attitudes from above, add trim.
-				const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
-						_landed);
+				const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed);
 
-				Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling;
+				Vector3f control_u = _gain_compression.getGains().emult(angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling);
+
+				_gain_compression.update(control_u, dt);
 
 				// Special case yaw in Acro: if the parameter FW_ACRO_YAW_EN is not set then don't rate-control yaw
 				if (!_vcontrol_mode.flag_control_attitude_enabled && _vcontrol_mode.flag_control_manual_enabled
@@ -417,6 +419,7 @@ void FixedwingRateControl::Run()
 
 		} else {
 			// full manual
+			_gain_compression.reset();
 			_rate_control.resetIntegral();
 		}
 

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -34,6 +34,7 @@
 #pragma once
 
 #include <lib/rate_control/rate_control.hpp>
+#include <lib/rate_control/gain_compression.hpp>
 
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
@@ -212,6 +213,7 @@ private:
 	)
 
 	RateControl _rate_control; ///< class for rate control calculations
+	GainCompression3d _gain_compression{this};
 
 	void updateActuatorControlsStatus(float dt);
 

src/modules/logger/logged_topics.cpp

@@ -73,6 +73,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("flaps_setpoint", 1000);
 	add_optional_topic("flight_phase_estimation", 1000);
 	add_optional_topic("fuel_tank_status", 10);
+	add_optional_topic("gain_compression", 100);
 	add_topic("gimbal_manager_set_attitude", 500);
 	add_optional_topic("generator_status");
 	add_optional_topic("gps_dump");