fix(tecs): low-pass the throttle energy rate error, not the estimate

The throttle feedback compared an unfiltered total energy rate setpoint
with an estimate low-passed by FW_T_STE_R_TC. On a setpoint change the
lag between the two showed up as a phantom error that the proportional
and integral paths acted on, pushing the throttle beyond what the
feedforward already demanded. Filter the error instead: setpoint and
estimate then see the same lag and only true tracking errors reach the
feedback, while the feedforward keeps acting on the unfiltered setpoint.

The airspeed-less altitude step test no longer needs to disable the
filter: on the step up the phantom error sped the vehicle up by 3 cm/s,
beyond the test's airspeed tolerance, and now it stays within it.

Assisted-by: Claude Fable 5.1
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Balduin authored and RomanBapst committed 2026-11-20 09:25:13 +03:00
1 parent 98cb30bf47
commit 418b3a4d8a
3 files changed
+13 -14

No files matched your search

+11 -10
View File
@@ -231,9 +231,8 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, const
_pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param);
_ste_rate_estimate_filter.reset(specific_energy_rate.spe_rate.estimate + specific_energy_rate.ske_rate.estimate);
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rate, param)};
_ste_rate_error_filter.reset(_getControlError(ste_rate));
_throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
@@ -559,11 +558,13 @@ float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValu
void TECSControl::_calcThrottleControl(float dt, const STERateLimit &limit,
const SpecificEnergyRates &specific_energy_rates, const Param &param, const Flag &flag)
{
// Update STE rate estimate LP filter
const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
// Low-pass the energy rate error for the feedback; the feedforward acts on the unfiltered setpoint. Filtering
// the error rather than the estimate keeps a setpoint change from appearing as a lagged phantom error.
_ste_rate_error_filter.setParameters(dt, param.ste_rate_time_const);
_ste_rate_error_filter.update(_getControlError(ste_rate));
// During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending
float throttle_setpoint{param.throttle_min};
@@ -602,7 +603,7 @@ TECSControl::ControlValues TECSControl::_calcThrottleControlSteRate(const STERat
ste_rate.setpoint += param.load_factor_correction * (param.load_factor - 1.f);
ste_rate.setpoint = constrain(ste_rate.setpoint, limit.STE_rate_min, limit.STE_rate_max);
ste_rate.estimate = _ste_rate_estimate_filter.getState();
ste_rate.estimate = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
return ste_rate;
}
@@ -616,7 +617,7 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit
if (flag.airspeed_enabled) {
if (param.integrator_gain_throttle > FLT_EPSILON) {
// underspeed conditions zero out integration
float throttle_integ_input = (_getControlError(ste_rate) * param.integrator_gain_throttle) * dt *
float throttle_integ_input = (_ste_rate_error_filter.getState() * param.integrator_gain_throttle) * dt *
STE_rate_to_throttle * (1.0f - _ratio_undersped);
// only allow integrator propagation into direction which unsaturates throttle
@@ -669,8 +670,8 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C
}
// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
float throttle_setpoint = (_getControlError(ste_rate) * param.throttle_damping_gain) * STE_rate_to_throttle +
// Add proportional control feedback to the predicted throttle and constrain to throttle limits
float throttle_setpoint = (_ste_rate_error_filter.getState() * param.throttle_damping_gain) * STE_rate_to_throttle +
throttle_predicted;
if (flag.airspeed_enabled) {
+1 -1
View File
@@ -626,7 +626,7 @@ private:
private:
// State
AlphaFilter<float> _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate.
AlphaFilter<float> _ste_rate_error_filter; ///< Low pass filter for the specific total energy rate error (feedback path).
float _pitch_integ_state{0.0f}; ///< Pitch integrator state [rad].
float _fast_descend_prev{0.0f}; ///< Fast descend ratio of the last update [0,1].
float _throttle_integ_state{0.0f}; ///< Throttle integrator state [-].
+1 -3
View File
@@ -591,14 +591,12 @@ TEST_F(TECSControlClosedLoopTest, AirspeedlessSteadyStateCruise)
// SEB_rate_sp / (V * g) asks for twice the demanded climb angle. A feedforward gain of 0.5 cancels this: the vehicle
// then flies exactly the demanded climb rate, the SEB rate error stays zero, and the altitude error decays first order
// with FW_T_ALT_TC. As in AltitudeStepDown, the pitch must follow the raw step as fast as the throttle and the demanded
// sink rate must not exceed FW_T_SINK_MIN. The total energy rate filter (FW_T_STE_R_TC) is disabled, as its lag
// makes the throttle damping briefly overfund the climb on the step up and speed the vehicle up.
// sink rate must not exceed FW_T_SINK_MIN.
TEST_F(TECSControlClosedLoopTest, AirspeedlessAltitudeStepHalfFeedforward)
{
disableAirspeed();
_params.seb_rate_ff = 0.5f;
_params.ste_rate_time_const = 0.f;
run(10.f);