mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 22:50:49 +08:00
test(tecs): clean up logging & variable names
Stop mixing up previous and current steps:
- _pitch and _throttle hold the last TECS command, which the plant
applies until the next update (drop the _prev naming)
- reorder logging, so each row pairs a state with the command
computed from it:
- find control input
- sim stats, csv output
- advance simulation
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Balduin <balduin@auterion.com>
This commit is contained in:
1 parent
b35a34efb1
commit
4d77b71c72
1 file changed
+21
-20
@@ -175,13 +175,13 @@ protected:
|
||||
|
||||
TECSControl::Param _params{defaultParams()}; // first: the plant and the setpoints derive from it
|
||||
AircraftState _state{_params.equivalent_airspeed_trim, ALT_INIT};
|
||||
TECSControl _tecs{};
|
||||
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_prev {0.f}; // pitch command from previous step, for altitude_rate input
|
||||
float _throttle_prev{_params.throttle_trim}; // throttle command from previous step, for tas_rate input
|
||||
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
|
||||
@@ -229,7 +229,7 @@ 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); }
|
||||
float plantAltitudeRate() const { return altitude_rate(_pitch, _state, _plant_pitch_offset); }
|
||||
|
||||
TECSControl::Setpoint makeSetpoint() const
|
||||
{
|
||||
@@ -249,14 +249,14 @@ protected:
|
||||
.altitude = _state.h,
|
||||
.altitude_rate = plantAltitudeRate(),
|
||||
.tas = airspeed ? _state.V : _params.equivalent_airspeed_trim,
|
||||
.tas_rate = airspeed ? airspeed_rate(_params, _throttle_prev, _pitch_prev, _state, _plant_pitch_offset, _throttle_failed) : 0.f,
|
||||
.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.initialize(makeSetpoint(), makeInput(), _params, _flag);
|
||||
_tecs_control.initialize(makeSetpoint(), makeInput(), _params, _flag);
|
||||
_tecs_initialised = true;
|
||||
}
|
||||
|
||||
@@ -309,25 +309,26 @@ protected:
|
||||
|
||||
while (_step < step_end) {
|
||||
|
||||
_tecs.update(DT, makeSetpoint(), makeInput(), _params, _flag);
|
||||
_tecs_control.update(DT, makeSetpoint(), makeInput(), _params, _flag);
|
||||
_pitch = _tecs_control.getPitchSetpoint();
|
||||
_throttle = _tecs_control.getThrottleSetpoint();
|
||||
|
||||
_pitch_prev = _tecs.getPitchSetpoint();
|
||||
_throttle_prev = _tecs.getThrottleSetpoint();
|
||||
_state = aircraft_step(_params, DT, _throttle_prev, _pitch_prev, _state, _plant_pitch_offset, _throttle_failed);
|
||||
updateSimStats(_state, _tecs.getDebugOutput());
|
||||
updateSimStats(_state, _tecs_control.getDebugOutput());
|
||||
|
||||
if (_output_csv_file) {
|
||||
const auto &dbg = _tecs.getDebugOutput();
|
||||
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_prev, (double) _tecs.getThrottleSetpoint(),
|
||||
(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;
|
||||
}
|
||||
@@ -351,9 +352,9 @@ TEST_F(TECSControlClosedLoopTest, FeedforwardIsEnergyConsistent)
|
||||
};
|
||||
const TECSControl::Input input{.altitude = ALT_INIT, .altitude_rate = h_dot, .tas = V, .tas_rate = 0.f};
|
||||
|
||||
_tecs.initialize(setpoint, input, _params, _flag);
|
||||
_tecs_control.initialize(setpoint, input, _params, _flag);
|
||||
|
||||
EXPECT_NEAR(airspeed_rate(_params, _tecs.getThrottleSetpoint(), _tecs.getPitchSetpoint(), {V, ALT_INIT}), 0.f,
|
||||
EXPECT_NEAR(airspeed_rate(_params, _tecs_control.getThrottleSetpoint(), _tecs_control.getPitchSetpoint(), {V, ALT_INIT}), 0.f,
|
||||
1e-5f) << "h_dot " << h_dot << " V " << V;
|
||||
}
|
||||
}
|
||||
@@ -440,7 +441,7 @@ TEST_F(TECSControlClosedLoopTest, SteadyStateCruisePitchOffset)
|
||||
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.getDebugOutput().pitch_integrator, _plant_pitch_offset, math::radians(0.1f));
|
||||
EXPECT_NEAR(_tecs_control.getDebugOutput().pitch_integrator, _plant_pitch_offset, math::radians(0.1f));
|
||||
}
|
||||
|
||||
TEST_F(TECSControlClosedLoopTest, AltitudeStepUpDefaultTuningPitchOffset)
|
||||
@@ -463,7 +464,7 @@ TEST_F(TECSControlClosedLoopTest, AltitudeStepUpDefaultTuningPitchOffset)
|
||||
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.getDebugOutput().pitch_integrator, _plant_pitch_offset, math::radians(0.1f));
|
||||
EXPECT_NEAR(_tecs_control.getDebugOutput().pitch_integrator, _plant_pitch_offset, math::radians(0.1f));
|
||||
}
|
||||
|
||||
// --- Gliding at speed weight 2 ---------------------------------------------------
|
||||
@@ -553,7 +554,7 @@ TEST_F(TECSControlClosedLoopTest, FastDescend)
|
||||
run(75.f);
|
||||
|
||||
// Throttle is forced to minimum during fast descend
|
||||
EXPECT_NEAR(_tecs.getThrottleSetpoint(), _params.throttle_min, 1e-3f);
|
||||
EXPECT_NEAR(_tecs_control.getThrottleSetpoint(), _params.throttle_min, 1e-3f);
|
||||
// Pitch loop drives airspeed toward tas_max
|
||||
EXPECT_NEAR(_state.V, _params.tas_max, TRACKING_ERROR_TOL);
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user