Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: HMC5843: Add support for MPU6000 auxiliary bus 

Allow HMC5843 to be on MPU6000's auxiliary bus.
This commit is contained in:
Lucas De Marchi 2015-08-10 20:30:32 -03:00 committed by Andrew Tridgell
parent a44ab9ed98
commit a66a201bf5
4 changed files with 129 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -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;
}

27
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -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

2
libraries/AP_Compass/Compass.cpp
View File

@ -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));

4
libraries/AP_InertialSensor/AuxiliaryBus.cpp
View File

@ -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);