Validation: Use error density instead of error count. Fail over to higher priority sensors once they become available.

src/lib/ecl/validation/data_validator.cpp

@@ -44,9 +44,10 @@
 
 DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_time_last(0),
-	_timeout_interval(70000),
+	_timeout_interval(20000),
 	_event_count(0),
 	_error_count(0),
+	_error_density(0),
 	_priority(0),
 	_mean{0.0f},
 	_lp{0.0f},
@@ -68,6 +69,13 @@ void
 DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
 {
 	_event_count++;
+
+	if (error_count_in > _error_count) {
+		_error_density += (error_count_in - _error_count);
+	} else if (_error_density > 0) {
+		_error_density--;
+	}
+
 	_error_count = error_count_in;
 	_priority = priority_in;
 
@@ -123,7 +131,13 @@ DataValidator::confidence(uint64_t timestamp)
 		return 0.0f;
 	}
 
-	return 1.0f;
+	/* cap error density counter at window size */
+	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_density = ERROR_DENSITY_WINDOW;
+	}
+
+	/* return local error density for last N measurements */
+	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
 }
 
 int
@@ -136,12 +150,13 @@ void
 DataValidator::print()
 {
 	if (_time_last == 0) {
-		ECL_INFO("\tno data\n");
+		ECL_INFO("\tno data");
 		return;
 	}
 
 	for (unsigned i = 0; i < _dimensions; i++) {
-		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
-			(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
+		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f",
+			(double) _value[i], (double)_lp[i], (double)_mean[i],
+			(double)_rms[i], (double)confidence(hrt_absolute_time()));
 	}
 }

src/lib/ecl/validation/data_validator.h

@@ -81,6 +81,12 @@ public:
 	 */
 	float*			value() { return _value; }
 
+	/**
+	 * Get the used status of this validator
+	 * @return		true if this validator ever saw data
+	 */
+	bool			used() { return (_time_last > 0); }
+
 	/**
 	 * Get the priority of this validator
 	 * @return		the stored priority
@@ -112,6 +118,7 @@ private:
 	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
 	uint64_t _event_count;			/**< total data counter */
 	uint64_t _error_count;			/**< error count */
+	int _error_density;			/**< ratio between successful reads and errors */
 	int _priority;				/**< sensor nominal priority */
 	float _mean[_dimensions];		/**< mean of value */
 	float _lp[3];				/**< low pass value */
@@ -120,7 +127,8 @@ private:
 	float _value[3];			/**< last value */
 	float _value_equal_count;		/**< equal values in a row */
 	DataValidator *_sibling;		/**< sibling in the group */
-	const unsigned NORETURN_ERRCOUNT = 100;	/**< if the error count reaches this value, return sensor as invalid */
+	const unsigned NORETURN_ERRCOUNT = 10000;	/**< if the error count reaches this value, return sensor as invalid */
+	const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
 	const unsigned VALUE_EQUAL_COUNT_MAX = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
 
 	/* we don't want this class to be copied */

src/lib/ecl/validation/data_validator_group.cpp

@@ -97,24 +97,35 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 	// XXX This should eventually also include voting
 	int pre_check_best = _curr_best;
+	float pre_check_confidence = 1.0f;
+	int pre_check_prio = -1;
 	float max_confidence = -1.0f;
 	int max_priority = -1000;
 	int max_index = -1;
-	uint64_t min_error_count = 30000;
 	DataValidator *best = nullptr;
 
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		float confidence = next->confidence(timestamp);
-		if (confidence > max_confidence ||
-			(fabsf(confidence - max_confidence) < 0.01f &&
-				((next->error_count() < min_error_count) &&
-				(next->priority() >= max_priority)))) {
+
+		if (i == pre_check_best) {
+			pre_check_prio = next->priority();
+			pre_check_confidence = confidence;
+		}
+
+		/*
+		 * Switch if:
+		 * 1) the confidence is higher and priority is equal or higher
+		 * 2) the confidence is no less than 1% different and the priority is higher
+		 */
+		if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
+			(confidence > max_confidence && (next->priority() >= max_priority)) ||
+			(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
+			) {
 			max_index = i;
 			max_confidence = confidence;
 			max_priority = next->priority();
-			min_error_count = next->error_count();
 			best = next;
 		}
 
@@ -125,17 +136,29 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	/* the current best sensor is not matching the previous best sensor */
 	if (max_index != _curr_best) {
 
+		bool true_failsafe = true;
+
+		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
+			fabsf(pre_check_confidence - max_confidence) < 0.1f) {
+			/* this is not a failover */
+			true_failsafe = false;
+		}
+
 		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
 		if (_curr_best < 0) {
 			_prev_best = max_index;
 		} else {
 			/* we were initialized before, this is a real failsafe */
 			_prev_best = pre_check_best;
-			_toggle_count++;
 
-			/* if this is the first time, log when we failed */
-			if (_first_failover_time == 0) {
-				_first_failover_time = timestamp;
+			if (true_failsafe) {
+				_toggle_count++;
+
+				/* if this is the first time, log when we failed */
+				if (_first_failover_time == 0) {
+					_first_failover_time = timestamp;
+				}
 			}
 		}
 
@@ -185,8 +208,10 @@ DataValidatorGroup::print()
 	unsigned i = 0;
 
 	while (next != nullptr) {
-		ECL_INFO("sensor #%u:\n", i);
-		next->print();
+		if (next->used()) {
+			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			next->print();
+		}
 		next = next->sibling();
 		i++;
 	}

src/lib/ecl/validation/data_validator_group.h

@@ -100,6 +100,7 @@ private:
 	int _prev_best;		/**< the previous best index */
 	uint64_t _first_failover_time;	/**< timestamp where the first failover occured or zero if none occured */
 	unsigned _toggle_count;		/**< number of back and forth switches between two sensors */
+	static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
 
 	/* we don't want this class to be copied */
 	DataValidatorGroup(const DataValidatorGroup&);