Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: AK8963: Use MPU9250 auxiliary bus

Browse Source
This commit is contained in:
José Roberto de Souza 2015-09-28 11:21:31 -03:00 committed by Andrew Tridgell
parent 588df53429
commit 1fc29a2654
3 changed files with 44 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -22,29 +22,6 @@
#include "AP_Compass_AK8963.h"
#include <AP_InertialSensor/AP_InertialSensor_MPU9250.h>
#define READ_FLAG 0x80
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_DO 0x63
/* bit definitions for MPUREG_USER_CTRL */
#define MPUREG_USER_CTRL 0x6A
/* Enable MPU to act as the I2C Master to external slave sensors */
# define BIT_USER_CTRL_I2C_MST_EN 0x20
# define BIT_USER_CTRL_I2C_IF_DIS 0x10
/* bit definitions for MPUREG_MST_CTRL */
#define MPUREG_I2C_MST_CTRL 0x24
# define I2C_SLV0_EN 0x80
# define I2C_MST_CLOCK_400KHZ 0x0D
# define I2C_MST_CLOCK_258KHZ 0x08
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
# define I2C_SLV0_DLY_EN 0x01
#define AK8963_I2C_ADDR 0x0c
#define AK8963_WIA 0x00
@ -93,9 +70,10 @@ AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus)
_reset_filter();
}
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass, AP_HAL::SPIDeviceDriver *spi)
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass, uint8_t mpu9250_instance)
{
AP_AK8963_SerialBus *bus = new AP_AK8963_SerialBus_MPU9250(spi);
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
AP_AK8963_SerialBus *bus = new AP_AK8963_SerialBus_MPU9250(ins, AK8963_I2C_ADDR, mpu9250_instance);
if (!bus)
return nullptr;
return _detect(compass, bus);
@ -148,11 +126,6 @@ bool AP_Compass_AK8963::init()
goto fail_sem;
}
if (!_bus->configure()) {
hal.console->printf("AK8963: Could not configure bus for AK8963\n");
goto fail;
}
if (!_check_id()) {
hal.console->printf("AK8963: Wrong id\n");
goto fail;
@ -403,94 +376,57 @@ void AP_Compass_AK8963::_dump_registers()
}
/* MPU9250 implementation of the AK8963 */
AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_HAL::SPIDeviceDriver *spi)
AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins,
uint8_t addr,
uint8_t mpu9250_instance)
{
_spi = spi;
// Only initialize members. Fails are handled by configure or while
// getting the semaphore
_bus = ins.get_auxiliary_bus(HAL_INS_MPU9250, mpu9250_instance);
if (!_bus)
hal.scheduler->panic("Cannot get MPU9250 auxiliary bus");
if (_spi == NULL) {
hal.console->printf("Cannot get SPIDevice_MPU9250\n");
return;
}
_slave = _bus->request_next_slave(addr);
}
AP_AK8963_SerialBus_MPU9250::~AP_AK8963_SerialBus_MPU9250()
{
/* After started it's owned by AuxiliaryBus */
if (!_started)
delete _slave;
}
void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t reg, uint8_t value)
{
const uint8_t count = 1;
_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR);
_write(MPUREG_I2C_SLV0_REG, reg);
_write(MPUREG_I2C_SLV0_DO, value);
_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count);
_slave->passthrough_write(reg, value);
}
void AP_AK8963_SerialBus_MPU9250::register_read(uint8_t reg, uint8_t *value, uint8_t count)
{
_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);
_write(MPUREG_I2C_SLV0_REG, reg);
_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count);
hal.scheduler->delay(10);
_read(MPUREG_EXT_SENS_DATA_00, value, count);
}
void AP_AK8963_SerialBus_MPU9250::_read(uint8_t reg, uint8_t *buf, uint32_t count)
{
ASSERT(count < 32);
reg |= READ_FLAG;
uint8_t tx[32] = { reg, };
uint8_t rx[32] = { };
_spi->transaction(tx, rx, count + 1);
memcpy(buf, rx + 1, count);
}
void AP_AK8963_SerialBus_MPU9250::_write(uint8_t reg, const uint8_t *buf, uint32_t count)
{
ASSERT(count < 2);
uint8_t tx[2] = { reg, };
memcpy(tx+1, buf, count);
_spi->transaction(tx, NULL, count + 1);
}
bool AP_AK8963_SerialBus_MPU9250::configure()
{
if (!AP_InertialSensor_MPU9250::initialize_driver_state(_spi))
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;
_slave->passthrough_read(reg, value, count);
}
void AP_AK8963_SerialBus_MPU9250::read_raw(struct raw_value *rv)
{
_read(MPUREG_EXT_SENS_DATA_00, (uint8_t *) rv, sizeof(*rv));
if (_started) {
_slave->read((uint8_t*)rv);
return;
}
_slave->passthrough_read(0x03, (uint8_t*)rv, sizeof(*rv));
}
AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
{
return _spi->get_semaphore();
return _bus ? _bus->get_semaphore() : nullptr;
}
bool AP_AK8963_SerialBus_MPU9250::start_measurements()
{
const uint8_t count = sizeof(struct raw_value);
if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(struct raw_value)) < 0)
return false;
/* Don't sample AK8963 at MPU9250's sample rate. See MPU9250's datasheet
* about registers below and registers 73-96, External Sensor Data */
_write(MPUREG_I2C_SLV4_CTRL, 31);
_write(MPUREG_I2C_MST_DELAY_CTRL, I2C_SLV0_DLY_EN);
/* Configure the registers from AK8963 that will be read by MPU9250's
* master: we will get the result directly from MPU9250's registers starting
* from MPUREG_EXT_SENS_DATA_00 when read_raw() is called */
_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);
_write(MPUREG_I2C_SLV0_REG, AK8963_HXL);
_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count);
_started = true;
return true;
}

21
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -9,6 +9,10 @@
#include "Compass.h"
#include "AP_Compass_Backend.h"
class AuxiliaryBus;
class AuxiliaryBusSlave;
class AP_InertialSensor;
class AP_AK8963_SerialBus
{
public:
@ -26,7 +30,6 @@ public:
}
virtual void register_write(uint8_t reg, uint8_t value) = 0;
virtual AP_HAL::Semaphore* get_semaphore() = 0;
virtual bool configure() = 0;
virtual bool start_measurements() = 0;
virtual void read_raw(struct raw_value *rv) = 0;
virtual uint32_t get_dev_id() = 0;
@ -35,7 +38,7 @@ public:
class AP_Compass_AK8963 : public AP_Compass_Backend
{
public:
static AP_Compass_Backend *detect_mpu9250(Compass &compass, AP_HAL::SPIDeviceDriver *spi);
static AP_Compass_Backend *detect_mpu9250(Compass &compass, uint8_t mpu9250_instance);
static AP_Compass_Backend *detect_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c,
uint8_t addr);
@ -85,21 +88,18 @@ private:
class AP_AK8963_SerialBus_MPU9250: public AP_AK8963_SerialBus
{
public:
AP_AK8963_SerialBus_MPU9250(AP_HAL::SPIDeviceDriver *spi);
AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins, uint8_t addr, uint8_t mpu9250_instance);
~AP_AK8963_SerialBus_MPU9250();
void register_read(uint8_t reg, uint8_t *value, uint8_t count);
void register_write(uint8_t reg, uint8_t value);
AP_HAL::Semaphore* get_semaphore();
bool configure();
bool start_measurements();
void read_raw(struct raw_value *rv);
uint32_t get_dev_id();
private:
void _read(uint8_t reg, uint8_t *value, uint32_t count);
void _write(uint8_t reg, const uint8_t *value, uint32_t count);
void _write(uint8_t reg, const uint8_t value) {
_write(reg, &value, 1);
}
AP_HAL::SPIDeviceDriver *_spi;
AuxiliaryBus *_bus = nullptr;
AuxiliaryBusSlave *_slave = nullptr;
bool _started;
};
class AP_AK8963_SerialBus_I2C: public AP_AK8963_SerialBus
@ -109,7 +109,6 @@ public:
void register_read(uint8_t reg, uint8_t *value, uint8_t count);
void register_write(uint8_t reg, uint8_t value);
AP_HAL::Semaphore* get_semaphore();
bool configure(){ return true; }
bool start_measurements() { return true; }
void read_raw(struct raw_value *rv);
uint32_t get_dev_id();

4
libraries/AP_Compass/Compass.cpp
View File

@ -432,7 +432,7 @@ void Compass::_detect_backends(void)
CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP && \
CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_MINLURE
_add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250)));
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
_add_backend(AP_Compass_HIL::detect(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
@ -444,6 +444,8 @@ void Compass::_detect_backends(void)
HAL_COMPASS_AK8963_I2C_ADDR));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
_add_backend(AP_Compass_PX4::detect(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0));
#else
#error Unrecognised HAL_COMPASS_TYPE setting
#endif