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

InertialSensor: poll data ready pin instead of an interrupt for MPU6k

This commit is contained in:
Andrew Tridgell 2012-12-22 09:16:31 +11:00
parent 563ca3f79b
commit 1a53bc783c
2 changed files with 11 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -180,6 +180,7 @@ const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 };
const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3; const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE; int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE;
AP_HAL::DigitalSource *AP_InertialSensor_MPU6000::_drdy_pin = NULL;
// variables to calculate time period over which a group of samples were // variables to calculate time period over which a group of samples were
// collected // collected
@ -191,9 +192,6 @@ static volatile uint32_t _delta_time_start_micros = 0;
// time latest sample was collected // time latest sample was collected
static volatile uint32_t _last_sample_time_micros = 0; static volatile uint32_t _last_sample_time_micros = 0;
// data ready interrupt fired
volatile bool AP_InertialSensor_MPU6000::_data_ready;
// DMP related static variables // DMP related static variables
bool AP_InertialSensor_MPU6000::_dmp_initialised = false; bool AP_InertialSensor_MPU6000::_dmp_initialised = false;
// high byte of number of elements in fifo buffer // high byte of number of elements in fifo buffer
@ -301,10 +299,10 @@ float AP_InertialSensor_MPU6000::temperature() {
/*================ HARDWARE FUNCTIONS ==================== */ /*================ HARDWARE FUNCTIONS ==================== */
/* /*
* this is called from the data_interrupt which fires when the MPU6000 has new * this is called from the num_samples_available() when the MPU6000
* sensor data available and add it to _sum[] to ensure this is the case, * has new sensor data available and add it to _sum[] to ensure this
* these other devices must perform their spi reads after being called by the * is the case, these other devices must perform their spi reads
* AP_TimerProcess. * after being called by the AP_TimerProcess.
*/ */
void AP_InertialSensor_MPU6000::read_data(void) void AP_InertialSensor_MPU6000::read_data(void)
{ {
@ -380,14 +378,6 @@ void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
_spi->transaction(tx, rx, 2); _spi->transaction(tx, rx, 2);
} }
// MPU6000 new data interrupt on INT6
void AP_InertialSensor_MPU6000::data_interrupt(void)
{
_data_ready = true;
// record time that data was available
_last_sample_time_micros = hal.scheduler->micros();
}
void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
{ {
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000); _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
@ -483,11 +473,10 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR); // clear interrupt on any read register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR); // clear interrupt on any read
hal.scheduler->delay(1); hal.scheduler->delay(1);
if (!hal.gpio->attach_interrupt(6, data_interrupt, GPIO_RISING)) { /* Pin 70 defined especially to hook
hal.console->println_P( up PE6 to the hal.gpio abstraction.
PSTR("Critical Error: AP_InertialSensor_MPU6000 could not " (It is not a valid pin under Arduino.) */
"attach data ready interrupt.")); _drdy_pin = hal.gpio->channel(70);
}
} }
float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval ) float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval )
@ -507,13 +496,12 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
// get number of samples read from the sensors // get number of samples read from the sensors
uint16_t AP_InertialSensor_MPU6000::num_samples_available() uint16_t AP_InertialSensor_MPU6000::num_samples_available()
{ {
if (_data_ready) { if (_drdy_pin->read()) {
// a data interrupt has occurred - read the data. // a data interrupt has occurred - read the data.
// Note that doing it this way means we doing the read out of // Note that doing it this way means we doing the read out of
// interrupt context, called from the main loop. This avoids // interrupt context, called from the main loop. This avoids
// all possible conflicts with the DataFlash SPI bus // all possible conflicts with the DataFlash SPI bus
read_data(); read_data();
_data_ready = false;
} }
return _count; return _count;
} }

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -50,7 +50,7 @@ protected:
private: private:
static void read_data(); static void read_data();
static void data_interrupt(void); static AP_HAL::DigitalSource *_drdy_pin;
static uint8_t register_read( uint8_t reg ); static uint8_t register_read( uint8_t reg );
static void register_write( uint8_t reg, uint8_t val ); static void register_write( uint8_t reg, uint8_t val );
void hardware_init(Sample_rate sample_rate); void hardware_init(Sample_rate sample_rate);
@ -58,9 +58,6 @@ private:
static AP_HAL::SPIDeviceDriver *_spi; static AP_HAL::SPIDeviceDriver *_spi;
static AP_HAL::Semaphore *_spi_sem; static AP_HAL::Semaphore *_spi_sem;
// set to true when data interrupt from MPU6k fires
static volatile bool _data_ready;
float _temp; float _temp;
float _temp_to_celsius( uint16_t ); float _temp_to_celsius( uint16_t );