fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 17:14:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tecs: restore pitch integrator when fat descends ends

Browse Source
This commit is contained in:
Konrad 2025-01-15 10:52:52 +01:00
parent 974446c0e8
commit 443d96873c
2 changed files with 24 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/lib/tecs/TECS.cpp
View File

@ -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,

1
src/lib/tecs/TECS.hpp
View File

@ -535,6 +535,7 @@ private:
// State
AlphaFilter<float> _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