ROI (Region Of Interest) support

Author: Jinnie

examples/roi.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+
+import sys, signal
+sys.path.insert(0, "build/lib.linux-armv7l-2.7/")
+
+import VL53L1X
+import time
+from datetime import datetime
+
+tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
+print("Python: Initialized")
+tof.open()
+print("Python: Opened")
+
+# Left, right, top and bottom are relative to the SPAD matrix coordinates,
+# which will be mirrored in real scene coordinates.
+# (or even rotated, depending on the VM53L1X element alignment on the board and on the board position)
+#
+# ROI in SPAD matrix coords:
+#
+# 15  top-left
+# |  X____
+# |  |    |
+# |  |____X
+# |        bottom-right
+# 0__________15
+#
+
+def scan(type="w"):
+    if type == "w":
+        # Wide scan forward ~30deg angle
+        print "Scan: wide"
+        return VL53L1X.VL53L1xUserRoi(0, 15, 15, 0)
+    elif type == "c":
+        # Focused scan forward
+        print "Scan: center"
+        return VL53L1X.VL53L1xUserRoi(6, 9, 9, 6)
+    elif type == "t":
+        # Focused scan top
+        print "Scan: top"
+        return VL53L1X.VL53L1xUserRoi(6, 15, 9, 12)
+    elif type == "b":
+        # Focused scan bottom
+        print "Scan: bottom"
+        return VL53L1X.VL53L1xUserRoi(6, 3, 9, 0)
+    elif type == "l":
+        # Focused scan left
+        print "Scan: left"
+        return VL53L1X.VL53L1xUserRoi(0, 9, 3, 6)
+    elif type == "r":
+        # Focused scan right
+        print "Scan: right"
+        return VL53L1X.VL53L1xUserRoi(12, 9, 15, 6)
+    else:
+        print("Scan: wide (default)")
+        return VL53L1X.VL53L1xUserRoi(0, 15, 15, 0)
+
+if len(sys.argv) == 2:
+    roi = scan(sys.argv[1])
+else:
+    roi = scan("default")
+
+tof.set_user_roi(roi)
+
+tof.start_ranging(1)
+
+def exit_handler(signal, frame):
+    tof.stop_ranging()
+    tof.close()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, exit_handler)
+
+while True:
+    distance_mm = tof.get_distance()
+    if distance_mm < 0:
+        # Error -1185 may occur if you didn't stop ranging in a previous test
+        print("Error: {}".format(distance_mm))
+    else:
+        print("Distance: {}cm".format(distance_mm/10))
+    time.sleep(0.5)

python/VL53L1X.py

@@ -39,6 +39,14 @@ class VL53L1xDistanceMode:
     LONG = 3
 
 
+class VL53L1xUserRoi:
+    def __init__(self, tlx=0, tly=0, brx=15, bry=15):
+        self.top_left_x = tlx
+        self.top_left_y = tly
+        self.bot_right_x = brx
+        self.bot_right_y = bry
+
+
 # Read/write function pointer types.
 _I2C_MULTI_FUNC = CFUNCTYPE(c_int, c_ubyte, c_ubyte)
 _I2C_READ_FUNC = CFUNCTYPE(c_int, c_ubyte, c_ubyte, POINTER(c_ubyte), c_ubyte)
@@ -150,6 +158,16 @@ def _i2c_multi(address, reg):
         self._i2c_write_func = _I2C_WRITE_FUNC(_i2c_write)
         _TOF_LIBRARY.VL53L1_set_i2c(self._i2c_multi_func, self._i2c_read_func, self._i2c_write_func)
 
+    # The ROI is a square or rectangle defined by two corners: top left and bottom right.
+    # Default ROI is 16x16 (indices 0-15). The minimum ROI size is 4x4.
+    def set_user_roi(self, user_roi):
+        """Set Region Of Interest (ROI)"""
+        _TOF_LIBRARY.setUserRoi(self._dev,
+                                user_roi.top_left_x,
+                                user_roi.top_left_y,
+                                user_roi.bot_right_x,
+                                user_roi.bot_right_y)
+
     def start_ranging(self, mode=VL53L1xDistanceMode.LONG):
         """Start VL53L1X ToF Sensor Ranging"""
         _TOF_LIBRARY.startRanging(self._dev, mode)

python_lib/vl53l1x_python.c

@@ -125,6 +125,25 @@ VL53L1_Error setDistanceMode(VL53L1_Dev_t *dev, int mode)
     return Status;
 }
 
+/******************************************************************************
+ * @brief   Set User ROI (Region Of Interest)
+ * @param   userRoi - user ROI
+ * @retval  Error code, 0 for success.
+ *****************************************************************************/
+VL53L1_Error setUserRoi(VL53L1_Dev_t *dev, int topLeftX, int topLeftY, int botRightX, int botRightY)
+{
+    VL53L1_UserRoi_t userRoi;
+    userRoi.TopLeftX = topLeftX;
+    userRoi.TopLeftY = topLeftY;
+    userRoi.BotRightX = botRightX;
+    userRoi.BotRightY = botRightY;
+
+    VL53L1_Error Status = VL53L1_ERROR_NONE;
+    Status = VL53L1_SetUserROI(dev, &userRoi);
+    return Status;
+}
+
+
 /******************************************************************************
  * @brief   Start Ranging
  * @param   mode - ranging mode