/**
* 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);
}
