mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-24 17:48:35 -04:00
AP_InertialSensor: add fifo support for MPU6000
And remove the use of data rdy in this case
This commit is contained in:
parent
8a76ff53bd
commit
3cf952d1f8
@ -407,10 +407,9 @@ AP_InertialSensor::_detect_backends(void)
|
|||||||
#if HAL_INS_DEFAULT == HAL_INS_HIL
|
#if HAL_INS_DEFAULT == HAL_INS_HIL
|
||||||
_add_backend(AP_InertialSensor_HIL::detect);
|
_add_backend(AP_InertialSensor_HIL::detect);
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
|
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
|
||||||
_backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this, new AP_MPU6000_BusDriver_SPI());
|
_add_backend(AP_InertialSensor_MPU6000::detect_spi);
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && HAL_INS_MPU60XX_I2C_BUS == 2
|
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && HAL_INS_MPU60XX_I2C_BUS == 2
|
||||||
_backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this,
|
_add_backend(AP_InertialSensor_MPU6000::detect_i2c2);
|
||||||
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
|
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
|
||||||
_add_backend(AP_InertialSensor_PX4::detect);
|
_add_backend(AP_InertialSensor_PX4::detect);
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_OILPAN
|
#elif HAL_INS_DEFAULT == HAL_INS_OILPAN
|
||||||
|
@ -61,6 +61,14 @@ extern const AP_HAL::HAL& hal;
|
|||||||
#define MPUREG_ZRMOT_THR 0x21 // detection threshold for Zero Motion interrupt generation.
|
#define MPUREG_ZRMOT_THR 0x21 // detection threshold for Zero Motion interrupt generation.
|
||||||
#define MPUREG_ZRMOT_DUR 0x22 // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms.
|
#define MPUREG_ZRMOT_DUR 0x22 // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms.
|
||||||
#define MPUREG_FIFO_EN 0x23
|
#define MPUREG_FIFO_EN 0x23
|
||||||
|
# define BIT_TEMP_FIFO_EN 0x80
|
||||||
|
# define BIT_XG_FIFO_EN 0x40
|
||||||
|
# define BIT_YG_FIFO_EN 0x20
|
||||||
|
# define BIT_ZG_FIFO_EN 0x10
|
||||||
|
# define BIT_ACCEL_FIFO_EN 0x08
|
||||||
|
# define BIT_SLV2_FIFO_EN 0x04
|
||||||
|
# define BIT_SLV1_FIFO_EN 0x02
|
||||||
|
# define BIT_SLV0_FIFI_EN0 0x01
|
||||||
#define MPUREG_INT_PIN_CFG 0x37
|
#define MPUREG_INT_PIN_CFG 0x37
|
||||||
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
|
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
|
||||||
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
|
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
|
||||||
@ -158,20 +166,41 @@ 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 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])
|
||||||
|
|
||||||
|
#if !defined(HAL_INS_MPU60XX_I2C_ADDR)
|
||||||
|
#define HAL_INS_MPU60XX_I2C_ADDR 0x68
|
||||||
|
#endif
|
||||||
|
|
||||||
/* SPI bus driver implementation */
|
/* SPI bus driver implementation */
|
||||||
void AP_MPU6000_BusDriver_SPI::init()
|
void AP_MPU6000_BusDriver_SPI::init(bool &fifo_mode, uint8_t &max_samples)
|
||||||
{
|
{
|
||||||
|
fifo_mode = false;
|
||||||
|
_error_count = 0;
|
||||||
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
|
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
|
||||||
// Disable I2C bus if SPI selected (Recommended in Datasheet
|
// Disable I2C bus if SPI selected (Recommended in Datasheet
|
||||||
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
|
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
|
||||||
|
/* 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;
|
||||||
};
|
};
|
||||||
|
|
||||||
void AP_MPU6000_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
|
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 tx[2];
|
||||||
uint8_t rx[2];
|
uint8_t rx[2];
|
||||||
|
|
||||||
tx[0] = reg;
|
tx[0] = addr;
|
||||||
tx[1] = 0;
|
tx[1] = 0;
|
||||||
_spi->transaction(tx, rx, 2);
|
_spi->transaction(tx, rx, 2);
|
||||||
|
|
||||||
@ -190,10 +219,12 @@ void AP_MPU6000_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
|
|||||||
|
|
||||||
void AP_MPU6000_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
|
void AP_MPU6000_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
|
||||||
{
|
{
|
||||||
set_bus_speed(speed);
|
_spi->set_bus_speed(speed);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t AP_MPU6000_BusDriver_SPI::read_burst(uint8_t v[14])
|
void AP_MPU6000_BusDriver_SPI::read_burst(uint8_t *samples,
|
||||||
|
AP_HAL::DigitalSource *_drdy_pin,
|
||||||
|
uint8_t &n_samples)
|
||||||
{
|
{
|
||||||
/* one resister address followed by seven 2-byte registers */
|
/* one resister address followed by seven 2-byte registers */
|
||||||
struct PACKED {
|
struct PACKED {
|
||||||
@ -204,9 +235,28 @@ uint8_t AP_MPU6000_BusDriver_SPI::read_burst(uint8_t v[14])
|
|||||||
|
|
||||||
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
|
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
|
||||||
|
|
||||||
memcpy(v, rx.d, 14);
|
/*
|
||||||
|
detect a bad SPI bus transaction by looking for all 14 bytes
|
||||||
|
zero, or the wrong INT_STATUS register value. This is used to
|
||||||
|
detect a too high SPI bus speed.
|
||||||
|
*/
|
||||||
|
uint8_t i;
|
||||||
|
for (i=0; i<14; i++) {
|
||||||
|
if (rx.d[i] != 0) break;
|
||||||
|
}
|
||||||
|
if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
|
||||||
|
// likely a bad bus transaction
|
||||||
|
if (++_error_count > 4) {
|
||||||
|
set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
return rx.int_status;
|
n_samples = 1;
|
||||||
|
/* remove temperature and cmd from data sample */
|
||||||
|
memcpy(&samples[0], &rx.d[0], 6);
|
||||||
|
memcpy(&samples[6], &rx.d[8], 6);
|
||||||
|
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
|
AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
|
||||||
@ -220,8 +270,25 @@ AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8
|
|||||||
_addr(addr)
|
_addr(addr)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void AP_MPU6000_BusDriver_I2C::init()
|
void AP_MPU6000_BusDriver_I2C::init(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);
|
||||||
|
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_SIG_COND_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
|
||||||
|
* accel_x
|
||||||
|
* accel_y
|
||||||
|
* accel_z
|
||||||
|
*/
|
||||||
|
max_samples = MPU6000_MAX_FIFO_SAMPLES;
|
||||||
|
}
|
||||||
|
|
||||||
void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
|
void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
|
||||||
{
|
{
|
||||||
@ -236,17 +303,53 @@ void AP_MPU6000_BusDriver_I2C::write8(uint8_t reg, uint8_t val)
|
|||||||
void AP_MPU6000_BusDriver_I2C::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
|
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])
|
void AP_MPU6000_BusDriver_I2C::read_burst(uint8_t *samples,
|
||||||
|
AP_HAL::DigitalSource *_drdy_pin,
|
||||||
|
uint8_t &n_samples)
|
||||||
{
|
{
|
||||||
struct PACKED {
|
uint16_t bytes_read;
|
||||||
uint8_t int_status;
|
uint8_t ret = 0;
|
||||||
uint8_t d[14];
|
|
||||||
} rx;
|
|
||||||
|
|
||||||
_i2c->readRegisters(_addr, MPUREG_INT_STATUS, 15, (uint8_t *) &rx);
|
ret = _i2c->readRegisters(_addr, MPUREG_FIFO_COUNTH, 2, _rx);
|
||||||
memcpy(v, rx.d, 14);
|
if(ret != 0) {
|
||||||
|
hal.console->printf_P(PSTR("MPU6000: error in i2c read\n"));
|
||||||
|
n_samples = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
return rx.int_status;
|
bytes_read = uint16_val(_rx, 0);
|
||||||
|
|
||||||
|
n_samples = bytes_read / MPU6000_SAMPLE_SIZE;
|
||||||
|
|
||||||
|
if(n_samples > 3) {
|
||||||
|
hal.console->printf_P(PSTR("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 | BIT_USER_CTRL_SIG_COND_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_P(PSTR("MPU6000: error in i2c read %d bytes\n"),
|
||||||
|
n_samples * MPU6000_SAMPLE_SIZE);
|
||||||
|
n_samples = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
memcpy(samples, _rx, MPU6000_SAMPLE_SIZE * MPU6000_MAX_FIFO_SAMPLES);
|
||||||
|
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
|
AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
|
||||||
@ -275,7 +378,6 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
|||||||
_bus_sem(NULL),
|
_bus_sem(NULL),
|
||||||
_last_accel_filter_hz(-1),
|
_last_accel_filter_hz(-1),
|
||||||
_last_gyro_filter_hz(-1),
|
_last_gyro_filter_hz(-1),
|
||||||
_error_count(0),
|
|
||||||
#if MPU6000_FAST_SAMPLING
|
#if MPU6000_FAST_SAMPLING
|
||||||
_accel_filter(1000, 15),
|
_accel_filter(1000, 15),
|
||||||
_gyro_filter(1000, 15),
|
_gyro_filter(1000, 15),
|
||||||
@ -285,6 +387,43 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
|||||||
_gyro_sum(),
|
_gyro_sum(),
|
||||||
#endif
|
#endif
|
||||||
_sum_count(0)
|
_sum_count(0)
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
detect the sensor
|
||||||
|
*/
|
||||||
|
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_spi(AP_InertialSensor &_imu)
|
||||||
|
{
|
||||||
|
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, new AP_MPU6000_BusDriver_SPI());
|
||||||
|
if (sensor == NULL) {
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
if (!sensor->_init_sensor()) {
|
||||||
|
delete sensor;
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
return sensor;
|
||||||
|
}
|
||||||
|
|
||||||
|
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_i2c2(AP_InertialSensor &_imu)
|
||||||
|
{
|
||||||
|
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu,
|
||||||
|
new AP_MPU6000_BusDriver_I2C(hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR));
|
||||||
|
if (sensor == NULL) {
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
if (!sensor->_init_sensor()) {
|
||||||
|
delete sensor;
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
return sensor;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_InertialSensor_MPU6000::_init_sensor(void)
|
||||||
{
|
{
|
||||||
_bus_sem = _bus->get_semaphore();
|
_bus_sem = _bus->get_semaphore();
|
||||||
|
|
||||||
@ -301,19 +440,17 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
|||||||
if (success) {
|
if (success) {
|
||||||
hal.scheduler->delay(5+2);
|
hal.scheduler->delay(5+2);
|
||||||
if (!_bus_sem->take(100)) {
|
if (!_bus_sem->take(100)) {
|
||||||
return;
|
return false;
|
||||||
}
|
}
|
||||||
if (_data_ready()) {
|
if (_data_ready()) {
|
||||||
_bus_sem->give();
|
_bus_sem->give();
|
||||||
break;
|
break;
|
||||||
} else {
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
_bus_sem->give();
|
_bus_sem->give();
|
||||||
}
|
}
|
||||||
if (tries++ > 5) {
|
if (tries++ > 5) {
|
||||||
hal.console->print_P(PSTR("failed to boot MPU6000 5 times"));
|
hal.console->print_P(PSTR("failed to boot MPU6000 5 times"));
|
||||||
return;
|
return false;
|
||||||
}
|
}
|
||||||
} while (1);
|
} while (1);
|
||||||
|
|
||||||
@ -330,10 +467,8 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
|||||||
_dump_registers();
|
_dump_registers();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
process any
|
process any
|
||||||
*/
|
*/
|
||||||
@ -429,59 +564,50 @@ void AP_InertialSensor_MPU6000::_poll_data(void)
|
|||||||
if (!_bus_sem->take_nonblocking()) {
|
if (!_bus_sem->take_nonblocking()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (_data_ready()) {
|
if (_fifo_mode || _data_ready()) {
|
||||||
_read_data_transaction();
|
_read_data_transaction();
|
||||||
}
|
}
|
||||||
_bus_sem->give();
|
_bus_sem->give();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
|
||||||
void AP_InertialSensor_MPU6000::_read_data_transaction() {
|
{
|
||||||
/* one resister address followed by seven 2-byte registers */
|
for(uint8_t i=0; i < n_samples; i++) {
|
||||||
uint8_t v[14];
|
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
|
||||||
uint8_t int_status = _bus->read_burst(v);
|
|
||||||
/*
|
|
||||||
detect a bad SPI bus transaction by looking for all 14 bytes
|
|
||||||
zero, or the wrong INT_STATUS register value. This is used to
|
|
||||||
detect a too high SPI bus speed.
|
|
||||||
*/
|
|
||||||
uint8_t i;
|
|
||||||
for (i=0; i<14; i++) {
|
|
||||||
if (v[i] != 0) break;
|
|
||||||
}
|
|
||||||
if ((int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
|
|
||||||
// likely a bad bus transaction
|
|
||||||
if (++_error_count > 4) {
|
|
||||||
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
|
||||||
#if MPU6000_FAST_SAMPLING
|
#if MPU6000_FAST_SAMPLING
|
||||||
_accel_filtered = _accel_filter.apply(Vector3f(int16_val(v, 1),
|
_accel_filtered = _accel_filter.apply(Vector3f(int16_val(data, 1),
|
||||||
int16_val(v, 0),
|
int16_val(data, 0),
|
||||||
-int16_val(v, 2)));
|
-int16_val(data, 2)));
|
||||||
|
|
||||||
_gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(v, 5),
|
_gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(data, 4),
|
||||||
int16_val(v, 4),
|
int16_val(data, 3),
|
||||||
-int16_val(v, 6)));
|
-int16_val(data, 5)));
|
||||||
#else
|
#else
|
||||||
_accel_sum.x += int16_val(v, 1);
|
_accel_sum.x += int16_val(data, 1);
|
||||||
_accel_sum.y += int16_val(v, 0);
|
_accel_sum.y += int16_val(data, 0);
|
||||||
_accel_sum.z -= int16_val(v, 2);
|
_accel_sum.z -= int16_val(data, 2);
|
||||||
_gyro_sum.x += int16_val(v, 5);
|
_gyro_sum.x += int16_val(data, 4);
|
||||||
_gyro_sum.y += int16_val(v, 4);
|
_gyro_sum.y += int16_val(data, 3);
|
||||||
_gyro_sum.z -= int16_val(v, 6);
|
_gyro_sum.z -= int16_val(data, 5);
|
||||||
#endif
|
#endif
|
||||||
_sum_count++;
|
_sum_count++;
|
||||||
|
|
||||||
#if !MPU6000_FAST_SAMPLING
|
#if !MPU6000_FAST_SAMPLING
|
||||||
if (_sum_count == 0) {
|
if (_sum_count == 0) {
|
||||||
// rollover - v unlikely
|
// rollover - v unlikely
|
||||||
_accel_sum.zero();
|
_accel_sum.zero();
|
||||||
_gyro_sum.zero();
|
_gyro_sum.zero();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_InertialSensor_MPU6000::_read_data_transaction()
|
||||||
|
{
|
||||||
|
uint8_t n_samples;
|
||||||
|
|
||||||
|
_bus->read_burst(_samples, _drdy_pin, n_samples);
|
||||||
|
_accumulate(_samples, n_samples);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg )
|
uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg )
|
||||||
@ -541,6 +667,8 @@ 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)
|
||||||
{
|
{
|
||||||
|
uint8_t max_samples;
|
||||||
|
|
||||||
if (!_bus_sem->take(100)) {
|
if (!_bus_sem->take(100)) {
|
||||||
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
|
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
|
||||||
}
|
}
|
||||||
@ -561,9 +689,9 @@ 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;
|
||||||
}
|
|
||||||
#if MPU6000_DEBUG
|
#if MPU6000_DEBUG
|
||||||
_dump_registers();
|
_dump_registers();
|
||||||
#endif
|
#endif
|
||||||
@ -577,7 +705,9 @@ 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
|
_register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
|
||||||
hal.scheduler->delay(1);
|
hal.scheduler->delay(1);
|
||||||
|
|
||||||
_bus->init();
|
_bus->init(_fifo_mode, max_samples);
|
||||||
|
/* each sample is on 16 bits */
|
||||||
|
_samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE];
|
||||||
hal.scheduler->delay(1);
|
hal.scheduler->delay(1);
|
||||||
|
|
||||||
#if MPU6000_FAST_SAMPLING
|
#if MPU6000_FAST_SAMPLING
|
||||||
@ -609,7 +739,11 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
|||||||
_set_filter_register(256);
|
_set_filter_register(256);
|
||||||
|
|
||||||
// set sample rate to 1000Hz and apply a software filter
|
// set sample rate to 1000Hz and apply a software filter
|
||||||
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
|
// In this configuration, the gyro sample rate is 8kHz
|
||||||
|
// Therefore the sample rate value is 8kHz/(SMPLRT_DIV + 1)
|
||||||
|
// So we have to set it to 7 to have a 1kHz sampling
|
||||||
|
// rate on the gyro
|
||||||
|
_register_write(MPUREG_SMPLRT_DIV, 7);
|
||||||
#else
|
#else
|
||||||
_set_filter_register(_accel_filter_cutoff());
|
_set_filter_register(_accel_filter_cutoff());
|
||||||
|
|
||||||
|
@ -24,17 +24,23 @@
|
|||||||
#include <LowPassFilter2p.h>
|
#include <LowPassFilter2p.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define MPU6000_SAMPLE_SIZE 12
|
||||||
|
#define MPU6000_MAX_FIFO_SAMPLES 3
|
||||||
|
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
|
||||||
|
|
||||||
class AP_MPU6000_BusDriver
|
class AP_MPU6000_BusDriver
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
virtual void init() = 0;
|
virtual void init(bool &fifo_mode, uint8_t &max_samples) = 0;
|
||||||
virtual void read8(uint8_t reg, uint8_t *val) = 0;
|
virtual void read8(uint8_t reg, uint8_t *val) = 0;
|
||||||
virtual void write8(uint8_t reg, uint8_t val) = 0;
|
virtual void write8(uint8_t reg, uint8_t val) = 0;
|
||||||
enum bus_speed {
|
enum bus_speed {
|
||||||
SPEED_LOW, SPEED_HIGH
|
SPEED_LOW, SPEED_HIGH
|
||||||
};
|
};
|
||||||
virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
|
virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
|
||||||
virtual uint8_t read_burst(uint8_t v[14]) = 0;
|
virtual void read_burst(uint8_t* samples,
|
||||||
|
AP_HAL::DigitalSource *_drdy_pin,
|
||||||
|
uint8_t &n_samples) = 0;
|
||||||
virtual AP_HAL::Semaphore* get_semaphore() = 0;
|
virtual AP_HAL::Semaphore* get_semaphore() = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -42,6 +48,8 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
|
AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
|
||||||
|
static AP_InertialSensor_Backend *detect_i2c2(AP_InertialSensor &_imu);
|
||||||
|
static AP_InertialSensor_Backend *detect_spi(AP_InertialSensor &_imu);
|
||||||
|
|
||||||
/* update accel and gyro state */
|
/* update accel and gyro state */
|
||||||
bool update();
|
bool update();
|
||||||
@ -59,19 +67,19 @@ private:
|
|||||||
uint8_t _accel_instance;
|
uint8_t _accel_instance;
|
||||||
|
|
||||||
AP_HAL::DigitalSource *_drdy_pin;
|
AP_HAL::DigitalSource *_drdy_pin;
|
||||||
|
|
||||||
bool _init_sensor(void);
|
bool _init_sensor(void);
|
||||||
bool _sample_available();
|
bool _sample_available();
|
||||||
void _read_data_transaction();
|
void _read_data_transaction();
|
||||||
bool _data_ready();
|
bool _data_ready();
|
||||||
void _poll_data(void);
|
void _poll_data(void);
|
||||||
uint8_t _register_read( uint8_t reg );
|
uint8_t _register_read( uint8_t reg);
|
||||||
void _register_write( uint8_t reg, uint8_t val );
|
void _register_write( uint8_t reg, uint8_t val );
|
||||||
void _register_write_check(uint8_t reg, uint8_t val);
|
void _register_write_check(uint8_t reg, uint8_t val);
|
||||||
bool _hardware_init(void);
|
bool _hardware_init(void);
|
||||||
|
void _accumulate(uint8_t *samples, uint8_t n_samples);
|
||||||
|
|
||||||
AP_MPU6000_BusDriver *_bus;
|
AP_MPU6000_BusDriver *_bus;
|
||||||
AP_HAL::Semaphore *_bus_sem;
|
AP_HAL::Semaphore *_bus_sem;
|
||||||
|
|
||||||
static const float _gyro_scale;
|
static const float _gyro_scale;
|
||||||
|
|
||||||
@ -81,9 +89,6 @@ private:
|
|||||||
|
|
||||||
void _set_filter_register(uint16_t filter_hz);
|
void _set_filter_register(uint16_t filter_hz);
|
||||||
|
|
||||||
// count of bus errors
|
|
||||||
uint16_t _error_count;
|
|
||||||
|
|
||||||
// how many hardware samples before we report a sample to the caller
|
// how many hardware samples before we report a sample to the caller
|
||||||
uint8_t _sample_count;
|
uint8_t _sample_count;
|
||||||
|
|
||||||
@ -101,37 +106,47 @@ private:
|
|||||||
Vector3l _gyro_sum;
|
Vector3l _gyro_sum;
|
||||||
#endif
|
#endif
|
||||||
volatile uint16_t _sum_count;
|
volatile uint16_t _sum_count;
|
||||||
|
bool _fifo_mode;
|
||||||
|
uint8_t *_samples;
|
||||||
};
|
};
|
||||||
|
|
||||||
class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
|
class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
void init();
|
void init(bool &fifo_mode, uint8_t &max_samples);
|
||||||
void read8(uint8_t reg, uint8_t *val);
|
void read8(uint8_t reg, uint8_t *val);
|
||||||
void write8(uint8_t reg, uint8_t val);
|
void write8(uint8_t reg, uint8_t val);
|
||||||
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
|
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
|
||||||
uint8_t read_burst(uint8_t v[14]);
|
void read_burst(uint8_t* samples,
|
||||||
|
AP_HAL::DigitalSource *_drdy_pin,
|
||||||
|
uint8_t &n_samples);
|
||||||
AP_HAL::Semaphore* get_semaphore();
|
AP_HAL::Semaphore* get_semaphore();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
AP_HAL::SPIDeviceDriver *_spi;
|
AP_HAL::SPIDeviceDriver *_spi;
|
||||||
AP_HAL::Semaphore *_spi_sem;
|
AP_HAL::Semaphore *_spi_sem;
|
||||||
|
// count of bus errors
|
||||||
|
uint16_t _error_count;
|
||||||
};
|
};
|
||||||
|
|
||||||
class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver
|
class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
|
AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
|
||||||
void init();
|
void init(bool &fifo_mode, uint8_t &max_samples);
|
||||||
void read8(uint8_t reg, uint8_t *val);
|
void read8(uint8_t reg, uint8_t *val);
|
||||||
void write8(uint8_t reg, uint8_t val);
|
void write8(uint8_t reg, uint8_t val);
|
||||||
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
|
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
|
||||||
uint8_t read_burst(uint8_t v[14]);
|
void read_burst(uint8_t* samples,
|
||||||
|
AP_HAL::DigitalSource *_drdy_pin,
|
||||||
|
uint8_t &n_samples);
|
||||||
AP_HAL::Semaphore* get_semaphore();
|
AP_HAL::Semaphore* get_semaphore();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint8_t _addr;
|
uint8_t _addr;
|
||||||
|
AP_HAL::I2CDriver *_i2c;
|
||||||
AP_HAL::Semaphore *_i2c_sem;
|
AP_HAL::Semaphore *_i2c_sem;
|
||||||
|
uint8_t _rx[MAX_DATA_READ];
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__
|
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__
|
||||||
|
Loading…
Reference in New Issue
Block a user