diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5aae7118df4..253024588dc 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -98,6 +98,7 @@ set(msg_files GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg + GainCompression.msg GimbalControls.msg GimbalDeviceAttitudeStatus.msg GimbalDeviceInformation.msg diff --git a/msg/GainCompression.msg b/msg/GainCompression.msg new file mode 100644 index 00000000000..cd045355569 --- /dev/null +++ b/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] diff --git a/src/lib/rate_control/CMakeLists.txt b/src/lib/rate_control/CMakeLists.txt index f95b133eea8..2370b2eb5d1 100644 --- a/src/lib/rate_control/CMakeLists.txt +++ b/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}) diff --git a/src/lib/rate_control/gain_compression.cpp b/src/lib/rate_control/gain_compression.cpp new file mode 100644 index 00000000000..220d3768b16 --- /dev/null +++ b/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; +} diff --git a/src/lib/rate_control/gain_compression.hpp b/src/lib/rate_control/gain_compression.hpp new file mode 100644 index 00000000000..20611e9ac96 --- /dev/null +++ b/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 + +// Libraries +#include +#include + +// uORB includes +#include +#include + +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 _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_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) _param_fw_gc_en, + (ParamFloat) _param_fw_gc_gain_min + ) +}; diff --git a/src/lib/rate_control/gain_compression_params.c b/src/lib/rate_control/gain_compression_params.c new file mode 100644 index 00000000000..7a3cda5660c --- /dev/null +++ b/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); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index bb9d55b1e03..3ae3272cfa5 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/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(); } diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 7791d0820c5..54cf23be001 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -212,6 +213,7 @@ private: ) RateControl _rate_control; ///< class for rate control calculations + GainCompression3d _gain_compression{this}; void updateActuatorControlsStatus(float dt); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5ab67745f1e..3a9e51704d6 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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");