From 6dd05db3b7a20228101a06543fcdee5457a14318 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Tue, 9 Apr 2019 02:10:10 -0700
Subject: [PATCH] AP_Rangefinder: Fix VL53L1X ignoring return status

---
 .../AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 16 ++++++++++-
 .../AP_RangeFinder/AP_RangeFinder_VL53L1X.h   | 27 +++++++++++++++++++
 2 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
index f075d31009..d233c66b44 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
@@ -421,6 +421,8 @@ bool AP_RangeFinder_VL53L1X::get_reading(uint16_t &reading_mm)
         }
     }
 
+    const uint8_t range_status = read_register(RESULT__RANGE_STATUS);
+
     reading_mm = read_register16(RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0);
     // "apply correction gain"
     // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT)
@@ -435,7 +437,18 @@ bool AP_RangeFinder_VL53L1X::get_reading(uint16_t &reading_mm)
     }
 
     write_register(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
-    return true;
+
+    switch ((DeviceError)range_status) {
+      case RANGECOMPLETE:
+        return true;
+
+      default:
+#ifdef VL53L1X_DEBUG
+        hal.console->printf("VL53L1X: %d ms status %d\n", AP_HAL::millis(), (int)range_status);
+#endif // VL53L1X_DEBUG
+        return false;
+
+    }
 }
 
 uint8_t AP_RangeFinder_VL53L1X::read_register(uint16_t reg)
@@ -492,6 +505,7 @@ void AP_RangeFinder_VL53L1X::timer(void)
 {
     uint16_t range_mm;
     if ((get_reading(range_mm)) && (range_mm <= 4000)) {
+        WITH_SEMAPHORE(_sem);
         sum_mm += range_mm;
         counter++;
     }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
index 1979f47822..be5e6b91a6 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
@@ -21,6 +21,33 @@ protected:
     }
 
 private:
+    enum DeviceError : uint8_t
+    {
+        NOUPDATE                    = 0,
+        VCSELCONTINUITYTESTFAILURE  = 1,
+        VCSELWATCHDOGTESTFAILURE    = 2,
+        NOVHVVALUEFOUND             = 3,
+        MSRCNOTARGET                = 4,
+        RANGEPHASECHECK             = 5,
+        SIGMATHRESHOLDCHECK         = 6,
+        PHASECONSISTENCY            = 7,
+        MINCLIP                     = 8,
+        RANGECOMPLETE               = 9,
+        ALGOUNDERFLOW               = 10,
+        ALGOOVERFLOW                = 11,
+        RANGEIGNORETHRESHOLD        = 12,
+        USERROICLIP                 = 13,
+        REFSPADCHARNOTENOUGHDPADS   = 14,
+        REFSPADCHARMORETHANTARGET   = 15,
+        REFSPADCHARLESSTHANTARGET   = 16,
+        MULTCLIPFAIL                = 17,
+        GPHSTREAMCOUNT0READY        = 18,
+        RANGECOMPLETE_NO_WRAP_CHECK = 19,
+        EVENTCONSISTENCY            = 20,
+        MINSIGNALEVENTCHECK         = 21,
+        RANGECOMPLETE_MERGED_PULSE  = 22,
+    };
+
     // register addresses from API vl53l1x_register_map.h
     enum regAddr : uint16_t
     {