/**************************************************************************** * * Copyright (C) 2026 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * Closed-loop unit tests for TECSControl. * * A minimal aircraft model is closed-loop coupled to the TECS controller with * perfect state knowledge and no noise. The model is deliberately chosen to be * consistent with the energy model used by TECS internally, so that we can find * correctness errors in the controller under zero model mismatch. Any air * density / weight corrections that would come from the FW performance model * are ignored. * * Physical model * -------------- * * States: V (TAS), h (altitude) * Inputs: throttle, gamma (flight path angle) = pitch angle (zero AoA, zero wind) * * d(STE)/dt = piecewise linear in throttle, matching TECS's above/below-trim slopes * h' = V * gamma * V' = (STE_rate - g * h') / V [remainder goes to kinetic energy] * */ #include #include "TECS.hpp" #include #include #include #include #include #include namespace { // --- Vehicle constants -------------------------------------------------------- static constexpr float ALT_INIT = 300.f; // initial altitude [m] // Compile-time default of a firmware parameter. constexpr float paramDefault(px4::params p) { return px4::parameters[static_cast(p)].val.f; } // Firmware parameter defaults, mapped the way FwLateralLongitudinalControl feeds them into TECS. constexpr TECSControl::Param defaultParams() { return { .max_sink_rate = paramDefault(px4::params::FW_T_SINK_MAX), .min_sink_rate = paramDefault(px4::params::FW_T_SINK_MIN), .max_climb_rate = paramDefault(px4::params::FW_T_CLMB_MAX), .vert_accel_limit = INFINITY, // Ignore FW_T_VERT_ACC, sim assumes perfect pitch tracking .equivalent_airspeed_trim = paramDefault(px4::params::FW_AIRSPD_TRIM), .tas_min = paramDefault(px4::params::FW_AIRSPD_MIN), .tas_stall = paramDefault(px4::params::FW_AIRSPD_STALL), .tas_max = paramDefault(px4::params::FW_AIRSPD_MAX), .tas_trim = paramDefault(px4::params::FW_AIRSPD_TRIM), // EAS = CAS = TAS in this sim .pitch_max = math::radians(paramDefault(px4::params::FW_P_LIM_MAX)), .pitch_min = math::radians(paramDefault(px4::params::FW_P_LIM_MIN)), .throttle_trim = paramDefault(px4::params::FW_THR_TRIM), .throttle_max = paramDefault(px4::params::FW_THR_MAX), .throttle_min = paramDefault(px4::params::FW_THR_MIN), .altitude_error_gain = 1.f / paramDefault(px4::params::FW_T_ALT_TC), .altitude_setpoint_gain_ff = paramDefault(px4::params::FW_T_HRATE_FF), .airspeed_error_gain = 1.f / paramDefault(px4::params::FW_T_TAS_TC), .ste_rate_time_const = paramDefault(px4::params::FW_T_STE_R_TC), .seb_rate_ff = paramDefault(px4::params::FW_T_SEB_R_FF), .pitch_speed_weight = paramDefault(px4::params::FW_T_SPDWEIGHT), .integrator_gain_pitch = paramDefault(px4::params::FW_T_I_GAIN_PIT), .pitch_damping_gain = paramDefault(px4::params::FW_T_PTCH_DAMP), .integrator_gain_throttle = paramDefault(px4::params::FW_T_THR_INTEG), .throttle_damping_gain = paramDefault(px4::params::FW_T_THR_DAMPING), .throttle_slewrate = paramDefault(px4::params::FW_THR_SLEW_MAX), .load_factor_correction = paramDefault(px4::params::FW_T_RLL2THR), .load_factor = 1.f, .fast_descend = 0.f, }; } static constexpr float DT = 0.02f; // Simulation time step [s] // Maximum numerical errors we expect to show up in tracking errors (both speed // and altitude) which in theory should be 0. Does not need to equal DT, but if // the only source of error is euler discretisation, then the error should scale // linearly with DT (first order method). static constexpr float MAX_NUMERICAL_ERR_PER_DT = 1.0f; static constexpr float MAX_NUMERICAL_ERR = MAX_NUMERICAL_ERR_PER_DT * DT; // Tolerance for settled tracking errors which do not go to zero with smaller DT // (but which do go to zero as time progresses). Choose settling time so the // error is below this with some margin. static constexpr float TRACKING_ERROR_TOL = 0.02f; // Altitude step chosen so that neither climbing nor sinking will saturate throttle static constexpr float ALT_STEP = math::min(defaultParams().min_sink_rate, defaultParams().max_climb_rate) / defaultParams().altitude_error_gain; // [m] static constexpr float AIRSPEED_STEP = 1.0f; // [m/s] struct AircraftState { float V{0.f}; float h{ALT_INIT}; }; float ste_rate_modelled(const TECSControl::Param &p, float throttle) { // Mirrors the formula in _calcThrottleControlOutput const float ste_rate_max = p.max_climb_rate * CONSTANTS_ONE_G; const float ste_rate_min_abs = p.min_sink_rate * CONSTANTS_ONE_G; return (throttle >= p.throttle_trim) ? (throttle - p.throttle_trim) * ste_rate_max / (p.throttle_max - p.throttle_trim) : (throttle - p.throttle_trim) * ste_rate_min_abs / (p.throttle_trim - p.throttle_min); } // Only correct in a linearised sense (close to level) but matches TECS float altitude_rate(float pitch_cmd, AircraftState s, float pitch_offset = 0.f) { return s.V * (pitch_cmd - pitch_offset); } // Continuous-time airspeed rate of the plant at the given commands: the energy rate the throttle delivers that // the climb does not absorb float airspeed_rate(const TECSControl::Param &p, float throttle, float pitch_cmd, AircraftState s, float pitch_offset = 0.f, bool throttle_failed = false) { const float ste_rate = ste_rate_modelled(p, throttle_failed ? 0.0f : throttle); const float ske_rate = ste_rate - CONSTANTS_ONE_G * altitude_rate(pitch_cmd, s, pitch_offset); return (s.V > 0.1f) ? ske_rate / s.V : 0.f; // If V < 0.1 we have long stalled... } AircraftState aircraft_step(const TECSControl::Param &p, float dt, float throttle, float pitch_cmd, AircraftState s, float pitch_offset = 0.f, bool throttle_failed = false) { return {s.V + dt * airspeed_rate(p, throttle, pitch_cmd, s, pitch_offset, throttle_failed), s.h + dt * altitude_rate(pitch_cmd, s, pitch_offset)}; } } // --- Test fixture ------------------------------------------------------------- class TECSControlClosedLoopTest : public ::testing::Test { protected: int _step{0}; // Simulation step count float _t{0.f}; // Simulation time [s], _step * DT TECSControl::Param _params{defaultParams()}; // first: the plant and the setpoints derive from it AircraftState _state{_params.equivalent_airspeed_trim, ALT_INIT}; TECSControl _tecs_control{}; TECSControl::Flag _flag{.airspeed_enabled = true, .detect_underspeed_enabled = true}; float _V_sp {_params.equivalent_airspeed_trim}; float _alt_sp {ALT_INIT}; float _pitch {0.f}; float _throttle {_params.throttle_trim}; // Options to re-introduce some basic model mismatch float _plant_pitch_offset {0.f}; // pitch-to-flight-path offset of the plant, unknown to TECS bool _throttle_failed {false}; // simulate failed throttle (giving min total energy rate) bool _tecs_initialised {false}; FILE *_output_csv_file {nullptr}; void TearDown() override { if (_output_csv_file) { std::fclose(_output_csv_file); _output_csv_file = nullptr; } } struct SimulationStats { float max_airspeed_error{0.f}; float max_altitude_error{0.f}; float t_max_airspeed_error{0.f}; // simulation time of the peak (independent of CSV logging) float t_max_altitude_error{0.f}; float max_throttle_integ_norm{0.f}; float max_pitch_integ_norm{0.f}; }; SimulationStats _sim_stats{}; void resetSimStats() { _sim_stats = {}; } void updateSimStats(const AircraftState &state, const TECSControl::DebugOutput &tecs_debug) { auto track_max_norm = [this](float & field, float val, float *t_field = nullptr) { if (std::fabs(val) > field) { field = std::fabs(val); if (t_field) { *t_field = _t; } } }; track_max_norm(_sim_stats.max_airspeed_error, state.V - _V_sp, &_sim_stats.t_max_airspeed_error); track_max_norm(_sim_stats.max_altitude_error, state.h - _alt_sp, &_sim_stats.t_max_altitude_error); track_max_norm(_sim_stats.max_throttle_integ_norm, tecs_debug.throttle_integrator); track_max_norm(_sim_stats.max_pitch_integ_norm, tecs_debug.pitch_integrator); } // Climb rate the plant is flying at the current state under the previous pitch command float plantAltitudeRate() const { return altitude_rate(_pitch, _state, _plant_pitch_offset); } TECSControl::Setpoint makeSetpoint() const { return { .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, }; } TECSControl::Input makeInput() const { // Without an airspeed sensor the TECS airspeed filter falls back to the trim airspeed and a zero // airspeed rate, so that is what the controller sees; the plant airspeed is then open loop. const bool airspeed = _flag.airspeed_enabled; return { .altitude = _state.h, .altitude_rate = plantAltitudeRate(), .tas = airspeed ? _state.V : _params.equivalent_airspeed_trim, .tas_rate = airspeed ? airspeed_rate(_params, _throttle, _pitch, _state, _plant_pitch_offset, _throttle_failed) : 0.f, }; } // Called in first run() call, only use in tests if reset is explicitly needed in the middle of the run. void initTecs() { _tecs_control.initialize(makeSetpoint(), makeInput(), _params, _flag); _tecs_initialised = true; } void setAirspeedSetpoint(float v_sp) { _V_sp = v_sp; } void setAltitudeSetpoint(float alt_sp) { _alt_sp = alt_sp; } // Airspeed sensor failed/absent: TECS flies altitude with pitch and funds it with the throttle feedforward, // the airspeed is open loop. void disableAirspeed() { _flag.airspeed_enabled = false; _flag.detect_underspeed_enabled = false; } // Advance simulation for `duration` seconds, updating _sim_stats. // If TECS_TEST_LOG_DIR is set in the environment, writes a CSV to // $TECS_TEST_LOG_DIR/.csv with one row per timestep. void run(float duration) { if (!_tecs_initialised) { initTecs(); } if (!_output_csv_file) { const char *log_dir = std::getenv("TECS_TEST_LOG_DIR"); if (log_dir) { const auto *test_info = ::testing::UnitTest::GetInstance()->current_test_info(); std::string path = std::string(log_dir) + "/" + test_info->name() + ".csv"; _output_csv_file = std::fopen(path.c_str(), "w"); if (_output_csv_file) { std::fprintf(_output_csv_file, "t,V,h,V_sp,alt_sp,pitch_cmd,throttle," "pitch_integ,throttle_integ," "ste_rate_sp,ste_rate_est,seb_rate_sp,seb_rate_est\n"); } } } const int step_end = _step + static_cast(roundf(duration / DT)); while (_step < step_end) { _tecs_control.update(DT, makeSetpoint(), makeInput(), _params, _flag); _pitch = _tecs_control.getPitchSetpoint(); _throttle = _tecs_control.getThrottleSetpoint(); updateSimStats(_state, _tecs_control.getDebugOutput()); if (_output_csv_file) { const auto &dbg = _tecs_control.getDebugOutput(); std::fprintf(_output_csv_file, "%.4f,%.6f,%.4f,%.4f,%.4f,%.6f,%.6f,%.6f,%.6f,%.4f,%.4f,%.4f,%.4f\n", (double) _t, (double) _state.V, (double) _state.h, (double) _V_sp, (double) _alt_sp, (double) _pitch, (double) _throttle, (double) dbg.pitch_integrator, (double) dbg.throttle_integrator, (double) dbg.total_energy_rate_sp, (double) dbg.total_energy_rate_estimate, (double) dbg.energy_balance_rate_sp, (double) dbg.energy_balance_rate_estimate); } _state = aircraft_step(_params, DT, _throttle, _pitch, _state, _plant_pitch_offset, _throttle_failed); _step++; _t = _step * DT; } } }; // --- Feedforward energy consistency ---------------------------------------------- // With zero errors and zero integrators the throttle and pitch feedforward invert the plant model exactly: // the climb absorbs precisely the energy rate the throttle adds, so the plant airspeed rate is zero in closed // form over the climb rates the throttle can fund and the whole airspeed range. TEST_F(TECSControlClosedLoopTest, FeedforwardIsEnergyConsistent) { _params.altitude_setpoint_gain_ff = 1.f; // the altitude loop passes the reference rate through unchanged for (float h_dot = -_params.min_sink_rate; h_dot <= _params.max_climb_rate; h_dot += 0.1f) { for (float V = _params.tas_min; V <= _params.tas_max; V += 0.1f) { const TECSControl::Setpoint setpoint{ .altitude_reference = {ALT_INIT, h_dot}, .altitude_rate_setpoint_direct = NAN, .tas_setpoint = V }; const TECSControl::Input input{.altitude = ALT_INIT, .altitude_rate = h_dot, .tas = V, .tas_rate = 0.f}; _tecs_control.initialize(setpoint, input, _params, _flag); EXPECT_NEAR(airspeed_rate(_params, _tecs_control.getThrottleSetpoint(), _tecs_control.getPitchSetpoint(), {V, ALT_INIT}), 0.f, 1e-5f) << "h_dot " << h_dot << " V " << V; } } } // --- Steady-state cruise ------------------------------------------------------ // Starting at perfect trim the feedforward is exact, integrators remain zero, // and airspeed must not drift at all. This exercises that the model and TECS // are self-consistent at the operating point. TEST_F(TECSControlClosedLoopTest, SteadyStateCruiseDefaultTuning) { run(60.f); EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_LT(_sim_stats.max_altitude_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; EXPECT_LT(_sim_stats.max_throttle_integ_norm, MAX_NUMERICAL_ERR); EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR); } TEST_F(TECSControlClosedLoopTest, SteadyStateCruiseHighSpeedWeight) { _params.pitch_speed_weight = 1.5f; run(60.f); EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_LT(_sim_stats.max_altitude_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; EXPECT_LT(_sim_stats.max_throttle_integ_norm, MAX_NUMERICAL_ERR); EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR); } TEST_F(TECSControlClosedLoopTest, SteadyStateCruiseLowSpeedWeight) { _params.pitch_speed_weight = 0.5f; run(60.f); EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_LT(_sim_stats.max_altitude_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; EXPECT_LT(_sim_stats.max_throttle_integ_norm, MAX_NUMERICAL_ERR); EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR); } // --- Altitude step ------------------------------------------------------------ TEST_F(TECSControlClosedLoopTest, AltitudeStepDown) { run(10.f); setAltitudeSetpoint(ALT_INIT - ALT_STEP); run(200.f); EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_NEAR(_state.h, _alt_sp, TRACKING_ERROR_TOL); } // Disabled temporarily: // - Throttle damping acts on the energy rate error against the low-passed estimate // - This lag creates a fake error, which the feedback acts on (fix: #29877) // - Because the feedback gain is assumed to be equal across the entire throttle range, but the actual // gain is higher above trim, this only fails the test in the upward step TEST_F(TECSControlClosedLoopTest, DISABLED_AltitudeStepUp) { run(10.f); setAltitudeSetpoint(ALT_INIT + ALT_STEP); run(200.f); EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_NEAR(_state.h, _alt_sp, TRACKING_ERROR_TOL); } // --- Plant pitch-to-flight-path offset (model mismatch) ----------------------- // // The plant flies level at pitch_cmd = 10 deg instead of 0 (e.g. incorrect FW_PSP_OFF). // The pitch integrator should converge to this offset. TEST_F(TECSControlClosedLoopTest, SteadyStateCruisePitchOffset) { _plant_pitch_offset = math::radians(10.f); // Let integrator converge run(120.f); // Reset stats to caputre errors after convergence resetSimStats(); run(60.f); // We fly with low altitude and airspeed error EXPECT_LT(_sim_stats.max_altitude_error, TRACKING_ERROR_TOL) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; EXPECT_LT(_sim_stats.max_airspeed_error, TRACKING_ERROR_TOL) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; // And the pitch integrator has converged to the plant pitch offset EXPECT_NEAR(_tecs_control.getDebugOutput().pitch_integrator, _plant_pitch_offset, math::radians(0.1f)); } TEST_F(TECSControlClosedLoopTest, AltitudeStepUpDefaultTuningPitchOffset) { _plant_pitch_offset = math::radians(10.f); // Let integrator converge run(120.0f); // Altitude step & time to converge setAltitudeSetpoint(ALT_INIT + ALT_STEP); run(120.f); // Reset stats to capture errors after convergence resetSimStats(); run(60.0f); // We fly with low altitude and airspeed error EXPECT_LT(_sim_stats.max_altitude_error, TRACKING_ERROR_TOL) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; EXPECT_LT(_sim_stats.max_airspeed_error, TRACKING_ERROR_TOL) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; // And the pitch integrator has converged to the plant pitch offset EXPECT_NEAR(_tecs_control.getDebugOutput().pitch_integrator, _plant_pitch_offset, math::radians(0.1f)); } // --- Gliding at speed weight 2 --------------------------------------------------- // // Speed weight 2 is the glider setting: the pitch flies airspeed only, so it tracks an airspeed setpoint with no // propulsion at all. TEST_F(TECSControlClosedLoopTest, GlideSpeedWeightTwoAirspeedStep) { _params.pitch_speed_weight = 2.f; run(10.f); _throttle_failed = true; run(60.f); // settle into the glide setAirspeedSetpoint(_params.equivalent_airspeed_trim + AIRSPEED_STEP); run(60.f); EXPECT_NEAR(_state.V, _V_sp, TRACKING_ERROR_TOL); setAirspeedSetpoint(_params.equivalent_airspeed_trim - AIRSPEED_STEP); run(60.f); EXPECT_NEAR(_state.V, _V_sp, TRACKING_ERROR_TOL); } // --- Airspeed setpoint change ------------------------------------------------- TEST_F(TECSControlClosedLoopTest, AirspeedStepUpDefaultTuning) { run(10.f); setAirspeedSetpoint(_params.equivalent_airspeed_trim + AIRSPEED_STEP); // Above trim, only the throttle accelerates and pitch holds altitude (#29009): the airspeed error decays as // a first-order response with the airspeed loop time constant FW_T_TAS_TC, up to a small discretisation error const float t_step = _t; for (int i = 0; i < 10; i++) { run(5.0f); const float expected_error = AIRSPEED_STEP * expf(-(_t - t_step) * _params.airspeed_error_gain); EXPECT_NEAR(_V_sp - _state.V, expected_error, MAX_NUMERICAL_ERR) << "at t=" << _t << " s"; } // Altitude holds to numerical precision EXPECT_LT(_sim_stats.max_altitude_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; } TEST_F(TECSControlClosedLoopTest, AirspeedStepDownDefaultTuning) { run(10.f); setAirspeedSetpoint(_params.equivalent_airspeed_trim - AIRSPEED_STEP); // Capture the rough shape of the airspeed step response. It is not first // order: the SEB rate to pitch conversion ignores the total energy rate, so // the pitch feedforward climbs with the kinetic energy the throttle is // already removing, and the pitch integrator has to take it out again. run(5.0f); EXPECT_NEAR(_state.V, _V_sp, 0.3f * AIRSPEED_STEP) << "at t=" << _t << " s"; run(5.0f); EXPECT_NEAR(_state.V, _V_sp, 0.2f * AIRSPEED_STEP) << "at t=" << _t << " s"; run(5.0f); EXPECT_NEAR(_state.V, _V_sp, 0.1f * AIRSPEED_STEP) << "at t=" << _t << " s"; run(5.0f); EXPECT_NEAR(_state.V, _V_sp, 0.1f * AIRSPEED_STEP) << "at t=" << _t << " s"; run(10.0f); EXPECT_NEAR(_state.V, _V_sp, 0.05f * AIRSPEED_STEP) << "at t=" << _t << " s"; run(10.0f); EXPECT_NEAR(_state.V, _V_sp, 0.02f * AIRSPEED_STEP) << "at t=" << _t << " s"; run(10.0f); EXPECT_NEAR(_state.V, _V_sp, 0.01f * AIRSPEED_STEP) << "at t=" << _t << " s"; // This is possible without sacrificing a lot of altitude EXPECT_LT(_sim_stats.max_altitude_error, 0.5f) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; } TEST_F(TECSControlClosedLoopTest, FastDescend) { // The decision whether or not fast descend runs happens in the outer // TECS class, not TECSControl which we test here - so we can only test // whether fast descend runs correctly when enabling it, NOT whether the // outer logic enables it correctly. run(10.0f); _params.fast_descend = 1.0f; setAirspeedSetpoint(_params.tas_max); initTecs(); // reset the controller state on the mode switch, as the firmware does // Run until we are fully in fast descend run(75.f); // Throttle is forced to minimum during fast descend EXPECT_NEAR(_tecs_control.getThrottleSetpoint(), _params.throttle_min, FLT_EPSILON); // Pitch loop drives airspeed toward tas_max EXPECT_NEAR(_state.V, _params.tas_max, TRACKING_ERROR_TOL); } // --- Airspeed-less mode --------------------------------------------------------- // Without airspeed feedback, pitch and throttle are both only driven by altitude feedback. TEST_F(TECSControlClosedLoopTest, AirspeedlessSteadyStateCruise) { disableAirspeed(); run(60.f); // Level cruise is held very precisely even with open airspeed loop EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_LT(_sim_stats.max_altitude_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_altitude_error << " s"; 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(TECSControlClosedLoopTest, AirspeedlessAltitudeStepHalfFeedforward) { disableAirspeed(); _params.seb_rate_ff = 0.5f; run(10.f); for (const float step : {-ALT_STEP, ALT_STEP}) { 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 does not move EXPECT_LT(_sim_stats.max_airspeed_error, MAX_NUMERICAL_ERR) << "peak at t=" << _sim_stats.t_max_airspeed_error << " s"; EXPECT_LT(_sim_stats.max_pitch_integ_norm, MAX_NUMERICAL_ERR); }