AP_RangeFinder: use thread per bus for LightWareI2C driver

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -30,6 +30,9 @@ AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, u
     : AP_RangeFinder_Backend(_ranger, instance, _state)
     , _dev(std::move(dev))
 {
+    // call timer() at 20Hz
+    _dev->register_periodic_callback(50000,
+                                     FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::timer, bool));
 }
 
 /*
@@ -42,13 +45,21 @@ AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger
     AP_RangeFinder_LightWareI2C *sensor
         = new AP_RangeFinder_LightWareI2C(_ranger, instance, _state, std::move(dev));
 
-    uint16_t reading_cm;
-
-    if (!sensor || !sensor->get_reading(reading_cm)) {
+    if (!sensor) {
         delete sensor;
         return nullptr;
     }
 
+    if (sensor->_dev->get_semaphore()->take(0)) {
+        uint16_t reading_cm;
+        if (!sensor->get_reading(reading_cm)) {
+            sensor->_dev->get_semaphore()->give();
+            delete sensor;
+            return nullptr;
+        }
+        sensor->_dev->get_semaphore()->give();
+    }
+
     return sensor;
 }
 
@@ -61,12 +72,6 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
         return false;
     }
 
-
-    // exit immediately if we can't take the semaphore
-    if (!_dev || !_dev->get_semaphore()->take(1)) {
-        return false;
-    }
-
     // read the high and low byte distance registers
     bool ret = _dev->read((uint8_t *) &val, sizeof(val));
     if (ret) {
@@ -74,8 +79,6 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
         reading_cm = be16toh(val);
     }
 
-    _dev->get_semaphore()->give();
-
     return ret;
 }
 
@@ -83,11 +86,17 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
    update the state of the sensor
 */
 void AP_RangeFinder_LightWareI2C::update(void)
+{
+    // nothing to do - its all done in the timer()
+}
+
+bool AP_RangeFinder_LightWareI2C::timer(void)
 {
     if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
         update_status();
     } else {
         set_status(RangeFinder::RangeFinder_NoData);
-    }
+    }    
+    return true;
 }

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h

@@ -18,6 +18,8 @@ private:
     // constructor
     AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
 
+    bool timer();
+    
     // get a reading
     bool get_reading(uint16_t &reading_cm);
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;