mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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>
618 lines
25 KiB
C++
618 lines
25 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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 <gtest/gtest.h>
|
|
#include "TECS.hpp"
|
|
#include <lib/geo/geo.h>
|
|
#include <px4_platform_common/param.h>
|
|
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <string>
|
|
|
|
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<size_t>(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/<test_name>.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<int>(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);
|
|
}
|