AP_InertialSensor: support AuxiliaryBus without register_periodic_callback()

This commit is contained in:
Andrew Tridgell 2016-11-03 09:52:05 +11:00
parent e79b4f1c11
commit 919aa61918
2 changed files with 17 additions and 1 deletions

View File

@ -388,7 +388,10 @@ void AP_InertialSensor_MPU6000::start()
hal.scheduler->resume_timer_procs();
// start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
if (_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool)) == nullptr) {
// cope with AuxiliaryBus which does not support register_periodic_callback yet
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data_timer, void));
}
}
/*
@ -445,6 +448,18 @@ bool AP_InertialSensor_MPU6000::_poll_data()
return true;
}
/*
varient of _poll_data for systems where register_periodic_callback
fails. We need to handle the semaphore ourselves.
*/
void AP_InertialSensor_MPU6000::_poll_data_timer()
{
if (_dev->get_semaphore()->take_nonblocking()) {
_poll_data();
_dev->get_semaphore()->give();
}
}
void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
{
for (uint8_t i = 0; i < n_samples; i++) {

View File

@ -78,6 +78,7 @@ private:
/* Poll for new data (non-blocking) */
bool _poll_data();
void _poll_data_timer();
/* Read and write functions taking the differences between buses into
* account */