AP_Compass: support compass health status on multiple compasses

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -105,7 +105,7 @@ void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
         _hil_mag.rotate(_board_orientation);
     }
 
-    healthy = true;
+    _healthy[0] = true;
 }
 
 void AP_Compass_HIL::accumulate(void)

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -61,7 +61,7 @@ extern const AP_HAL::HAL& hal;
 bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
 {
     if (hal.i2c->readRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
-        healthy = false;
+        _healthy[0] = false;
         return false;
     }
     return true;
@@ -71,7 +71,7 @@ bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
 bool AP_Compass_HMC5843::write_register(uint8_t address, uint8_t value)
 {
     if (hal.i2c->writeRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
-        healthy = false;
+        _healthy[0] = false;
         return false;
     }
     return true;
@@ -83,10 +83,10 @@ bool AP_Compass_HMC5843::read_raw()
     uint8_t buff[6];
 
     if (hal.i2c->readRegisters(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
-        if (healthy) {
+        if (_healthy[0]) {
 			hal.i2c->setHighSpeed(false);
         }
-        healthy = false;
+        _healthy[0] = false;
         _i2c_sem->give();
         return false;
     }
@@ -123,7 +123,7 @@ void AP_Compass_HMC5843::accumulate(void)
         return;
     }
    uint32_t tnow = hal.scheduler->micros();
-   if (healthy && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
+   if (_healthy[0] && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
 	  // the compass gets new data at 75Hz
 	  return;
    }
@@ -191,7 +191,7 @@ AP_Compass_HMC5843::init()
     _base_config = 0;
     if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
         !read_register(ConfigRegA, &_base_config)) {
-        healthy = false;
+        _healthy[0] = false;
         _i2c_sem->give();
         return false;
     }
@@ -280,7 +280,7 @@ AP_Compass_HMC5843::init()
     _initialised = true;
 
 	// perform an initial read
-	healthy = true;
+	_healthy[0] = true;
 	read();
 
     return success;
@@ -295,7 +295,7 @@ bool AP_Compass_HMC5843::read()
         // have the right orientation!)
         return false;
     }
-    if (!healthy) {
+    if (!_healthy[0]) {
         if (hal.scheduler->millis() < _retry_time) {
             return false;
         }
@@ -308,7 +308,7 @@ bool AP_Compass_HMC5843::read()
 
 	if (_accum_count == 0) {
 	   accumulate();
-	   if (!healthy || _accum_count == 0) {
+	   if (!_healthy[0] || _accum_count == 0) {
 		  // try again in 1 second, and set I2c clock speed slower
 		  _retry_time = hal.scheduler->millis() + 1000;
 		  hal.i2c->setHighSpeed(false);
@@ -353,7 +353,7 @@ bool AP_Compass_HMC5843::read()
         _motor_offset.z = 0;
     }
 
-    healthy = true;
+    _healthy[0] = true;
 
     return true;
 }

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -89,14 +89,8 @@ bool AP_Compass_PX4::read(void)
     accumulate();
 
     // consider the compass healthy if we got a reading in the last 0.2s
-    healthy = (hrt_absolute_time() - _last_timestamp[0] < 200000);
-    if (!healthy || _count[0] == 0) {
-        if (was_healthy) {
-            hal.console->printf("Compass unhealthy deltat=%u _count=%u\n",
-                                (unsigned)(hrt_absolute_time() - _last_timestamp[0]),
-                                (unsigned)_count[0]);
-        }
-        return healthy;
+    for (uint8_t i=0; i<_num_instances; i++) {
+        _healthy[i] = (hrt_absolute_time() - _last_timestamp[i] < 200000);
     }
 
     for (uint8_t i=0; i<_num_instances; i++) {
@@ -136,7 +130,7 @@ bool AP_Compass_PX4::read(void)
 
     last_update = _last_timestamp[0];
     
-    return true;
+    return _healthy[0];
 }
 
 void AP_Compass_PX4::accumulate(void)

libraries/AP_Compass/Compass.h

@@ -52,7 +52,6 @@ class Compass
 public:
     int16_t product_id;                         /// product id
     uint32_t last_update;               ///< micros() time of last update
-    bool healthy;                               ///< true if last read OK
 
     /// Constructor
     ///
@@ -103,6 +102,10 @@ public:
     const Vector3f &get_field(uint8_t i) const { return _field[i]; }
     const Vector3f &get_field(void) const { return get_field(0); }
 
+    /// Return the health of a compass
+    bool healthy(uint8_t i) const { return _healthy[i]; }
+    bool healthy(void) const { return healthy(0); }
+
     /// set the current field as a Vector3f
     void set_field(const Vector3f &field) { _field[0] = field; }
 
@@ -136,7 +139,7 @@ public:
 
     /// return true if the compass should be used for yaw calculations
     bool use_for_yaw(void) const {
-        return healthy && _use_for_yaw;
+        return _healthy[0] && _use_for_yaw;
     }
 
     /// Sets the local magnetic field declination.
@@ -214,6 +217,7 @@ public:
     AP_Int8 _learn;                             ///<enable calibration learning
 
 protected:
+    bool _healthy[COMPASS_MAX_INSTANCES];
     Vector3f _field[COMPASS_MAX_INSTANCES];     ///< magnetic field strength
 
     AP_Int8 _orientation;

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -73,7 +73,7 @@ void loop()
         unsigned long read_time = hal.scheduler->micros() - timer;
         float heading;
 
-        if (!compass.healthy) {
+        if (!compass.healthy()) {
             hal.console->println("not healthy");
             return;
         }