Added pitot tube icing detection (PX4#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 <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Co-authored-by: Silvan Fuhrer <[email protected]>

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
 #

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
 #

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
 #

ROMFS/px4fmu_common/init.d/airframes/4500_clover4

@@ -8,6 +8,7 @@
 # @maintainer Oleg Kalachev <[email protected]>
 #
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v4pro exclude
 # @board bitcraze_crazyflie exclude
 #
 

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]

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

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 <[email protected]>
+ */
+
+#pragma once
+
+// #include <float.h>
+// #include <mathlib/math/Functions.hpp>
+#include <mathlib/math/filter/AlphaFilter.hpp>
+
+using namespace math;
+
+template <typename T>
+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<T> _alpha_filter;
+	float _sample_interval{0.f};
+	T _previous_sample{0.f};
+	bool _initialized{false};
+};

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<float>(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;
 	}

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -41,6 +41,8 @@
 #include <airspeed/airspeed.h>
 #include <lib/wind_estimator/WindEstimator.hpp>
 #include <uORB/topics/airspeed_wind.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/mathlib/math/filter/FilteredDerivative.hpp>
 
 
 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<float> _IAS_derivative; ///< indicated airspeed derivative for first principle check
+	AlphaFilter<float> _throttle_filtered; ///< filtered throttle for first principle check
+	AlphaFilter<float> _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();