AP_Compass: rework to make the bus generic for AK8963

Supporting only MPU9250, prepare to implement another bus, i.e i2c on the
bebop

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -106,36 +106,21 @@
 
 extern const AP_HAL::HAL& hal;
 
-AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass) :
+AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) :
     AP_Compass_Backend(compass),
     _state(STATE_UNKNOWN),
     _initialized(false),
     _last_update_timestamp(0),
-    _last_accum_time(0)
+    _last_accum_time(0),
+    _bus(bus)
 {
     _mag_x_accum =_mag_y_accum = _mag_z_accum = 0;
     _mag_x =_mag_y = _mag_z = 0;
     _accum_count = 0;
     _magnetometer_adc_resolution = AK8963_16BIT_ADC;
+    init();
 }
 
-AP_Compass_Backend *AP_Compass_AK8963::detect(Compass &compass)
-{
-    AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass);
-
-    if (sensor == nullptr) {
-        return nullptr;
-    }
-
-    if (!sensor->init()) {
-        delete sensor;
-        return nullptr;
-    }
-
-    return sensor;
-}
-
-
 /* stub to satisfy Compass API*/
 void AP_Compass_AK8963::accumulate(void)
 {
@@ -143,24 +128,17 @@ void AP_Compass_AK8963::accumulate(void)
 
 bool AP_Compass_AK8963::init()
 {
-    _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
+    _bus_sem = _bus->get_semaphore();
-
-    if (_spi == NULL) {
-        hal.console->println_P(PSTR("Cannot get SPIDevice_MPU9250"));
-        return false;
-    }
-
-    _spi_sem = _spi->get_semaphore();
 
     hal.scheduler->suspend_timer_procs();
 
-    if (!_spi_sem->take(100)) {
+    if (!_bus_sem->take(100)) {
-        hal.console->printf("AK8963: Unable to get MPU9250 semaphore");
+        hal.console->printf("AK8963: Unable to get bus semaphore");
         goto fail_sem;
     }
 
-    if (!_configure_mpu9250()) {
+    if (!_bus->configure()) {
-        hal.console->printf_P(PSTR("AK8963: MPU9250 not configured for AK8963\n"));
+        hal.console->printf_P(PSTR("AK8963: Bus not configured for AK8963\n"));
         goto fail;
     }
 
@@ -179,7 +157,7 @@ bool AP_Compass_AK8963::init()
         goto fail;
     }
 
-    if (!_start_conversion()) {
+    if (!_bus->start_conversion()) {
         hal.console->printf_P(PSTR("AK8963: conversion not started\n"));
         goto fail;
     }
@@ -193,14 +171,14 @@ bool AP_Compass_AK8963::init()
 
     hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));
 
-    _spi_sem->give();
+    _bus_sem->give();
 
     hal.scheduler->resume_timer_procs();
 
     return true;
 
 fail:
-    _spi_sem->give();
+    _bus_sem->give();
 fail_sem:
     hal.scheduler->resume_timer_procs();
 
@@ -227,9 +205,6 @@ void AP_Compass_AK8963::read()
     _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
     _accum_count = 0;
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
-    field.rotate(ROTATION_YAW_90);
-#endif
     publish_field(field, _compass_instance);
 }
 
@@ -251,7 +226,7 @@ void AP_Compass_AK8963::_update()
             }
             break;
         case STATE_ERROR:
-            if (_start_conversion()) {
+            if (_bus->start_conversion()) {
                 _state = STATE_SAMPLE;
             }
             break;
@@ -267,7 +242,7 @@ bool AP_Compass_AK8963::_check_id()
 {
     for (int i = 0; i < 5; i++) {
         uint8_t deviceid;
-        _register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */
+        _bus->register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */
 
         if (deviceid == AK8963_Device_ID) {
             return true;
@@ -277,40 +252,26 @@ bool AP_Compass_AK8963::_check_id()
     return false;
 }
 
-bool AP_Compass_AK8963::_configure_mpu9250()
-{
-    if (!AP_InertialSensor_MPU9250::initialize_driver_state())
-        return false;
-
-    uint8_t user_ctrl;
-    _register_read(MPUREG_USER_CTRL, &user_ctrl, 1);
-    _bus_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
-    _bus_write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ);
-
-    return true;
-}
-
 bool AP_Compass_AK8963::_configure() {
-    _register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution);
+    _bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution);
     return true;
 }
 
 bool AP_Compass_AK8963::_reset()
 {
-    _register_write(AK8963_CNTL2, AK8963_RESET);
+    _bus->register_write(AK8963_CNTL2, AK8963_RESET);
-
     return true;
 }
 
 
 bool AP_Compass_AK8963::_calibrate()
 {
-    uint8_t cntl1 = _register_read(AK8963_CNTL1);
+    uint8_t cntl1 = _bus->register_read(AK8963_CNTL1);
 
-    _register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */
+    _bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */
 
     uint8_t response[3];
-    _register_read(AK8963_ASAX, response, 3);
+    _bus->register_read(AK8963_ASAX, response, 3);
 
     for (int i = 0; i < 3; i++) {
         float data = response[i];
@@ -318,21 +279,7 @@ bool AP_Compass_AK8963::_calibrate()
         error("%d: %lf\n", i, _magnetometer_ASA[i]);
     }
 
-    _register_write(AK8963_CNTL1, cntl1);
+    _bus->register_write(AK8963_CNTL1, cntl1);
-
-    return true;
-}
-
-bool AP_Compass_AK8963::_start_conversion()
-{
-    static const uint8_t address = AK8963_INFO;
-    /* Read registers from INFO through ST2 */
-    static const uint8_t count = 0x09;
-
-    _configure_mpu9250();
-    _bus_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);  /* Set the I2C slave addres of AK8963 and set for read. */
-    _bus_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
-    _bus_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */
 
     return true;
 }
@@ -343,7 +290,7 @@ bool AP_Compass_AK8963::_collect_samples()
         return false;
     }
 
-    if (!_read_raw()) {
+    if (!_bus->read_raw(_mag_x, _mag_y, _mag_z)) {
         return false;
     } else {
         _mag_x_accum += _mag_x;
@@ -363,27 +310,27 @@ bool AP_Compass_AK8963::_collect_samples()
 
 bool AP_Compass_AK8963::_sem_take_blocking()
 {
-    return _spi_sem->take(10);
+    return _bus_sem->take(10);
 }
 
 bool AP_Compass_AK8963::_sem_give()
 {
-    return _spi_sem->give();
+    return _bus_sem->give();
 }
 
 bool AP_Compass_AK8963::_sem_take_nonblocking()
 {
     static int _sem_failure_count = 0;
 
-    bool got = _spi_sem->take_nonblocking();
+    bool got = _bus_sem->take_nonblocking();
 
     if (!got) {
         if (!hal.scheduler->system_initializing()) {
             _sem_failure_count++;
             if (_sem_failure_count > 100) {
-                hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem "
+                hal.scheduler->panic(PSTR("PANIC: failed to take _bus->sem "
                                           "100 times in a row, in "
-                                          "AP_Compass_AK8963::_update"));
+                                          "AP_Compass_AK8963"));
             }
         }
         return false; /* never reached */
@@ -412,51 +359,35 @@ void AP_Compass_AK8963::_dump_registers()
 #endif
 }
 
-bool AP_Compass_AK8963::_read_raw()
+AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250()
 {
-    uint8_t rx[14] = {0};
+    _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
-
-    const uint8_t count = 9;
-    _bus_read(MPUREG_EXT_SENS_DATA_00, rx, count);
-
-    uint8_t st2 = rx[8]; /* End data read by reading ST2 register */
-
-#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx]))
-
-    if(!(st2 & 0x08)) {
-        _mag_x = (float) int16_val(rx, 1);
-        _mag_y = (float) int16_val(rx, 2);
-        _mag_z = (float) int16_val(rx, 3);
-
-        if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) {
-            return false;
-        }
 
-        return true;
+    if (_spi == NULL) {
-    } else {
+        hal.console->println_P(PSTR("Cannot get SPIDevice_MPU9250"));
-        return false;
+        return;
     }
-
 }
-void AP_Compass_AK8963::_register_write(uint8_t address, uint8_t value)
+
+void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t address, uint8_t value)
 {
-    _bus_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR);  /* Set the I2C slave addres of AK8963 and set for _register_write. */
+    _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR);  /* Set the I2C slave addres of AK8963 and set for register_write. */
-    _bus_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
+    _write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
-    _bus_write(MPUREG_I2C_SLV0_DO, value); /* Register value to continuous measurement in 16-bit */
+    _write(MPUREG_I2C_SLV0_DO, value); /* Register value to continuous measurement in 16-bit */
-    _bus_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | 0x01); /* Enable I2C and set 1 byte */
+    _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | 0x01); /* Enable I2C and set 1 byte */
 }
 
-void AP_Compass_AK8963::_register_read(uint8_t address, uint8_t *value, uint8_t count)
+void AP_AK8963_SerialBus_MPU9250::register_read(uint8_t address, uint8_t *value, uint8_t count)
 {
-    _bus_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);  /* Set the I2C slave addres of AK8963 and set for read. */
+    _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);  /* Set the I2C slave addres of AK8963 and set for read. */
-    _bus_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
+    _write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
-    _bus_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */
+    _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */
 
     hal.scheduler->delay(10);
-    _bus_read(MPUREG_EXT_SENS_DATA_00, value, count);
+    _read(MPUREG_EXT_SENS_DATA_00, value, count);
 }
 
-void AP_Compass_AK8963::_bus_read(uint8_t address, uint8_t *buf, uint32_t count)
+void AP_AK8963_SerialBus_MPU9250::_read(uint8_t address, uint8_t *buf, uint32_t count)
 {
     ASSERT(count < 150);
     uint8_t tx[150];
@@ -469,7 +400,7 @@ void AP_Compass_AK8963::_bus_read(uint8_t address, uint8_t *buf, uint32_t count)
     memcpy(buf, rx + 1, count);
 }
 
-void AP_Compass_AK8963::_bus_write(uint8_t address, const uint8_t *buf, uint32_t count)
+void AP_AK8963_SerialBus_MPU9250::_write(uint8_t address, const uint8_t *buf, uint32_t count)
 {
     ASSERT(count < 2);
     uint8_t tx[2];
@@ -479,4 +410,62 @@ void AP_Compass_AK8963::_bus_write(uint8_t address, const uint8_t *buf, uint32_t
 
     _spi->transaction(tx, NULL, count + 1);
 }
+
+bool AP_AK8963_SerialBus_MPU9250::configure()
+{
+    if (!AP_InertialSensor_MPU9250::initialize_driver_state())
+        return false;
+
+    uint8_t user_ctrl;
+    register_read(MPUREG_USER_CTRL, &user_ctrl, 1);
+    _write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
+    _write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ);
+
+    return true;
+}
+
+bool AP_AK8963_SerialBus_MPU9250::read_raw(float &mag_x, float &mag_y, float &mag_z)
+{
+    uint8_t rx[14] = {0};
+
+    const uint8_t count = 9;
+    _read(MPUREG_EXT_SENS_DATA_00, rx, count);
+
+    uint8_t st2 = rx[8]; /* End data read by reading ST2 register */
+
+#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx]))
+
+    if(!(st2 & 0x08)) {
+        mag_x = (float) int16_val(rx, 1);
+        mag_y = (float) int16_val(rx, 2);
+        mag_z = (float) int16_val(rx, 3);
+
+        if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
+            return false;
+        }
+
+        return true;
+    } else {
+        return false;
+    }
+}
+
+AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
+{
+    return _spi->get_semaphore();
+}
+
+bool AP_AK8963_SerialBus_MPU9250::start_conversion()
+{
+    static const uint8_t address = AK8963_INFO;
+    /* Read registers from INFO through ST2 */
+    static const uint8_t count = 0x09;
+
+    configure();
+    _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);  /* Set the I2C slave addres of AK8963 and set for read. */
+    _write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
+    _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */
+
+    return true;
+}
 #endif // CONFIG_HAL_BOARD

libraries/AP_Compass/AP_Compass_AK8963.h

@@ -9,10 +9,26 @@
 #include "Compass.h"
 #include "AP_Compass_Backend.h"
 
+class AP_AK8963_SerialBus
+{
+public:
+    virtual void register_read(uint8_t address, uint8_t *value, uint8_t count) = 0;
+    uint8_t register_read(uint8_t address) {
+        uint8_t reg;
+        register_read(address, &reg, 1);
+        return reg;
+    }
+    virtual void register_write(uint8_t address, uint8_t value) = 0;
+    virtual AP_HAL::Semaphore* get_semaphore() = 0;
+    virtual bool start_conversion() = 0;
+    virtual bool configure() = 0;
+    virtual bool read_raw(float &mag_x, float &mag_y, float &mag_z) = 0;
+};
+
 class AP_Compass_AK8963 : public AP_Compass_Backend
 {
 public:
-    AP_Compass_AK8963(Compass &compass);
+    AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus);
 
     static AP_Compass_Backend *detect(Compass &compass);
 
@@ -29,38 +45,16 @@ private:
         STATE_ERROR
     } state_t;
 
-    bool _read_raw();
-
     bool _reset();
     bool _configure();
     bool _check_id();
     bool _calibrate();
 
     void _update();
-    bool _start_conversion();
     bool _collect_samples();
     void _dump_registers();
-
-    bool _configure_mpu9250();
-    void _bus_read(uint8_t address, uint8_t *value, uint32_t count);
-    void _bus_write(uint8_t address, const uint8_t *value,  uint32_t count);
-
-    void _bus_write(uint8_t address, const uint8_t value) {
-        _bus_write(address, &value, 1);
-    }
-
-    void _register_read(uint8_t address, uint8_t *value, uint8_t count);
-
-    uint8_t _register_read(uint8_t address) {
-        uint8_t reg;
-        _register_read(address, &reg, 1);
-        return reg;
-    }
-
-    void _register_write(uint8_t address, uint8_t value);
-
-    bool _sem_take_nonblocking();
     bool _sem_take_blocking();
+    bool _sem_take_nonblocking();
     bool _sem_give();
 
     state_t             _state;
@@ -80,6 +74,29 @@ private:
     uint8_t             _magnetometer_adc_resolution;
     uint32_t            _last_update_timestamp;
     uint32_t            _last_accum_time;
+
+    AP_AK8963_SerialBus *_bus;
+    AP_HAL::Semaphore *_bus_sem;
+};
+
+class AP_AK8963_SerialBus_MPU9250: public AP_AK8963_SerialBus
+{
+public:
+    AP_AK8963_SerialBus_MPU9250();
+    void register_read(uint8_t address, uint8_t *value, uint8_t count);
+    void register_write(uint8_t address, uint8_t value);
+    AP_HAL::Semaphore* get_semaphore();
+    bool start_conversion();
+    bool configure();
+    bool read_raw(float &mag_x, float &mag_y, float &mag_z);
+private:
+    void _read(uint8_t address, uint8_t *value, uint32_t count);
+    void _write(uint8_t address, const uint8_t *value,  uint32_t count);
+    void _write(uint8_t address, const uint8_t value) {
+        _write(address, &value, 1);
+    }
+    AP_HAL::SPIDeviceDriver *_spi;
+    AP_HAL::Semaphore *_spi_sem;
 };
 
 #endif

libraries/AP_Compass/Compass.cpp

@@ -351,7 +351,8 @@ Compass::_detect_backends(void)
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
     _add_backend(AP_Compass_HMC5843::detect);
-    _add_backend(AP_Compass_AK8963::detect);
+    _backends[_backend_count++] = new AP_Compass_AK8963(*this,
+                                    new AP_AK8963_SerialBus_MPU9250());
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
     _add_backend(AP_Compass_HIL::detect);
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843