mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-13 03:13:57 -04:00
AP_InertialSensor: fixed timer bug in HIL sensors
This commit is contained in:
parent
3a8d4cdcda
commit
fae45b29fa
@ -63,7 +63,7 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
|
|||||||
if (_sample_available()) {
|
if (_sample_available()) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
uint32_t start = hal.scheduler->micros();
|
uint32_t start = hal.scheduler->millis();
|
||||||
while ((hal.scheduler->millis() - start) < timeout_ms) {
|
while ((hal.scheduler->millis() - start) < timeout_ms) {
|
||||||
uint32_t tnow = hal.scheduler->micros();
|
uint32_t tnow = hal.scheduler->micros();
|
||||||
uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
|
uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
|
||||||
|
Loading…
Reference in New Issue
Block a user