PiicoDev_VL53L1X.py

"""
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'

VL51L1X_DEFAULT_CONFIGURATION = bytes([
0x00, # 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
0x00, # 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x00, # 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x01, # 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
0x02, # 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
0x00, # 0x32 : not user-modifiable (NUM)*/
0x02, # 0x33 : NUM */
0x08, # 0x34 : NUM */
0x00, # 0x35 : NUM */
0x08, # 0x36 : NUM */
0x10, # 0x37 : NUM */
0x01, # 0x38 : NUM */
0x01, # 0x39 : NUM */
0x00, # 0x3a : NUM */
0x00, # 0x3b : NUM */
0x00, # 0x3c : NUM */
0x00, # 0x3d : NUM */
0xff, # 0x3e : NUM */
0x00, # 0x3f : NUM */
0x0F, # 0x40 : NUM */
0x00, # 0x41 : NUM */
0x00, # 0x42 : NUM */
0x00, # 0x43 : NUM */
0x00, # 0x44 : NUM */
0x00, # 0x45 : NUM */
0x20, # 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
0x0b, # 0x47 : NUM */
0x00, # 0x48 : NUM */
0x00, # 0x49 : NUM */
0x02, # 0x4a : NUM */
0x0a, # 0x4b : NUM */
0x21, # 0x4c : NUM */
0x00, # 0x4d : NUM */
0x00, # 0x4e : NUM */
0x05, # 0x4f : NUM */
0x00, # 0x50 : NUM */
0x00, # 0x51 : NUM */
0x00, # 0x52 : NUM */
0x00, # 0x53 : NUM */
0xc8, # 0x54 : NUM */
0x00, # 0x55 : NUM */
0x00, # 0x56 : NUM */
0x38, # 0x57 : NUM */
0xff, # 0x58 : NUM */
0x01, # 0x59 : NUM */
0x00, # 0x5a : NUM */
0x08, # 0x5b : NUM */
0x00, # 0x5c : NUM */
0x00, # 0x5d : NUM */
0x01, # 0x5e : NUM */
0xdb, # 0x5f : NUM */
0x0f, # 0x60 : NUM */
0x01, # 0x61 : NUM */
0xf1, # 0x62 : NUM */
0x0d, # 0x63 : NUM */
0x01, # 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm  */
0x68, # 0x65 : Sigma threshold LSB */
0x00, # 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
0x80, # 0x67 : Min count Rate LSB */
0x08, # 0x68 : NUM */
0xb8, # 0x69 : NUM */
0x00, # 0x6a : NUM */
0x00, # 0x6b : NUM */
0x00, # 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
0x00, # 0x6d : Intermeasurement period */
0x0f, # 0x6e : Intermeasurement period */
0x89, # 0x6f : Intermeasurement period LSB */
0x00, # 0x70 : NUM */
0x00, # 0x71 : NUM */
0x00, # 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, # 0x73 : distance threshold high LSB */
0x00, # 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, # 0x75 : distance threshold low LSB */
0x00, # 0x76 : NUM */
0x01, # 0x77 : NUM */
0x0f, # 0x78 : NUM */
0x0d, # 0x79 : NUM */
0x0e, # 0x7a : NUM */
0x0e, # 0x7b : NUM */
0x00, # 0x7c : NUM */
0x00, # 0x7d : NUM */
0x02, # 0x7e : NUM */
0xc7, # 0x7f : ROI center, use SetROI() */
0xff, # 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
0x9B, # 0x81 : NUM */
0x00, # 0x82 : NUM */
0x00, # 0x83 : NUM */
0x00, # 0x84 : NUM */
0x01, # 0x85 : NUM */
0x01, # 0x86 : clear interrupt, use ClearInterrupt() */
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):
        try:
            if compat_ind >= 1:
                pass
            else:
                print(compat_str)
        except:
            print(compat_str)
        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:
            raise RuntimeError('Failed to find expected ID register values. Check wiring!')
        # write default configuration
        self.i2c.writeto_mem(self.addr, 0x2D, VL51L1X_DEFAULT_CONFIGURATION, addrsize=16)
        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(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(IDENTIFICATION__MODEL_ID) 
    def reset(self):
        self.writeReg(0x0000, 0x00)
        sleep_ms(100)
        self.writeReg(0x0000, 0x01)
    
    def read(self):
        try:
            data = self.i2c.readfrom_mem(self.addr, 0x0089, 17, addrsize=16) # RESULT__RANGE_STATUS
        except:
            print(i2c_err_str.format(self.addr))
            return float('NaN')
        range_status = data[0]
        # 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]
        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:
            self.status = "MinRangeFail"
        elif range_status == 18:
            self.status = "SynchronizationInt"
        elif range_status == 5:
            self.status = "OutOfBoundsFail"
        elif range_status == 4:
            self.status = "SignalFail"
        elif range_status == 6:
            self.status = "SignalFail"
        elif range_status == 7:
            self.status = "WrapTargetFail"
        elif range_status == 12:
            self.status = "XtalkSignalFail"
        elif range_status == 8:
            self.status = "RangeValidMinRangeClipped"
        elif range_status == 9:
            if stream_count == 0:
                self.status = "RangeValidNoWrapCheckFail"
            else:
                self.status = "OK"
        return final_crosstalk_corrected_range_mm_sd0
    
    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))