AP_InertialSensor: fixed timer bug in HIL sensors

This commit is contained in:
Andrew Tridgell 2014-08-19 19:25:08 +10:00
parent 8f280d212b
commit 532e9aace4

View File

@ -63,7 +63,7 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->micros();
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
uint32_t tnow = hal.scheduler->micros();
uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;