Improved sensor failsafe reporting

src/lib/ecl/validation/data_validator.cpp

@@ -43,6 +43,7 @@
 #include <ecl/ecl.h>
 
 DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_error_mask(ERROR_FLAG_NO_ERROR),
 	_time_last(0),
 	_timeout_interval(20000),
 	_event_count(0),
@@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
 float
 DataValidator::confidence(uint64_t timestamp)
 {
+	float ret = 1.0f;
+
 	/* check if we have any data */
 	if (_time_last == 0) {
-		return 0.0f;
+		_error_mask |= ERROR_FLAG_NO_DATA;
+		ret = 0.0f;
 	}
-
-	/* check error count limit */
-	if (_error_count > NORETURN_ERRCOUNT) {
-		return 0.0f;
+
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		_error_mask |= ERROR_FLAG_TIMEOUT;
+		ret = 0.0f;
 	}
 
 	/* we got the exact same sensor value N times in a row */
 	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
-		return 0.0f;
+		_error_mask |= ERROR_FLAG_STALE_DATA;
+		ret = 0.0f;
 	}
-
-	/* timed out - that's it */
-	if (timestamp - _time_last > _timeout_interval) {
-		return 0.0f;
+
+	/* check error count limit */
+	if (_error_count > NORETURN_ERRCOUNT) {
+		_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
+		ret = 0.0f;
 	}
 
 	/* cap error density counter at window size */
 	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
 		_error_density = ERROR_DENSITY_WINDOW;
 	}
-
-	/* return local error density for last N measurements */
-	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
-}
-
-int
-DataValidator::priority()
-{
-	return _priority;
+
+	/* no critical errors */
+	if(ret > 0.0f) {
+		/* return local error density for last N measurements */
+		ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
+	}
+
+	return ret;
 }
 
 void

src/lib/ecl/validation/data_validator.h

@@ -91,7 +91,18 @@ class __EXPORT DataValidator {
 	 * Get the priority of this validator
 	 * @return		the stored priority
 	 */
-	int			priority();
+	int			priority() { return (_priority); }
+
+	/**
+	 * Get the error state of this validator
+	 * @return		the bitmask with the error status
+	 */
+	uint32_t		state() { return (_error_mask); }
+
+	/**
+	 * Reset the error state of this validator
+	 */
+	void			reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
 
 	/**
 	 * Get the RMS values of this validator
@@ -111,9 +122,20 @@ class __EXPORT DataValidator {
 	 * @param timeout_interval_us The timeout interval in microseconds
 	 */
 	void			set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+
+	/**
+	 * Data validator error states 
+	 */
+	static constexpr uint32_t ERROR_FLAG_NO_ERROR      	= (0x00000000U);
+	static constexpr uint32_t ERROR_FLAG_NO_DATA       	= (0x00000001U);
+	static constexpr uint32_t ERROR_FLAG_STALE_DATA    	= (0x00000001U << 1);
+	static constexpr uint32_t ERROR_FLAG_TIMEOUT 	   	= (0x00000001U << 2);
+	static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT 	= (0x00000001U << 3);
+	static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY 	= (0x00000001U << 4);
 
 private:
 	static const unsigned _dimensions = 3;
+	uint32_t _error_mask;			/**< sensor error state */
 	uint64_t _time_last;			/**< last timestamp */
 	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
 	uint64_t _event_count;			/**< total data counter */

src/lib/ecl/validation/data_validator_group.cpp

@@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 		bool true_failsafe = true;
 
-		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		/* check whether 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;
+			/* reset error flags, this is likely a hotplug sensor coming online late */
+			best->reset_state();
 		}
 
 		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@@ -199,17 +201,25 @@ void
 DataValidatorGroup::print()
 {
 	/* print the group's state */
-	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
 		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
 		_toggle_count);
 
-
 	DataValidator *next = _first;
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		if (next->used()) {
-			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			uint32_t flags = next->state();
+
+			ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
+			((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""),
+			((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""),
+			((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""),
+			((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""),
+			((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""),
+			((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
+
 			next->print();
 		}
 		next = next->sibling();
@@ -222,3 +232,35 @@ DataValidatorGroup::failover_count()
 {
 	return _toggle_count;
 }
+
+int
+DataValidatorGroup::failover_index()
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+			return i;
+		}
+		next = next->sibling();
+		i++;
+	}
+	return -1;
+}
+
+uint32_t
+DataValidatorGroup::failover_state()
+{	
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+			return next->state();
+		}
+		next = next->sibling();
+		i++;
+	}
+	return DataValidator::ERROR_FLAG_NO_ERROR;
+}

src/lib/ecl/validation/data_validator_group.h

@@ -80,6 +80,20 @@ class __EXPORT DataValidatorGroup {
 	 * @return		the number of failovers
 	 */
 	unsigned		failover_count();
+
+	/**
+	 * Get the index of the failed sensor in the group
+	 *
+	 * @return		index of the failed sensor
+	 */
+	int			failover_index();
+
+	/**
+	 * Get the error state of the failed sensor in the group
+	 *
+	 * @return		bitmask with erro states of the failed sensor
+	 */
+	uint32_t		failover_state();
 
 	/**
 	 * Print the validator value

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -348,10 +348,17 @@ void AttitudeEstimatorQ::task_main()
 					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
 				}
 
-				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
-						 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
-				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
-					       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+				/* ignore empty fields */
+				if (sensors.accelerometer_timestamp[i] > 0) {
+					_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
+							 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+				}
+
+				/* ignore empty fields */
+				if (sensors.magnetometer_timestamp[i] > 0) {
+					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
+						       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+				}
 			}
 
 			int best_gyro, best_accel, best_mag;
@@ -369,12 +376,48 @@ void AttitudeEstimatorQ::task_main()
 
 			_data_good = true;
 
-			if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
-					   _voter_accel.failover_count() > 0 ||
-					   _voter_mag.failover_count() > 0)) {
+			if (!_failsafe) {
+				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
+
+				if (_voter_gyro.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_gyro.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
+									  _voter_gyro.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
 
-				_failsafe = true;
-				mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+				if (_voter_accel.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_accel.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
+									  _voter_accel.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_voter_mag.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_mag.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!",
+									  _voter_mag.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_failsafe) {
+					mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+				}
 			}
 
 			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||