Compass: added compass.accumulate() API

this allows us to accumulate mag readings using spare CPU cycles

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -30,3 +30,8 @@ void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
     mag_z = _mag_z + ofs.z;
     healthy = true;
 }
+
+void AP_Compass_HIL::accumulate(void)
+{
+	// nothing to do
+}

libraries/AP_Compass/AP_Compass_HIL.h

@@ -11,6 +11,7 @@ public:
         product_id = AP_COMPASS_TYPE_HIL;
     }
     bool        read(void);
+    void        accumulate(void);
     void        setHIL(float Mag_X, float Mag_Y, float Mag_Z);
 };
 

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -95,14 +95,42 @@ bool AP_Compass_HMC5843::read_raw()
         return false;
     }
 
-    mag_x = -rx;
-    mag_y = ry;
-    mag_z = -rz;
+    _mag_x = -rx;
+    _mag_y =  ry;
+    _mag_z = -rz;
 
     return true;
 }
 
 
+// accumulate a reading from the magnetometer
+void AP_Compass_HMC5843::accumulate(void)
+{
+   uint32_t tnow = micros();
+   if (healthy && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
+	  // the compass gets new data at 75Hz
+	  return;
+   }
+   if (read_raw()) {
+	  // the _mag_N values are in the range -2048 to 2047, so we can
+	  // accumulate up to 15 of them in an int16_t. Let's make it 14
+	  // for ease of calculation. We expect to do reads at 10Hz, and
+	  // we get new data at most 75Hz, so we don't expect to
+	  // accumulate more than 8 before a read
+	  _mag_x_accum += _mag_x;
+	  _mag_y_accum += _mag_y;
+	  _mag_z_accum += _mag_z;
+	  _accum_count++;
+	  if (_accum_count == 14) {
+		 _mag_x_accum /= 2;
+		 _mag_y_accum /= 2;
+		 _mag_z_accum /= 2;
+		 _accum_count = 7;
+	  }
+	  _last_accum_time = tnow;
+   }
+}
+
 
 /*
  *  re-initialise after a IO error
@@ -178,9 +206,9 @@ AP_Compass_HMC5843::init()
 
         float cal[3];
 
-        cal[0] = fabs(expected_x / (float)mag_x);
-        cal[1] = fabs(expected_yz / (float)mag_y);
-        cal[2] = fabs(expected_yz / (float)mag_z);
+        cal[0] = fabs(expected_x / (float)_mag_x);
+        cal[1] = fabs(expected_yz / (float)_mag_y);
+        cal[2] = fabs(expected_yz / (float)_mag_z);
 
         if (cal[0] > 0.7 && cal[0] < 1.3 &&
             cal[1] > 0.7 && cal[1] < 1.3 &&
@@ -194,11 +222,11 @@ AP_Compass_HMC5843::init()
 #if 0
         /* useful for debugging */
         Serial.print("mag_x: ");
-        Serial.print(mag_x);
+        Serial.print(_mag_x);
         Serial.print(" mag_y: ");
-        Serial.print(mag_y);
+        Serial.print(_mag_y);
         Serial.print(" mag_z: ");
-        Serial.println(mag_z);
+        Serial.println(_mag_z);
         Serial.print("CalX: ");
         Serial.print(calibration[0]/good_count);
         Serial.print(" CalY: ");
@@ -227,6 +255,10 @@ AP_Compass_HMC5843::init()
 
     _initialised = true;
 
+	// perform an initial read
+	healthy = true;
+	read();
+
     return success;
 }
 
@@ -249,16 +281,21 @@ bool AP_Compass_HMC5843::read()
         }
     }
 
-    if (!read_raw()) {
-        // try again in 1 second, and set I2c clock speed slower
-        _retry_time = millis() + 1000;
-        I2c.setSpeed(false);
-        return false;
-    }
+	if (_accum_count == 0) {
+	   accumulate();
+	   if (!healthy || _accum_count == 0) {
+		  // try again in 1 second, and set I2c clock speed slower
+		  _retry_time = millis() + 1000;
+		  I2c.setSpeed(false);
+		  return false;
+	   }
+	}
 
-    mag_x *= calibration[0];
-    mag_y *= calibration[1];
-    mag_z *= calibration[2];
+	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;
+	_accum_count = 0;
+	_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
 
     last_update = micros(); // record time of update
 

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -56,12 +56,22 @@ private:
     bool                write_register(uint8_t address, byte value);
     uint32_t            _retry_time; // when unhealthy the millis() value to retry at
 
+    int16_t			    _mag_x;
+    int16_t			    _mag_y;
+    int16_t			    _mag_z;
+    int16_t             _mag_x_accum;
+    int16_t             _mag_y_accum;
+    int16_t             _mag_z_accum;
+    uint8_t			    _accum_count;
+    uint32_t            _last_accum_time;
+
 public:
     AP_Compass_HMC5843() : Compass() {
     }
-    virtual bool        init(void);
-    virtual bool        read(void);
-    virtual void        set_orientation(enum Rotation rotation);
+    bool        init(void);
+    bool        read(void);
+    void        accumulate(void);
+    void        set_orientation(enum Rotation rotation);
 
 };
 #endif

libraries/AP_Compass/Compass.h

@@ -38,6 +38,10 @@ public:
     ///
     virtual bool read(void) = 0;
 
+    /// use spare CPU cycles to accumulate values from the compass if
+    /// possible
+    virtual void accumulate(void) = 0;
+
     /// Calculate the tilt-compensated heading_ variables.
     ///
     /// @param  roll                The current airframe roll angle.

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -26,7 +26,7 @@ void setup()
     I2c.begin();
     I2c.timeOut(20);
 
-    // I2c.setSpeed(true);
+    I2c.setSpeed(true);
 
     if (!compass.init()) {
         Serial.println("compass initialisation failed!");
@@ -61,6 +61,8 @@ void loop()
 {
     static float min[3], max[3], offset[3];
 
+    compass.accumulate();
+
     if((micros()- timer) > 100000L)
     {
         timer = micros();
@@ -73,6 +75,7 @@ void loop()
             return;
         }
         heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0 for this example
+	compass.null_offsets();
 
         // capture min
         if( compass.mag_x < min[0] )