MPU6000: use data ready interrupt to prevent stale data

We listen for a data ready interrupt and only read new data in read()
if there is new data
This commit is contained in:
Andrew Tridgell 2011-12-25 20:37:50 +11:00
parent 71e08f8484
commit 3ef707a2c3
2 changed files with 41 additions and 14 deletions

View File

@ -12,8 +12,10 @@
#define MPUREG_CONFIG 0x1A //
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_FIFO_EN 0x23
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B //
#define MPUREG_ACCEL_XOUT_L 0x3C //
#define MPUREG_ACCEL_YOUT_H 0x3D //
@ -31,6 +33,10 @@
#define MPUREG_USER_CTRL 0x6A //
#define MPUREG_PWR_MGMT_1 0x6B //
#define MPUREG_PWR_MGMT_2 0x6C //
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
@ -56,6 +62,7 @@
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
int AP_InertialSensor_MPU6000::_cs_pin;
@ -81,6 +88,8 @@ const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 };
const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
static volatile uint8_t _new_data;
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( int cs_pin )
{
_cs_pin = cs_pin; /* can't use initializer list, is static */
@ -200,18 +209,26 @@ static int16_t spi_transfer_16(void)
return (((int16_t)byte_H)<<8) | byte_L;
}
/*
this is called from a timer interrupt to read data from the MPU6000
and add it to _sum[]
*/
void AP_InertialSensor_MPU6000::read(uint32_t )
{
// We start a multibyte SPI read of sensors
byte addr = MPUREG_ACCEL_XOUT_H | 0x80; // Set most significant bit
if (_new_data == 0) {
// no new data is ready from the MPU6000
return;
}
_new_data = 0;
// now read the data
digitalWrite(_cs_pin, LOW);
byte addr = MPUREG_ACCEL_XOUT_H | 0x80;
SPI.transfer(addr);
for (uint8_t i=0; i<7; i++) {
_sum[i] += spi_transfer_16();
}
_count++;
if (_count == 0) {
// rollover - v unlikely
@ -246,6 +263,13 @@ void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
digitalWrite(_cs_pin, HIGH);
}
// MPU6000 new data interrupt on INT6
void AP_InertialSensor_MPU6000::data_interrupt(void)
{
// tell the timer routine that there is data to be read
_new_data = 1;
}
void AP_InertialSensor_MPU6000::hardware_init()
{
// MPU6000 chip select setup
@ -272,6 +296,7 @@ void AP_InertialSensor_MPU6000::hardware_init()
delay(1);
register_write(MPUREG_ACCEL_CONFIG,0x08); // Accel scele 4g (4096LSB/g)
delay(1);
// INT CFG => Interrupt on Data Ready
register_write(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready
delay(1);
@ -281,6 +306,7 @@ void AP_InertialSensor_MPU6000::hardware_init()
// register_write(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
delay(1);
attachInterrupt(6,data_interrupt,RISING);
}
float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval )

View File

@ -34,6 +34,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
void reset_sample_time();
static void read(uint32_t);
static void data_interrupt(void);
static uint8_t register_read( uint8_t reg );
static void register_write( uint8_t reg, uint8_t val );
static void hardware_init();