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

AP_InertialSensor: add i2c bus driver for MPU6050

This commit is contained in:
Julien BERAUD 2015-06-15 14:58:30 +02:00 committed by Andrew Tridgell
parent 1679728730
commit 8a76ff53bd
3 changed files with 74 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -406,8 +406,11 @@ AP_InertialSensor::_detect_backends(void)
}
#if HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect);
#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
_add_backend(AP_InertialSensor_MPU6000::detect);
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
_backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this, new AP_MPU6000_BusDriver_SPI());
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && HAL_INS_MPU60XX_I2C_BUS == 2
_backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this,
new AP_MPU6000_BusDriver_I2C(hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR));
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
_add_backend(AP_InertialSensor_PX4::detect);
#elif HAL_INS_DEFAULT == HAL_INS_OILPAN

92
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -158,30 +158,32 @@ extern const AP_HAL::HAL& hal;
#define MPU6000_REV_D8 0x58 // 0101 1000
#define MPU6000_REV_D9 0x59 // 0101 1001
/* SPI bus driver implementation */
void AP_MPU6000_BusDriver_SPI::init()
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
// Disable I2C bus if SPI selected (Recommended in Datasheet
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
};
void AP_MPU6000_BusDriver_SPI::read8(uint8_t addr, uint8_t *val)
void AP_MPU6000_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
{
uint8_t tx[2];
uint8_t rx[2];
tx[0] = addr;
tx[0] = reg;
tx[1] = 0;
_spi->transaction(tx, rx, 2);
*val = rx[1];
}
void AP_MPU6000_BusDriver_SPI::write8(uint8_t addr, uint8_t val)
void AP_MPU6000_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
{
uint8_t tx[2];
uint8_t rx[2];
tx[0] = addr;
tx[0] = reg;
tx[1] = val;
_spi->transaction(tx, rx, 2);
}
@ -212,6 +214,46 @@ AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
return _spi->get_semaphore();
}
/* I2C bus driver implementation */
AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_i2c(i2c),
_addr(addr)
{}
void AP_MPU6000_BusDriver_I2C::init()
{}
void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
{
_i2c->readRegister(_addr, reg, val);
}
void AP_MPU6000_BusDriver_I2C::write8(uint8_t reg, uint8_t val)
{
_i2c->writeRegister(_addr, reg, val);
}
void AP_MPU6000_BusDriver_I2C::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
{}
uint8_t AP_MPU6000_BusDriver_I2C::read_burst(uint8_t v[14])
{
struct PACKED {
uint8_t int_status;
uint8_t d[14];
} rx;
_i2c->readRegisters(_addr, MPUREG_INT_STATUS, 15, (uint8_t *) &rx);
memcpy(v, rx.d, 14);
return rx.int_status;
}
AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
{
return _i2c->get_semaphore();
}
/*
* 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)
@ -226,10 +268,10 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
* variants however
*/
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus) :
AP_InertialSensor_Backend(imu),
_drdy_pin(NULL),
_bus(NULL),
_bus(bus),
_bus_sem(NULL),
_last_accel_filter_hz(-1),
_last_gyro_filter_hz(-1),
@ -244,31 +286,6 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
#endif
_sum_count(0)
{
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
/*
initialise the sensor
*/
bool AP_InertialSensor_MPU6000::_init_sensor(void)
{
_bus = new AP_MPU6000_BusDriver_SPI();
_bus_sem = _bus->get_semaphore();
#ifdef MPU6000_DRDY_PIN
@ -284,17 +301,19 @@ bool AP_InertialSensor_MPU6000::_init_sensor(void)
if (success) {
hal.scheduler->delay(5+2);
if (!_bus_sem->take(100)) {
return false;
return;
}
if (_data_ready()) {
_bus_sem->give();
break;
} else {
return;
}
_bus_sem->give();
}
if (tries++ > 5) {
hal.console->print_P(PSTR("failed to boot MPU6000 5 times"));
return false;
return;
}
} while (1);
@ -311,7 +330,7 @@ bool AP_InertialSensor_MPU6000::_init_sensor(void)
_dump_registers();
#endif
return true;
return;
}
@ -352,6 +371,9 @@ bool AP_InertialSensor_MPU6000::update( void )
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
accel.rotate(ROTATION_YAW_270);
gyro.rotate(ROTATION_YAW_270);
#endif
_publish_accel(_accel_instance, accel);
@ -555,9 +577,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
_register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
hal.scheduler->delay(1);
// Disable I2C bus if SPI selected (Recommended in Datasheet
_bus->init();
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1);
#if MPU6000_FAST_SAMPLING

28
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -27,21 +27,21 @@
class AP_MPU6000_BusDriver
{
public:
virtual void init();
virtual void read8(uint8_t addr, uint8_t *val);
virtual void write8(uint8_t addr, uint8_t val);
virtual void init() = 0;
virtual void read8(uint8_t reg, uint8_t *val) = 0;
virtual void write8(uint8_t reg, uint8_t val) = 0;
enum bus_speed {
SPEED_LOW, SPEED_HIGH
};
virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
virtual uint8_t read_burst(uint8_t v[14]);
virtual AP_HAL::Semaphore* get_semaphore();
virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
virtual uint8_t read_burst(uint8_t v[14]) = 0;
virtual AP_HAL::Semaphore* get_semaphore() = 0;
};
class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_MPU6000(AP_InertialSensor &imu);
AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
/* update accel and gyro state */
bool update();
@ -49,9 +49,6 @@ public:
bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
bool accel_sample_available(void) { return _sum_count >= _sample_count; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
#if MPU6000_DEBUG
void _dump_registers(void);
@ -110,8 +107,8 @@ class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
{
public:
void init();
void read8(uint8_t addr, uint8_t *val);
void write8(uint8_t addr, uint8_t val);
void read8(uint8_t reg, uint8_t *val);
void write8(uint8_t reg, uint8_t val);
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
uint8_t read_burst(uint8_t v[14]);
AP_HAL::Semaphore* get_semaphore();
@ -124,15 +121,16 @@ class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver
{
public:
AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
void init();
void read8(uint8_t addr, uint8_t *val);
void write8(uint8_t addr, uint8_t val);
void read8(uint8_t reg, uint8_t *val);
void write8(uint8_t reg, uint8_t val);
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
uint8_t read_burst(uint8_t v[14]);
AP_HAL::Semaphore* get_semaphore();
private:
AP_HAL::I2CDriver *_i2c;
uint8_t _addr;
AP_HAL::Semaphore *_i2c_sem;
};