From 38c63c9c52594f2519a4fd51dfb1252c5c0f6acc Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 12 Feb 2026 13:31:12 -0800 Subject: [PATCH] Limit TECS integrators --- src/lib/tecs/TECS.cpp | 11 +++++++ src/lib/tecs/TECS.hpp | 6 ++++ .../FwLateralLongitudinalControl.cpp | 2 ++ .../FwLateralLongitudinalControl.hpp | 2 ++ .../fw_lat_long_params.c | 30 +++++++++++++++++++ 5 files changed, 51 insertions(+) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 5ff73eb83c..b5cd1dd7a8 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -485,6 +485,11 @@ void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const Co // Update the pitch integrator state. _pitch_integ_state = _pitch_integ_state + pitch_integ_input * dt; + // Limit the integrator state + if (param.pitch_integrator_max > FLT_EPSILON) { + _pitch_integ_state = constrain(_pitch_integ_state, -param.pitch_integrator_max, param.pitch_integrator_max); + } + } else { _pitch_integ_state = 0.0f; } @@ -598,6 +603,12 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit // This will be added to the total throttle demand to compensate for steady state errors _throttle_integ_state = PX4_ISFINITE(throttle_integ_input) ? _throttle_integ_state + throttle_integ_input : 0.f; + // Limit the integrator state + if (param.throttle_integrator_max > FLT_EPSILON) { + _throttle_integ_state = constrain(_throttle_integ_state, -param.throttle_integrator_max, + param.throttle_integrator_max); + } + } else { _throttle_integ_state = 0.0f; } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a45faeb2e3..dc447de382 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -228,11 +228,13 @@ public: float pitch_speed_weight; ///< Speed control weighting used by pitch demand calculation. float integrator_gain_pitch; ///< Integrator gain used by the pitch demand calculation. float pitch_damping_gain; ///< Damping gain of the pitch demand calculation [s]. + float pitch_integrator_max; ///< Maximum pitch integrator state [rad]. // Throttle control param float integrator_gain_throttle; ///< Integrator gain used by the throttle demand calculation. float throttle_damping_gain; ///< Damping gain of the throttle demand calculation [s]. float throttle_slewrate; ///< Throttle demand slew rate limit [1/s]. + float throttle_integrator_max; ///< Maximum throttle integrator state [-]. float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³]. float load_factor; ///< Additional normal load factor. @@ -604,6 +606,8 @@ public: void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;}; void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; }; + void set_throttle_integrator_max(float max) { _control_param.throttle_integrator_max = max; }; + void set_pitch_integrator_max(float max) { _control_param.pitch_integrator_max = max; }; void set_max_sink_rate(float max_sink_rate) { _control_param.max_sink_rate = max_sink_rate; _reference_param.max_sink_rate = max_sink_rate; }; void set_min_sink_rate(float min_sink_rate) { _control_param.min_sink_rate = min_sink_rate; }; @@ -746,9 +750,11 @@ private: .pitch_speed_weight = 1.0f, .integrator_gain_pitch = 0.0f, .pitch_damping_gain = 0.0f, + .pitch_integrator_max = 0.5f, .integrator_gain_throttle = 0.0f, .throttle_damping_gain = 0.0f, .throttle_slewrate = 0.0f, + .throttle_integrator_max = 0.3f, .load_factor_correction = 0.0f, .load_factor = 1.0f, .fast_descend = 0.f diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 00283b9848..9774d653f5 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -97,7 +97,9 @@ FwLateralLongitudinalControl::parameters_update() _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed()); _tecs.set_throttle_damp(_param_fw_t_thr_damping.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get()); + _tecs.set_throttle_integrator_max(_param_fw_t_thr_i_max.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); + _tecs.set_pitch_integrator_max(_param_fw_t_ptch_i_max.get()); _tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get()); _tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index 13b21e4529..18e0789559 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -148,7 +148,9 @@ private: (ParamFloat) _param_fw_t_h_error_tc, (ParamFloat) _param_fw_t_fast_alt_err, (ParamFloat) _param_fw_t_thr_integ, + (ParamFloat) _param_fw_t_thr_i_max, (ParamFloat) _param_fw_t_I_gain_pit, + (ParamFloat) _param_fw_t_ptch_i_max, (ParamFloat) _param_fw_t_ptch_damp, (ParamFloat) _param_fw_t_rll2thr, (ParamFloat) _param_fw_t_sink_max, diff --git a/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c b/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c index 0856e8c123..858afb944f 100644 --- a/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c +++ b/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c @@ -90,6 +90,21 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f); */ PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f); +/** + * Throttle integrator limit + * + * Maximum integrator state contribution to throttle output (in the range + * between minimum and maximum throttle). This prevents integrator windup, + * especially important in soaring mode where throttle authority may be limited. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_I_MAX, 0.3f); + /** * Integrator gain pitch * @@ -104,6 +119,21 @@ PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f); */ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); +/** + * Pitch integrator limit + * + * Maximum integrator state contribution to pitch output (in radians). + * This prevents integrator windup. + * + * @unit rad + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_I_MAX, 0.5f); + /** * Maximum vertical acceleration *