From beeae1f67303f8c61aa8d27a2b3a0ebb578fba99 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 4 Apr 2023 10:46:03 +0200 Subject: [PATCH] TECS: rename TECSReferenceModel to TECSAltitudeReferenceModel Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 20 +++++++++----------- src/lib/tecs/TECS.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 2a68e135d9..106bc19716 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -133,8 +133,8 @@ TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const return _airspeed_state; } -void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude, - const Param ¶m) +void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude, + const Param ¶m) { // Input checks if (!TIMESTAMP_VALID(dt)) { @@ -147,7 +147,6 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se altitude = 0.0f; } - // Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference. if (PX4_ISFINITE(setpoint.alt_rate)) { _alt_rate_ref = setpoint.alt_rate; @@ -155,7 +154,6 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se _alt_rate_ref = 0.0f; } - // Altitude setpoint reference const bool altitude_control_enable{PX4_ISFINITE(setpoint.alt)}; _alt_control_traj_generator.setMaxJerk(param.jerk_max); @@ -182,9 +180,9 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se } } -TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeReference() const +TECSAltitudeReferenceModel::AltitudeReferenceState TECSAltitudeReferenceModel::getAltitudeReference() const { - TECSReferenceModel::AltitudeReferenceState ref{ + TECSAltitudeReferenceModel::AltitudeReferenceState ref{ .alt = _alt_control_traj_generator.getCurrentPosition(), .alt_rate = _alt_control_traj_generator.getCurrentVelocity(), }; @@ -192,12 +190,12 @@ TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeRefere return ref; } -float TECSReferenceModel::getAltitudeRateReference() const +float TECSAltitudeReferenceModel::getAltitudeRateReference() const { return _alt_rate_ref; } -void TECSReferenceModel::initialize(const AltitudeReferenceState &state) +void TECSAltitudeReferenceModel::initialize(const AltitudeReferenceState &state) { float init_state_alt{state.alt}; _alt_rate_ref = state.alt_rate; @@ -639,7 +637,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo const float eas_to_tas) { // Init subclasses - TECSReferenceModel::AltitudeReferenceState current_state{ .alt = altitude, + TECSAltitudeReferenceModel::AltitudeReferenceState current_state{ .alt = altitude, .alt_rate = altitude_rate}; _reference_model.initialize(current_state); _airspeed_filter.initialize(equivalent_airspeed); @@ -661,7 +659,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState(); _debug_status.true_airspeed_filtered = eas_to_tas * eas.speed; _debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate; - const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()}; + const TECSAltitudeReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()}; _debug_status.altitude_sp_ref = ref_alt.alt; _debug_status.altitude_rate_alt_ref = ref_alt.alt_rate; _debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference(); @@ -710,7 +708,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState(); // Update Reference model submodule - const TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, + const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, .alt_rate = hgt_rate_sp}; _reference_model.update(dt, setpoint, altitude, _reference_param); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a7b80d3657..c9f13833ac 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -113,7 +113,7 @@ private: AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state }; -class TECSReferenceModel +class TECSAltitudeReferenceModel { public: /** @@ -139,8 +139,8 @@ public: }; public: - TECSReferenceModel() = default; - ~TECSReferenceModel() = default; + TECSAltitudeReferenceModel() = default; + ~TECSAltitudeReferenceModel() = default; /** * @brief Initialize reference models. @@ -249,7 +249,7 @@ public: * */ struct Setpoint { - TECSReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model. + TECSAltitudeReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model. float altitude_rate_setpoint; ///< Altitude rate setpoint. float tas_setpoint; ///< True airspeed setpoint. }; @@ -638,7 +638,7 @@ public: */ void handle_alt_step(float altitude, float altitude_rate) { - TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude, + TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ .alt = altitude, .alt_rate = altitude_rate}; // reset altitude reference model. @@ -654,7 +654,7 @@ public: private: TECSControl _control; ///< Control submodule. TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule. - TECSReferenceModel _reference_model; ///< Setpoint reference model submodule. + TECSAltitudeReferenceModel _reference_model; ///< Setpoint reference model submodule. enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode. @@ -677,7 +677,7 @@ private: .airspeed_rate_noise_std_dev = 0.02f }; /// Reference model parameters. - TECSReferenceModel::Param _reference_param{ + TECSAltitudeReferenceModel::Param _reference_param{ .target_climbrate = 2.0f, .target_sinkrate = 2.0f, .jerk_max = 1000.0f,