diff --git a/PiicoDev_VL53L1X.py b/PiicoDev_VL53L1X.py index 2a9baae..4ef4c12 100644 --- a/PiicoDev_VL53L1X.py +++ b/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))