AirspeedSelector: do checks for data stopped in airspeed selector, not validator

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/airspeed_validator/AirspeedValidator.cpp

@@ -46,12 +46,6 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	// 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_CAS_scale();
 	update_CAS_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,
@@ -63,6 +57,13 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	update_airspeed_valid_status(input_data.timestamp);
 }
 
+void
+AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)
+{
+	_airspeed_valid = false;
+	_time_checks_failed = 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,
@@ -148,16 +149,14 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 		_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 {
 		const 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());
+			const 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
@@ -185,8 +184,9 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 			}
 		}
 
-		_time_last_aspd_innov_check = time_now;
 	}
+
+	_time_last_aspd_innov_check = time_now;
 }
 
 
@@ -212,9 +212,6 @@ AirspeedValidator::check_load_factor(float accel_z)
 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
-	// 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) {
 		// either innovation or load factor check failed, so record timestamp
 		_time_checks_failed = timestamp;
@@ -233,12 +230,12 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 		// 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) {
+		if (both_checks_failed || single_check_fail_timeout) {
 
 			_airspeed_valid = false;
 		}
 
-	} else if (!_data_stopped_failed && (timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
+	} else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
 		_airspeed_valid = true;
 	}
 }

src/lib/airspeed_validator/AirspeedValidator.hpp

@@ -78,6 +78,8 @@ public:
 
 	void update_airspeed_validator(const airspeed_validator_update_data &input_data);
 
+	void reset_airspeed_to_invalid(const uint64_t timestamp);
+
 	float get_IAS() { return _IAS; }
 	float get_CAS() { return _CAS; }
 	float get_TAS() { return _TAS; }
@@ -125,12 +127,6 @@ private:
 	float _TAS{0.0f}; ///< true airspeed in m/s
 	float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS
 
-	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
@@ -149,7 +145,7 @@ private:
 	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)
+	bool _airspeed_valid{true}; ///< 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)
 	uint64_t	_time_checks_passed{0};	///< time the checks have last passed (uSec)
@@ -167,5 +163,6 @@ private:
 				       float estimator_status_mag_test_ratio);
 	void check_load_factor(float accel_z);
 	void update_airspeed_valid_status(const uint64_t timestamp);
+	void reset();
 
 };

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -146,6 +146,8 @@ private:
 
 	bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
 
+	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
+
 	perf_counter_t _perf_elapsed{};
 
 	DEFINE_PARAMETERS(
@@ -352,6 +354,13 @@ AirspeedModule::Run()
 
 				// push input data into airspeed validator
 				_airspeed_validator[i].update_airspeed_validator(input_data);
+
+				_time_last_airspeed_update[i] = _time_now_usec;
+
+			} else if (_time_last_airspeed_update[i] - _time_now_usec > 1_s) {
+				// declare airspeed invalid if more then 1s since last raw airspeed update
+				_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
+
 			}
 		}
 	}