AP_Compass: HMC5843: Add support for MPU6000 auxiliary bus

Allow HMC5843 to be on MPU6000's auxiliary bus.

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -28,6 +28,8 @@
 #include <AP_HAL/AP_HAL.h>
 
 #include "AP_Compass_HMC5843.h"
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_InertialSensor/AuxiliaryBus.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -89,6 +91,15 @@ AP_Compass_Backend *AP_Compass_HMC5843::detect_i2c(Compass &compass,
     return _detect(compass, bus);
 }
 
+AP_Compass_Backend *AP_Compass_HMC5843::detect_mpu6000(Compass &compass)
+{
+    AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
+    AP_HMC5843_SerialBus *bus = new AP_HMC5843_SerialBus_MPU6000(ins, HMC5843_I2C_ADDR);
+    if (!bus)
+        return nullptr;
+    return _detect(compass, bus);
+}
+
 AP_Compass_Backend *AP_Compass_HMC5843::_detect(Compass &compass,
                                                 AP_HMC5843_SerialBus *bus)
 {
@@ -130,7 +141,7 @@ bool AP_Compass_HMC5843::read_raw()
 {
     struct AP_HMC5843_SerialBus::raw_value rv;
 
-    if (_bus->register_read(0x03, (uint8_t*)&rv, sizeof(rv)) != 0) {
+    if (_bus->read_raw(&rv) != 0) {
         _bus->set_high_speed(false);
         _retry_time = hal.scheduler->millis() + 1000;
         return false;
@@ -247,11 +258,18 @@ AP_Compass_HMC5843::init()
     _bus_sem = _bus->get_semaphore();
     hal.scheduler->suspend_timer_procs();
 
-    if (!_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore"));
+    if (!_bus_sem || !_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        hal.console->printf_P(PSTR("HMC5843: Unable to get bus semaphore\n"));
+        goto fail_sem;
+    }
+
+    if (!_bus->configure()) {
+        hal.console->printf_P(PSTR("HMC5843: Could not configure the bus\n"));
+        goto errout;
     }
 
     if (!_detect_version()) {
+        hal.console->printf_P(PSTR("HMC5843: Could not detect version\n"));
         goto errout;
     }
 
@@ -268,6 +286,7 @@ AP_Compass_HMC5843::init()
     }
 
     if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
+        hal.console->printf_P(PSTR("HMC5843: Could not calibrate sensor\n"));
         goto errout;
     }
 
@@ -276,7 +295,12 @@ AP_Compass_HMC5843::init()
         goto errout;
     }
 
+    if (!_bus->start_measurements()) {
+        hal.console->printf_P(PSTR("HMC5843: Could not start measurements on bus\n"));
+        goto errout;
+    }
     _initialised = true;
+
     _bus_sem->give();
     hal.scheduler->resume_timer_procs();
 
@@ -295,6 +319,7 @@ AP_Compass_HMC5843::init()
 
 errout:
     _bus_sem->give();
+fail_sem:
     hal.scheduler->resume_timer_procs();
     return false;
 }
@@ -441,6 +466,7 @@ void AP_Compass_HMC5843::read()
     _retry_time = 0;
 }
 
+/* I2C implementation of the HMC5843 */
 AP_HMC5843_SerialBus_I2C::AP_HMC5843_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr)
     : _i2c(i2c)
     , _addr(addr)
@@ -466,3 +492,72 @@ AP_HAL::Semaphore* AP_HMC5843_SerialBus_I2C::get_semaphore()
 {
     return _i2c->get_semaphore();
 }
+
+uint8_t AP_HMC5843_SerialBus_I2C::read_raw(struct raw_value *rv)
+{
+    return register_read(0x03, (uint8_t*)rv, sizeof(*rv));
+}
+
+
+/* MPU6000 implementation of the HMC5843 */
+AP_HMC5843_SerialBus_MPU6000::AP_HMC5843_SerialBus_MPU6000(AP_InertialSensor &ins,
+                                                           uint8_t addr)
+{
+    // Only initialize members. Fails are handled by configure or while
+    // getting the semaphore
+    _bus = ins.get_auxiliar_bus(HAL_INS_MPU60XX_SPI);
+    if (!_bus)
+        return;
+    _slave = _bus->request_next_slave(addr);
+}
+
+AP_HMC5843_SerialBus_MPU6000::~AP_HMC5843_SerialBus_MPU6000()
+{
+    /* After started it's owned by AuxiliaryBus */
+    if (!_started)
+        delete _slave;
+}
+
+bool AP_HMC5843_SerialBus_MPU6000::configure()
+{
+    if (!_bus || !_slave)
+        return false;
+    return true;
+}
+
+void AP_HMC5843_SerialBus_MPU6000::set_high_speed(bool val)
+{
+}
+
+uint8_t AP_HMC5843_SerialBus_MPU6000::register_read(uint8_t reg, uint8_t *buf, uint8_t size)
+{
+    return _slave->passthrough_read(reg, buf, size) == size ? 0 : 1;
+}
+
+uint8_t AP_HMC5843_SerialBus_MPU6000::register_write(uint8_t reg, uint8_t val)
+{
+    return _slave->passthrough_write(reg, val) >= 0 ? 0 : 1;
+}
+
+AP_HAL::Semaphore* AP_HMC5843_SerialBus_MPU6000::get_semaphore()
+{
+    return _bus ? _bus->get_semaphore() : nullptr;
+}
+
+uint8_t AP_HMC5843_SerialBus_MPU6000::read_raw(struct raw_value *rv)
+{
+    if (_started)
+        return _slave->read((uint8_t*)rv) >= 0 ? 0 : 1;
+
+    return _slave->passthrough_read(0x03, (uint8_t*)rv, sizeof(*rv)) >= 0 ? 0 : 1;
+}
+
+bool AP_HMC5843_SerialBus_MPU6000::start_measurements()
+{
+    if (_bus->register_periodic_read(_slave, 0x03, sizeof(struct raw_value)) < 0)
+        return false;
+
+    _started = true;
+
+    return true;
+}

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -9,6 +9,9 @@
 #include "Compass.h"
 #include "AP_Compass_Backend.h"
 
+class AuxiliaryBus;
+class AuxiliaryBusSlave;
+class AP_InertialSensor;
 class AP_HMC5843_SerialBus;
 
 class AP_Compass_HMC5843 : public AP_Compass_Backend
@@ -49,6 +52,7 @@ public:
     // detect the sensor
     static AP_Compass_Backend *detect_i2c(Compass &compass,
                                           AP_HAL::I2CDriver *i2c);
+    static AP_Compass_Backend *detect_mpu6000(Compass &compass);
 
     AP_Compass_HMC5843(Compass &compass, AP_HMC5843_SerialBus *bus);
     ~AP_Compass_HMC5843();
@@ -75,6 +79,9 @@ public:
 
     virtual uint8_t register_write(uint8_t reg, uint8_t val) = 0;
     virtual AP_HAL::Semaphore* get_semaphore() = 0;
+    virtual uint8_t read_raw(struct raw_value *rv) = 0;
+    virtual bool configure() { return true; }
+    virtual bool start_measurements() { return true; }
 };
 
 class AP_HMC5843_SerialBus_I2C : public AP_HMC5843_SerialBus
@@ -85,10 +92,30 @@ public:
     uint8_t register_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
     uint8_t register_write(uint8_t reg, uint8_t val) override;
     AP_HAL::Semaphore* get_semaphore() override;
+    uint8_t read_raw(struct raw_value *rv) override;
 
 private:
     AP_HAL::I2CDriver *_i2c;
     uint8_t _addr;
 };
 
+class AP_HMC5843_SerialBus_MPU6000 : public AP_HMC5843_SerialBus
+{
+public:
+    AP_HMC5843_SerialBus_MPU6000(AP_InertialSensor &ins, uint8_t addr);
+    ~AP_HMC5843_SerialBus_MPU6000();
+    void set_high_speed(bool val) override;
+    uint8_t register_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
+    uint8_t register_write(uint8_t reg, uint8_t val) override;
+    AP_HAL::Semaphore* get_semaphore() override;
+    uint8_t read_raw(struct raw_value *rv) override;
+    bool configure() override;
+    bool start_measurements() override;
+
+private:
+    AuxiliaryBus *_bus = nullptr;
+    AuxiliaryBusSlave *_slave = nullptr;
+    bool _started = false;
+};
+
 #endif

libraries/AP_Compass/Compass.cpp

@@ -347,6 +347,8 @@ void Compass::_detect_backends(void)
     _add_backend(AP_Compass_HIL::detect(*this));
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
     _add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
+#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
+    _add_backend(AP_Compass_HMC5843::detect_mpu6000(*this));
 #elif  HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C && HAL_INS_AK8963_I2C_BUS == 1
     _add_backend(AP_Compass_AK8963::detect_i2c(*this, hal.i2c1,
                                                HAL_COMPASS_AK8963_I2C_ADDR));

libraries/AP_InertialSensor/AuxiliaryBus.cpp

@@ -5,7 +5,7 @@
 #include "AuxiliaryBus.h"
 
 AuxiliaryBusSlave::AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
-                                   uint8_t instance)
+                                     uint8_t instance)
     : _bus(bus)
     , _addr(addr)
     , _instance(instance)
@@ -83,7 +83,7 @@ AuxiliaryBusSlave *AuxiliaryBus::request_next_slave(uint8_t addr)
  * Return 0 on success or < 0 on error.
  */
 int AuxiliaryBus::register_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
-                                        uint8_t size)
+                                         uint8_t size)
 {
     assert(slave->_instance == _n_slaves);
     assert(_n_slaves < _max_slaves);