mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: don't skip delay if we are a bit early
this produces a bit more even timing
This commit is contained in:
parent
619196b6b3
commit
04bef5ccf3
|
@ -1072,9 +1072,7 @@ void AP_InertialSensor::wait_for_sample(void)
|
||||||
if (_next_sample_usec - now <=_sample_period_usec) {
|
if (_next_sample_usec - now <=_sample_period_usec) {
|
||||||
// we're ahead on time, schedule next sample at expected period
|
// we're ahead on time, schedule next sample at expected period
|
||||||
uint32_t wait_usec = _next_sample_usec - now;
|
uint32_t wait_usec = _next_sample_usec - now;
|
||||||
if (wait_usec > 200) {
|
hal.scheduler->delay_microseconds(wait_usec);
|
||||||
hal.scheduler->delay_microseconds(wait_usec);
|
|
||||||
}
|
|
||||||
_next_sample_usec += _sample_period_usec;
|
_next_sample_usec += _sample_period_usec;
|
||||||
} else if (now - _next_sample_usec < _sample_period_usec/8) {
|
} else if (now - _next_sample_usec < _sample_period_usec/8) {
|
||||||
// we've overshot, but only by a small amount, keep on
|
// we've overshot, but only by a small amount, keep on
|
||||||
|
|
Loading…
Reference in New Issue