From 80eaa52ed830ae686ac63991e3b2fa925d9afcb5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 22 Jan 2013 20:08:33 +1100
Subject: [PATCH] AP_Compass: use report timestamp on PX4 for accurate timing

---
 libraries/AP_Compass/AP_Compass_PX4.cpp | 6 ++++--
 libraries/AP_Compass/AP_Compass_PX4.h   | 1 +
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp
index f4022b50cb..a85c969735 100644
--- a/libraries/AP_Compass/AP_Compass_PX4.cpp
+++ b/libraries/AP_Compass/AP_Compass_PX4.cpp
@@ -74,7 +74,7 @@ bool AP_Compass_PX4::read(void)
     _sum.zero();
     _count = 0;
 
-    last_update = hal.scheduler->micros();
+    last_update = _last_timestamp;
 
     return true;
 }
@@ -82,9 +82,11 @@ bool AP_Compass_PX4::read(void)
 void AP_Compass_PX4::accumulate(void)
 {
     struct mag_report mag_report;
-    while (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report)) {
+    while (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
+           mag_report.timestamp != _last_timestamp) {
         _sum += Vector3f(mag_report.x, mag_report.y, mag_report.z);
         _count++;
+        _last_timestamp = mag_report.timestamp;
         healthy = true;
     }
 }
diff --git a/libraries/AP_Compass/AP_Compass_PX4.h b/libraries/AP_Compass/AP_Compass_PX4.h
index 22f205546d..c438490aca 100644
--- a/libraries/AP_Compass/AP_Compass_PX4.h
+++ b/libraries/AP_Compass/AP_Compass_PX4.h
@@ -19,6 +19,7 @@ private:
     int _mag_fd;
     Vector3f _sum;
     uint32_t _count;
+    uint64_t _last_timestamp;
 };
 
 #endif // AP_Compass_PX4_H