AP_InertialSensor: fixed semaphore error on startup for MPU6000

This commit is contained in:
Andrew Tridgell 2013-11-09 11:26:50 +11:00
parent 3262022195
commit 80def01fbe
1 changed files with 4 additions and 1 deletions

View File

@ -217,7 +217,10 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data));