From 5928d7f067f03ad9da1d5f4c2c919a6e9c462aaa Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 7 Dec 2023 10:45:08 +0100 Subject: [PATCH] TECS: init to airspeed filter to trim airspeed if airspeed-less Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 16 ++++++++++++---- src/lib/tecs/TECS.hpp | 5 ++++- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index ae5715d86c..b42a6c2d95 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -52,10 +52,17 @@ using namespace time_literals; static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);} -void TECSAirspeedFilter::initialize(const float equivalent_airspeed) +void TECSAirspeedFilter::initialize(const float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available) { - _airspeed_state.speed = equivalent_airspeed; - _airspeed_state.speed_rate = 0.0f; + if (airspeed_sensor_available && PX4_ISFINITE(equivalent_airspeed)) { + _airspeed_state.speed = equivalent_airspeed; + + } else { + _airspeed_state.speed = equivalent_airspeed_trim; + } + + _airspeed_state.speed_rate = 0.f; } void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m, @@ -654,7 +661,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude, .alt_rate = altitude_rate}; _altitude_reference_model.initialize(current_state); - _airspeed_filter.initialize(equivalent_airspeed); + _airspeed_filter.initialize(equivalent_airspeed, _airspeed_filter_param.equivalent_airspeed_trim, + _control_flag.airspeed_enabled); TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 5673342772..1848a8765b 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -88,8 +88,11 @@ public: * @brief Initialize filter * * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s]. + * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s]. + * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available. */ - void initialize(float equivalent_airspeed); + void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available); /** * @brief Update filter