From 8532e2bff852d370ac32981126f201bb23507510 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 13 Oct 2013 22:15:50 +1100
Subject: [PATCH] AP_InertialSensor: fixed timing of PX4 sensor samples

---
 libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp | 10 +++++-----
 libraries/AP_InertialSensor/AP_InertialSensor_PX4.h   |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index 26b5355893..e8f97f8c2c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -108,7 +108,7 @@ bool AP_InertialSensor_PX4::update(void)
         _last_filter_hz = _mpu6000_filter;
     }
 
-    _num_samples_available = 0;
+    _have_sample_available = false;
 
     return true;
 }
@@ -145,11 +145,11 @@ void AP_InertialSensor_PX4::_get_sample(void)
 bool AP_InertialSensor_PX4::sample_available(void)
 {
     uint64_t tnow = hrt_absolute_time();
-    if (tnow - _last_sample_timestamp > _sample_time_usec) {
-        _num_samples_available++;
-        _last_sample_timestamp = tnow;
+    while (tnow - _last_sample_timestamp > _sample_time_usec) {
+        _have_sample_available = true;
+        _last_sample_timestamp += _sample_time_usec;
     }
-    return _num_samples_available > 0;
+    return _have_sample_available;
 }
 
 bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
index 3fa01c24ec..868bc0852d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
@@ -36,7 +36,7 @@ private:
     uint64_t _last_accel_timestamp;
     uint64_t _last_gyro_timestamp;
     uint64_t _last_sample_timestamp;
-    uint16_t _num_samples_available;
+    bool     _have_sample_available;
     uint32_t _sample_time_usec;
 
     // support for updating filter at runtime