From 9633de05ff940f45dd2a74520224cfa4c629346c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 5 Feb 2024 14:24:22 +0100 Subject: [PATCH 1/9] lib: add FilteredDerivative class Signed-off-by: Silvan Fuhrer --- src/lib/mathlib/CMakeLists.txt | 1 + .../math/filter/FilteredDerivative.hpp | 111 ++++++++++++++++++ 2 files changed, 112 insertions(+) create mode 100644 src/lib/mathlib/math/filter/FilteredDerivative.hpp diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index aad1d3446c51..7f3a3d534e0a 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 000000000000..09b1fd49777e --- /dev/null +++ b/src/lib/mathlib/math/filter/FilteredDerivative.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 { + // 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}; +}; From bedde0164263ac9fcb816270f53a6e6efca68f73 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 2 Feb 2024 09:36:34 +0100 Subject: [PATCH 2/9] 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. Signed-off-by: Silvan Fuhrer --- msg/AirspeedValidated.msg | 4 ++ .../airspeed_selector/AirspeedValidator.cpp | 66 ++++++++++++++++++- .../airspeed_selector/AirspeedValidator.hpp | 26 ++++++++ .../airspeed_selector_main.cpp | 29 +++++++- .../airspeed_selector_params.c | 18 ++++- 5 files changed, 138 insertions(+), 5 deletions(-) diff --git a/msg/AirspeedValidated.msg b/msg/AirspeedValidated.msg index 06731cc4106a..9ee0f518314e 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/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index b83d419b5fdd..9bae948e3eab 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, 4.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 10% above trim + const float high_throttle_threshold = math::min(throttle_trim + 0.05f, _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() < 0.1f && 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 58f79e0009ce..350d0b42b099 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 @@ class AirspeedValidator 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 @@ class AirspeedValidator 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,6 +139,7 @@ class AirspeedValidator 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°) @@ -158,6 +171,17 @@ class AirspeedValidator 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 +209,8 @@ class AirspeedValidator 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 28fb87c2b6c1..0c432cb63c73 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 @@ class AirspeedModule : public ModuleBase, public ModuleParams, 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 @@ class AirspeedModule : public ModuleBase, public ModuleParams, 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,7 +165,8 @@ class AirspeedModule : public ModuleBase, public ModuleParams, 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) }; DEFINE_PARAMETERS( @@ -185,8 +189,14 @@ class AirspeedModule : public ModuleBase, public ModuleParams, (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, + (ParamFloat) _param_fw_psp_off, + (ParamFloat) _param_fw_thr_max ) void init(); /**< initialization of the airspeed validator instances */ @@ -355,6 +365,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++) { @@ -476,6 +489,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_fw_psp_off.get())); + _airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max.get()); + _airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get()); } } @@ -501,6 +519,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 +681,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 9ef58a78c8bd..9c79cdc6dfc0 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,18 @@ 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 + * + * Time window for comparing airspeed change to throttle and pitch change. + * If the airspeed change within this window is negative while throttle increases + * and the vehicle pitches down, the check will trigger. + * This check is meant to catch degrading airspeed blockages as can happen when flying through icing conditions. + * + * @unit s + * @min 0 + * @decimal 1 + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ASPD_FP_T_WINDOW, 2.0f); From a90af6ab0fb050d9ecd8a098cbc75b04dcbedd42 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 30 May 2024 10:11:54 +0200 Subject: [PATCH 3/9] AirspeedValidator: define constants for first principle check Signed-off-by: RomanBapst --- src/modules/airspeed_selector/AirspeedValidator.cpp | 6 +++--- src/modules/airspeed_selector/AirspeedValidator.hpp | 6 ++++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 9bae948e3eab..cfc91dfba749 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -320,13 +320,13 @@ AirspeedValidator::check_first_principle(const uint64_t timestamp, const float t _pitch_filtered.update(pitch); } - // declare high throttle if more than 10% above trim - const float high_throttle_threshold = math::min(throttle_trim + 0.05f, _param_throttle_max); + // 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() < 0.1f && high_throttle && pitching_down; + const bool check_failing = _IAS_derivative.getState() < kIASDerivateThreshold && high_throttle && pitching_down; if (!check_failing) { _time_last_first_principle_check_passing = timestamp; diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 350d0b42b099..19c3cc88d1b1 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -144,6 +144,12 @@ class AirspeedValidator // 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 From 38bdd05034ea09be7dd5065af0f03a2dcb576c35 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 31 May 2024 07:50:56 +0200 Subject: [PATCH 4/9] FilteredDerivative: set initialised to false if sample interval is invalid Signed-off-by: RomanBapst --- src/lib/mathlib/math/filter/FilteredDerivative.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/mathlib/math/filter/FilteredDerivative.hpp b/src/lib/mathlib/math/filter/FilteredDerivative.hpp index 09b1fd49777e..f0099334d2e7 100644 --- a/src/lib/mathlib/math/filter/FilteredDerivative.hpp +++ b/src/lib/mathlib/math/filter/FilteredDerivative.hpp @@ -89,6 +89,9 @@ class FilteredDerivative if (_initialized) { if (_sample_interval > FLT_EPSILON) { _alpha_filter.update((sample - _previous_sample) / _sample_interval); + + } else { + _initialized = false; } } else { From 9e3502c6b223599a37eb4df05a6b980d1d36670c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Jun 2024 08:44:14 +0200 Subject: [PATCH 5/9] airspeed_selector: improved comment Signed-off-by: RomanBapst --- src/modules/airspeed_selector/airspeed_selector_params.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 9c79cdc6dfc0..0462fbbc34f3 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -244,10 +244,11 @@ PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f); /** * First principle airspeed check time window * - * Time window for comparing airspeed change to throttle and pitch change. - * If the airspeed change within this window is negative while throttle increases - * and the vehicle pitches down, the check will trigger. - * This check is meant to catch degrading airspeed blockages as can happen when flying through icing conditions. + * 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 From 0fa2446a3ce44ea28b9e4fcf061750de15ee33f3 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Jun 2024 12:54:36 +0200 Subject: [PATCH 6/9] increase IAS derivative filter time constant from 4 to 5 Signed-off-by: RomanBapst --- src/modules/airspeed_selector/AirspeedValidator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index cfc91dfba749..4ebacdbeb804 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -311,7 +311,7 @@ AirspeedValidator::check_first_principle(const uint64_t timestamp, const float t } else { // update filters, with different time constant - _IAS_derivative.setParameters(dt, 4.f); + _IAS_derivative.setParameters(dt, 5.f); _throttle_filtered.setParameters(dt, 0.5f); _pitch_filtered.setParameters(dt, 1.5f); From 9c8df36b5096317eea66d38de76507b18030c50a Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 11 Jun 2024 16:15:00 +0200 Subject: [PATCH 7/9] use legacy parameter handling for FW_PSP_OFF Signed-off-by: RomanBapst --- .../airspeed_selector/airspeed_selector_main.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 0c432cb63c73..9fbb116cdb90 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -169,6 +169,10 @@ class AirspeedModule : public ModuleBase, public ModuleParams, CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4) }; + + param_t _param_handle_pitch_sp_offset{PARAM_INVALID}; + float _param_pitch_sp_offset{0.0f}; + DEFINE_PARAMETERS( (ParamFloat) _param_aspd_wind_nsd, (ParamFloat) _param_aspd_scale_nsd, @@ -195,7 +199,6 @@ class AirspeedModule : public ModuleBase, public ModuleParams, // external parameters (ParamFloat) _param_fw_airspd_stall, (ParamFloat) _param_fw_airspd_trim, - (ParamFloat) _param_fw_psp_off, (ParamFloat) _param_fw_thr_max ) @@ -213,6 +216,7 @@ AirspeedModule::AirspeedModule(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _param_handle_pitch_sp_offset = param_find("FW_PSP_OFF"); // initialise parameters update_params(); @@ -455,6 +459,10 @@ void AirspeedModule::update_params() { updateParams(); + if (_param_handle_pitch_sp_offset != PARAM_INVALID) { + param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset); + } + _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(); @@ -491,7 +499,7 @@ void AirspeedModule::update_params() 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_fw_psp_off.get())); + _airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset)); _airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max.get()); _airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get()); } From b3dd9c774e31e90d3334e3a29ae25c58909e797d Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 14 Jun 2024 08:50:58 +0200 Subject: [PATCH 8/9] handle FW_THR_MAX as well Signed-off-by: RomanBapst --- .../airspeed_selector/airspeed_selector_main.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 9fbb116cdb90..a3f8bcbc2351 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -172,6 +172,8 @@ class AirspeedModule : public ModuleBase, public ModuleParams, 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, @@ -198,8 +200,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, // external parameters (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_fw_airspd_trim, - (ParamFloat) _param_fw_thr_max + (ParamFloat) _param_fw_airspd_trim ) void init(); /**< initialization of the airspeed validator instances */ @@ -217,6 +218,7 @@ AirspeedModule::AirspeedModule(): 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(); @@ -463,6 +465,10 @@ void AirspeedModule::update_params() 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(); @@ -500,7 +506,7 @@ void AirspeedModule::update_params() _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.get()); + _airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max); _airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get()); } } From e6f610d468e11f284a0d4d2a5a827dae60e01361 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Jul 2024 10:09:45 +0200 Subject: [PATCH 9/9] ROMFS/airframes: exclude some airframes for v6x and v4pro to save flash on them Signed-off-by: Silvan Fuhrer --- ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu | 1 + ROMFS/px4fmu_common/init.d/airframes/4071_ifo | 1 + ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s | 1 + ROMFS/px4fmu_common/init.d/airframes/4500_clover4 | 1 + 4 files changed, 4 insertions(+) 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 66d0973caa52..2e52f24ac1d4 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 ce1c3f8f0e54..8cfc14ae1ef7 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 a544a49e0367..276e1b45db27 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 22f8d74462be..885f0239d589 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 #