From 443d96873ccc9903df323efe0afae358d223916e Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 15 Jan 2025 10:52:52 +0100 Subject: [PATCH] Tecs: restore pitch integrator when fat descends ends --- src/lib/tecs/TECS.cpp | 24 +++++++++++++++++++++++- src/lib/tecs/TECS.hpp | 1 + 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 6a09b87759..10606a0767 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -417,6 +417,15 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; + // Store the pitch integrator value when entering fast descend and reset when leaving fast descend + if ((param.fast_descend > FLT_EPSILON) && !PX4_ISFINITE(_pitch_integ_reset_val)) { + _pitch_integ_reset_val = _pitch_integ_state; + + } else if (fabsf(param.fast_descend) <= FLT_EPSILON && PX4_ISFINITE(_pitch_integ_reset_val)) { + _pitch_integ_state = _pitch_integ_reset_val; + _pitch_integ_reset_val = NAN; + } + _calcPitchControlUpdate(dt, input, seb_rate, param); const float pitch_setpoint{_calcPitchControlOutput(input, seb_rate, param, flag)}; @@ -506,7 +515,16 @@ float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValu // a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop. // b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of // pitch transients due to control action or turbulence. - const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate + _pitch_integ_state; + + float pitch_setpoint_unc{0.f}; + + if (param.fast_descend > FLT_EPSILON && PX4_ISFINITE(_pitch_integ_reset_val)) { + pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate + lerp(_pitch_integ_reset_val, _pitch_integ_state, + param.fast_descend); + + } else { + pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate + _pitch_integ_state; + } return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max); } @@ -654,6 +672,10 @@ void TECSControl::resetIntegrals() { _pitch_integ_state = 0.0f; _throttle_integ_state = 0.0f; + + if (!PX4_ISFINITE(_pitch_integ_reset_val)) { + _pitch_integ_reset_val = NAN; + } } void TECS::initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max, diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a45faeb2e3..12544de6c2 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -535,6 +535,7 @@ private: // State AlphaFilter _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate. float _pitch_integ_state{0.0f}; ///< Pitch integrator state [rad]. + float _pitch_integ_reset_val{NAN}; float _throttle_integ_state{0.0f}; ///< Throttle integrator state [-]. // Output