mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
1 parent
98cb30bf47
commit
418b3a4d8a
3 files changed
+13
-14
No files matched your search
+11
-10
@@ -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) {
|
||||
|
||||
@@ -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 [-].
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
Reference in new issue
Block a user