attitude_q : verbose failure reporting

PX4 · Nov 14, 2015 · 5a1f7ca · 5a1f7ca
1 parent 0d7cd22
commit 5a1f7ca
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 7 deletions.
diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp
@@ -240,7 +240,7 @@ DataValidatorGroup::failover_index()
 	unsigned i = 0;
 
 	while (next != nullptr) {
-		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
 			return i;
 		}
 		next = next->sibling();
@@ -256,7 +256,7 @@ DataValidatorGroup::failover_state()
 	unsigned i = 0;
 
 	while (next != nullptr) {
-		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
 			return next->state();
 		}
 		next = next->sibling();

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -347,15 +347,17 @@ void AttitudeEstimatorQ::task_main()
 
 					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_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]);
+							 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]);
+						       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
 				}
 			}
 
@@ -375,19 +377,42 @@ void AttitudeEstimatorQ::task_main()
 			_data_good = true;
 
 			if (!_failsafe) {
+				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
+
 				if (_voter_gyro.failover_count() > 0) {
 					_failsafe = true;
-					mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!");
+					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" : ""));
 				}
 
 				if (_voter_accel.failover_count() > 0) {
 					_failsafe = true;
-					mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!");
+					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;
-					mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!");
+					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) {