test(tecs): show the airspeed-less altitude step with half the SEB feedforward

Without airspeed the speed weight is 0, so the SEB rate is twice the
potential energy rate, while the pitch feedforward converts it to a climb
angle as if they were equal and asks for twice the climb rate. With the
feedforward gain at 0.5 the vehicle flies the demanded climb rate exactly
and the altitude error decays first order with FW_T_ALT_TC, which the
test checks for a step down and a step up. At the default gain of 1 the
same checks miss by up to 1.8 m.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst authored and Roman Bapst committed 2026-11-20 06:33:50 +03:00
1 parent 00aed233ef
commit fc970a9b3f
1 file changed
+34
+34
View File
@@ -570,3 +570,37 @@ TEST_F(TECSClosedLoopTest, AirspeedlessSteadyStateCruise)
EXPECT_LT(_sim_stats.max_throttle_integ_norm, MAX_NUMERICAL_ERR);
EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR);
}
// Without airspeed the speed weight is 0, so the SEB rate is twice the SPE rate and the pitch feedforward
// 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.
TEST_F(TECSClosedLoopTest, AirspeedlessAltitudeStepHalfFeedforward)
{
disableAirspeed();
_params.seb_rate_ff = 0.5f;
_params.vert_accel_limit = INFINITY;
run(10.f);
const float step_size = _params.min_sink_rate / _params.altitude_error_gain;
for (const float step : {-step_size, step_size}) {
setAltitudeSetpoint(_state.h + step);
const float t_step = _t;
for (int i = 0; i < 30; i++) {
run(1.f);
const float expected_error = step * expf(-(_t - t_step) * _params.altitude_error_gain);
EXPECT_NEAR(_alt_sp - _state.h, expected_error, MAX_NUMERICAL_ERR) << "at t=" << _t << " s";
}
}
// The throttle feedforward funds the climb exactly, so the open loop airspeed only moves because the throttle
// damping acts on the low-pass filtered total energy rate estimate (FW_T_STE_R_TC), which lags the step. On the
// step down the throttle saturates at idle and clips this; on the step up it adds a few cm/s of airspeed.
EXPECT_LT(_sim_stats.max_airspeed_error, 0.05f) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s";
EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR);
}