AP_Compass: added compass offsets for 2nd compass

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -51,7 +51,7 @@ void AP_Compass_HIL::_setup_earth_field(void)
 bool AP_Compass_HIL::read()
 {
     // get offsets
-    Vector3f ofs = _offset.get();
+    Vector3f ofs = _offset[0].get();
 
     // apply motor compensation
     if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -341,7 +341,7 @@ bool AP_Compass_HMC5843::read()
         _field[0].rotate(_board_orientation);
     }
 
-    _field[0] += _offset.get();
+    _field[0] += _offset[0].get();
 
     // apply motor compensation
     if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -118,7 +118,7 @@ bool AP_Compass_PX4::read(void)
             _sum[i].rotate(_board_orientation);
         }
 
-        _sum[i] += _offset.get();
+        _sum[i] += _offset[i].get();
 
         // apply motor compensation
         if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {

libraries/AP_Compass/Compass.cpp

@@ -22,7 +22,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     // @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
     // @Range: -400 400
     // @Increment: 1
-    AP_GROUPINFO("OFS",    1, Compass, _offset, 0),
+    AP_GROUPINFO("OFS",    1, Compass, _offset[0], 0),
 
     // @Param: DEC
     // @DisplayName: Compass declination
@@ -98,6 +98,10 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("EXTERNAL", 9, Compass, _external, 0),
 
+#if COMPASS_MAX_INSTANCES > 1
+    AP_GROUPINFO("OFS2",    10, Compass, _offset[1], 0),
+#endif
+
     AP_GROUPEND
 };
 
@@ -123,19 +127,15 @@ Compass::init()
 void
 Compass::set_offsets(const Vector3f &offsets)
 {
-    _offset.set(offsets);
+    _offset[0].set(offsets);
 }
 
 void
 Compass::save_offsets()
 {
-    _offset.save();
-}
-
-const Vector3f &
-Compass::get_offsets() const
-{
-    return _offset;
+    for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
+        _offset[k].save();
+    }
 }
 
 void
@@ -248,34 +248,37 @@ Compass::null_offsets(void)
     const float gain = 0.01;
     const float max_change = 10.0;
     const float min_diff = 50.0;
-    Vector3f ofs;
-
-    ofs = _offset.get();
 
     if (!_null_init_done) {
         // first time through
         _null_init_done = true;
+        for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
+            const Vector3f &ofs = _offset[k].get();
             for (uint8_t i=0; i<_mag_history_size; i++) {
                 // fill the history buffer with the current mag vector,
                 // with the offset removed
-            _mag_history[i] = Vector3i((_field[0].x+0.5f) - ofs.x, (_field[0].y+0.5f) - ofs.y, (_field[0].z+0.5f) - ofs.z);
+                _mag_history[k][i] = Vector3i((_field[k].x+0.5f) - ofs.x, (_field[k].y+0.5f) - ofs.y, (_field[k].z+0.5f) - ofs.z);
+            }
+            _mag_history_index[k] = 0;
         }
-        _mag_history_index = 0;
         return;
     }
 
-    Vector3f b1, b2, diff;
+    for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
+        const Vector3f &ofs = _offset[k].get();
+        Vector3f b1, diff;
         float length;
 
         // get a past element
-    b1 = Vector3f(_mag_history[_mag_history_index].x,
-                  _mag_history[_mag_history_index].y,
-                  _mag_history[_mag_history_index].z);
+        b1 = Vector3f(_mag_history[k][_mag_history_index[k]].x,
+                      _mag_history[k][_mag_history_index[k]].y,
+                      _mag_history[k][_mag_history_index[k]].z);
+
         // the history buffer doesn't have the offsets
         b1 += ofs;
 
         // get the current vector
-    b2 = Vector3f(_field[0].x, _field[0].y, _field[0].z);
+        const Vector3f &b2 = _field[k];
 
         // calculate the delta for this sample
         diff = b2 - b1;
@@ -288,13 +291,15 @@ Compass::null_offsets(void)
             // build up before calculating an offset change, as accuracy
             // of the offset change is highly dependent on the size of the
             // rotation.
-        _mag_history_index = (_mag_history_index + 1) % _mag_history_size;
-        return;
+            _mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
+            continue;
         }
 
         // put the vector in the history
-    _mag_history[_mag_history_index] = Vector3i((_field[0].x+0.5f) - ofs.x, (_field[0].y+0.5f) - ofs.y, (_field[0].z+0.5f) - ofs.z);
-    _mag_history_index = (_mag_history_index + 1) % _mag_history_size;
+        _mag_history[k][_mag_history_index[k]] = Vector3i((_field[k].x+0.5f) - ofs.x, 
+                                                          (_field[k].y+0.5f) - ofs.y, 
+                                                          (_field[k].z+0.5f) - ofs.z);
+        _mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
 
         // equation 6 of Bills paper
         diff = diff * (gain * (b2.length() - b1.length()) / length);
@@ -308,5 +313,6 @@ Compass::null_offsets(void)
         }
 
         // set the new offsets
-    _offset.set(_offset.get() - diff);
+        _offset[k].set(_offset[k].get() - diff);
+    }
 }

libraries/AP_Compass/Compass.h

@@ -110,7 +110,8 @@ public:
     ///
     /// @returns                    The current compass offsets.
     ///
-    const Vector3f &get_offsets() const;
+    const Vector3f &get_offsets(uint8_t i) const { return _offset[i]; }
+    const Vector3f &get_offsets(void) const { return get_offsets(0); }
 
     /// Sets the initial location used to get declination
     ///
@@ -216,7 +217,7 @@ protected:
     Vector3f _field[COMPASS_MAX_INSTANCES];     ///< magnetic field strength
 
     AP_Int8 _orientation;
-    AP_Vector3f _offset;
+    AP_Vector3f _offset[COMPASS_MAX_INSTANCES];
     AP_Float _declination;
     AP_Int8 _use_for_yaw;                       ///<enable use for yaw calculation
     AP_Int8 _auto_declination;                  ///<enable automatic declination code
@@ -226,8 +227,8 @@ protected:
 
     ///< used by offset correction
     static const uint8_t _mag_history_size = 20;
-    uint8_t _mag_history_index;
-    Vector3i _mag_history[_mag_history_size];
+    uint8_t _mag_history_index[COMPASS_MAX_INSTANCES];
+    Vector3i _mag_history[COMPASS_MAX_INSTANCES][_mag_history_size];
 
     // motor compensation
     AP_Int8     _motor_comp_type;               // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current