Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: MPU6000: export auxiliary bus

Browse Source
This commit is contained in:
Lucas De Marchi 2015-08-16 16:23:24 -03:00 committed by Andrew Tridgell
parent caae933c28
commit 565c18603d
2 changed files with 263 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

200
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -71,9 +71,17 @@ extern const AP_HAL::HAL& hal;
# define BIT_SLV2_FIFO_EN 0x04
# define BIT_SLV1_FIFO_EN 0x02
# define BIT_SLV0_FIFI_EN0 0x01
#define MPUREG_I2C_MST_CTRL 0x24
# define BIT_I2C_MST_P_NSR 0x10
# define BIT_I2C_MST_CLK_400KHZ 0x0D
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_INT_PIN_CFG 0x37
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_INT_ENABLE 0x38
// bit definitions for MPUREG_INT_ENABLE
# define BIT_RAW_RDY_EN 0x01
@ -108,6 +116,13 @@ extern const AP_HAL::HAL& hal;
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_DO 0x63
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
# define BIT_I2C_SLV0_DLY_EN 0x01
# define BIT_I2C_SLV1_DLY_EN 0x02
# define BIT_I2C_SLV2_DLY_EN 0x04
# define BIT_I2C_SLV3_DLY_EN 0x08
#define MPUREG_USER_CTRL 0x6A
// bit definitions for MPUREG_USER_CTRL
# define BIT_USER_CTRL_SIG_COND_RESET 0x01 // resets signal paths and results registers for all sensors (gyros, accel, temp)
@ -142,6 +157,7 @@ extern const AP_HAL::HAL& hal;
#define MPUREG_WHOAMI 0x75
#define BIT_READ_FLAG 0x80
#define BIT_I2C_SLVX_EN 0x80
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
@ -289,6 +305,11 @@ AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
return _spi->get_semaphore();
}
bool AP_MPU6000_BusDriver_SPI::has_auxiliar_bus()
{
return true;
}
/* I2C bus driver implementation */
AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_addr(addr),
@ -392,6 +413,11 @@ AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
return _i2c->get_semaphore();
}
bool AP_MPU6000_BusDriver_I2C::has_auxiliar_bus()
{
return false;
}
/*
* RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
@ -430,6 +456,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000()
{
delete _bus;
delete _auxiliar_bus;
delete _samples;
}
@ -675,6 +702,17 @@ bool AP_InertialSensor_MPU6000::update( void )
return true;
}
AuxiliaryBus *AP_InertialSensor_MPU6000::get_auxiliar_bus()
{
if (_auxiliar_bus)
return _auxiliar_bus;
if (_bus->has_auxiliar_bus())
_auxiliar_bus = new AP_MPU6000_AuxiliaryBus(*this);
return _auxiliar_bus;
}
/*================ HARDWARE FUNCTIONS ==================== */
/**
@ -819,6 +857,16 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
// Chip reset
uint8_t tries;
for (tries = 0; tries<5; tries++) {
uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL);
/* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
hal.scheduler->delay(10);
}
/* reset device */
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
hal.scheduler->delay(100);
@ -878,3 +926,155 @@ void AP_InertialSensor_MPU6000::_dump_registers(void)
}
}
#endif
AP_MPU6000_AuxiliaryBusSlave::AP_MPU6000_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
uint8_t instance)
: AuxiliaryBusSlave(bus, addr, instance)
, _mpu6000_addr(MPUREG_I2C_SLV0_ADDR + _instance * 3)
, _mpu6000_reg(_mpu6000_addr + 1)
, _mpu6000_ctrl(_mpu6000_addr + 2)
, _mpu6000_do(MPUREG_I2C_SLV0_DO + _instance)
{
}
int AP_MPU6000_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
uint8_t *out)
{
AP_InertialSensor_MPU6000 &backend = static_cast<AP_InertialSensor_MPU6000&>(_bus.get_backend());
uint8_t addr;
/* Ensure the slave read/write is disabled before changing the registers */
backend._register_write(_mpu6000_ctrl, 0);
if (out) {
backend._register_write(_mpu6000_do, *out);
addr = _addr;
} else {
addr = _addr | BIT_READ_FLAG;
}
backend._register_write(_mpu6000_addr, addr);
backend._register_write(_mpu6000_reg, reg);
backend._register_write(_mpu6000_ctrl, BIT_I2C_SLVX_EN | size);
return 0;
}
int AP_MPU6000_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
uint8_t size)
{
assert(buf);
if (_registered) {
hal.console->println_P(PSTR("Error: can't passthrough when slave is already configured"));
return -1;
}
int r = _set_passthrough(reg, size);
if (r < 0)
return r;
/* wait the value to be read from the slave and read it back */
hal.scheduler->delay(10);
AP_InertialSensor_MPU6000 &backend = static_cast<AP_InertialSensor_MPU6000&>(_bus.get_backend());
backend._read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size);
/* disable new reads */
backend._register_write(_mpu6000_ctrl, 0);
return size;
}
int AP_MPU6000_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
{
if (_registered) {
hal.console->println_P(PSTR("Error: can't passthrough when slave is already configured"));
return -1;
}
int r = _set_passthrough(reg, 1, &val);
if (r < 0)
return r;
/* wait the value to be written to the slave */
hal.scheduler->delay(10);
AP_InertialSensor_MPU6000 &backend = static_cast<AP_InertialSensor_MPU6000&>(_bus.get_backend());
/* disable new writes */
backend._register_write(_mpu6000_ctrl, 0);
return 0;
}
int AP_MPU6000_AuxiliaryBusSlave::read(uint8_t *buf)
{
if (!_registered) {
hal.console->println_P(PSTR("Error: can't read before configuring slave"));
return -1;
}
AP_InertialSensor_MPU6000 &backend = static_cast<AP_InertialSensor_MPU6000&>(_bus.get_backend());
backend._read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size);
return 0;
}
/* MPU6000 provides up to 5 slave devices, but the 5th is way too different to
* configure and is seldom used */
AP_MPU6000_AuxiliaryBus::AP_MPU6000_AuxiliaryBus(AP_InertialSensor_MPU6000 &backend)
: AuxiliaryBus(backend, 4)
{
}
AP_HAL::Semaphore *AP_MPU6000_AuxiliaryBus::get_semaphore()
{
return static_cast<AP_InertialSensor_MPU6000&>(_ins_backend)._bus_sem;
}
AuxiliaryBusSlave *AP_MPU6000_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance)
{
/* Enable slaves on MPU6000 if this is the first time */
if (_ext_sens_data == 0)
_configure_slaves();
return new AP_MPU6000_AuxiliaryBusSlave(*this, addr, instance);
}
void AP_MPU6000_AuxiliaryBus::_configure_slaves()
{
AP_InertialSensor_MPU6000 &backend = static_cast<AP_InertialSensor_MPU6000&>(_ins_backend);
uint8_t user_ctrl;
user_ctrl = backend._register_read(MPUREG_USER_CTRL);
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
/* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL,
BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);
/* Hard-code divider for internal sample rate, 1 kHz, resulting in a
* sample rate of 100Hz */
backend._register_write(MPUREG_I2C_SLV4_CTRL, 9);
/* All slaves are subject to the sample rate */
backend._register_write(MPUREG_I2C_MST_DELAY_CTRL,
BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN |
BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN);
}
int AP_MPU6000_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,
uint8_t reg, uint8_t size)
{
if (_ext_sens_data + size > MAX_EXT_SENS_DATA)
return -1;
AP_MPU6000_AuxiliaryBusSlave *mpu_slave =
static_cast<AP_MPU6000_AuxiliaryBusSlave*>(slave);
mpu_slave->_set_passthrough(reg, size);
mpu_slave->_ext_sens_data = _ext_sens_data;
_ext_sens_data += size;
return 0;
}

63
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -7,7 +7,9 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Progmem/AP_Progmem.h>
#include "AP_InertialSensor.h"
#include "AuxiliaryBus.h"
// enable debug to see a register dump on startup
#define MPU6000_DEBUG 0
@ -28,6 +30,9 @@
#define MPU6000_MAX_FIFO_SAMPLES 3
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
class AP_MPU6000_AuxiliaryBus;
class AP_MPU6000_AuxiliaryBusSlave;
class AP_MPU6000_BusDriver
{
public:
@ -45,10 +50,14 @@ public:
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples) = 0;
virtual AP_HAL::Semaphore* get_semaphore() = 0;
virtual bool has_auxiliar_bus() = 0;
};
class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
{
friend AP_MPU6000_AuxiliaryBus;
friend AP_MPU6000_AuxiliaryBusSlave;
public:
AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
~AP_InertialSensor_MPU6000();
@ -63,6 +72,11 @@ public:
bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
bool accel_sample_available(void) { return _sum_count >= _sample_count; }
/*
* Return an AuxiliaryBus if the bus driver allows it
*/
AuxiliaryBus *get_auxiliar_bus() override;
void start() override;
private:
@ -94,6 +108,8 @@ private:
AP_MPU6000_BusDriver *_bus;
AP_HAL::Semaphore *_bus_sem;
AP_MPU6000_AuxiliaryBus *_auxiliar_bus = nullptr;
static const float _gyro_scale;
// support for updating filter at runtime
@ -137,6 +153,7 @@ public:
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples);
AP_HAL::Semaphore* get_semaphore();
bool has_auxiliar_bus() override;
private:
AP_HAL::SPIDeviceDriver *_spi;
@ -159,6 +176,7 @@ public:
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples);
AP_HAL::Semaphore* get_semaphore();
bool has_auxiliar_bus() override;
private:
uint8_t _addr;
@ -167,4 +185,49 @@ private:
uint8_t _rx[MAX_DATA_READ];
};
class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave
{
friend class AP_MPU6000_AuxiliaryBus;
public:
int passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
int passthrough_write(uint8_t reg, uint8_t val) override;
int read(uint8_t *buf) override;
protected:
AP_MPU6000_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance);
int _set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr);
private:
const uint8_t _mpu6000_addr;
const uint8_t _mpu6000_reg;
const uint8_t _mpu6000_ctrl;
const uint8_t _mpu6000_do;
uint8_t _ext_sens_data = 0;
};
class AP_MPU6000_AuxiliaryBus : public AuxiliaryBus
{
friend class AP_InertialSensor_MPU6000;
public:
AP_HAL::Semaphore *get_semaphore() override;
protected:
AP_MPU6000_AuxiliaryBus(AP_InertialSensor_MPU6000 &backend);
AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance) override;
int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
uint8_t size) override;
private:
void _configure_slaves();
static const uint8_t MAX_EXT_SENS_DATA = 24;
uint8_t _ext_sens_data = 0;
};
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__