AP_InertialSensor: prefer sensors that have zero error counts

if a sensor has failed in flight then try not to use it if another
error-free sensor is available

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -229,6 +229,10 @@ AP_InertialSensor::AP_InertialSensor() :
     for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
         _backends[i] = NULL;
     }
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        _accel_error_count[i] = 0;
+        _gyro_error_count[i] = 0;
+    }
 }
 
 
@@ -953,7 +957,7 @@ void AP_InertialSensor::update(void)
     wait_for_sample();
 
     if (!_hil_mode) {
-        for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             // mark sensors unhealthy and let update() in each backend
             // mark them healthy via _rotate_and_offset_gyro() and
             // _rotate_and_offset_accel() 
@@ -963,14 +967,35 @@ void AP_InertialSensor::update(void)
         for (uint8_t i=0; i<_backend_count; i++) {
             _backends[i]->update();
         }
+
+        // adjust health status if a sensor has a non-zero error count
+        // but another sensor doesn't. 
+        bool have_zero_accel_error_count = false;
+        bool have_zero_gyro_error_count = false;
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_accel_healthy[i] && _accel_error_count[i] == 0) {
+                have_zero_accel_error_count = true;
+            }
+            if (_gyro_healthy[i] && _gyro_error_count[i] == 0) {
+                have_zero_gyro_error_count = true;
+            }
+        }
         // set primary to first healthy accel and gyro
-        for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_gyro_healthy[i] && _gyro_error_count[i] != 0 && have_zero_gyro_error_count) {
+                // we prefer not to use a gyro that has had errors
+                _gyro_healthy[i] = false;
+            }
             if (_gyro_healthy[i]) {
                 _primary_gyro = i;
                 break;
             }
         }
-        for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) {
+                // we prefer not to use a accel that has had errors
+                _accel_healthy[i] = false;
+            }
             if (_accel_healthy[i]) {
                 _primary_accel = i;
                 break;

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -124,6 +124,9 @@ public:
     const Vector3f     &get_accel(void) const { return get_accel(_primary_accel); }
     void               set_accel(uint8_t instance, const Vector3f &accel);
 
+    uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; }
+    uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; }
+
     // multi-device interface
     bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; }
     bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
@@ -276,6 +279,9 @@ private:
     // health of gyros and accels
     bool _gyro_healthy[INS_MAX_INSTANCES];
     bool _accel_healthy[INS_MAX_INSTANCES];
+
+    uint32_t _accel_error_count[INS_MAX_INSTANCES];
+    uint32_t _gyro_error_count[INS_MAX_INSTANCES];
 };
 
 #include "AP_InertialSensor_Backend.h"

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -36,6 +36,18 @@ void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const
     _imu._accel_healthy[instance] = true;
 }
 
+// set accelerometer error_count
+void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
+{
+    _imu._accel_error_count[instance] = error_count;
+}
+
+// set gyro error_count
+void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count)
+{
+    _imu._gyro_error_count[instance] = error_count;
+}
+
 /*
   return the default filter frequency in Hz for the sample rate
   

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -67,6 +67,12 @@ protected:
     // rotate accel vector, scale and offset
     void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel);
 
+    // set accelerometer error_count
+    void _set_accel_error_count(uint8_t instance, uint32_t error_count);
+
+    // set gyro error_count
+    void _set_gyro_error_count(uint8_t instance, uint32_t error_count);
+
     // backend should fill in its product ID from AP_PRODUCT_ID_*
     int16_t _product_id;
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -139,6 +139,7 @@ void AP_InertialSensor_PX4::_get_sample(void)
                accel_report.timestamp != _last_accel_timestamp[i]) {        
             _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
             _last_accel_timestamp[i] = accel_report.timestamp;
+            _set_accel_error_count(_accel_instance[i], accel_report.error_count);
         }
     }
     for (uint8_t i=0; i<_num_gyro_instances; i++) {
@@ -148,6 +149,7 @@ void AP_InertialSensor_PX4::_get_sample(void)
                gyro_report.timestamp != _last_gyro_timestamp[i]) {        
             _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
             _last_gyro_timestamp[i] = gyro_report.timestamp;
+            _set_gyro_error_count(_gyro_instance[i], gyro_report.error_count);
         }
     }
     _last_get_sample_timestamp = hal.scheduler->micros64();