AP_Baro_PX4: fixed baro driver to run at full rate

we were skipping samples due to a timer handling bug. It now checks
for new samples at 100Hz

libraries/AP_Baro/AP_Baro_PX4.cpp

@@ -12,8 +12,7 @@
 #include <unistd.h>
 
 #include <drivers/drv_baro.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_hrt.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -22,7 +21,6 @@ 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 //////////////////////////////////////////////////////////////
@@ -41,6 +39,9 @@ bool AP_Baro_PX4::init(void)
         ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 10);
 
         hal.scheduler->register_timer_process(_baro_timer);
+
+        // give the timer a chance to run and gather one sample
+        hal.scheduler->delay(40);
     }
 
     return true;
@@ -49,12 +50,19 @@ bool AP_Baro_PX4::init(void)
 // Read the sensor
 uint8_t AP_Baro_PX4::read(void)
 {
-    accumulate();
-    if (_sum_count == 0) {
-        // no data available
-        return 0;
+    hal.scheduler->suspend_timer_procs();
+
+    // try to accumulate one more sample, so we have the latest data
+    _accumulate();
+
+    // consider the baro healthy if we got a reading in the last 0.2s
+    healthy = (hrt_absolute_time() - _last_timestamp < 200000);
+    if (!healthy || _sum_count == 0) {
+        hal.scheduler->resume_timer_procs();
+        return healthy;
     }
 
+
     _pressure    = (_pressure_sum / _sum_count) * 100.0f;
     _temperature = (_temperature_sum / _sum_count) * 10.0f;
     _pressure_samples = _sum_count;
@@ -63,7 +71,8 @@ uint8_t AP_Baro_PX4::read(void)
     _temperature_sum = 0;
     _sum_count = 0;
 
-    healthy = true;
+    hal.scheduler->resume_timer_procs();
+
     return 1;
 }
 
@@ -71,11 +80,6 @@ uint8_t AP_Baro_PX4::read(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) &&
            baro_report.timestamp != _last_timestamp) {
 		_pressure_sum += baro_report.pressure; // Pressure in mbar
@@ -83,19 +87,12 @@ void AP_Baro_PX4::_accumulate(void)
         _sum_count++;
         _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) {
+    if (now - _last_timer < 10000) {
         return;
     }
     _last_timer = hal.scheduler->micros();

libraries/AP_Baro/AP_Baro_PX4.h

@@ -14,7 +14,6 @@ public:
     float get_temperature();
     int32_t get_raw_pressure();
     int32_t get_raw_temp();
-    void accumulate(void);
 
 private:
     float _temperature;
@@ -22,7 +21,6 @@ private:
     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;