From f8ceccce329c91000f326bd37812a33ea4e2d763 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 9 Aug 2014 21:22:51 +1000 Subject: [PATCH] AP_InertialSensor: improved HIL timing consistancy try to keep the average time for each sample right --- .../AP_InertialSensor_HIL.cpp | 21 ++++++++++++------- .../AP_InertialSensor/AP_InertialSensor_HIL.h | 2 +- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index 6bda0bb961..313652d408 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -30,7 +30,7 @@ uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) { bool AP_InertialSensor_HIL::update( void ) { uint32_t now = hal.scheduler->micros(); - _last_update_usec = now; + _last_sample_usec = now; return true; } @@ -45,10 +45,13 @@ float AP_InertialSensor_HIL::get_gyro_drift_rate(void) { bool AP_InertialSensor_HIL::_sample_available() { - uint16_t ret = (hal.scheduler->micros() - _last_update_usec) - / _sample_period_usec; - - return ret > 0; + uint32_t tnow = hal.scheduler->micros(); + bool have_sample = false; + while (tnow - _last_sample_usec > _sample_period_usec) { + have_sample = true; + _last_sample_usec += _sample_period_usec; + } + return have_sample; } bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms) @@ -56,9 +59,13 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms) if (_sample_available()) { return true; } - uint32_t start = hal.scheduler->millis(); + uint32_t start = hal.scheduler->micros(); while ((hal.scheduler->millis() - start) < timeout_ms) { - hal.scheduler->delay(1); + uint32_t tnow = hal.scheduler->micros(); + uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow; + if (tdelay < 100000) { + hal.scheduler->delay_microseconds(tdelay); + } if (_sample_available()) { return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index aba162fce8..55ae828ad8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -28,7 +28,7 @@ private: bool _sample_available(); uint16_t _init_sensor( Sample_rate sample_rate ); uint32_t _sample_period_usec; - uint32_t _last_update_usec; + uint32_t _last_sample_usec; uint32_t _last_accel_usec[INS_MAX_INSTANCES]; uint32_t _last_gyro_usec[INS_MAX_INSTANCES]; };