Airspeed Selector: added airspeed validator class

This validator takes measurements from a single airspeed sensor and: -checks validity of measurement -can estimate an airspeed scale factor (estimation enabled with parameter setting) to accout for errors in airspeed due due placement of sensor Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2026-05-20 11:23:06 +08:00 · 2019-08-07 13:03:40 +02:00
parent 248e1818e2
commit 50dc8aee7a
4 changed files with 466 additions and 0 deletions
@@ -0,0 +1,254 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 AirspeedValidator.cpp
+ * Estimates airspeed scale error (from indicated to equivalent airspeed), performes
+ * checks on airspeed measurement input and reports airspeed valid or invalid.
+ */
+
+#include "AirspeedValidator.hpp"
+
+
+void
+AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data)
+{
+	// get indicated airspeed from input data (raw airspeed)
+	_IAS = input_data.airspeed_indicated_raw;
+
+	// to be able to detect missing data, save timestamp (used in data_missing check)
+	if (input_data.airspeed_timestamp != _previous_airspeed_timestamp && input_data.airspeed_timestamp > 0) {
+		_time_last_airspeed = input_data.timestamp;
+		_previous_airspeed_timestamp = input_data.airspeed_timestamp;
+	}
+
+	update_EAS_scale();
+	update_EAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
+	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
+			      input_data.lpos_vy,
+			      input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
+	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
+	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
+	check_load_factor(input_data.accel_z);
+	update_airspeed_valid_status(input_data.timestamp);
+}
+
+void
+AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
+		float lpos_vx, float lpos_vy,
+		float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
+{
+	bool att_valid = true; // att_valid could also be a input_data state
+
+	_wind_estimator.update(time_now_usec);
+
+	if (lpos_valid && att_valid && _in_fixed_wing_flight) {
+
+		Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
+		Quatf q(att_q);
+
+		// sideslip fusion
+		_wind_estimator.fuse_beta(time_now_usec, vI, q);
+
+		// airspeed fusion (with raw TAS)
+		const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
+		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)});
+	}
+}
+
+// this function returns the current states of the wind estimator to be published in the airspeed module
+wind_estimate_s
+AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
+{
+	wind_estimate_s wind_est = {};
+
+	wind_est.timestamp = timestamp;
+	float wind[2];
+	_wind_estimator.get_wind(wind);
+	wind_est.windspeed_north = wind[0];
+	wind_est.windspeed_east = wind[1];
+	float wind_cov[2];
+	_wind_estimator.get_wind_var(wind_cov);
+	wind_est.variance_north = wind_cov[0];
+	wind_est.variance_east = wind_cov[1];
+	wind_est.tas_innov = _wind_estimator.get_tas_innov();
+	wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
+	wind_est.beta_innov = _wind_estimator.get_beta_innov();
+	wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
+	wind_est.tas_scale = _wind_estimator.get_tas_scale();
+	return wind_est;
+}
+
+void
+AirspeedValidator::update_EAS_scale()
+{
+	_EAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.75f, 1.25f);
+}
+
+void
+AirspeedValidator::update_EAS_TAS(float air_pressure_pa, float air_temperature_celsius)
+{
+	_EAS = calc_EAS_from_IAS(_IAS, _EAS_scale);
+	_TAS = calc_TAS_from_EAS(_EAS, air_pressure_pa, air_temperature_celsius);
+}
+
+void
+AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
+		float estimator_status_mag_test_ratio)
+{
+	// Check normalised innovation levels with requirement for continuous data and use of hysteresis
+	// to prevent false triggering.
+
+	if (_wind_estimator.get_wind_estimator_reset()) {
+		_time_wind_estimator_initialized = time_now;
+	}
+
+	/* Reset states if we are not flying */
+	if (!_in_fixed_wing_flight) {
+		// not in a flight condition that enables use of this check, thus pass check
+		_innovations_check_failed = false;
+		_time_last_tas_pass = time_now;
+		_time_last_tas_fail = 0;
+		_airspeed_valid = true;
+		_time_last_aspd_innov_check = time_now;
+
+	} else {
+		float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz
+
+		if (dt_s < 1.0f) {
+			// Compute the ratio of innovation to gate size
+			float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
+					       / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.0f) * _wind_estimator.get_tas_innov_var());
+
+			if (tas_test_ratio <= _tas_innov_threshold) {
+				// record pass and reset integrator used to trigger
+				_time_last_tas_pass = time_now;
+				_apsd_innov_integ_state = 0.0f;
+
+			} else {
+				// integrate exceedance
+				_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold);
+			}
+
+			if ((estimator_status_vel_test_ratio < 1.0f) && (estimator_status_mag_test_ratio < 1.0f)) {
+				// nav velocity data is likely good so airspeed innovations are able to be used
+				if ((_tas_innov_integ_threshold > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold)) {
+					_time_last_tas_fail = time_now;
+				}
+			}
+
+			if (!_innovations_check_failed) {
+				_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;
+
+			} else {
+				_innovations_check_failed = ((time_now - _time_last_tas_fail) < TAS_INNOV_FAIL_DELAY * 100)
+							    || (time_now - _time_wind_estimator_initialized) < TAS_INNOV_FAIL_DELAY * 100;
+			}
+		}
+
+		_time_last_aspd_innov_check = time_now;
+	}
+}
+
+
+void
+AirspeedValidator::check_load_factor(float accel_z)
+{
+	// Check if the airpeed reading is lower than physically possible given the load factor
+
+	bool bad_number_fail = false; // disable this for now
+
+	if (_in_fixed_wing_flight) {
+
+		if (!bad_number_fail) {
+			float max_lift_ratio = fmaxf(_EAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f);
+			max_lift_ratio *= max_lift_ratio;
+
+			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel_z) / 9.81f) / max_lift_ratio;
+			_load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f);
+			_load_factor_check_failed = (_load_factor_ratio > 1.1f);
+
+		} else {
+			_load_factor_check_failed = true; // bad number fail
+		}
+
+	} else {
+
+		_load_factor_ratio = 0.5f; // reset if not in fixed-wing flight (and not in takeoff condition)
+	}
+
+}
+
+
+void
+AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
+{
+
+	bool bad_number_fail = false; // disable this for now
+
+	// Check if sensor data is missing - assume a minimum 5Hz data rate.
+	const bool data_missing = (timestamp - _time_last_airspeed) > 200_ms;
+
+	// Declare data stopped if not received for longer than 1 second
+	_data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s;
+
+	if (_innovations_check_failed || _load_factor_check_failed || data_missing || bad_number_fail) {
+		// either innovation, load factor or data missing check failed, so declare airspeed failing and record timestamp
+		_time_checks_failed = timestamp;
+		_airspeed_failing = true;
+
+	} else if (!_innovations_check_failed && !_load_factor_check_failed && !data_missing && !bad_number_fail) {
+		// All checks must pass to declare airspeed good
+		_time_checks_passed = timestamp;
+		_airspeed_failing = false;
+
+	}
+
+	if (_airspeed_valid) {
+		// A simultaneous load factor and innovaton check fail makes it more likely that a large
+		// airspeed measurement fault has developed, so a fault should be declared immediately
+		const bool both_checks_failed = (_innovations_check_failed && _load_factor_check_failed);
+
+		// Because the innovation, load factor and data missing checks are subject to short duration false positives
+		// a timeout period is applied.
+		const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;
+
+		if (_data_stopped_failed || both_checks_failed || single_check_fail_timeout || bad_number_fail) {
+
+			_airspeed_valid = false;
+		}
+
+	} else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
+		_airspeed_valid = true;
+	}
+}
@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 AirspeedValidator.hpp
+ * Calculates airspeed from differential pressure and checks if this airspeed is valid.
+ */
+
+#pragma once
+
+#include <airspeed/airspeed.h>
+#include <ecl/airdata/WindEstimator.hpp>
+#include <uORB/topics/wind_estimate.h>
+
+
+using matrix::Dcmf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+
+using namespace time_literals;
+
+struct airspeed_validator_update_data {
+	uint64_t timestamp;
+	float airspeed_indicated_raw;
+	float airspeed_true_raw;
+	uint64_t airspeed_timestamp;
+	float lpos_vx;
+	float lpos_vy;
+	float lpos_vz;
+	bool lpos_valid;
+	float lpos_evh;
+	float lpos_evv;
+	float att_q[4];
+	float air_pressure_pa;
+	float air_temperature_celsius;
+	float accel_z;
+	float vel_test_ratio;
+	float mag_test_ratio;
+	bool in_fixed_wing_flight;
+};
+
+class AirspeedValidator
+{
+public:
+	AirspeedValidator() = default;
+	~AirspeedValidator() = default;
+
+	void update_airspeed_validator(const airspeed_validator_update_data &input_data);
+
+	float get_IAS() { return _IAS; }
+	float get_EAS() { return _EAS; }
+	float get_TAS() { return _TAS; }
+	bool get_airspeed_valid() { return _airspeed_valid; }
+	float get_EAS_scale() {return _EAS_scale;}
+
+	wind_estimate_s get_wind_estimator_states(uint64_t timestamp);
+
+	// setters wind estimator parameters
+	void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); }
+	void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); }
+	void set_wind_estimator_tas_noise(float tas_sigma) { _wind_estimator.set_tas_noise(tas_sigma); }
+	void set_wind_estimator_beta_noise(float beta_var) { _wind_estimator.set_beta_noise(beta_var); }
+	void set_wind_estimator_tas_gate(uint8_t gate_size)
+	{
+		_tas_gate = gate_size;
+		_wind_estimator.set_tas_gate(gate_size);
+	}
+
+	void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); }
+	void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) {_wind_estimator_scale_estimation_on = scale_estimation_on;}
+	void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(airspeed_scale_manual);}
+
+	// setters for failure detection tuning parameters
+	void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; }
+	void set_tas_innov_integ_threshold(float tas_innov_integ_threshold) { _tas_innov_integ_threshold = tas_innov_integ_threshold; }
+	void set_checks_fail_delay(float checks_fail_delay) { _checks_fail_delay = checks_fail_delay; }
+	void set_checks_clear_delay(float checks_clear_delay) { _checks_clear_delay = checks_clear_delay; }
+
+	void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; }
+
+private:
+
+	WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator
+
+	// wind estimator parameter
+	bool _wind_estimator_scale_estimation_on{false};	///< online scale estimation (IAS-->CAS/EAS) is on
+
+	// 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
+	float _EAS{0.0f}; ///< equivalent airspeed in m/s
+	float _TAS{0.0f}; ///< true airspeed in m/s
+	float _EAS_scale{1.0f}; ///< scale factor from IAS to EAS
+
+	uint64_t	_time_last_airspeed{0};		///< time last airspeed measurement was received (uSec)
+
+	// states of data stopped check
+	bool _data_stopped_failed{false}; ///< data_stopp check has detected failure
+	hrt_abstime _previous_airspeed_timestamp{0}; ///< timestamp from previous measurement input, to check validity of measurement
+
+	// states of innovation check
+	float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio)
+	bool _innovations_check_failed{false};  ///< true when airspeed innovations have failed consistency checks
+	float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure
+	float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure
+	uint64_t	_time_last_aspd_innov_check{0};	///< time airspeed innovation was last checked (uSec)
+	uint64_t	_time_last_tas_pass{0};		///< last time innovation checks passed
+	uint64_t	_time_last_tas_fail{0};		///< last time innovation checks failed
+	float		_apsd_innov_integ_state{0.0f};	///< inegral of excess normalised airspeed innovation (sec)
+	static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s};	///< time required for innovation levels to pass or fail (usec)
+	uint64_t	_time_wind_estimator_initialized{0};		///< time last time wind estimator was initialized (uSec)
+
+	// states of load factor check
+	bool _load_factor_check_failed{false}; ///< load_factor check has detected failure
+	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
+
+	// states of airspeed valid declaration
+	bool _airspeed_valid{false}; ///< airspeed valid (pitot or groundspeed-windspeed)
+	int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec)
+	int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
+	bool		_airspeed_failing{false};	///< airspeed sensor checks have detected failure, but not yet declared failed
+	uint64_t	_time_checks_passed{0};	///< time the checks have last passed (uSec)
+	uint64_t	_time_checks_failed{0};	///< time the checks have last not passed (uSec)
+
+	void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }
+
+	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx,
+				   float lpos_vy,
+				   float lpos_vz,
+				   float lpos_evh, float lpos_evv, const float att_q[4]);
+	void update_EAS_scale();
+	void update_EAS_TAS(float air_pressure_pa, float air_temperature_celsius);
+	void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
+				       float estimator_status_mag_test_ratio);
+	void check_load_factor(float accel_z);
+	void update_airspeed_valid_status(const uint64_t timestamp);
+
+};
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+px4_add_library(AirspeedValidator AirspeedValidator.cpp)
+
+target_include_directories(AirspeedValidator
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(AirspeedValidator PUBLIC ecl_airdata)
@@ -35,6 +35,7 @@ px4_add_git_submodule(TARGET git_ecl PATH "ecl")
 px4_add_git_submodule(TARGET git_matrix PATH "matrix")

 add_subdirectory(airspeed)
+add_subdirectory(AirspeedValidator)
 add_subdirectory(battery)
 add_subdirectory(bezier)
 add_subdirectory(cdev)