From e88519364fcfb8b22c12ac0491407cf9f4f5a45f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 4 Nov 2016 14:35:41 +1100
Subject: [PATCH] AP_RangeFinder: fixed maxbotix I2C semaphore and bus usage

---
 .../AP_RangeFinder_MaxsonarI2CXL.cpp          | 85 +++++++++++++------
 .../AP_RangeFinder_MaxsonarI2CXL.h            |  6 ++
 .../AP_RangeFinder/RangeFinder_Backend.cpp    |  3 +
 .../AP_RangeFinder/RangeFinder_Backend.h      |  3 +
 4 files changed, 71 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
index b7226abb38..0540e7f3e0 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
@@ -51,16 +51,11 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range
 {
     AP_RangeFinder_MaxsonarI2CXL *sensor
         = new AP_RangeFinder_MaxsonarI2CXL(_ranger, instance, _state);
-    if (!sensor || !sensor->start_reading()) {
-        delete sensor;
+    if (!sensor) {
         return nullptr;
     }
 
-    // give time for the sensor to process the request
-    hal.scheduler->delay(50);
-
-    uint16_t reading_cm;
-    if (!sensor->get_reading(reading_cm)) {
+    if (!sensor->_init()) {
         delete sensor;
         return nullptr;
     }
@@ -68,21 +63,45 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range
     return sensor;
 }
 
-// start_reading() - ask sensor to make a range reading
-bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
+/*
+  initialise sensor
+ */
+bool AP_RangeFinder_MaxsonarI2CXL::_init(void)
 {
-    if (!_dev || !_dev->get_semaphore()->take(1)) {
+    if (!_dev->get_semaphore()->take(0)) {
+        return false;
+    }
+    
+
+    if (!start_reading()) {
+        _dev->get_semaphore()->give();
         return false;
     }
 
+    // give time for the sensor to process the request
+    hal.scheduler->delay(50);
+
+    uint16_t reading_cm;
+    if (!get_reading(reading_cm)) {
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    _dev->get_semaphore()->give();
+    
+    _dev->register_periodic_callback(50000,
+                                     FUNCTOR_BIND_MEMBER(&AP_RangeFinder_MaxsonarI2CXL::_timer, bool));
+    
+    return true;
+}
+
+// start_reading() - ask sensor to make a range reading
+bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
+{
     uint8_t cmd = AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING;
 
     // send command to take reading
-    bool ret = _dev->transfer(&cmd, sizeof(cmd), nullptr, 0);
-
-    _dev->get_semaphore()->give();
-
-    return ret;
+    return _dev->transfer(&cmd, sizeof(cmd), nullptr, 0);
 }
 
 // read - return last value measured by sensor
@@ -90,14 +109,8 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
 {
     be16_t val;
 
-    // exit immediately if we can't take the semaphore
-    if (!_dev->get_semaphore()->take(1)) {
-        return false;
-    }
-
     // take range reading and read back results
     bool ret = _dev->transfer(nullptr, 0, (uint8_t *) &val, sizeof(val));
-    _dev->get_semaphore()->give();
 
     if (ret) {
         // combine results into distance
@@ -110,16 +123,36 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
     return ret;
 }
 
+/*
+  timer called at 20Hz
+*/
+bool AP_RangeFinder_MaxsonarI2CXL::_timer(void)
+{
+    uint16_t d;
+    if (get_reading(d)) {
+        if (_sem->take(0)) {
+            distance = d;
+            new_distance = true;
+            _sem->give();
+        }
+    }
+    return true;
+}
+    
 
 /*
    update the state of the sensor
 */
 void AP_RangeFinder_MaxsonarI2CXL::update(void)
 {
-    if (get_reading(state.distance_cm)) {
-        // update range_valid state based on distance measured
-        update_status();
-    } else {
-        set_status(RangeFinder::RangeFinder_NoData);
+    if (_sem->take_nonblocking()) {
+        if (new_distance) {
+            state.distance_cm = distance;
+            new_distance = false;
+            update_status();
+        } else {
+            set_status(RangeFinder::RangeFinder_NoData);
+        }
+         _sem->give();
     }
 }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h
index d256742688..f65a0e38c9 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h
@@ -28,6 +28,12 @@ private:
     AP_RangeFinder_MaxsonarI2CXL(RangeFinder &ranger, uint8_t instance,
                                  RangeFinder::RangeFinder_State &_state);
 
+    bool _init(void);    
+    bool _timer(void);
+
+    uint16_t distance;
+    bool new_distance;
+    
     // start a reading
     bool start_reading(void);
     bool get_reading(uint16_t &reading_cm);
diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp
index 9af355163d..6f95f54a23 100644
--- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp
@@ -18,6 +18,8 @@
 #include "RangeFinder.h"
 #include "RangeFinder_Backend.h"
 
+extern const AP_HAL::HAL& hal;
+
 /*
   base class constructor. 
   This incorporates initialisation as well.
@@ -26,6 +28,7 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t ins
         ranger(_ranger),
         state(_state) 
 {
+    _sem = hal.util->new_semaphore();    
 }
 
 // update status based on distance measurement
diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h
index 9fafa3fb7f..5227be1419 100644
--- a/libraries/AP_RangeFinder/RangeFinder_Backend.h
+++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h
@@ -48,4 +48,7 @@ protected:
 
     RangeFinder &ranger;
     RangeFinder::RangeFinder_State &state;
+
+    // semaphore for access to shared frontend data
+    AP_HAL::Semaphore *_sem;    
 };