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 # 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/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..f0099334d2e7 --- /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 b83d419b5fdd..4ebacdbeb804 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 58f79e0009ce..19c3cc88d1b1 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,10 +139,17 @@ 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°) + 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 @@ 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 +215,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..a3f8bcbc2351 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,9 +165,16 @@ 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) }; + + 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 @@ 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 ) 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 9ef58a78c8bd..0462fbbc34f3 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);