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

AP_InertialSensor: MPU60x0: use AP_HAL::Device abstraction

This commit is contained in:
Lucas De Marchi 2016-01-12 16:22:11 -02:00
parent f1ade970a3
commit af846636e4
4 changed files with 270 additions and 473 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -213,8 +213,8 @@
#define HAL_BOARD_LOG_DIRECTORY "/data/ftp/internal_000/APM/logs" #define HAL_BOARD_LOG_DIRECTORY "/data/ftp/internal_000/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/data/ftp/internal_000/APM/terrain" #define HAL_BOARD_TERRAIN_DIRECTORY "/data/ftp/internal_000/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU60XX_I2C #define HAL_INS_DEFAULT HAL_INS_MPU60XX_I2C
#define HAL_INS_MPU60XX_I2C_BUS 2 #define HAL_INS_MPU60x0_I2C_BUS 2
#define HAL_INS_MPU60XX_I2C_ADDR 0x68 #define HAL_INS_MPU60x0_I2C_ADDR 0x68
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C
#define HAL_INS_AK8963_I2C_BUS 1 #define HAL_INS_AK8963_I2C_BUS 1
#define HAL_COMPASS_AK8963_I2C_ADDR 0x0d #define HAL_COMPASS_AK8963_I2C_ADDR 0x0d
@ -249,6 +249,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU60XX_SPI #define HAL_INS_DEFAULT HAL_INS_MPU60XX_SPI
#define HAL_INS_MPU60x0_NAME "mpu6000"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C #define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
#define HAL_BARO_MS5611_I2C_BUS 10 #define HAL_BARO_MS5611_I2C_BUS 10
#define HAL_BARO_MS5611_I2C_ADDR 0x77 #define HAL_BARO_MS5611_I2C_ADDR 0x77
@ -288,6 +289,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_RASPILOT #define HAL_INS_DEFAULT HAL_INS_RASPILOT
#define HAL_INS_MPU60x0_NAME "mpu6000"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
#define HAL_BARO_MS5611_NAME "ms5611" #define HAL_BARO_MS5611_NAME "ms5611"
#define HAL_COMPASS_DEFAULT HAL_COMPASS_RASPILOT #define HAL_COMPASS_DEFAULT HAL_COMPASS_RASPILOT
@ -345,7 +347,8 @@
#define HAL_BARO_MS5611_I2C_ADDR 0x77 #define HAL_BARO_MS5611_I2C_ADDR 0x77
#define HAL_BARO_MS5611_USE_TIMER true #define HAL_BARO_MS5611_USE_TIMER true
#define HAL_INS_DEFAULT HAL_INS_BH #define HAL_INS_DEFAULT HAL_INS_BH
#define HAL_INS_MPU60XX_I2C_ADDR 0x69 #define HAL_INS_MPU60x0_I2C_BUS 1
#define HAL_INS_MPU60x0_I2C_ADDR 0x69
#define HAL_COMPASS_DEFAULT HAL_COMPASS_BH #define HAL_COMPASS_DEFAULT HAL_COMPASS_BH
#define HAL_GPIO_A_LED_PIN 17 #define HAL_GPIO_A_LED_PIN 17
#define HAL_GPIO_B_LED_PIN 18 #define HAL_GPIO_B_LED_PIN 18

12
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -4,6 +4,8 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/SPIDevice.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Notify/AP_Notify.h> #include <AP_Notify/AP_Notify.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
@ -519,12 +521,12 @@ AP_InertialSensor::detect_backends(void)
#elif HAL_INS_DEFAULT == HAL_INS_HIL #elif HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect(*this)); _add_backend(AP_InertialSensor_HIL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
_add_backend(AP_InertialSensor_MPU6000::detect_spi(*this)); _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_BH #elif HAL_INS_DEFAULT == HAL_INS_BH
_add_backend(AP_InertialSensor_MPU6000::detect_i2c(*this, hal.i2c, HAL_INS_MPU60XX_I2C_ADDR)); _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
_add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250))); _add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && HAL_INS_MPU60XX_I2C_BUS == 2
_add_backend(AP_InertialSensor_MPU6000::detect_i2c(*this, hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR));
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
_add_backend(AP_InertialSensor_PX4::detect(*this)); _add_backend(AP_InertialSensor_PX4::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250 #elif HAL_INS_DEFAULT == HAL_INS_MPU9250
@ -538,7 +540,7 @@ AP_InertialSensor::detect_backends(void)
#elif HAL_INS_DEFAULT == HAL_INS_RASPILOT #elif HAL_INS_DEFAULT == HAL_INS_RASPILOT
//_add_backend(AP_InertialSensor_L3GD20::detect); //_add_backend(AP_InertialSensor_L3GD20::detect);
//_add_backend(AP_InertialSensor_LSM303D::detect); //_add_backend(AP_InertialSensor_LSM303D::detect);
_add_backend(AP_InertialSensor_MPU6000::detect_spi(*this)); _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
_add_backend(AP_InertialSensor_MPU9250::detect_i2c(*this, _add_backend(AP_InertialSensor_MPU9250::detect_i2c(*this,
HAL_INS_MPU9250_I2C_POINTER, HAL_INS_MPU9250_I2C_POINTER,

512
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -1,8 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <assert.h> #include <assert.h>
#include <utility>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_MPU6000.h"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -187,245 +189,23 @@ extern const AP_HAL::HAL& hal;
#define MPU6000_REV_D8 0x58 // 0101 1000 #define MPU6000_REV_D8 0x58 // 0101 1000
#define MPU6000_REV_D9 0x59 // 0101 1001 #define MPU6000_REV_D9 0x59 // 0101 1001
#define MPU6000_SAMPLE_SIZE 14
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define MPU6000_MAX_FIFO_SAMPLES 6
#else
#define MPU6000_MAX_FIFO_SAMPLES 3
#endif
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]) #define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
/* SPI bus driver implementation */
AP_MPU6000_BusDriver_SPI::AP_MPU6000_BusDriver_SPI(void) :
_error_count(0)
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
}
void AP_MPU6000_BusDriver_SPI::init()
{
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be done
* just after the device is reset) */
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
}
void AP_MPU6000_BusDriver_SPI::start(bool &fifo_mode, uint8_t &max_samples)
{
fifo_mode = false;
_error_count = 0;
/* maximum number of samples read by a burst
* a sample is an array containing :
* gyro_x
* gyro_y
* gyro_z
* accel_x
* accel_y
* accel_z
*/
max_samples = 1;
};
/*
* This implementation is limited to a block of at most 32 bytes
*/
void AP_MPU6000_BusDriver_SPI::read_block(uint8_t reg, uint8_t *buf, uint32_t size)
{
assert(size < 32);
reg |= BIT_READ_FLAG;
uint8_t tx[32] = { reg, };
uint8_t rx[32] = { };
_spi->transaction(tx, rx, size + 1);
memcpy(buf, rx + 1, size);
}
void AP_MPU6000_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
{
uint8_t addr = reg | 0x80; // Set most significant bit
uint8_t tx[2];
uint8_t rx[2];
tx[0] = addr;
tx[1] = 0;
_spi->transaction(tx, rx, 2);
*val = rx[1];
}
void AP_MPU6000_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
{
uint8_t tx[2];
uint8_t rx[2];
tx[0] = reg;
tx[1] = val;
_spi->transaction(tx, rx, 2);
}
void AP_MPU6000_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
{
_spi->set_bus_speed(speed);
}
void AP_MPU6000_BusDriver_SPI::read_data_transaction(uint8_t *samples,
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples)
{
/* one register address followed by seven 2-byte registers */
struct PACKED {
uint8_t cmd;
uint8_t int_status;
uint8_t d[14];
} rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, };
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
/*
detect a bad SPI bus transaction by looking for all 14 bytes
zero. This can happen with some boards with hw that end up
needing a lower bus speed
*/
uint8_t i;
for (i=0; i<14; i++) {
if (rx.d[i] != 0) break;
}
if (i == 14) {
// likely a bad bus transaction
if (++_error_count > 4) {
set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
}
}
n_samples = 1;
/* remove cmd from data sample */
memcpy(&samples[0], &rx.d[0], 14);
return;
}
AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
{
return _spi->get_semaphore();
}
bool AP_MPU6000_BusDriver_SPI::has_auxiliary_bus()
{
return true;
}
/* I2C bus driver implementation */
AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_addr(addr),
_i2c(i2c),
_i2c_sem(NULL)
{}
void AP_MPU6000_BusDriver_I2C::init()
{
}
void AP_MPU6000_BusDriver_I2C::start(bool &fifo_mode, uint8_t &max_samples)
{
// enable fifo mode
fifo_mode = true;
write8(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
write8(MPUREG_USER_CTRL, 0);
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
/* maximum number of samples read by a burst
* a sample is an array containing :
* gyro_x
* gyro_y
* gyro_z
* temperature
* accel_x
* accel_y
* accel_z
*/
max_samples = MPU6000_MAX_FIFO_SAMPLES;
}
void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
{
_i2c->readRegister(_addr, reg, val);
}
void AP_MPU6000_BusDriver_I2C::read_block(uint8_t reg, uint8_t *buf, uint32_t size)
{
_i2c->readRegisters(_addr, reg, size, buf);
}
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)
{}
void AP_MPU6000_BusDriver_I2C::read_data_transaction(uint8_t *samples,
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples)
{
uint16_t bytes_read;
uint8_t ret = 0;
ret = _i2c->readRegisters(_addr, MPUREG_FIFO_COUNTH, 2, _rx);
if(ret != 0) {
hal.console->printf("MPU6000: error in i2c read\n");
n_samples = 0;
return;
}
bytes_read = uint16_val(_rx, 0);
n_samples = bytes_read / MPU6000_SAMPLE_SIZE;
if(n_samples > MPU6000_MAX_FIFO_SAMPLES) {
hal.console->printf("bytes_read = %d, n_samples %d > 3, dropping samples\n",
bytes_read, n_samples);
/* Too many samples, do a FIFO RESET */
write8(MPUREG_USER_CTRL, 0);
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
n_samples = 0;
return;
}
else if (n_samples == 0) {
/* Not enough data in FIFO */
return;
}
else {
ret = _i2c->readRegisters(_addr, MPUREG_FIFO_R_W, n_samples * MPU6000_SAMPLE_SIZE, _rx);
}
if(ret != 0) {
hal.console->printf("MPU6000: error in i2c read %d bytes\n",
n_samples * MPU6000_SAMPLE_SIZE);
n_samples = 0;
return;
}
memcpy(samples, _rx, n_samples * MPU6000_SAMPLE_SIZE);
return;
}
AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
{
return _i2c->get_semaphore();
}
bool AP_MPU6000_BusDriver_I2C::has_auxiliary_bus()
{
return false;
}
/* /*
* RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of * 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) * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
*/ */
const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f); static const float GYRO_SCALE = (0.0174532f / 16.4f);
/* /*
* RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of * RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
@ -435,70 +215,56 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
* variants however * variants however
*/ */
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus) : AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
AP_InertialSensor_Backend(imu), AP_HAL::OwnPtr<AP_HAL::Device> dev,
_drdy_pin(NULL), enum bus_type type,
_bus(bus), bool use_fifo,
_bus_sem(NULL), uint8_t read_flag)
_temp_filter(1000, 1), : AP_InertialSensor_Backend(imu)
_samples(NULL) , _read_flag(read_flag)
, _use_fifo(use_fifo)
, _bus_type(type)
, _temp_filter(1000, 1)
, _dev(std::move(dev))
{ {
} }
AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000() AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000()
{ {
delete _bus;
delete _auxiliary_bus; delete _auxiliary_bus;
delete _samples;
} }
/* Detect the sensor on SPI bus. It must have a corresponding device on AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &imu,
* SPIDriver table */ AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_spi(AP_InertialSensor &imu)
{ {
AP_MPU6000_BusDriver *bus = new AP_MPU6000_BusDriver_SPI(); AP_InertialSensor_MPU6000 *sensor =
if (!bus) new AP_InertialSensor_MPU6000(imu, std::move(dev), BUS_TYPE_I2C, true, 0);
return nullptr; if (!sensor || !sensor->_init()) {
return _detect(imu, bus, HAL_INS_MPU60XX_SPI);
}
/* Detect the sensor on the specified I2C bus and address */
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_i2c(AP_InertialSensor &imu,
AP_HAL::I2CDriver *i2c,
uint8_t addr)
{
AP_MPU6000_BusDriver *bus = new AP_MPU6000_BusDriver_I2C(i2c, addr);
if (!bus)
return nullptr;
return _detect(imu, bus);
}
/* Common detection method - it takes ownership of the bus, freeing it if it's
* not possible to return an AP_InertialSensor_Backend */
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::_detect(AP_InertialSensor &_imu,
AP_MPU6000_BusDriver *bus,
int16_t id)
{
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, bus);
if (sensor == NULL) {
delete bus;
return nullptr;
}
if (!sensor->_init_sensor()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
} }
sensor->_id = HAL_INS_MPU60XX_I2C;
sensor->_id = id;
return sensor; return sensor;
} }
bool AP_InertialSensor_MPU6000::_init_sensor(void)
{
_bus_sem = _bus->get_semaphore();
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev)
{
AP_InertialSensor_MPU6000 *sensor =
new AP_InertialSensor_MPU6000(imu, std::move(dev), BUS_TYPE_SPI, false, 0x80);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
}
sensor->_id = HAL_INS_MPU60XX_SPI;
return sensor;
}
bool AP_InertialSensor_MPU6000::_init()
{
#ifdef MPU6000_DRDY_PIN #ifdef MPU6000_DRDY_PIN
_drdy_pin = hal.gpio->channel(MPU6000_DRDY_PIN); _drdy_pin = hal.gpio->channel(MPU6000_DRDY_PIN);
_drdy_pin->mode(HAL_GPIO_INPUT); _drdy_pin->mode(HAL_GPIO_INPUT);
@ -515,28 +281,44 @@ bool AP_InertialSensor_MPU6000::_init_sensor(void)
return success; return success;
} }
void AP_InertialSensor_MPU6000::_fifo_reset()
{
_register_write(MPUREG_USER_CTRL, 0);
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
}
void AP_InertialSensor_MPU6000::_fifo_enable()
{
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
_fifo_reset();
hal.scheduler->delay(1);
}
bool AP_InertialSensor_MPU6000::_has_auxiliary_bus()
{
return _bus_type != BUS_TYPE_I2C;
}
void AP_InertialSensor_MPU6000::start() void AP_InertialSensor_MPU6000::start()
{ {
uint8_t max_samples;
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
if (!_bus_sem->take(100)) { if (!_dev->get_semaphore()->take(100)) {
AP_HAL::panic("MPU6000: Unable to get semaphore"); AP_HAL::panic("MPU6000: Unable to get semaphore");
} }
// initially run the bus at low speed // initially run the bus at low speed
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _dev->set_speed(AP_HAL::Device::SPEED_LOW);
// only used for wake-up in accelerometer only low power mode // only used for wake-up in accelerometer only low power mode
_register_write(MPUREG_PWR_MGMT_2, 0x00); _register_write(MPUREG_PWR_MGMT_2, 0x00);
hal.scheduler->delay(1); hal.scheduler->delay(1);
_bus->start(_fifo_mode, max_samples); if (_use_fifo) {
_fifo_enable();
/* each sample is on 16 bits */ }
_samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE];
hal.scheduler->delay(1);
// disable sensor filtering // disable sensor filtering
_set_filter_register(256); _set_filter_register(256);
@ -547,10 +329,10 @@ void AP_InertialSensor_MPU6000::start()
// So we have to set it to 7 to have a 1kHz sampling // So we have to set it to 7 to have a 1kHz sampling
// rate on the gyro // rate on the gyro
_register_write(MPUREG_SMPLRT_DIV, 7); _register_write(MPUREG_SMPLRT_DIV, 7);
hal.scheduler->delay(1); hal.scheduler->delay(1);
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s // Gyro scale 2000º/s
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS);
hal.scheduler->delay(1); hal.scheduler->delay(1);
// read the product ID rev c has 1/2 the sensitivity of rev d // read the product ID rev c has 1/2 the sensitivity of rev d
@ -580,10 +362,10 @@ void AP_InertialSensor_MPU6000::start()
// until we clear the interrupt // until we clear the interrupt
_register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN); _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
// now that we have initialised, we set the SPI bus speed to high // now that we have initialised, we set the bus speed to high
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
_bus_sem->give(); _dev->get_semaphore()->give();
// grab the used instances // grab the used instances
_gyro_instance = _imu.register_gyro(1000); _gyro_instance = _imu.register_gyro(1000);
@ -599,7 +381,7 @@ void AP_InertialSensor_MPU6000::start()
/* /*
process any process any
*/ */
bool AP_InertialSensor_MPU6000::update( void ) bool AP_InertialSensor_MPU6000::update()
{ {
update_accel(_accel_instance); update_accel(_accel_instance);
update_gyro(_gyro_instance); update_gyro(_gyro_instance);
@ -614,18 +396,18 @@ bool AP_InertialSensor_MPU6000::update( void )
AuxiliaryBus *AP_InertialSensor_MPU6000::get_auxiliary_bus() AuxiliaryBus *AP_InertialSensor_MPU6000::get_auxiliary_bus()
{ {
if (_auxiliary_bus) if (_auxiliary_bus) {
return _auxiliary_bus; return _auxiliary_bus;
}
if (_bus->has_auxiliary_bus()) if (_has_auxiliary_bus()) {
_auxiliary_bus = new AP_MPU6000_AuxiliaryBus(*this); _auxiliary_bus = new AP_MPU6000_AuxiliaryBus(*this);
}
return _auxiliary_bus; return _auxiliary_bus;
} }
/*================ HARDWARE FUNCTIONS ==================== */ /*
/**
* Return true if the MPU6000 has new data available for reading. * Return true if the MPU6000 has new data available for reading.
* *
* We use the data ready pin if it is available. Otherwise, read the * We use the data ready pin if it is available. Otherwise, read the
@ -640,18 +422,22 @@ bool AP_InertialSensor_MPU6000::_data_ready()
return (status & BIT_RAW_RDY_INT) != 0; return (status & BIT_RAW_RDY_INT) != 0;
} }
/** /*
* Timer process to poll for new data from the MPU6000. * Timer process to poll for new data from the MPU6000.
*/ */
void AP_InertialSensor_MPU6000::_poll_data(void) void AP_InertialSensor_MPU6000::_poll_data()
{ {
if (!_bus_sem->take_nonblocking()) { if (!_dev->get_semaphore()->take_nonblocking()) {
return; return;
} }
if (_fifo_mode || _data_ready()) {
_read_data_transaction(); if (_use_fifo) {
_read_fifo();
} else if (_data_ready()) {
_read_sample();
} }
_bus_sem->give();
_dev->get_semaphore()->give();
} }
void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
@ -669,7 +455,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
gyro = Vector3f(int16_val(data, 5), gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4), int16_val(data, 4),
-int16_val(data, 6)); -int16_val(data, 6));
gyro *= _gyro_scale; gyro *= GYRO_SCALE;
temp = int16_val(data, 3); temp = int16_val(data, 3);
/* scaling/offset values from the datasheet */ /* scaling/offset values from the datasheet */
@ -696,31 +482,84 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
} }
} }
void AP_InertialSensor_MPU6000::_read_data_transaction() void AP_InertialSensor_MPU6000::_read_fifo()
{ {
uint8_t n_samples; uint8_t n_samples;
uint16_t bytes_read;
uint8_t rx[MAX_DATA_READ];
_bus->read_data_transaction(_samples, _drdy_pin, n_samples); static_assert(MAX_DATA_READ <= 100, "Too big to keep on stack");
_accumulate(_samples, n_samples);
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
hal.console->printf("MPU60x0: error in fifo read\n");
return;
} }
void AP_InertialSensor_MPU6000::_read_block(uint8_t reg, uint8_t *buf, bytes_read = uint16_val(rx, 0);
n_samples = bytes_read / MPU6000_SAMPLE_SIZE;
if (n_samples == 0) {
/* Not enough data in FIFO */
return;
}
if (n_samples > MPU6000_MAX_FIFO_SAMPLES) {
hal.console->printf("bytes_read = %u, n_samples %u > %u, dropping samples\n",
bytes_read, n_samples, MPU6000_MAX_FIFO_SAMPLES);
/* Too many samples, do a FIFO RESET */
_fifo_reset();
return;
}
if (!_block_read(MPUREG_FIFO_R_W, rx, n_samples * MPU6000_SAMPLE_SIZE)) {
hal.console->printf("MPU60x0: error in fifo read %u bytes\n",
n_samples * MPU6000_SAMPLE_SIZE);
return;
}
_accumulate(rx, n_samples);
}
void AP_InertialSensor_MPU6000::_read_sample()
{
/* one register address followed by seven 2-byte registers */
struct PACKED {
uint8_t int_status;
uint8_t d[14];
} rx;
if (!_block_read(MPUREG_INT_STATUS, (uint8_t *) &rx, sizeof(rx))) {
if (++_error_count > 4) {
// TODO: set bus speed low for this (and only this) device
hal.console->printf("MPU60x0: error reading sample\n");
return;
}
}
_accumulate(rx.d, 1);
}
bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
uint32_t size) uint32_t size)
{ {
_bus->read_block(reg, buf, size); reg |= _read_flag;
return _dev->read_registers(reg, buf, size);
} }
uint8_t AP_InertialSensor_MPU6000::_register_read(uint8_t reg) uint8_t AP_InertialSensor_MPU6000::_register_read(uint8_t reg)
{ {
uint8_t val; uint8_t val = 0;
reg |= _read_flag;
_dev->read_registers(reg, &val, 1);
_bus->read8(reg, &val);
return val; return val;
} }
void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val) void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val)
{ {
_bus->write8(reg, val); _dev->write_register(reg, val);
} }
/* /*
@ -767,12 +606,12 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz)
bool AP_InertialSensor_MPU6000::_hardware_init(void) bool AP_InertialSensor_MPU6000::_hardware_init(void)
{ {
if (!_bus_sem->take(100)) { if (!_dev->get_semaphore()->take(100)) {
AP_HAL::panic("MPU6000: Unable to get semaphore"); AP_HAL::panic("MPU6000: Unable to get semaphore");
} }
// initially run the bus at low speed // initially run the bus at low speed
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _dev->set_speed(AP_HAL::Device::SPEED_LOW);
// Chip reset // Chip reset
uint8_t tries; uint8_t tries;
@ -792,7 +631,11 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
hal.scheduler->delay(100); hal.scheduler->delay(100);
/* bus-dependent initialization */ /* bus-dependent initialization */
_bus->init(); if (_bus_type == BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
}
// Wake up device and select GyroZ clock. Note that the // Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time // MPU6000 starts up in sleep mode, and it can take some time
@ -801,31 +644,29 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
hal.scheduler->delay(5); hal.scheduler->delay(5);
// check it has woken up // check it has woken up
if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
break; break;
}
hal.scheduler->delay(10); hal.scheduler->delay(10);
if (_data_ready()) if (_data_ready()) {
break; break;
}
#if MPU6000_DEBUG #if MPU6000_DEBUG
_dump_registers(); _dump_registers();
#endif #endif
} }
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev->get_semaphore()->give();
if (tries == 5) { if (tries == 5) {
hal.console->println("Failed to boot MPU6000 5 times"); hal.console->println("Failed to boot MPU6000 5 times");
goto fail_tries; return false;
} }
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_bus_sem->give();
return true; return true;
fail_tries:
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_bus_sem->give();
return false;
} }
#if MPU6000_DEBUG #if MPU6000_DEBUG
@ -833,7 +674,10 @@ fail_tries:
void AP_InertialSensor_MPU6000::_dump_registers(void) void AP_InertialSensor_MPU6000::_dump_registers(void)
{ {
hal.console->println("MPU6000 registers"); hal.console->println("MPU6000 registers");
if (_bus_sem->take(100)) { if (!_dev->get_semaphore()->take(100)) {
return;
}
for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
uint8_t v = _register_read(reg); uint8_t v = _register_read(reg);
hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
@ -842,8 +686,8 @@ void AP_InertialSensor_MPU6000::_dump_registers(void)
} }
} }
hal.console->println(); hal.console->println();
_bus_sem->give();
} _dev->get_semaphore()->give();
} }
#endif #endif
@ -899,7 +743,7 @@ int AP_MPU6000_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
hal.scheduler->delay(10); hal.scheduler->delay(10);
auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend()); auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend());
backend._read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size); backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size);
/* disable new reads */ /* disable new reads */
backend._register_write(_mpu6000_ctrl, 0); backend._register_write(_mpu6000_ctrl, 0);
@ -938,7 +782,7 @@ int AP_MPU6000_AuxiliaryBusSlave::read(uint8_t *buf)
} }
auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend()); auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend());
backend._read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size); backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size);
return 0; return 0;
} }
@ -952,7 +796,7 @@ AP_MPU6000_AuxiliaryBus::AP_MPU6000_AuxiliaryBus(AP_InertialSensor_MPU6000 &back
AP_HAL::Semaphore *AP_MPU6000_AuxiliaryBus::get_semaphore() AP_HAL::Semaphore *AP_MPU6000_AuxiliaryBus::get_semaphore()
{ {
return static_cast<AP_InertialSensor_MPU6000&>(_ins_backend)._bus_sem; return static_cast<AP_InertialSensor_MPU6000&>(_ins_backend)._dev->get_semaphore();
} }
AuxiliaryBusSlave *AP_MPU6000_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance) AuxiliaryBusSlave *AP_MPU6000_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance)

174
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -1,15 +1,16 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
#ifndef __AP_INERTIAL_SENSOR_MPU6000_H__
#define __AP_INERTIAL_SENSOR_MPU6000_H__
#include <stdint.h> #include <stdint.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/SPIDevice.h>
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <Filter/Filter.h> #include <Filter/Filter.h>
#include <Filter/LowPassFilter2p.h>
#include <Filter/LowPassFilter.h> #include <Filter/LowPassFilter.h>
#include <Filter/LowPassFilter2p.h>
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_Backend.h"
@ -18,53 +19,27 @@
// enable debug to see a register dump on startup // enable debug to see a register dump on startup
#define MPU6000_DEBUG 0 #define MPU6000_DEBUG 0
#define MPU6000_SAMPLE_SIZE 14
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define MPU6000_MAX_FIFO_SAMPLES 6
#else
#define MPU6000_MAX_FIFO_SAMPLES 3
#endif
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
class AP_MPU6000_AuxiliaryBus; class AP_MPU6000_AuxiliaryBus;
class AP_MPU6000_AuxiliaryBusSlave; class AP_MPU6000_AuxiliaryBusSlave;
class AP_MPU6000_BusDriver
{
public:
virtual ~AP_MPU6000_BusDriver() { };
virtual void init() = 0;
virtual void start(bool &fifo_mode, uint8_t &max_samples) = 0;
virtual void read8(uint8_t reg, uint8_t *val) = 0;
/// Copy data from the device to @p buf starting at @p reg with @size
/// length.
virtual void read_block(uint8_t reg, uint8_t *buf, uint32_t size) = 0;
virtual void write8(uint8_t reg, uint8_t val) = 0;
virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
virtual void read_data_transaction(uint8_t* samples,
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples) = 0;
virtual AP_HAL::Semaphore* get_semaphore() = 0;
virtual bool has_auxiliary_bus() = 0;
};
class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
{ {
friend AP_MPU6000_AuxiliaryBus; friend AP_MPU6000_AuxiliaryBus;
friend AP_MPU6000_AuxiliaryBusSlave; friend AP_MPU6000_AuxiliaryBusSlave;
public: public:
AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus); virtual ~AP_InertialSensor_MPU6000();
~AP_InertialSensor_MPU6000();
static AP_InertialSensor_Backend *detect_i2c(AP_InertialSensor &_imu,
AP_HAL::I2CDriver *i2c,
uint8_t addr);
static AP_InertialSensor_Backend *detect_spi(AP_InertialSensor &_imu);
static AP_InertialSensor_MPU6000 &from(AP_InertialSensor_Backend &backend) { static AP_InertialSensor_MPU6000 &from(AP_InertialSensor_Backend &backend) {
return static_cast<AP_InertialSensor_MPU6000&>(backend); return static_cast<AP_InertialSensor_MPU6000&>(backend);
} }
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev);
/* update accel and gyro state */ /* update accel and gyro state */
bool update(); bool update();
@ -76,94 +51,69 @@ public:
void start() override; void start() override;
private: private:
static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu, enum bus_type {
AP_MPU6000_BusDriver *bus, BUS_TYPE_I2C = 0,
int16_t id = -1); BUS_TYPE_SPI,
};
AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum bus_type bus_type,
bool use_fifo,
uint8_t read_flag);
#if MPU6000_DEBUG #if MPU6000_DEBUG
void _dump_registers(void); void _dump_registers();
#endif #endif
/* Initialize sensor*/
bool _init();
bool _hardware_init();
void _set_filter_register(uint16_t filter_hz);
void _fifo_reset();
void _fifo_enable();
bool _has_auxiliary_bus();
/* Read samples from FIFO (FIFO enabled) */
void _read_fifo();
/* Read a single sample (FIFO disabled) */
void _read_sample();
/* Check if there's data available by either reading DRDY pin or register */
bool _data_ready();
/* Poll for new data (non-blocking) */
void _poll_data();
/* Read and write functions taking the differences between buses into
* account */
bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size);
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val );
void _register_write_check(uint8_t reg, uint8_t val);
void _accumulate(uint8_t *samples, uint8_t n_samples);
// instance numbers of accel and gyro data // instance numbers of accel and gyro data
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;
AP_HAL::DigitalSource *_drdy_pin; const uint8_t _read_flag;
bool _init_sensor(void); const bool _use_fifo;
void _read_data_transaction(); const enum bus_type _bus_type;
bool _data_ready();
void _poll_data(void);
void _read_block(uint8_t reg, uint8_t *buf, uint32_t size);
uint8_t _register_read( uint8_t reg);
void _register_write( uint8_t reg, uint8_t val );
void _register_write_check(uint8_t reg, uint8_t val);
bool _hardware_init(void);
void _accumulate(uint8_t *samples, uint8_t n_samples);
AP_MPU6000_BusDriver *_bus; uint16_t _error_count;
AP_HAL::Semaphore *_bus_sem;
AP_MPU6000_AuxiliaryBus *_auxiliary_bus = nullptr;
static const float _gyro_scale;
void _set_filter_register(uint16_t filter_hz);
float _temp_filtered; float _temp_filtered;
LowPassFilter2pFloat _temp_filter; LowPassFilter2pFloat _temp_filter;
bool _fifo_mode; AP_HAL::DigitalSource *_drdy_pin;
uint8_t *_samples = nullptr; AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_MPU6000_AuxiliaryBus *_auxiliary_bus;
}; };
class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
{
public:
AP_MPU6000_BusDriver_SPI(void);
void init();
void start(bool &fifo_mode, uint8_t &max_samples);
void read8(uint8_t reg, uint8_t *val);
void read_block(uint8_t reg, uint8_t *buf, uint32_t size) override;
void write8(uint8_t reg, uint8_t val);
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
void read_data_transaction(uint8_t* samples,
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples);
AP_HAL::Semaphore* get_semaphore();
bool has_auxiliary_bus() override;
private:
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
// count of bus errors
uint16_t _error_count;
};
class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver
{
public:
AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
void init();
void start(bool &fifo_mode, uint8_t &max_samples);
void read8(uint8_t reg, uint8_t *val);
void read_block(uint8_t reg, uint8_t *buf, uint32_t size) override;
void write8(uint8_t reg, uint8_t val);
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
void read_data_transaction(uint8_t* samples,
AP_HAL::DigitalSource *_drdy_pin,
uint8_t &n_samples);
AP_HAL::Semaphore* get_semaphore();
bool has_auxiliary_bus() override;
private:
uint8_t _addr;
AP_HAL::I2CDriver *_i2c;
AP_HAL::Semaphore *_i2c_sem;
uint8_t _rx[MAX_DATA_READ];
};
class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave
{ {
friend class AP_MPU6000_AuxiliaryBus; friend class AP_MPU6000_AuxiliaryBus;
@ -207,5 +157,3 @@ private:
static const uint8_t MAX_EXT_SENS_DATA = 24; static const uint8_t MAX_EXT_SENS_DATA = 24;
uint8_t _ext_sens_data = 0; uint8_t _ext_sens_data = 0;
}; };
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__