AP_Compass: allow for more than one compass in HIL/SITL

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -27,9 +27,9 @@ extern const AP_HAL::HAL& hal;
 
 // constructor
 AP_Compass_HIL::AP_Compass_HIL(Compass &compass):
-    AP_Compass_Backend(compass),
+    AP_Compass_Backend(compass)
-    _compass_instance(0)
 {
+    memset(_compass_instance, 0, sizeof(_compass_instance));
     _compass._setup_earth_field();
 }
 
@@ -50,12 +50,18 @@ AP_Compass_Backend *AP_Compass_HIL::detect(Compass &compass)
 bool
 AP_Compass_HIL::init(void)
 {
-    // register the compass instance in the frontend
+    // register two compass instances
-    _compass_instance = register_compass();
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        _compass_instance[i] = register_compass();
+    }
     return true;
 }
 
 void AP_Compass_HIL::read()
 {
-    publish_field(_compass._hil.field, _compass_instance);
+    for (uint8_t i=0; i<sizeof(_compass_instance)/sizeof(_compass_instance[0]); i++) {
+        if (_compass._hil.healthy[i]) {
+            publish_field(_compass._hil.field[_compass_instance[i]], _compass_instance[i]);
+        }
+    }
 }

libraries/AP_Compass/AP_Compass_HIL.h

@@ -15,7 +15,7 @@ public:
     static AP_Compass_Backend *detect(Compass &compass);
 
 private:
-    uint8_t     _compass_instance;    
+    uint8_t     _compass_instance[COMPASS_MAX_INSTANCES];
 };
 
 #endif

libraries/AP_Compass/Compass.cpp

@@ -568,7 +568,7 @@ bool Compass::configured(void)
 
 // Update raw magnetometer values from HIL data
 //
-void Compass::setHIL(float roll, float pitch, float yaw)
+void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
 {
     Matrix3f R;
 
@@ -582,31 +582,34 @@ void Compass::setHIL(float roll, float pitch, float yaw)
 
     // convert the earth frame magnetic vector to body frame, and
     // apply the offsets
-    _hil.field = R.mul_transpose(_hil.Bearth);
+    _hil.field[instance] = R.mul_transpose(_hil.Bearth);
 
     // apply default board orientation for this compass type. This is
     // a noop on most boards
-    _hil.field.rotate(MAG_BOARD_ORIENTATION);
+    _hil.field[instance].rotate(MAG_BOARD_ORIENTATION);
 
     // add user selectable orientation
-    _hil.field.rotate((enum Rotation)_state[0].orientation.get());
+    _hil.field[instance].rotate((enum Rotation)_state[0].orientation.get());
 
     if (!_state[0].external) {
         // and add in AHRS_ORIENTATION setting if not an external compass
-        _hil.field.rotate(_board_orientation);
+        _hil.field[instance].rotate(_board_orientation);
     }
+    _hil.healthy[instance] = true;
 }
 
 // Update raw magnetometer values from HIL mag vector
 //
-void Compass::setHIL(const Vector3f &mag)
+void Compass::setHIL(uint8_t instance, const Vector3f &mag)
 {
-    _hil.field = mag;
+    _hil.field[instance] = mag;
+    _hil.healthy[instance] = true;
     _last_update_usec = hal.scheduler->micros();
 }
 
-const Vector3f& Compass::getHIL() const {
+const Vector3f& Compass::getHIL(uint8_t instance) const 
-    return _hil.field;
+{
+    return _hil.field[instance];
 }
 
 // setup _Bearth

libraries/AP_Compass/Compass.h

@@ -35,7 +35,7 @@
    maximum number of compass instances available on this platform. If more
    than 1 then redundent sensors may be available
  */
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
 #define COMPASS_MAX_INSTANCES 3
 #define COMPASS_MAX_BACKEND   3   
 #else
@@ -223,9 +223,9 @@ public:
     uint8_t get_primary(void) const { return _primary; }
 
     // HIL methods
-    void        setHIL(float roll, float pitch, float yaw);
+    void        setHIL(uint8_t instance, float roll, float pitch, float yaw);
-    void        setHIL(const Vector3f &mag);
+    void        setHIL(uint8_t instance, const Vector3f &mag);
-    const Vector3f&   getHIL() const;
+    const Vector3f&   getHIL(uint8_t instance) const;
     void        _setup_earth_field();
 
     // enable HIL mode
@@ -240,7 +240,8 @@ public:
     struct {
         Vector3f Bearth;
         float last_declination;
-        Vector3f field;
+        bool healthy[COMPASS_MAX_INSTANCES];
+        Vector3f field[COMPASS_MAX_INSTANCES];
     } _hil;
 
 private: