From 80e494bb79035daead28d52533e9fb2983186e1e Mon Sep 17 00:00:00 2001 From: Andrea Bernasconi Date: Tue, 29 Sep 2026 19:09:12 +0200 Subject: [PATCH] refactor(rate_control): replace gain compression template with setters (#28910) Templating GainCompression3d on its parameter IDs made the compiler emit every method twice on boards that build both the fixed-wing and the multicopter rate controller, as each instantiation is a separate class. Inlining does not remove the duplicate because update() is too large to be inlined. Make GainCompression3d a plain class configured through setters and let each rate controller own its FW_GC_* or MC_GC_* parameters. This saves 1040 bytes of flash on px4_fmu-v6x_default with no change in behaviour. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Andrea Bernasconi --- src/lib/rate_control/gain_compression.cpp | 22 ++++---------- src/lib/rate_control/gain_compression.hpp | 30 ++++++------------- .../fw_rate_control/FixedwingRateControl.cpp | 3 ++ .../fw_rate_control/FixedwingRateControl.hpp | 5 +++- .../MulticopterRateControl.cpp | 3 ++ .../MulticopterRateControl.hpp | 7 +++-- 6 files changed, 30 insertions(+), 40 deletions(-) diff --git a/src/lib/rate_control/gain_compression.cpp b/src/lib/rate_control/gain_compression.cpp index 19d1cf5a781..52ceb9ea05b 100644 --- a/src/lib/rate_control/gain_compression.cpp +++ b/src/lib/rate_control/gain_compression.cpp @@ -36,15 +36,12 @@ using matrix::Vector3f; using namespace time_literals; -template -GainCompression3dT::GainCompression3dT(ModuleParams *parent) : ModuleParams(parent) +GainCompression3d::GainCompression3d() { - updateParams(); _gain_compression_pub.advertise(); } -template -void GainCompression3dT::reset() +void GainCompression3d::reset() { for (unsigned i = 0; i < 3; i++) { _compression_gains[i].reset(); @@ -53,20 +50,16 @@ void GainCompression3dT::reset() _gains.setOne(); } -template -void GainCompression3dT::updateParams() +void GainCompression3d::setCompressionGainMin(const float gain_min) { - ModuleParams::updateParams(); - for (unsigned i = 0; i < 3; i++) { - _compression_gains[i].setCompressionGainMin(_param_gc_gain_min.get()); + _compression_gains[i].setCompressionGainMin(gain_min); } } -template -void GainCompression3dT::update(const Vector3f &input, const float dt) +void GainCompression3d::update(const Vector3f &input, const float dt) { - if (!_param_gc_en.get()) { + if (!_enabled) { reset(); return; } @@ -99,9 +92,6 @@ void GainCompression3dT::update(const Vector3f &input } } -template class GainCompression3dT; -template class GainCompression3dT; - float GainCompression::update(const float input, const float dt) { if (!PX4_ISFINITE(input)) { diff --git a/src/lib/rate_control/gain_compression.hpp b/src/lib/rate_control/gain_compression.hpp index 478fa9a1f39..5a8557d1cdb 100644 --- a/src/lib/rate_control/gain_compression.hpp +++ b/src/lib/rate_control/gain_compression.hpp @@ -44,11 +44,12 @@ #pragma once // PX4 includes -#include +#include // Libraries #include #include +#include // uORB includes #include @@ -93,25 +94,18 @@ private: }; -/* - * Templated on the enable and minimum gain parameter IDs so that the same - * implementation can be used by the fixed-wing and multicopter rate controllers - * with their own set of parameters. - */ -template -class GainCompression3dT : public ModuleParams +class GainCompression3d { public: - GainCompression3dT(ModuleParams *parent); - ~GainCompression3dT() = default; + GainCompression3d(); + ~GainCompression3d() = default; void reset(); void update(const matrix::Vector3f &input, float dt); const matrix::Vector3f &getGains() const { return _gains; }; - -protected: - void updateParams() override; + void setEnabled(bool enabled) { _enabled = enabled; } + void setCompressionGainMin(float gain_min); private: // uORB publications @@ -120,16 +114,10 @@ private: GainCompression _compression_gains[3]; matrix::Vector3f _gains{1.f, 1.f, 1.f}; + bool _enabled{false}; + 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) _param_gc_en, - (ParamFloat) _param_gc_gain_min - ) }; - -using GainCompression3d = GainCompression3dT; -using GainCompression3dMc = GainCompression3dT; diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 0e0b39e4659..6adde92c5d5 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -85,6 +85,9 @@ FixedwingRateControl::parameters_update() _rate_control.setIntegratorLimit( Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); + _gain_compression.setEnabled(_param_fw_gc_en.get()); + _gain_compression.setCompressionGainMin(_param_fw_gc_gain_min.get()); + if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) { param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en); } diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 4ef6690fe0d..90711000273 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -186,6 +186,9 @@ private: (ParamFloat) _param_fw_dtrim_y_vmax, (ParamFloat) _param_fw_dtrim_y_vmin, + (ParamBool) _param_fw_gc_en, + (ParamFloat) _param_fw_gc_gain_min, + (ParamFloat) _param_fw_man_p_sc, (ParamFloat) _param_fw_man_r_sc, (ParamFloat) _param_fw_man_y_sc, @@ -218,7 +221,7 @@ private: ) RateControl _rate_control; ///< class for rate control calculations - GainCompression3d _gain_compression{this}; + GainCompression3d _gain_compression; void updateActuatorControlsStatus(float dt); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 12ca513107d..94947772cd9 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -93,6 +93,9 @@ MulticopterRateControl::parameters_updated() _rate_control.setFeedForwardGain( Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + _gain_compression.setEnabled(_param_mc_gc_en.get()); + _gain_compression.setCompressionGainMin(_param_mc_gc_gain_min.get()); + // manual rate control acro mode rate limits _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 892419f7fdc..a25a23130af 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -94,7 +94,7 @@ private: void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); RateControl _rate_control; ///< class for rate control calculations - GainCompression3dMc _gain_compression{this}; ///< reduces the loop gain when an oscillation is detected + GainCompression3d _gain_compression; ///< reduces the loop gain when an oscillation is detected uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; @@ -167,6 +167,9 @@ private: (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ - (ParamBool) _param_mc_bat_scale_en + (ParamBool) _param_mc_bat_scale_en, + + (ParamBool) _param_mc_gc_en, + (ParamFloat) _param_mc_gc_gain_min ) };