diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 44c24578fd0..305a65e3619 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -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 ¶m, 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) { diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 2e66dc40154..75dd8347948 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -626,7 +626,7 @@ private: private: // State - AlphaFilter _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate. + AlphaFilter _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 [-]. diff --git a/src/lib/tecs/TECSControlClosedLoopTest.cpp b/src/lib/tecs/TECSControlClosedLoopTest.cpp index 3eca1f59a36..f84e05ac635 100644 --- a/src/lib/tecs/TECSControlClosedLoopTest.cpp +++ b/src/lib/tecs/TECSControlClosedLoopTest.cpp @@ -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);