mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_InertialSensor: add fifo support for MPU6000
And remove the use of data rdy in this case
This commit is contained in:
Julien BERAUD
Andrew Tridgell
parent
8a76ff53bd
commit
3cf952d1f8
@ -407,10 +407,9 @@ AP_InertialSensor::_detect_backends(void)
|
||||
#if HAL_INS_DEFAULT == HAL_INS_HIL
|
||||
_add_backend(AP_InertialSensor_HIL::detect);
|
||||
#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
|
||||
_backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this,
|
||||
new AP_MPU6000_BusDriver_I2C(hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR));
|
||||
_add_backend(AP_InertialSensor_MPU6000::detect_i2c2);
|
||||
#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
|
||||
|
||||
@ -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_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 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 BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
|
||||
# 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_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 */
|
||||
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);
|
||||
// Disable I2C bus if SPI selected (Recommended in Datasheet
|
||||
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)
|
||||
{
|
||||
uint8_t addr = reg | 0x80; // Set most significant bit
|
||||
|
||||
uint8_t tx[2];
|
||||
uint8_t rx[2];
|
||||
|
||||
tx[0] = reg;
|
||||
tx[0] = addr;
|
||||
tx[1] = 0;
|
||||
_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)
|
||||
{
|
||||
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 */
|
||||
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));
|
||||
|
||||
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()
|
||||
@ -220,8 +270,25 @@ AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8
|
||||
_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)
|
||||
{
|
||||
@ -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)
|
||||
{}
|
||||
|
||||
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 {
|
||||
uint8_t int_status;
|
||||
uint8_t d[14];
|
||||
} rx;
|
||||
uint16_t bytes_read;
|
||||
uint8_t ret = 0;
|
||||
|
||||
_i2c->readRegisters(_addr, MPUREG_INT_STATUS, 15, (uint8_t *) &rx);
|
||||
memcpy(v, rx.d, 14);
|
||||
ret = _i2c->readRegisters(_addr, MPUREG_FIFO_COUNTH, 2, _rx);
|
||||
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()
|
||||
@ -275,7 +378,6 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
||||
_bus_sem(NULL),
|
||||
_last_accel_filter_hz(-1),
|
||||
_last_gyro_filter_hz(-1),
|
||||
_error_count(0),
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
_accel_filter(1000, 15),
|
||||
_gyro_filter(1000, 15),
|
||||
@ -285,6 +387,43 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
||||
_gyro_sum(),
|
||||
#endif
|
||||
_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();
|
||||
|
||||
@ -301,19 +440,17 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
||||
if (success) {
|
||||
hal.scheduler->delay(5+2);
|
||||
if (!_bus_sem->take(100)) {
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
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;
|
||||
return false;
|
||||
}
|
||||
} while (1);
|
||||
|
||||
@ -330,10 +467,8 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
||||
_dump_registers();
|
||||
#endif
|
||||
|
||||
return;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
process any
|
||||
*/
|
||||
@ -429,59 +564,50 @@ void AP_InertialSensor_MPU6000::_poll_data(void)
|
||||
if (!_bus_sem->take_nonblocking()) {
|
||||
return;
|
||||
}
|
||||
if (_data_ready()) {
|
||||
if (_fifo_mode || _data_ready()) {
|
||||
_read_data_transaction();
|
||||
}
|
||||
_bus_sem->give();
|
||||
}
|
||||
|
||||
|
||||
void AP_InertialSensor_MPU6000::_read_data_transaction() {
|
||||
/* one resister address followed by seven 2-byte registers */
|
||||
uint8_t v[14];
|
||||
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]))
|
||||
void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
for(uint8_t i=0; i < n_samples; i++) {
|
||||
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
_accel_filtered = _accel_filter.apply(Vector3f(int16_val(v, 1),
|
||||
int16_val(v, 0),
|
||||
-int16_val(v, 2)));
|
||||
_accel_filtered = _accel_filter.apply(Vector3f(int16_val(data, 1),
|
||||
int16_val(data, 0),
|
||||
-int16_val(data, 2)));
|
||||
|
||||
_gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(v, 5),
|
||||
int16_val(v, 4),
|
||||
-int16_val(v, 6)));
|
||||
_gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(data, 4),
|
||||
int16_val(data, 3),
|
||||
-int16_val(data, 5)));
|
||||
#else
|
||||
_accel_sum.x += int16_val(v, 1);
|
||||
_accel_sum.y += int16_val(v, 0);
|
||||
_accel_sum.z -= int16_val(v, 2);
|
||||
_gyro_sum.x += int16_val(v, 5);
|
||||
_gyro_sum.y += int16_val(v, 4);
|
||||
_gyro_sum.z -= int16_val(v, 6);
|
||||
_accel_sum.x += int16_val(data, 1);
|
||||
_accel_sum.y += int16_val(data, 0);
|
||||
_accel_sum.z -= int16_val(data, 2);
|
||||
_gyro_sum.x += int16_val(data, 4);
|
||||
_gyro_sum.y += int16_val(data, 3);
|
||||
_gyro_sum.z -= int16_val(data, 5);
|
||||
#endif
|
||||
_sum_count++;
|
||||
_sum_count++;
|
||||
|
||||
#if !MPU6000_FAST_SAMPLING
|
||||
if (_sum_count == 0) {
|
||||
if (_sum_count == 0) {
|
||||
// rollover - v unlikely
|
||||
_accel_sum.zero();
|
||||
_gyro_sum.zero();
|
||||
}
|
||||
_accel_sum.zero();
|
||||
_gyro_sum.zero();
|
||||
}
|
||||
#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 )
|
||||
@ -541,6 +667,8 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz)
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
||||
{
|
||||
uint8_t max_samples;
|
||||
|
||||
if (!_bus_sem->take(100)) {
|
||||
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
|
||||
}
|
||||
@ -561,9 +689,9 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
||||
hal.scheduler->delay(5);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
#if MPU6000_DEBUG
|
||||
_dump_registers();
|
||||
#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
|
||||
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);
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
@ -609,7 +739,11 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
||||
_set_filter_register(256);
|
||||
|
||||
// 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
|
||||
_set_filter_register(_accel_filter_cutoff());
|
||||
|
||||
|
||||
@ -24,17 +24,23 @@
|
||||
#include <LowPassFilter2p.h>
|
||||
#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
|
||||
{
|
||||
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 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) = 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;
|
||||
};
|
||||
|
||||
@ -42,6 +48,8 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
|
||||
{
|
||||
public:
|
||||
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 */
|
||||
bool update();
|
||||
@ -59,19 +67,19 @@ private:
|
||||
uint8_t _accel_instance;
|
||||
|
||||
AP_HAL::DigitalSource *_drdy_pin;
|
||||
|
||||
bool _init_sensor(void);
|
||||
bool _sample_available();
|
||||
void _read_data_transaction();
|
||||
bool _data_ready();
|
||||
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_check(uint8_t reg, uint8_t val);
|
||||
bool _hardware_init(void);
|
||||
void _accumulate(uint8_t *samples, uint8_t n_samples);
|
||||
|
||||
AP_MPU6000_BusDriver *_bus;
|
||||
AP_HAL::Semaphore *_bus_sem;
|
||||
AP_HAL::Semaphore *_bus_sem;
|
||||
|
||||
static const float _gyro_scale;
|
||||
|
||||
@ -81,9 +89,6 @@ private:
|
||||
|
||||
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
|
||||
uint8_t _sample_count;
|
||||
|
||||
@ -101,37 +106,47 @@ private:
|
||||
Vector3l _gyro_sum;
|
||||
#endif
|
||||
volatile uint16_t _sum_count;
|
||||
bool _fifo_mode;
|
||||
uint8_t *_samples;
|
||||
};
|
||||
|
||||
class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
|
||||
{
|
||||
public:
|
||||
void init();
|
||||
void init(bool &fifo_mode, uint8_t &max_samples);
|
||||
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]);
|
||||
void read_burst(uint8_t* samples,
|
||||
AP_HAL::DigitalSource *_drdy_pin,
|
||||
uint8_t &n_samples);
|
||||
AP_HAL::Semaphore* get_semaphore();
|
||||
|
||||
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 init(bool &fifo_mode, uint8_t &max_samples);
|
||||
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]);
|
||||
void read_burst(uint8_t* samples,
|
||||
AP_HAL::DigitalSource *_drdy_pin,
|
||||
uint8_t &n_samples);
|
||||
AP_HAL::Semaphore* get_semaphore();
|
||||
|
||||
private:
|
||||
uint8_t _addr;
|
||||
AP_HAL::I2CDriver *_i2c;
|
||||
AP_HAL::Semaphore *_i2c_sem;
|
||||
uint8_t _rx[MAX_DATA_READ];
|
||||
};
|
||||
|
||||
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__
|
||||
|
||||
Loading…
Reference in New Issue
Block a user