AP_Compass: auto-select healthy compass

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -144,4 +144,12 @@ void AP_Compass_PX4::accumulate(void)
     }
 }
 
+uint8_t AP_Compass_PX4::_get_primary(void) const
+{
+    for (uint8_t i=0; i<_num_instances; i++) {
+        if (_healthy[i]) return i;
+    }    
+    return 0;
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_Compass/AP_Compass_PX4.h

@@ -20,6 +20,7 @@ public:
     uint8_t get_count(void) const { return _num_instances; }
 
 private:
+    uint8_t _get_primary(void) const;
     uint8_t _num_instances;
     int _mag_fd[COMPASS_MAX_INSTANCES];
     Vector3f _sum[COMPASS_MAX_INSTANCES];

libraries/AP_Compass/Compass.h

@@ -100,11 +100,11 @@ public:
 
     /// Return the current field as a Vector3f
     const Vector3f &get_field(uint8_t i) const { return _field[i]; }
-    const Vector3f &get_field(void) const { return get_field(0); }
+    const Vector3f &get_field(void) const { return get_field(_get_primary()); }
 
     /// Return the health of a compass
     bool healthy(uint8_t i) const { return _healthy[i]; }
-    bool healthy(void) const { return healthy(0); }
+    bool healthy(void) const { return healthy(_get_primary()); }
 
     /// set the current field as a Vector3f
     void set_field(const Vector3f &field) { _field[0] = field; }
@@ -114,7 +114,7 @@ public:
     /// @returns                    The current compass offsets.
     ///
     const Vector3f &get_offsets(uint8_t i) const { return _offset[i]; }
-    const Vector3f &get_offsets(void) const { return get_offsets(0); }
+    const Vector3f &get_offsets(void) const { return get_offsets(_get_primary()); }
 
     /// Sets the initial location used to get declination
     ///
@@ -217,6 +217,8 @@ public:
     AP_Int8 _learn;                             ///<enable calibration learning
 
 protected:
+    virtual uint8_t _get_primary(void) const { return 0; }
+
     bool _healthy[COMPASS_MAX_INSTANCES];
     Vector3f _field[COMPASS_MAX_INSTANCES];     ///< magnetic field strength