diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 1ad3739153..8fa1d8a881 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -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; 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 // collected @@ -191,9 +192,6 @@ static volatile uint32_t _delta_time_start_micros = 0; // time latest sample was collected static volatile uint32_t _last_sample_time_micros = 0; -// data ready interrupt fired -volatile bool AP_InertialSensor_MPU6000::_data_ready; - // DMP related static variables bool AP_InertialSensor_MPU6000::_dmp_initialised = false; // high byte of number of elements in fifo buffer @@ -301,10 +299,10 @@ float AP_InertialSensor_MPU6000::temperature() { /*================ HARDWARE FUNCTIONS ==================== */ /* - * this is called from the data_interrupt which fires when the MPU6000 has new - * sensor data available and add it to _sum[] to ensure this is the case, - * these other devices must perform their spi reads after being called by the - * AP_TimerProcess. + * this is called from the num_samples_available() when the MPU6000 + * has new sensor data available and add it to _sum[] to ensure this + * is the case, these other devices must perform their spi reads + * after being called by the AP_TimerProcess. */ 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); } -// 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) { _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 hal.scheduler->delay(1); - if (!hal.gpio->attach_interrupt(6, data_interrupt, GPIO_RISING)) { - hal.console->println_P( - PSTR("Critical Error: AP_InertialSensor_MPU6000 could not " - "attach data ready interrupt.")); - } + /* Pin 70 defined especially to hook + up PE6 to the hal.gpio abstraction. + (It is not a valid pin under Arduino.) */ + _drdy_pin = hal.gpio->channel(70); } 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 uint16_t AP_InertialSensor_MPU6000::num_samples_available() { - if (_data_ready) { + if (_drdy_pin->read()) { // a data interrupt has occurred - read the data. // Note that doing it this way means we doing the read out of // interrupt context, called from the main loop. This avoids // all possible conflicts with the DataFlash SPI bus read_data(); - _data_ready = false; } return _count; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 12b2d68ad6..f9bcfd1ae0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -50,7 +50,7 @@ protected: private: static void read_data(); - static void data_interrupt(void); + static AP_HAL::DigitalSource *_drdy_pin; static uint8_t register_read( uint8_t reg ); static void register_write( uint8_t reg, uint8_t val ); void hardware_init(Sample_rate sample_rate); @@ -58,9 +58,6 @@ private: static AP_HAL::SPIDeviceDriver *_spi; static AP_HAL::Semaphore *_spi_sem; - // set to true when data interrupt from MPU6k fires - static volatile bool _data_ready; - float _temp; float _temp_to_celsius( uint16_t );