test(tecs): drop the altitude reference model from the closed-loop test

The closed-loop harness is meant to test TECSControl on its own, but the
altitude setpoint went through TECSAltitudeReferenceModel first, so the
altitude step results also depended on how the reference model shapes
the setpoint. Feed TECSControl the raw altitude setpoint instead.

A raw step drops the throttle to idle at once, so the altitude step only
keeps airspeed exact if the pitch can follow just as fast. In TECSControl
the vertical acceleration limit only rate limits the pitch, so the step
test now disables it, and sizes the step so that the demanded sink rate
stays within FW_T_SINK_MIN, the sink rate flown at idle without speeding
up.

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 97b31cf048
commit 00aed233ef
1 file changed
+11 -26
+11 -26
View File
@@ -165,18 +165,12 @@ AircraftState aircraft_step(const TECSControl::Param &p, float dt, float throttl
class TECSClosedLoopTest : public ::testing::Test
{
protected:
TECSClosedLoopTest() { _alt_ref_model.initialize({ALT_INIT, 0.f}); }
float _t{0.f}; // Simulation time [s]
TECSControl::Param _params{defaultParams()}; // first: the plant and the setpoints derive from it
AircraftState _state{_params.equivalent_airspeed_trim, ALT_INIT};
TECSControl _tecs{};
TECSControl::Flag _flag{.airspeed_enabled = true, .detect_underspeed_enabled = true};
TECSAltitudeReferenceModel _alt_ref_model{};
float _target_climbrate{paramDefault(px4::params::FW_T_CLMB_R_SP)};
float _target_sinkrate {paramDefault(px4::params::FW_T_SINK_R_SP)};
float _V_sp {_params.equivalent_airspeed_trim};
float _alt_sp {ALT_INIT};
@@ -231,24 +225,10 @@ protected:
// Climb rate the plant is flying at the current state under the previous pitch command
float plantAltitudeRate() const { return altitude_rate(_pitch_prev, _state, _plant_pitch_offset); }
void updateAltRef()
{
// Mapped the way FwLateralLongitudinalControl feeds the reference model: the target rates come from
// FW_T_CLMB_R_SP / FW_T_SINK_R_SP, bounded by the performance limits
const TECSAltitudeReferenceModel::Param param{
.target_climbrate = math::min(_target_climbrate, _params.max_climb_rate),
.target_sinkrate = math::min(_target_sinkrate, _params.max_sink_rate),
.vert_accel_limit = _params.vert_accel_limit,
.max_climb_rate = _params.max_climb_rate,
.max_sink_rate = _params.max_sink_rate,
};
_alt_ref_model.update(DT, {_alt_sp, NAN}, _state.h, plantAltitudeRate(), param);
}
TECSControl::Setpoint makeSetpoint() const
{
return {
.altitude_reference = _alt_ref_model.getAltitudeReference(),
.altitude_reference = {_alt_sp, 0.f}, // no reference model: TECSControl sees the raw setpoint
.altitude_rate_setpoint_direct = NAN, // use altitude control loop
.tas_setpoint = _V_sp,
};
@@ -323,7 +303,6 @@ protected:
while (_t < t_start + duration) {
updateAltRef();
_tecs.update(DT, makeSetpoint(), makeInput(), _params, _flag);
_pitch_prev = _tecs.getPitchSetpoint();
@@ -412,15 +391,21 @@ TEST_F(TECSClosedLoopTest, SteadyStateCruiseLowSpeedWeight)
EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR);
}
// --- Altitude step with a low vertical acceleration limit -------------------
// --- Altitude step ------------------------------------------------------------
//
// TECSControl gets the raw altitude setpoint (no reference model), so the altitude loop demands its full sink rate at
// once and the throttle drops to idle at once. The pitch has to follow just as fast, or the vehicle loses total energy
// before it starts descending and the energy comes out of airspeed: the vertical acceleration limit, which in
// TECSControl only rate limits the pitch, is therefore disabled. The step is sized so that the demanded sink rate
// (step / FW_T_ALT_TC) does not exceed FW_T_SINK_MIN, the sink rate the vehicle flies at idle without accelerating.
TEST_F(TECSClosedLoopTest, AltitudeStepDownLowAccel)
TEST_F(TECSClosedLoopTest, AltitudeStepDown)
{
_params.vert_accel_limit /= 10.f;
_params.vert_accel_limit = INFINITY;
run(10.f);
setAltitudeSetpoint(ALT_INIT - 100.f);
setAltitudeSetpoint(ALT_INIT - _params.min_sink_rate / _params.altitude_error_gain);
run(200.f);
EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s";