/** * Timer process to poll for new data from the MPU6000. */ void AP_InertialSensor_MPU6000::_poll_data(void) { if (_data_ready()) { if (hal.scheduler->in_timerprocess()) { _read_data_from_timerprocess(); } else { /* Synchronous read - take semaphore */ bool got = _spi_sem->take(10); if (got) { _last_sample_time_micros = hal.scheduler->micros(); _read_data_transaction(); _spi_sem->give(); } else { hal.scheduler->panic( PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data " "failed to take SPI semaphore synchronously")); } } } }
void AP_InertialSensor_ITG3200::_poll_data(uint32_t now) { if (now - _ins_timer > _micros_per_sample) { _ins_timer = now; if (hal.scheduler->in_timerprocess()) { _read_data_from_timerprocess(); } else { /* Synchronous read - take semaphore */ bool got = _i2c_sem->take(10); if (got) { _read_data_transaction(); _i2c_sem->give(); } else { hal.scheduler->panic( PSTR("PANIC: AP_InertialSensor_ITG3200::_poll_data " "failed to take I2C semaphore synchronously")); } } } }
void AP_InertialSensor_MPU6000_I2C::_poll_data(void) { hal.gpio->write(46,1); uint32_t now = hal.scheduler->micros(); if ( (now - _ins_timer > _micros_per_sample) || (_sens_stage == 1)) { // hal.gpio->write(46,1); if (hal.scheduler->in_timerprocess()) { bool _stage = _sens_stage; _read_data_from_timerprocess(); if (_stage != _sens_stage && _stage == 0) { _ins_timer = now; } } else { /* Synchronous read - take semaphore */ bool got = _i2c_sem->take(10); if (got) { if (_sens_stage == 0) { _ins_timer = now; } _read_data_transaction(); _i2c_sem->give(); } else { hal.scheduler->panic( PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data " "failed to take I2C semaphore synchronously")); } } } hal.gpio->write(46,0); }