AP_Compass: switched to a vector based interface

this is more consistent with other APIs and makes multi-device support
easier

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -63,9 +63,7 @@ bool AP_Compass_HIL::read()
     }
 
     // return last values provided by setHIL function
-    mag_x = _hil_mag.x + ofs.x + _motor_offset.x;
-    mag_y = _hil_mag.y + ofs.y + _motor_offset.y;
-    mag_z = _hil_mag.z + ofs.z + _motor_offset.z;
+    _field = _hil_mag + ofs + _motor_offset;
 
     // values set by setHIL function
     last_update = hal.scheduler->micros();      // record time of update

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -316,16 +316,16 @@ bool AP_Compass_HMC5843::read()
 	   }
 	}
 
-	mag_x = _mag_x_accum * calibration[0] / _accum_count;
-	mag_y = _mag_y_accum * calibration[1] / _accum_count;
-	mag_z = _mag_z_accum * calibration[2] / _accum_count;
+	_field.x = _mag_x_accum * calibration[0] / _accum_count;
+	_field.y = _mag_y_accum * calibration[1] / _accum_count;
+	_field.z = _mag_z_accum * calibration[2] / _accum_count;
 	_accum_count = 0;
 	_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
 
     last_update = hal.scheduler->micros(); // record time of update
 
     // rotate to the desired orientation
-    Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
+    Vector3f rot_mag = Vector3f(_field.x,_field.y,_field.z);
     if (product_id == AP_COMPASS_TYPE_HMC5883L) {
         rot_mag.rotate(ROTATION_YAW_90);
     }
@@ -354,9 +354,9 @@ bool AP_Compass_HMC5843::read()
         _motor_offset.z = 0;
     }
 
-    mag_x = rot_mag.x;
-    mag_y = rot_mag.y;
-    mag_z = rot_mag.z;
+    _field.x = rot_mag.x;
+    _field.y = rot_mag.y;
+    _field.z = rot_mag.z;
     healthy = true;
 
     return true;

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -121,14 +121,10 @@ bool AP_Compass_PX4::read(void)
         _motor_offset = _motor_compensation.get() * _thr_or_curr;
         _sum += _motor_offset;
     } else {
-        _motor_offset.x = 0;
-        _motor_offset.y = 0;
-        _motor_offset.z = 0;
+        _motor_offset.zero();
     }
     
-    mag_x = _sum.x;
-    mag_y = _sum.y;
-    mag_z = _sum.z;
+    _field = _sum;
     
     _sum.zero();
     _count = 0;

libraries/AP_Compass/Compass.cpp

@@ -192,10 +192,10 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
     float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
 
     // Tilt compensated magnetic field Y component:
-    float headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y;
+    float headY = _field.y * dcm_matrix.c.z - _field.z * dcm_matrix.c.y;
 
     // Tilt compensated magnetic field X component:
-    float headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z);
+    float headX = _field.x * cos_pitch_sq - dcm_matrix.c.x * (_field.y * dcm_matrix.c.y + _field.z * dcm_matrix.c.z);
 
     // magnetic heading
     // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
@@ -258,7 +258,7 @@ Compass::null_offsets(void)
         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((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z);
+            _mag_history[i] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.z+0.5f) - ofs.z);
         }
         _mag_history_index = 0;
         return;
@@ -275,7 +275,7 @@ Compass::null_offsets(void)
     b1 += ofs;
 
     // get the current vector
-    b2 = Vector3f(mag_x, mag_y, mag_z);
+    b2 = Vector3f(_field.x, _field.y, _field.z);
 
     // calculate the delta for this sample
     diff = b2 - b1;
@@ -293,7 +293,7 @@ Compass::null_offsets(void)
     }
 
     // put the vector in the history
-    _mag_history[_mag_history_index] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z);
+    _mag_history[_mag_history_index] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.z+0.5f) - ofs.z);
     _mag_history_index = (_mag_history_index + 1) % _mag_history_size;
 
     // equation 6 of Bills paper

libraries/AP_Compass/Compass.h

@@ -41,9 +41,6 @@ class Compass
 {
 public:
     int16_t product_id;                         /// product id
-    int16_t mag_x;                      ///< magnetic field strength along the X axis
-    int16_t mag_y;                      ///< magnetic field strength along the Y axis
-    int16_t mag_z;                      ///< magnetic field strength along the Z axis
     uint32_t last_update;               ///< micros() time of last update
     bool healthy;                               ///< true if last read OK
 
@@ -62,6 +59,8 @@ public:
     ///
     virtual bool read(void) = 0;
 
+    
+
     /// use spare CPU cycles to accumulate values from the compass if
     /// possible
     virtual void accumulate(void) = 0;
@@ -87,6 +86,12 @@ public:
     ///
     void save_offsets();
 
+    /// Return the current field as a Vector3f
+    const Vector3f &get_field(void) const { return _field; }
+
+    /// set the current field as a Vector3f
+    void set_field(const Vector3f &field) { _field = field; }
+
     /// Returns the current offset values
     ///
     /// @returns                    The current compass offsets.
@@ -194,6 +199,8 @@ public:
     AP_Int8 _learn;                             ///<enable calibration learning
 
 protected:
+    Vector3f _field;                            ///< magnetic field strength
+
     AP_Int8 _orientation;
     AP_Vector3f _offset;
     AP_Float _declination;

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -84,20 +84,21 @@ void loop()
         compass.null_offsets();
 
         // capture min
-        if( compass.mag_x < min[0] )
-            min[0] = compass.mag_x;
-        if( compass.mag_y < min[1] )
-            min[1] = compass.mag_y;
-        if( compass.mag_z < min[2] )
-            min[2] = compass.mag_z;
+        const Vector3f &mag = compass.get_field();
+        if( mag.x < min[0] )
+            min[0] = mag.x;
+        if( mag.y < min[1] )
+            min[1] = mag.y;
+        if( mag.z < min[2] )
+            min[2] = mag.z;
 
         // capture max
-        if( compass.mag_x > max[0] )
-            max[0] = compass.mag_x;
-        if( compass.mag_y > max[1] )
-            max[1] = compass.mag_y;
-        if( compass.mag_z > max[2] )
-            max[2] = compass.mag_z;
+        if( mag.x > max[0] )
+            max[0] = mag.x;
+        if( mag.y > max[1] )
+            max[1] = mag.y;
+        if( mag.z > max[2] )
+            max[2] = mag.z;
 
         // calculate offsets
         offset[0] = -(max[0]+min[0])/2;
@@ -107,9 +108,9 @@ void loop()
         // display all to user
         hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
 			    ToDeg(heading),
-			    (int)compass.mag_x,
-			    (int)compass.mag_y,
-			    (int)compass.mag_z, 
+			    (int)mag.x,
+			    (int)mag.y,
+			    (int)mag.z, 
 			    (unsigned)hal.i2c->lockup_count());
 
         // display offsets