From a66a201bf586f221438cb10816c0fa010d2fa4bb Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 10 Aug 2015 20:30:32 -0300 Subject: [PATCH] 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 | 101 ++++++++++++++++++- libraries/AP_Compass/AP_Compass_HMC5843.h | 27 +++++ libraries/AP_Compass/Compass.cpp | 2 + libraries/AP_InertialSensor/AuxiliaryBus.cpp | 4 +- 4 files changed, 129 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 960f17238e..bba61da5d6 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -28,6 +28,8 @@ #include #include "AP_Compass_HMC5843.h" +#include +#include 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; +} diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 9bb258475e..e6a267329b 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/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 diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index baa3f5dcde..4223b9169f 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/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)); diff --git a/libraries/AP_InertialSensor/AuxiliaryBus.cpp b/libraries/AP_InertialSensor/AuxiliaryBus.cpp index ed0ce2b3a0..20f46065e1 100644 --- a/libraries/AP_InertialSensor/AuxiliaryBus.cpp +++ b/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);