AP_Compass: added register checking for AK09916 mode

this allows AK09916 on I2C to recover from a power outage in flight

libraries/AP_Compass/AP_Compass_AK09916.cpp

@@ -243,6 +243,9 @@ bool AP_Compass_AK09916::init()
         goto fail;
     }
 
+    // one checked register for mode
+    _bus->setup_checked_registers(1);
+
     if (!_setup_mode()) {
         hal.console->printf("AK09916: Could not setup mode\n");
         goto fail;
@@ -301,22 +304,22 @@ void AP_Compass_AK09916::_update()
     Vector3f raw_field;
 
     if (!_bus->block_read(REG_ST1, (uint8_t *) &regs, sizeof(regs))) {
-        return;
+        goto check_registers;
     }
 
     if (!(regs.st1 & 0x01)) {
-        return;
+        goto check_registers;
     }
 
     /* Check for overflow. See AK09916's datasheet*/
     if ((regs.st2 & 0x08)) {
-        return;
+        goto check_registers;
     }
 
     raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
 
     if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
-        return;
+        goto check_registers;
     }
 
     _make_adc_sensitivity_adjustment(raw_field);
@@ -336,6 +339,9 @@ void AP_Compass_AK09916::_update()
 #endif
 
     accumulate_sample(raw_field, _compass_instance, 10);
+
+check_registers:
+    _bus->check_next_register();
 }
 
 bool AP_Compass_AK09916::_check_id()
@@ -363,7 +369,7 @@ bool AP_Compass_AK09916::_check_id()
 }
 
 bool AP_Compass_AK09916::_setup_mode() {
-    return _bus->register_write(REG_CNTL2, 0x08); //Continuous Mode 2
+    return _bus->register_write(REG_CNTL2, 0x08, true); //Continuous Mode 2
 }
 
 bool AP_Compass_AK09916::_reset()
@@ -387,9 +393,9 @@ bool AP_AK09916_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
     return _dev->read_registers(reg, val, 1);
 }
 
-bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
+bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val, bool checked)
 {
-    return _dev->write_register(reg, val);
+    return _dev->write_register(reg, val, checked);
 }
 
 AP_HAL::Semaphore *AP_AK09916_BusDriver_HALDevice::get_semaphore()
@@ -453,8 +459,9 @@ bool AP_AK09916_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
     return _slave->passthrough_read(reg, val, 1) == 1;
 }
 
-bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
+bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val, bool checked)
 {
+    (void)checked;
     return _slave->passthrough_write(reg, val) == 1;
 }
 

libraries/AP_Compass/AP_Compass_AK09916.h

@@ -86,7 +86,6 @@ private:
 
     AP_AK09916_BusDriver *_bus;
 
-    float _magnetometer_ASA[3] {0, 0, 0};
     bool _force_external;
     uint8_t _compass_instance;
     bool _initialized;
@@ -102,7 +101,7 @@ public:
 
     virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) = 0;
     virtual bool register_read(uint8_t reg, uint8_t *val) = 0;
-    virtual bool register_write(uint8_t reg, uint8_t val) = 0;
+    virtual bool register_write(uint8_t reg, uint8_t val, bool checked=false) = 0;
 
     virtual AP_HAL::Semaphore  *get_semaphore() = 0;
 
@@ -115,6 +114,14 @@ public:
 
     // return 24 bit bus identifier
     virtual uint32_t get_bus_id(void) const = 0;
+
+    /**
+     setup for register value checking. Frequency is how often to
+     check registers. If set to 10 then every 10th call to
+     check_next_register will check a register
+     */
+    virtual void setup_checked_registers(uint8_t num_regs) {}
+    virtual void check_next_register(void) {}
 };
 
 class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver
@@ -124,7 +131,7 @@ public:
 
     virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override;
     virtual bool register_read(uint8_t reg, uint8_t *val) override;
-    virtual bool register_write(uint8_t reg, uint8_t val) override;
+    virtual bool register_write(uint8_t reg, uint8_t val, bool checked) override;
 
     virtual AP_HAL::Semaphore  *get_semaphore() override;
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
@@ -138,6 +145,18 @@ public:
     uint32_t get_bus_id(void) const override {
         return _dev->get_bus_id();
     }
+
+    /**
+     setup for register value checking. Frequency is how often to
+     check registers. If set to 10 then every 10th call to
+     check_next_register will check a register
+     */
+    void setup_checked_registers(uint8_t num_regs) override {
+        _dev->setup_checked_registers(num_regs);
+    }
+    void check_next_register(void) override {
+        _dev->check_next_register();
+    }
     
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
@@ -152,7 +171,7 @@ public:
 
     bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override;
     bool register_read(uint8_t reg, uint8_t *val) override;
-    bool register_write(uint8_t reg, uint8_t val) override;
+    bool register_write(uint8_t reg, uint8_t val, bool checked) override;
 
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;