From 9db86f7f0a0e571bcc783534df581ca2984676ac Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 11 Dec 2023 12:09:42 +0100 Subject: [PATCH] TECS: fix airspeed filter init in airspeed-less mode Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index f540a8a5e8..0b9a556811 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, @@ -651,7 +658,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();