Lots of new features added to this driver

PiicoDev_VL53L1X.py

@@ -1,3 +1,8 @@
+"""
+Modified driver for Core's VL53L1X device, additions by Trevor Norman, you can find me here:
+ https://forum.core-electronics.com.au/u/trevor277988
+
+"""
 from PiicoDev_Unified import *
 
 compat_str = '\nUnified PiicoDev library out of date.  Get the latest module: https://piico.dev/unified \n'
@@ -96,6 +101,30 @@
 0x40  # 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
 ])
 
+# Register addresses used by enhanced driver methods
+OSC_MEASURED__FAST_OSC__FREQUENCY               = 0x06
+ALGO__PART_TO_PART_RANGE_OFFSET_MM              = 0x001E
+MM_CONFIG__OUTER_OFFSET_MM                      = 0x0022
+PHASECAL_CONFIG__TIMEOUT_MACROP                 = 0x004B
+MM_CONFIG__TIMEOUT_MACROP_A                     = 0x005A
+MM_CONFIG__TIMEOUT_MACROP_B                     = 0x005C
+RANGE_CONFIG__TIMEOUT_MACROP_A                  = 0x005E
+RANGE_CONFIG__VCSEL_PERIOD_A                    = 0x0060
+RANGE_CONFIG__TIMEOUT_MACROP_B                  = 0x0061
+RANGE_CONFIG__VCSEL_PERIOD_B                    = 0x0063
+RANGE_CONFIG__VALID_PHASE_HIGH                  = 0x0069
+SD_CONFIG__WOI_SD0                              = 0x0078
+SD_CONFIG__WOI_SD1                              = 0x0079
+SD_CONFIG__INITIAL_PHASE_SD0                    = 0x007A
+SD_CONFIG__INITIAL_PHASE_SD1                    = 0x007B
+ROI_CONFIG__USER_ROI_CENTRE_SPAD                = 0x007F
+ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE   = 0x0080
+SYSTEM__INTERRUPT_CLEAR                         = 0x0086
+SYSTEM__MODE_START                              = 0x0087
+RESULT__OSC_CALIBRATE_VAL                       = 0x00DE
+IDENTIFICATION__MODEL_ID                        = 0x010F
+
+TimingGuard                                     = 4528      # used for Timing Budget calcs
 
 class PiicoDev_VL53L1X:
     def __init__(self, bus=None, freq=None, sda=None, scl=None, address=0x29):
@@ -109,6 +138,14 @@ def __init__(self, bus=None, freq=None, sda=None, scl=None, address=0x29):
         self.i2c = create_unified_i2c(bus=bus, freq=freq, sda=sda, scl=scl)
         self.addr = address
         self.status = None
+        self.distance_mode = 'unknown'
+        self.ambient_count = 0
+        self.peak_count = 0
+        self.spad_count = 0
+        # self.calibrated = False
+        # self.saved_vhv_init = 0
+        # self.saved_vhv_timeout = 0
+        # self.osc_calibrate_val = self.readReg16Bit(RESULT__OSC_CALIBRATE_VAL)
         self.reset()
         sleep_ms(1)
         if self.read_model_id() != 0xEACC:
@@ -118,20 +155,22 @@ def __init__(self, bus=None, freq=None, sda=None, scl=None, address=0x29):
         sleep_ms(100)
         # the API triggers this change in VL53L1_init_and_start_range() once a
         # measurement is started; assumes MM1 and MM2 are disabled
-        self.writeReg16Bit(0x001E, self.readReg16Bit(0x0022) * 4)
+        self.writeReg16Bit(ALGO__PART_TO_PART_RANGE_OFFSET_MM, self.readReg16Bit(MM_CONFIG__OUTER_OFFSET_MM) * 4)
         sleep_ms(200)
 
     def writeReg(self, reg, value):
         return self.i2c.writeto_mem(self.addr, reg, bytes([value]), addrsize=16)
     def writeReg16Bit(self, reg, value):
         return self.i2c.writeto_mem(self.addr, reg, bytes([(value >> 8) & 0xFF, value & 0xFF]), addrsize=16)
+    def writeReg32Bit(self, reg, value):
+        return self.i2c.writeto_mem(self.addr, reg, bytes([(value >> 24) & 0xFF, (value >> 16) & 0xFF, (value >> 8) & 0xFF, value & 0xFF]), addrsize=16)
     def readReg(self, reg):
         return self.i2c.readfrom_mem(self.addr, reg, 1, addrsize=16)[0]
     def readReg16Bit(self, reg):
         data = self.i2c.readfrom_mem(self.addr, reg, 2, addrsize=16)
         return (data[0]<<8) + data[1]
     def read_model_id(self):
-        return self.readReg16Bit(0x010F) 
+        return self.readReg16Bit(IDENTIFICATION__MODEL_ID) 
     def reset(self):
         self.writeReg(0x0000, 0x00)
         sleep_ms(100)
@@ -147,13 +186,15 @@ def read(self):
         # report_status = data[1]
         stream_count = data[2]
         dss_actual_effective_spads_sd0 = (data[3]<<8) + data[4]
+        self.spad_count = dss_actual_effective_spads_sd0 >> 4       # TN added right-shift, ref UM2356 2.6
         # peak_signal_count_rate_mcps_sd0 = (data[5]<<8) + data[6]
         ambient_count_rate_mcps_sd0 = (data[7]<<8) + data[8]
+        self.ambient_count = ambient_count_rate_mcps_sd0
         # sigma_sd0 = (data[9]<<8) + data[10]
         # phase_sd0 = (data[11]<<8) + data[12]
         final_crosstalk_corrected_range_mm_sd0 = (data[13]<<8) + data[14]
         peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = (data[15]<<8) + data[16]
-        status = None
+        self.peak_count = peak_signal_count_rate_crosstalk_corrected_mcps_sd0
         if range_status in (17, 2, 1, 3):
             self.status = "HardwareFail"
         elif range_status == 13:
@@ -183,3 +224,195 @@ def change_addr(self, new_addr):
         self.writeReg(0x0001, new_addr & 0x7F)
         sleep_ms(50)
         self.addr = new_addr
+
+    def decodeTimeout(self, reg_val):
+        return ((reg_val & 0xFF) << (reg_val >> 8)) + 1
+
+    def encodeTimeout(self, timeout_mclks):
+        if timeout_mclks > 0:
+            ls_byte = timeout_mclks - 1
+            ms_byte = 0
+            while (ls_byte & 0xFFFFFF00) > 0:
+                ls_byte >>= 1
+                ms_byte += 1
+            return (ms_byte << 8) | (ls_byte & 0xFF)
+        else:
+            return 0
+
+    def timeoutMclksToMicroseconds(self, timeout_mclks, macro_period_us):
+        # Returns integer microseconds
+        return ((timeout_mclks * macro_period_us + 0x800) >> 12)
+
+    def timeoutMicrosecondsToMclks(self, timeout_us, macro_period_us):
+        # Returns integer macro periods
+        return (((timeout_us << 12) + (macro_period_us >> 1)) // macro_period_us)
+
+    def calcMacroPeriod(self, vcsel_period):
+        # You must have self.fast_osc_frequency set (read from register 0x0006)
+        fast_osc_frequency = self.readReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY)
+
+        pll_period_us = (1 << 30) // fast_osc_frequency
+        vcsel_period_pclks = (vcsel_period + 1) << 1
+        macro_period_us = 2304 * pll_period_us
+        macro_period_us >>= 6
+        macro_period_us *= vcsel_period_pclks
+        macro_period_us >>= 6
+        return macro_period_us
+
+    # def startContinuous(self, period_ms):
+    #     SYSTEM__INTERMEASUREMENT_PERIOD = 0x006C
+
+    #     self.writeReg32Bit(SYSTEM__INTERMEASUREMENT_PERIOD, period_ms * self.osc_calibrate_val)
+    #     self.writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01)
+    #     self.writeReg(SYSTEM__MODE_START, 0x40)
+
+    # def stopContinuous(self):
+    #     VHV_CONFIG__INIT                        = 0x000B
+    #     VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND   = 0x0008
+    #     PHASECAL_CONFIG__OVERRIDE               = 0x004D
+
+    #     self.calibrated = False
+    #     if self.saved_vhv_init != 0:
+    #         self.writeReg(VHV_CONFIG__INIT, self.saved_vhv_init)
+    #     if self.saved_vhv_timeout != 0:
+    #         self.writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, self.saved_vhv_timeout)
+    #     self.writeReg(PHASECAL_CONFIG__OVERRIDE, 0x00)
+
+    def startRanging(self):
+        self.writeReg(SYSTEM__MODE_START, 0x40)
+
+    def singleMode(self):
+        self.writeReg(SYSTEM__MODE_START, 0x10)
+
+    def stopRanging(self):
+        self.writeReg(SYSTEM__MODE_START, 0x00)
+
+    def readSingle(self):
+        self.writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01)
+        self.writeReg(SYSTEM__MODE_START, 0x10)
+
+        return self.read()
+
+    def getMeasurementTimingBudget(self):
+        # Assumes PresetMode is LOWPOWER_AUTONOMOUS and sequence steps enabled: VHV, PHASECAL, DSS1, RANGE
+
+        macro_period_us = self.calcMacroPeriod(self.readReg(RANGE_CONFIG__VCSEL_PERIOD_A))
+        range_config_timeout_us = self.timeoutMclksToMicroseconds(
+            self.decodeTimeout(self.readReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A)), macro_period_us )
+        return 2 * range_config_timeout_us + TimingGuard
+
+    def setMeasurementTimingBudget(self, budget_us):
+        # Assumes PresetMode is LOWPOWER_AUTONOMOUS
+
+        if budget_us <= TimingGuard:
+            return False
+
+        range_config_timeout_us = budget_us - TimingGuard
+        if range_config_timeout_us > 1100000:
+            return False
+
+        range_config_timeout_us //= 2
+
+        macro_period_us = self.calcMacroPeriod(self.readReg(RANGE_CONFIG__VCSEL_PERIOD_A))
+        phasecal_timeout_mclks = self.timeoutMicrosecondsToMclks(1000, macro_period_us)
+        if phasecal_timeout_mclks > 0xFF:
+            phasecal_timeout_mclks = 0xFF
+        self.writeReg(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks)
+
+        self.writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_A,
+            self.encodeTimeout(self.timeoutMicrosecondsToMclks(1, macro_period_us))
+        )
+
+        self.writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A,
+            self.encodeTimeout(self.timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))
+        )
+
+        macro_period_us = self.calcMacroPeriod(self.readReg(RANGE_CONFIG__VCSEL_PERIOD_B))
+        self.writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_B,
+            self.encodeTimeout(self.timeoutMicrosecondsToMclks(1, macro_period_us))
+        )
+        self.writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_B,
+            self.encodeTimeout(self.timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))
+        )
+        return True
+
+    def getDistanceMode(self):
+        return self.distance_mode
+
+    def setDistanceMode(self, mode):
+    # """
+    # Set the distance mode: "short", "medium", or "long".
+    # Returns True on success, False on invalid mode.
+    # """
+        # Save existing timing budget
+        budget_us = self.getMeasurementTimingBudget()
+
+        if mode == "short":
+            # timing config
+            self.writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07)       # RANGE_CONFIG__VCSEL_PERIOD_A
+            self.writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05)       # RANGE_CONFIG__VCSEL_PERIOD_B
+            self.writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38)     # RANGE_CONFIG__VALID_PHASE_HIGH
+
+            # dynamic config
+            self.writeReg(SD_CONFIG__WOI_SD0, 0x07)                 # SD_CONFIG__WOI_SD0
+            self.writeReg(SD_CONFIG__WOI_SD1, 0x05)                 # SD_CONFIG__WOI_SD1
+            self.writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 6)          # SD_CONFIG__INITIAL_PHASE_SD0
+            self.writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 6)          # SD_CONFIG__INITIAL_PHASE_SD1
+
+        elif mode == "medium":
+            self.writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B)
+            self.writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09)
+            self.writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78)
+
+            self.writeReg(SD_CONFIG__WOI_SD0, 0x0B)
+            self.writeReg(SD_CONFIG__WOI_SD1, 0x09)
+            self.writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 10)
+            self.writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 10)
+
+        elif mode == "long":
+            self.writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F)
+            self.writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D)
+            self.writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8)
+
+            self.writeReg(SD_CONFIG__WOI_SD0, 0x0F)
+            self.writeReg(SD_CONFIG__WOI_SD1, 0x0D)
+            self.writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 14)
+            self.writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 14)
+
+        else:
+            # Unrecognized mode
+            return False
+
+        # Reapply timing budget
+        self.setMeasurementTimingBudget(budget_us)
+
+        # Optionally, store mode if you want to track it
+        self.distance_mode = mode
+
+        return True
+
+    def getROICenter(self):
+        return self.readReg(ROI_CONFIG__USER_ROI_CENTRE_SPAD)
+
+    def setROICenter(self, padnum):
+        self.writeReg(ROI_CONFIG__USER_ROI_CENTRE_SPAD, padnum & 0XFF)
+
+    def getROISize(self, list_WH:list):
+        """
+        Returns the width and height of the ROI in the list, which should probably be EMPTY when you call this
+        """
+        reg_val = self.readReg(ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE)
+        list_WH.append((reg_val & 0xF) + 1)
+        list_WH.append((reg_val >> 4) + 1)
+
+    def setROISize(self, width, height):
+        if ( width > 16): width  = 16
+        if (height > 16): height = 16
+
+#   Force ROI to be centered if width or height > 10, matching what the ULD API
+#   does. (This can probably be overridden by calling setROICenter()
+#   afterwards.)
+        if (width > 10 or height > 10): self.writeReg(ROI_CONFIG__USER_ROI_CENTRE_SPAD, 199)
+
+        self.writeReg(ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
+           (height - 1) << 4 | (width - 1))