From 8221940b60493b791f317064ccac9a38e3a8ea77 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 9 Jul 2024 11:16:40 +0200 Subject: [PATCH] Added pitot tube icing detection (#23206) * lib: add FilteredDerivative class * AirspeedValidator: add first principle check - filters throttle, pitch and airspeed rate, and triggers if the airspeed rate is negative even though the vehicle is pitching down and giving high throttle. Check has to fail for duration defined by ASPD_FP_T_WINDOW to trigger an airspeed failure. * AirspeedValidator: define constants for first principle check * FilteredDerivative: set initialised to false if sample interval is invalid * airspeed_selector: improved comment * increase IAS derivative filter time constant from 4 to 5 * use legacy parameter handling for FW_PSP_OFF * handle FW_THR_MAX as well * ROMFS/airframes: exclude some airframes for v6x and v4pro to save flash on them --------- Signed-off-by: Silvan Fuhrer Signed-off-by: RomanBapst Co-authored-by: Silvan Fuhrer --- .../init.d/airframes/4061_atl_mantis_edu | 1 + ROMFS/px4fmu_common/init.d/airframes/4071_ifo | 1 + .../px4fmu_common/init.d/airframes/4073_ifo-s | 1 + .../init.d/airframes/4500_clover4 | 1 + msg/AirspeedValidated.msg | 4 + src/lib/mathlib/CMakeLists.txt | 1 + .../math/filter/FilteredDerivative.hpp | 114 ++++++++++++++++++ .../airspeed_selector/AirspeedValidator.cpp | 66 +++++++++- .../airspeed_selector/AirspeedValidator.hpp | 32 +++++ .../airspeed_selector_main.cpp | 43 ++++++- .../airspeed_selector_params.c | 19 ++- 11 files changed, 278 insertions(+), 5 deletions(-) create mode 100644 src/lib/mathlib/math/filter/FilteredDerivative.hpp diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 66d0973caa..2e52f24ac1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -7,6 +7,7 @@ # # @maintainer # @board px4_fmu-v2 exclude +# @board px4_fmu-v4pro exclude # @board px4_fmu-v5x exclude # @board px4_fmu-v6x exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index ce1c3f8f0e..8cfc14ae1e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -12,6 +12,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board bitcraze_crazyflie exclude # @board cuav_x7pro exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s index a544a49e03..276e1b45db 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s +++ b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s @@ -10,6 +10,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board bitcraze_crazyflie exclude # @board cuav_x7pro exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 index 22f8d74462..885f0239d5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 +++ b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 @@ -8,6 +8,7 @@ # @maintainer Oleg Kalachev # # @board px4_fmu-v2 exclude +# @board px4_fmu-v4pro exclude # @board bitcraze_crazyflie exclude # diff --git a/msg/AirspeedValidated.msg b/msg/AirspeedValidated.msg index 06731cc410..9ee0f51831 100644 --- a/msg/AirspeedValidated.msg +++ b/msg/AirspeedValidated.msg @@ -10,3 +10,7 @@ float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspe bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid + +float32 airspeed_derivative_filtered # filtered indicated airspeed derivative [m/s/s] +float32 throttle_filtered # filtered fixed-wing throttle [-] +float32 pitch_filtered # filtered pitch [rad] diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index aad1d3446c..7f3a3d534e 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_library(mathlib math/test/test.cpp + math/filter/FilteredDerivative.hpp math/filter/LowPassFilter2p.hpp math/filter/MedianFilter.hpp math/filter/NotchFilter.hpp diff --git a/src/lib/mathlib/math/filter/FilteredDerivative.hpp b/src/lib/mathlib/math/filter/FilteredDerivative.hpp new file mode 100644 index 0000000000..f0099334d2 --- /dev/null +++ b/src/lib/mathlib/math/filter/FilteredDerivative.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * @file FilteredDerivative.hpp + * + * @brief Derivative function passed through a first order "alpha" IIR digital filter + * + * @author Silvan Fuhrer + */ + +#pragma once + +// #include +// #include +#include + +using namespace math; + +template +class FilteredDerivative +{ +public: + FilteredDerivative() = default; + ~FilteredDerivative() = default; + + /** + * Set filter parameters for time abstraction + * + * Both parameters have to be provided in the same units. + * + * @param sample_interval interval between two samples + * @param time_constant filter time constant determining convergence + */ + void setParameters(float sample_interval, float time_constant) + { + _alpha_filter.setParameters(sample_interval, time_constant); + _sample_interval = sample_interval; + } + + /** + * Set filter state to an initial value + * + * @param sample new initial value + */ + void reset(const T &sample) + { + _alpha_filter.reset(sample); + _initialized = false; + } + + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ + const T &update(const T &sample) + { + if (_initialized) { + if (_sample_interval > FLT_EPSILON) { + _alpha_filter.update((sample - _previous_sample) / _sample_interval); + + } else { + _initialized = false; + } + + } else { + // don't update in the first iteration + _initialized = true; + } + + _previous_sample = sample; + return _alpha_filter.getState(); + } + + const T &getState() const { return _alpha_filter.getState(); } + + +private: + AlphaFilter _alpha_filter; + float _sample_interval{0.f}; + T _previous_sample{0.f}; + bool _initialized{false}; +}; diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index b83d419b5f..4ebacdbeb8 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -60,6 +60,8 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat check_load_factor(input_data.accel_z); check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, input_data.ground_velocity, input_data.gnss_valid); + check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle, + input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att); update_airspeed_valid_status(input_data.timestamp); } @@ -277,16 +279,76 @@ AirspeedValidator::check_load_factor(float accel_z) } } +void +AirspeedValidator::check_first_principle(const uint64_t timestamp, const float throttle_fw, const float throttle_trim, + const uint64_t tecs_timestamp, const Quatf &att_q) +{ + if (! _first_principle_check_enabled) { + _first_principle_check_failed = false; + _time_last_first_principle_check_passing = timestamp; + return; + } + + const float pitch = matrix::Eulerf(att_q).theta(); + const hrt_abstime tecs_dt = timestamp - tecs_timestamp; // return if TECS data is old (TECS not running) + + if (!_in_fixed_wing_flight || tecs_dt > 500_ms || !PX4_ISFINITE(_IAS) || !PX4_ISFINITE(throttle_fw) + || !PX4_ISFINITE(throttle_trim) || !PX4_ISFINITE(pitch)) { + // do not do anything in that case + return; + } + + const float dt = static_cast(timestamp - _time_last_first_principle_check) / 1_s; + _time_last_first_principle_check = timestamp; + + // update filters + if (dt < FLT_EPSILON || dt > 1.f) { + // reset if dt is too large + _IAS_derivative.reset(0.f); + _throttle_filtered.reset(throttle_fw); + _pitch_filtered.reset(pitch); + _time_last_first_principle_check_passing = timestamp; + + } else { + // update filters, with different time constant + _IAS_derivative.setParameters(dt, 5.f); + _throttle_filtered.setParameters(dt, 0.5f); + _pitch_filtered.setParameters(dt, 1.5f); + + _IAS_derivative.update(_IAS); + _throttle_filtered.update(throttle_fw); + _pitch_filtered.update(pitch); + } + + // declare high throttle if more than 5% above trim + const float high_throttle_threshold = math::min(throttle_trim + kHighThrottleDelta, _param_throttle_max); + const bool high_throttle = _throttle_filtered.getState() > high_throttle_threshold; + const bool pitching_down = _pitch_filtered.getState() < _param_psp_off; + + // check if the airspeed derivative is too low given the throttle and pitch + const bool check_failing = _IAS_derivative.getState() < kIASDerivateThreshold && high_throttle && pitching_down; + + if (!check_failing) { + _time_last_first_principle_check_passing = timestamp; + _first_principle_check_failed = false; + } + + if (timestamp - _time_last_first_principle_check_passing > _aspd_fp_t_window * 1_s) { + // only update the test_failed flag once the timeout since first principle check failing is over + _first_principle_check_failed = check_failing; + } +} void AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) { - if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) { + if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed + || _first_principle_check_failed) { // at least one check (data stuck, innovation or load factor) failed, so record timestamp _time_checks_failed = timestamp; } else if (! _data_stuck_test_failed && !_innovations_check_failed - && !_load_factor_check_failed) { + && !_load_factor_check_failed && !_first_principle_check_failed) { // all checks(data stuck, innovation and load factor) must pass to declare airspeed good _time_checks_passed = timestamp; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 58f79e0009..19c3cc88d1 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include using matrix::Dcmf; @@ -66,6 +68,9 @@ struct airspeed_validator_update_data { float vel_test_ratio; float mag_test_ratio; bool in_fixed_wing_flight; + float fixed_wing_tecs_throttle; + float fixed_wing_tecs_throttle_trim; + uint64_t tecs_timestamp; }; class AirspeedValidator @@ -83,6 +88,9 @@ public: float get_TAS() { return _TAS; } bool get_airspeed_valid() { return _airspeed_valid; } float get_CAS_scale_validated() {return _CAS_scale_validated;} + float get_airspeed_derivative() { return _IAS_derivative.getState(); } + float get_throttle_filtered() { return _throttle_filtered.getState(); } + float get_pitch_filtered() { return _pitch_filtered.getState(); } airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); @@ -118,6 +126,10 @@ public: void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; } void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; } void set_enable_load_factor_check(bool enable) { _load_factor_check_enabled = enable; } + void set_enable_first_principle_check(bool enable) { _first_principle_check_enabled = enable; } + void set_psp_off_param(float psp_off_param) { _param_psp_off = psp_off_param; } + void set_throttle_max_param(float throttle_max_param) { _param_throttle_max = throttle_max_param; } + void set_fp_t_window(float t_window) { _aspd_fp_t_window = t_window; } private: @@ -127,10 +139,17 @@ private: bool _data_stuck_check_enabled{false}; bool _innovation_check_enabled{false}; bool _load_factor_check_enabled{false}; + bool _first_principle_check_enabled{false}; // airspeed scale validity check static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°) + static constexpr float kHighThrottleDelta = + 0.05f; ///< throttle delta above trim throttle required to consider throttle high + static constexpr float kIASDerivateThreshold = + 0.1f; ///< threshold for IAS derivative to detect airspeed failure. Failure is + // detected if in a high throttle and low pitch situation and the filtered IAS derivative is below this threshold + // general states bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks float _IAS{0.0f}; ///< indicated airsped in m/s @@ -158,6 +177,17 @@ private: float _airspeed_stall{8.0f}; ///< stall speed of aircraft used for load factor check float _load_factor_ratio{0.5f}; ///< ratio of maximum load factor predicted by stall speed to measured load factor + // first principle check + bool _first_principle_check_failed{false}; ///< first principle check has detected failure + float _aspd_fp_t_window{0.f}; ///< time window for first principle check + FilteredDerivative _IAS_derivative; ///< indicated airspeed derivative for first principle check + AlphaFilter _throttle_filtered; ///< filtered throttle for first principle check + AlphaFilter _pitch_filtered; ///< filtered pitch for first principle check + hrt_abstime _time_last_first_principle_check{0}; ///< time airspeed first principle was last checked (uSec) + hrt_abstime _time_last_first_principle_check_passing{0}; ///< time airspeed first principle was last passing (uSec) + float _param_psp_off{0.0f}; ///< parameter pitch in level flight [rad] + float _param_throttle_max{0.0f}; ///< parameter maximum throttle value + // states of airspeed valid declaration bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed) float _checks_fail_delay{2.f}; ///< delay for airspeed invalid declaration after single check failure (Sec) @@ -185,6 +215,8 @@ private: void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid); void check_load_factor(float accel_z); + void check_first_principle(const uint64_t timestamp, const float throttle, const float throttle_trim, + const uint64_t tecs_timestamp, const Quatf &att_q); void update_airspeed_valid_status(const uint64_t timestamp); void reset(); void reset_CAS_scale_check(); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 28fb87c2b6..a3f8bcbc23 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -112,6 +113,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; @@ -125,6 +127,7 @@ private: uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; + tecs_status_s _tecs_status {}; estimator_status_s _estimator_status {}; vehicle_acceleration_s _accel {}; vehicle_air_data_s _vehicle_air_data {}; @@ -162,9 +165,16 @@ private: CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0), CHECK_TYPE_DATA_STUCK_BIT = (1 << 1), CHECK_TYPE_INNOVATION_BIT = (1 << 2), - CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3) + CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3), + CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4) }; + + param_t _param_handle_pitch_sp_offset{PARAM_INVALID}; + float _param_pitch_sp_offset{0.0f}; + param_t _param_handle_fw_thr_max{PARAM_INVALID}; + float _param_fw_thr_max{0.0f}; + DEFINE_PARAMETERS( (ParamFloat) _param_aspd_wind_nsd, (ParamFloat) _param_aspd_scale_nsd, @@ -185,8 +195,12 @@ private: (ParamFloat) _checks_fail_delay, /**< delay to declare airspeed invalid */ (ParamFloat) _checks_clear_delay, /**< delay to declare airspeed valid again */ + (ParamFloat) _param_wind_sigma_max_synth_tas, + (ParamFloat) _aspd_fp_t_window, + + // external parameters (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_wind_sigma_max_synth_tas + (ParamFloat) _param_fw_airspd_trim ) void init(); /**< initialization of the airspeed validator instances */ @@ -203,6 +217,8 @@ AirspeedModule::AirspeedModule(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _param_handle_pitch_sp_offset = param_find("FW_PSP_OFF"); + _param_handle_fw_thr_max = param_find("FW_THR_MAX"); // initialise parameters update_params(); @@ -355,6 +371,9 @@ AirspeedModule::Run() input_data.accel_z = _accel.xyz[2]; input_data.vel_test_ratio = _estimator_status.vel_test_ratio; input_data.mag_test_ratio = _estimator_status.mag_test_ratio; + input_data.tecs_timestamp = _tecs_status.timestamp; + input_data.fixed_wing_tecs_throttle = _tecs_status.throttle_sp; + input_data.fixed_wing_tecs_throttle_trim = _tecs_status.throttle_trim; // iterate through all airspeed sensors, poll new data from them and update their validators for (int i = 0; i < _number_of_airspeed_sensors; i++) { @@ -442,6 +461,14 @@ void AirspeedModule::update_params() { updateParams(); + if (_param_handle_pitch_sp_offset != PARAM_INVALID) { + param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset); + } + + if (_param_handle_fw_thr_max != PARAM_INVALID) { + param_get(_param_handle_fw_thr_max, &_param_fw_thr_max); + } + _param_airspeed_scale[0] = _param_airspeed_scale_1.get(); _param_airspeed_scale[1] = _param_airspeed_scale_2.get(); _param_airspeed_scale[2] = _param_airspeed_scale_3.get(); @@ -476,6 +503,11 @@ void AirspeedModule::update_params() CheckTypeBits::CHECK_TYPE_INNOVATION_BIT); _airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() & CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT); + _airspeed_validator[i].set_enable_first_principle_check(_param_airspeed_checks_on.get() & + CheckTypeBits::CHECK_TYPE_FIRST_PRINCIPLE_BIT); + _airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset)); + _airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max); + _airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get()); } } @@ -501,6 +533,8 @@ void AirspeedModule::poll_topics() _vehicle_local_position_sub.update(&_vehicle_local_position); _position_setpoint_sub.update(&_position_setpoint); + _tecs_status_sub.update(&_tecs_status); + if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude; _vehicle_attitude_sub.update(&vehicle_attitude); @@ -661,6 +695,11 @@ void AirspeedModule::select_airspeed_and_publish() airspeed_validated.airspeed_sensor_measurement_valid = false; airspeed_validated.selected_airspeed_index = _valid_airspeed_index; + airspeed_validated.airspeed_derivative_filtered = _airspeed_validator[_valid_airspeed_index - + 1].get_airspeed_derivative(); + airspeed_validated.throttle_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_throttle_filtered(); + airspeed_validated.pitch_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_pitch_filtered(); + switch (_valid_airspeed_index) { case airspeed_index::DISABLED_INDEX: break; diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 9ef58a78c8..0462fbbc34 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -149,11 +149,12 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0. * * @min 0 - * @max 15 + * @max 31 * @bit 0 Only data missing check (triggers if more than 1s no data) * @bit 1 Data stuck (triggers if data is exactly constant for 2s in FW mode) * @bit 2 Innovation check (see ASPD_FS_INNOV) * @bit 3 Load factor check (triggers if measurement is below stall speed) + * @bit 4 First principle check (airspeed change vs. throttle and pitch) * @group Airspeed Validator */ PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7); @@ -239,3 +240,19 @@ PARAM_DEFINE_FLOAT(ASPD_FS_T_START, -1.f); * @group Airspeed Validator */ PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f); + +/** + * First principle airspeed check time window + * + * Window for comparing airspeed change to throttle and pitch change. + * Triggers when the airspeed change within this window is negative while throttle increases + * and the vehicle pitches down. + * Is meant to catch degrading airspeed blockages as can happen when flying through icing conditions. + * Relies on FW_THR_TRIM being set accurately. + * + * @unit s + * @min 0 + * @decimal 1 + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ASPD_FP_T_WINDOW, 2.0f);