From b9b3ef91a1e3f54a12c3dc1b699358b799888f1f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Jan 2013 18:20:05 +1100
Subject: [PATCH] AP_Baro: added timer to PX4 driver

this gives us more samples when main sketch is reading slowly
---
 libraries/AP_Baro/AP_Baro_PX4.cpp | 37 ++++++++++++++++++++++++++++---
 libraries/AP_Baro/AP_Baro_PX4.h   | 14 +++++++-----
 2 files changed, 43 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro_PX4.cpp b/libraries/AP_Baro/AP_Baro_PX4.cpp
index c1035c28e1..519e0b90a7 100644
--- a/libraries/AP_Baro/AP_Baro_PX4.cpp
+++ b/libraries/AP_Baro/AP_Baro_PX4.cpp
@@ -17,6 +17,14 @@
 
 extern const AP_HAL::HAL& hal;
 
+float AP_Baro_PX4::_pressure_sum;
+float AP_Baro_PX4::_temperature_sum;
+uint32_t AP_Baro_PX4::_sum_count;
+uint32_t AP_Baro_PX4::_last_timer;
+uint64_t AP_Baro_PX4::_last_timestamp;
+volatile bool AP_Baro_PX4::_in_accumulate;
+int AP_Baro_PX4::_baro_fd;
+
 // Public Methods //////////////////////////////////////////////////////////////
 bool AP_Baro_PX4::init(void)
 {
@@ -31,6 +39,8 @@ bool AP_Baro_PX4::init(void)
 
         // average over up to 10 samples
         ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 10);
+
+        hal.scheduler->register_timer_process(_baro_timer);
     }
 
     return true;
@@ -48,7 +58,7 @@ uint8_t AP_Baro_PX4::read(void)
     _pressure    = (_pressure_sum / _sum_count) * 100.0f;
     _temperature = (_temperature_sum / _sum_count) * 10.0f;
     _pressure_samples = _sum_count;
-
+    _last_update = (uint32_t)_last_timestamp;
     _pressure_sum = 0;
     _temperature_sum = 0;
     _sum_count = 0;
@@ -58,16 +68,37 @@ uint8_t AP_Baro_PX4::read(void)
 }
 
 // accumulate sensor values
-void AP_Baro_PX4::accumulate(void)
+void AP_Baro_PX4::_accumulate(void)
 {
     struct baro_report baro_report;
+    if (_in_accumulate) {
+        return;
+    }
+    _in_accumulate = true;
 
     while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report)) {
 		_pressure_sum += baro_report.pressure; // Pressure in mbar
 		_temperature_sum += baro_report.temperature; // degrees celcius
         _sum_count++;
-        _last_update = hal.scheduler->millis();
+        _last_timestamp = baro_report.timestamp;
     }
+    _in_accumulate = false;
+}
+
+// accumulate sensor values
+void AP_Baro_PX4::accumulate(void)
+{
+    _accumulate();
+}
+
+void AP_Baro_PX4::_baro_timer(uint32_t now)
+{
+    // accumulate samples at 100Hz
+    if (now - _last_timer > 10000) {
+        return;
+    }
+    _last_timer = hal.scheduler->micros();
+    _accumulate();
 }
 
 float AP_Baro_PX4::get_pressure() {
diff --git a/libraries/AP_Baro/AP_Baro_PX4.h b/libraries/AP_Baro/AP_Baro_PX4.h
index 5e45933afe..be7cc3f7fc 100644
--- a/libraries/AP_Baro/AP_Baro_PX4.h
+++ b/libraries/AP_Baro/AP_Baro_PX4.h
@@ -19,12 +19,16 @@ public:
 private:
     float _temperature;
     float _pressure;
-    float _pressure_sum;
-    float _temperature_sum;
-    uint32_t _sum_count;
-
+    static float _pressure_sum;
+    static float _temperature_sum;
+    static uint32_t _sum_count;
+    static volatile bool _in_accumulate;
+    static void _accumulate(void);
+    static void _baro_timer(uint32_t now);
+    static uint64_t _last_timestamp;
     // baro driver handle
-    int _baro_fd;
+    static int _baro_fd;
+    static uint32_t _last_timer;
 };
 
 #endif //  __AP_BARO_PX4_H__