fw rate: add gain compression algorithm

msg/CMakeLists.txt

@@ -98,6 +98,7 @@ set(msg_files
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GeofenceStatus.msg
+	GainCompression.msg
 	GimbalControls.msg
 	GimbalDeviceAttitudeStatus.msg
 	GimbalDeviceInformation.msg

msg/GainCompression.msg

@@ -0,0 +1,6 @@
+uint64 timestamp                # Time since system start (microseconds)
+
+float32[3] compression_gains	# [-] Multiplicative gain to modify the output of the controller per axis [@frame FRD], [@range 0, 1]
+float32[3] spectral_damper_hpf  # Squared output of the spectral damper’s high-pass filter [@frame FRD]
+float32[3] spectral_damper_out  # Spectral damper's output squared [@frame FRD]
+float32[3] input                # Torque input [@frame FRD]

src/lib/rate_control/CMakeLists.txt

@@ -34,6 +34,8 @@
 px4_add_library(RateControl
 	rate_control.cpp
 	rate_control.hpp
+	gain_compression.cpp
+	gain_compression.hpp
 )
 target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/rate_control/gain_compression.cpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "gain_compression.hpp"
+
+using matrix::Vector3f;
+using namespace time_literals;
+
+GainCompression3d::GainCompression3d(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_gain_compression_pub.advertise();
+}
+
+void GainCompression3d::reset()
+{
+	for (unsigned i = 0; i < 3; i++) {
+		_compression_gains[i].reset();
+	}
+}
+
+void GainCompression3d::updateParams()
+{
+	ModuleParams::updateParams();
+
+	for (unsigned i = 0; i < 3; i++) {
+		_compression_gains[i].setCompressionGainMin(_param_fw_gc_gain_min.get());
+	}
+}
+
+void GainCompression3d::update(const Vector3f &input, const float dt)
+{
+	if (!_param_fw_gc_en.get()) {
+		reset();
+		_gains.setOne();
+		return;
+	}
+
+	Vector3f hpf;
+	Vector3f lpf;
+	const float sample_freq = 1.f / math::constrain(dt, 1e-3f, 100e-3f);
+
+	for (unsigned i = 0; i < 3; i++) {
+		_compression_gains[i].setLpfCutoffFrequency(sample_freq, _kLpfCutoffFrequency);
+		_compression_gains[i].setHpfCutoffFrequency(sample_freq, _kHpfCutoffFrequency);
+
+		_gains(i) = _compression_gains[i].update(input(i), dt);
+
+		hpf(i) = _compression_gains[i].getSpectralDamperHpf();
+		lpf(i) = _compression_gains[i].getSpectralDamperLpf();
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if ((now - _time_last_publication) > 100_ms) {
+		gain_compression_s msg;
+		msg.timestamp = now;
+		_gains.copyTo(msg.compression_gains);
+		hpf.copyTo(msg.spectral_damper_hpf);
+		lpf.copyTo(msg.spectral_damper_out);
+		input.copyTo(msg.input);
+		_gain_compression_pub.publish(msg);
+
+		_time_last_publication = now;
+	}
+}
+
+float GainCompression::update(const float input, const float dt)
+{
+	_hpf = _alpha_hpf * _hpf + _alpha_hpf * (input - _input_prev);
+	_lpf.update(_hpf * _hpf);
+
+	_input_prev = input;
+
+	const float ka = fmaxf(_compression_gain - _compression_gain_min, 0.f);
+	const float spectral_damping = -_kSpectralDamperGain * ka * _lpf.getState();
+
+	const float leakage = _kLeakageGain * (1.f - _compression_gain);
+
+	const float ka_dot = spectral_damping + leakage;
+	_compression_gain = math::constrain(_compression_gain + ka_dot * dt, _compression_gain_min, 1.f);
+
+	return _compression_gain;
+}

src/lib/rate_control/gain_compression.hpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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.
+ *
+ ****************************************************************************/
+
+/* Description:
+ * This class produces a multiplicative gain that can be used on the output of a controller
+ * to dynamically reduce the loop gain when oscillations are detected.
+ *
+ * Algorithm based on
+ * Orr, Jeb, and Tannen Van Zwieten. "Robust, practical adaptive control for launch vehicles."
+ * AIAA Guidance, Navigation, and Control Conference. 2012.
+ * https://ntrs.nasa.gov/api/citations/20120015662/downloads/20120015662.pdf
+ */
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// Libraries
+#include <math.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/topics/gain_compression.h>
+
+class GainCompression
+{
+public:
+	void reset() { _compression_gain = 1.f; }
+	float update(float input, float dt);
+
+	void setLpfCutoffFrequency(float sample_freq, float cutoff)
+	{
+		_lpf.setCutoffFreq(sample_freq, cutoff);
+	}
+	void setHpfCutoffFrequency(float sample_freq, float cutoff) { _alpha_hpf = sample_freq / (sample_freq + 2.f * M_PI_F * cutoff); }
+
+	float getSpectralDamperHpf() const { return _hpf * _hpf; }
+	float getSpectralDamperLpf() const { return _lpf.getState(); }
+	void setCompressionGainMin(float gain_min) { _compression_gain_min = gain_min; }
+
+private:
+	float _compression_gain{1.f};
+	float _compression_gain_min{0.3f};
+
+	float _alpha_hpf{0.f};
+	float _hpf{0.f};
+
+	float _input_prev{0.f};
+
+	AlphaFilter<float> _lpf;
+
+	static constexpr float _kSpectralDamperGain{200.f}; // tuned based on flight test data to obtain a quick response
+	static constexpr float _kLeakageGain{0.1f}; // 1/time_constant, slow enough to not interfere with the controller
+
+};
+
+class GainCompression3d : public ModuleParams
+{
+public:
+	GainCompression3d(ModuleParams *parent);
+	~GainCompression3d() = default;
+
+	void reset();
+	void update(const matrix::Vector3f &input, float dt);
+	const matrix::Vector3f &getGains() const { return _gains; };
+
+
+protected:
+	void updateParams() override;
+
+private:
+	// uORB publications
+	uORB::Publication<gain_compression_s> _gain_compression_pub{ORB_ID(gain_compression)};
+
+	GainCompression _compression_gains[3];
+	matrix::Vector3f _gains{1.f, 1.f, 1.f};
+
+	hrt_abstime _time_last_publication{0};
+
+	static constexpr float _kLpfCutoffFrequency{5.f}; // Just above the control bandwidth of most UAVs
+	static constexpr float _kHpfCutoffFrequency{2.f * _kLpfCutoffFrequency}; // 1 Octave above LPF cutoff, as recommended by the reference paper
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::FW_GC_EN>) _param_fw_gc_en,
+		(ParamFloat<px4::params::FW_GC_GAIN_MIN>) _param_fw_gc_gain_min
+	)
+};

src/lib/rate_control/gain_compression_params.c

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable rate gain compression
+ *
+ * @boolean
+ * @group FW Rate Control
+ */
+PARAM_DEFINE_INT32(FW_GC_EN, 1);
+
+/**
+ * Compression gain lower limit
+ *
+ * The range of the compression gain is between this parameter and 1.0
+ *
+ * @min 0.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 2
+ * @group FW Rate Control
+ */
+PARAM_DEFINE_FLOAT(FW_GC_GAIN_MIN, 0.3f);

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");