AP_Compass: implement full state machine for MMC3416

this uses 100Hz readings most of the time, but does slow offset
readings every 50 samples

libraries/AP_Compass/AP_Compass_MMC3416.cpp

@@ -30,6 +30,12 @@ extern const AP_HAL::HAL &hal;
 #define REG_CONTROL0        0x07
 #define REG_CONTROL1        0x08
 
+// bits in REG_CONTROL0
+#define REG_CONTROL0_REFILL 0x80
+#define REG_CONTROL0_RESET  0x40
+#define REG_CONTROL0_SET    0x20
+#define REG_CONTROL0_TM     0x01
+
 AP_Compass_Backend *AP_Compass_MMC3416::probe(Compass &compass,
                                               AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
                                               bool force_external,
@@ -102,6 +108,9 @@ bool AP_Compass_MMC3416::init()
     dev->register_periodic_callback(10000,
                                     FUNCTOR_BIND_MEMBER(&AP_Compass_MMC3416::timer, bool));
 
+    // wait 250ms for the compass to make it's initial readings
+    hal.scheduler->delay(250);
+    
     return true;
 
 fail:
@@ -112,18 +121,29 @@ fail:
 bool AP_Compass_MMC3416::timer()
 {
     uint32_t now = AP_HAL::millis();
+    const uint8_t measure_count_limit = 50;
+    const uint16_t zero_offset = 32768; // 16 bit mode
+    const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
+    const float counts_to_milliGauss = 1.0e3f / sensitivity;
     
+    /*
+      we use the SET/RESET method to remove bridge offset every
+      measure_count_limit measurements. This involves a fairly complex
+      state machine, but means we are much less sensitive to
+      temperature changes
+     */
     switch (state) {
     case STATE_REFILL1:
-        if (dev->write_register(REG_CONTROL0, 0x80)) { // REFILL
+        if (dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
             state = STATE_REFILL1_WAIT;
+            last_state_ms = AP_HAL::millis();
         }
         break;
 
     case STATE_REFILL1_WAIT:
         if (now - last_state_ms >= 50) {
-            if (!dev->write_register(REG_CONTROL0, 0x20) || // SET
+            if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET) ||
-                !dev->write_register(REG_CONTROL0, 0x01)) { // Take Measurement
+                !dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
                 state = STATE_REFILL1;
             } else {
                 state = STATE_MEASURE_WAIT1;
@@ -138,10 +158,11 @@ bool AP_Compass_MMC3416::timer()
                 state = STATE_REFILL1;
                 break;
             }
-            if (!dev->write_register(REG_CONTROL0, 0x80)) { // REFILL
+            if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
                 state = STATE_REFILL1;
             } else {
                 state = STATE_REFILL2_WAIT;
+                last_state_ms = AP_HAL::millis();
             }
         }
         break;
@@ -149,8 +170,8 @@ bool AP_Compass_MMC3416::timer()
 
     case STATE_REFILL2_WAIT:
         if (now - last_state_ms >= 50) {
-            if (!dev->write_register(REG_CONTROL0, 0x40) || // RESET
+            if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET) ||
-                !dev->write_register(REG_CONTROL0, 0x01)) { // Take Measurement
+                !dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
                 state = STATE_REFILL1;
             } else {
                 state = STATE_MEASURE_WAIT2;
@@ -168,35 +189,69 @@ bool AP_Compass_MMC3416::timer()
             state = STATE_REFILL1;
             break;
         }
-        const uint16_t zero_offset = 32768; // 16 bit mode
-        const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
-        const float counts_to_milliGauss = 1.0e3f / sensitivity;
         Vector3f field;
 
+        /*
+          calculate field and offset
+         */
         Vector3f f1(float(data0[0]) - zero_offset,
                     float(data0[1]) - zero_offset,
                     float(data0[2]) - zero_offset);
         Vector3f f2(float(data1[0]) - zero_offset,
                     float(data1[1]) - zero_offset,
                     float(data1[2]) - zero_offset);
-        field = (f1 - f2) / 2;
+        field = (f1 - f2) * (counts_to_milliGauss / 2);
-        field * counts_to_milliGauss;
+        Vector3f new_offset = (f1 + f2) * (counts_to_milliGauss / 2);
-        
+        if (!have_initial_offset) {
-        /* rotate raw_field from sensor frame to body frame */
+            offset = new_offset;
-        rotate_field(field, compass_instance);
+        } else {
-        
+            // low pass changes to the offset
-        /* publish raw_field (uncorrected point sample) for calibration use */
+            offset = offset * 0.95 + new_offset * 0.05;
-        publish_raw_field(field, AP_HAL::micros(), compass_instance);
+        }
-        
+
-        /* correct raw_field for known errors */
+#if 0
-        correct_field(field, compass_instance);
+        printf("F(%.1f %.1f %.1f) O(%.1f %.1f %.1f)\n",
-        
+               field.x, field.y, field.z,
-        if (_sem->take(0)) {
+               offset.x, offset.y, offset.z);
-            accum += field;
+#endif
-            accum_count++;
+
-            _sem->give();
+        accumulate_field(field);
+
+        if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
+            state = STATE_REFILL1;
+        } else {
+            state = STATE_MEASURE_WAIT3;
+        }
+        break;
+    }
+
+    case STATE_MEASURE_WAIT3: {
+        uint8_t status;
+        if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
+            break;
+        }
+        uint16_t data1[3];
+        if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
+            state = STATE_REFILL1;
+            break;
+        }
+        Vector3f field(float(data1[0]) - zero_offset,
+                       float(data1[1]) - zero_offset,
+                       float(data1[2]) - zero_offset);
+        field *= -counts_to_milliGauss;
+        field += offset;
+
+        accumulate_field(field);
+
+        // we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles
+        if (measure_count++ >= measure_count_limit) {
+            measure_count = 0;
+            state = STATE_REFILL1;
+        } else {
+            if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
+                state = STATE_REFILL1;
+            }
         }
-        state = STATE_REFILL1;
         break;
     }
     }
@@ -204,9 +259,30 @@ bool AP_Compass_MMC3416::timer()
     return true;
 }
 
+/*
+  accumulate a field
+ */
+void AP_Compass_MMC3416::accumulate_field(Vector3f &field)
+{
+    /* rotate raw_field from sensor frame to body frame */
+    rotate_field(field, compass_instance);
+        
+    /* publish raw_field (uncorrected point sample) for calibration use */
+    publish_raw_field(field, AP_HAL::micros(), compass_instance);
+        
+    /* correct raw_field for known errors */
+    correct_field(field, compass_instance);
+        
+    if (_sem->take(0)) {
+        accum += field;
+        accum_count++;
+        _sem->give();
+    }
+}
+
 void AP_Compass_MMC3416::read()
 {
-    if (!_sem->take_nonblocking()) {
+    if (!_sem->take(0)) {
         return;
     }
     if (accum_count == 0) {

libraries/AP_Compass/AP_Compass_MMC3416.h

@@ -51,6 +51,7 @@ private:
         STATE_MEASURE_WAIT1,
         STATE_REFILL2_WAIT,
         STATE_MEASURE_WAIT2,
+        STATE_MEASURE_WAIT3,
     } state;
     
     /**
@@ -58,11 +59,15 @@ private:
      */
     bool init();
     bool timer();
+    void accumulate_field(Vector3f &field);
 
     uint8_t compass_instance;
     Vector3f accum;
     uint16_t accum_count;
     bool force_external;
+    Vector3f offset;
+    uint8_t measure_count;
+    bool have_initial_offset;
 
     uint16_t data0[3];
     uint32_t last_state_ms;