AP_Baro: fixed semaphore and thread usage in baro drivers

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -80,22 +80,17 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice>
     _state = 0;
 
     sem->give();
+
+    _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, bool));
 }
 
 /*
   This is a state machine. Acumulate a new sensor reading.
  */
-void AP_Baro_BMP085::accumulate(void)
+bool AP_Baro_BMP085::_timer(void)
 {
-    AP_HAL::Semaphore *sem = _dev->get_semaphore();
-
     if (!_data_ready()) {
-        return;
+        return true;
-    }
-
-    // take i2c bus sempahore
-    if (!sem->take(1)) {
-        return;
     }
 
     if (_state == 0) {
@@ -111,8 +106,7 @@ void AP_Baro_BMP085::accumulate(void)
     } else {
         _cmd_read_pressure();
     }
-
+    return true;
-    sem->give();
 }
 
 /*
@@ -120,17 +114,18 @@ void AP_Baro_BMP085::accumulate(void)
  */
 void AP_Baro_BMP085::update(void)
 {
-    if (!_has_sample && _data_ready()) {
+    if (_sem->take_nonblocking()) {
-        accumulate();
-    }
         if (!_has_sample) {
+            _sem->give();
             return;
         }
 
         float temperature = 0.1f * _temp;
-
         float pressure = _pressure_filter.getf();
+
         _copy_to_frontend(_instance, pressure, temperature);
+        _sem->give();
+    }
 }
 
 // Send command to Read Pressure
@@ -217,8 +212,11 @@ void AP_Baro_BMP085::_calculate()
     x2 = (-7357 * p) >> 16;
     p += ((x1 + x2 + 3791) >> 4);
 
+    if (_sem->take(0)) {
         _pressure_filter.apply(p);
         _has_sample = true;
+        _sem->give();
+    }
 }
 
 bool AP_Baro_BMP085::_data_ready()

libraries/AP_Baro/AP_Baro_BMP085.h

@@ -14,7 +14,6 @@ public:
 
     /* AP_Baro public interface: */
     void update();
-    void accumulate(void);
 
 private:
     void _cmd_read_pressure();
@@ -24,6 +23,8 @@ private:
     void _calculate();
     bool _data_ready();
 
+    bool _timer(void);
+    
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
     AP_HAL::DigitalSource *_eoc;
 

libraries/AP_Baro/AP_Baro_Backend.cpp

@@ -5,7 +5,9 @@ extern const AP_HAL::HAL& hal;
 // constructor
 AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) : 
     _frontend(baro) 
-{}
+{
+    _sem = hal.util->new_semaphore();    
+}
 
 /*
   copy latest data to the frontend from a backend

libraries/AP_Baro/AP_Baro_Backend.h

@@ -21,4 +21,7 @@ protected:
     AP_Baro &_frontend;
 
     void _copy_to_frontend(uint8_t instance, float pressure, float temperature);
+
+    // semaphore for access to shared frontend data
+    AP_HAL::Semaphore *_sem;    
 };

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -69,11 +69,6 @@ void AP_Baro_MS56XX::_init()
         AP_HAL::panic("AP_Baro_MS56XX: failed to use device");
     }
 
-    _sem = hal.util->new_semaphore();
-    if (!_sem) {
-        AP_HAL::panic("AP_Baro_MS56XX: failed to create semaphore");
-    }
-
     _instance = _frontend.register_sensor();
 
     if (!_dev->get_semaphore()->take(10)) {
@@ -280,7 +275,7 @@ void AP_Baro_MS56XX::update()
     uint32_t sD1, sD2;
     uint8_t d1count, d2count;
 
-    if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_sem->take_nonblocking()) {
         return;
     }
 

libraries/AP_Baro/AP_Baro_MS5611.h

@@ -33,12 +33,6 @@ protected:
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
 
-    /*
-     * Synchronize access to _accum between thread sampling the HW and main
-     * thread using the values
-     */
-    AP_HAL::Semaphore *_sem;
-
     /* Shared values between thread sampling the HW and main thread */
     struct {
         uint32_t s_D1;

libraries/AP_Baro/AP_Baro_qflight.cpp

@@ -40,27 +40,35 @@ void AP_Baro_QFLIGHT::timer_update(void)
         return;
     }
 
+    if (!_sem->take(0)) {
+        return;
+    }
+
     for (uint16_t i=0; i<barobuf->num_samples; i++) {
         DSPBuffer::BARO::BUF &b = barobuf->buf[i];
         pressure_sum += b.pressure_pa;
         temperature_sum += b.temperature_C;
         sum_count++;
     }
+
+    _sem->give();
 }
 
 // Read the sensor
 void AP_Baro_QFLIGHT::update(void)
 {
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
     if (sum_count > 0) {
-        hal.scheduler->suspend_timer_procs();
         _copy_to_frontend(instance,
                           pressure_sum/sum_count,
                           temperature_sum/sum_count);
         sum_count = 0;
         pressure_sum = 0;
         temperature_sum = 0;
-        hal.scheduler->resume_timer_procs();
     }
+    _sem->give();
 }
 
 #endif // CONFIG_HAL_BOARD_SUBTYPE