031 - MicroPython TechNotes: TOF Distance Sensor

 1# More details can be found in TechToTinker.blogspot.com 
 2# George Bantique | tech.to.tinker@gmail.com
 3
 4from machine import Pin
 5from machine import I2C
 6from vl53l0x import VL53L0X
 7
 8I2C_bus = I2C(0, sda=Pin(21), scl=Pin(22))
 9tof = VL53L0X(I2C_bus)
10
11# The following lines of code should be tested using the REPL:
12# 1. To enable/start the tof:
13# tof.start()
14# 2. To read the current tof value:
15# tof.read()
16# 3. To disable/stop the tof:
17# tof.stop()

 1# More details can be found in TechToTinker.blogspot.com 
 2# George Bantique | tech.to.tinker@gmail.com
 3
 4from vl53l0x import VL53L0X
 5from machine import I2C
 6from machine import Pin
 7from time import ticks_ms
 8
 9led = Pin(2, Pin.OUT)
10
11I2C_bus = I2C(0, sda=Pin(21), scl=Pin(22))
12tof = VL53L0X(I2C_bus)
13timer_start = ticks_ms()
14
15while True:
16    tof.start()
17    distance = tof.read()
18    tof.stop()
19    
20    if ticks_ms() - timer_start >= distance:
21        led.value(not led.value())
22        timer_start = ticks_ms()

  1# https://github.com/uceeatz/VL53L0X/blob/master/VL53L0X.py
  2from micropython import const
  3import ustruct
  4import utime
  5from machine import Timer
  6import time
  7
  8_IO_TIMEOUT = 1000
  9_SYSRANGE_START = const(0x00)
 10_EXTSUP_HV = const(0x89)
 11_MSRC_CONFIG = const(0x60)
 12_FINAL_RATE_RTN_LIMIT = const(0x44)
 13_SYSTEM_SEQUENCE = const(0x01)
 14_SPAD_REF_START = const(0x4f)
 15_SPAD_ENABLES = const(0xb0)
 16_REF_EN_START_SELECT = const(0xb6)
 17_SPAD_NUM_REQUESTED = const(0x4e)
 18_INTERRUPT_GPIO = const(0x0a)
 19_INTERRUPT_CLEAR = const(0x0b)
 20_GPIO_MUX_ACTIVE_HIGH = const(0x84)
 21_RESULT_INTERRUPT_STATUS = const(0x13)
 22_RESULT_RANGE_STATUS = const(0x14)
 23_OSC_CALIBRATE = const(0xf8)
 24_MEASURE_PERIOD = const(0x04)
 25
 26SYSRANGE_START = 0x00
 27
 28SYSTEM_THRESH_HIGH = 0x0C
 29SYSTEM_THRESH_LOW = 0x0E
 30
 31SYSTEM_SEQUENCE_CONFIG = 0x01
 32SYSTEM_RANGE_CONFIG = 0x09
 33SYSTEM_INTERMEASUREMENT_PERIOD = 0x04
 34
 35SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A
 36
 37GPIO_HV_MUX_ACTIVE_HIGH = 0x84
 38
 39SYSTEM_INTERRUPT_CLEAR = 0x0B
 40
 41RESULT_INTERRUPT_STATUS = 0x13
 42RESULT_RANGE_STATUS = 0x14
 43
 44RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC
 45RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0
 46RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0
 47RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4
 48RESULT_PEAK_SIGNAL_RATE_REF = 0xB6
 49
 50ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28
 51
 52I2C_SLAVE_DEVICE_ADDRESS = 0x8A
 53
 54MSRC_CONFIG_CONTROL = 0x60
 55
 56PRE_RANGE_CONFIG_MIN_SNR = 0x27
 57PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56
 58PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57
 59PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64
 60
 61FINAL_RANGE_CONFIG_MIN_SNR = 0x67
 62FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47
 63FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48
 64FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44
 65
 66PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61
 67PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62
 68
 69PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50
 70PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51
 71PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52
 72
 73SYSTEM_HISTOGRAM_BIN = 0x81
 74HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33
 75HISTOGRAM_CONFIG_READOUT_CTRL = 0x55
 76
 77FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70
 78FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71
 79FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72
 80CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20
 81
 82MSRC_CONFIG_TIMEOUT_MACROP = 0x46
 83
 84SOFT_RESET_GO2_SOFT_RESET_N = 0xBF
 85IDENTIFICATION_MODEL_ID = 0xC0
 86IDENTIFICATION_REVISION_ID = 0xC2
 87
 88OSC_CALIBRATE_VAL = 0xF8
 89
 90GLOBAL_CONFIG_VCSEL_WIDTH = 0x32
 91GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0
 92GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1
 93GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2
 94GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3
 95GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4
 96GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5
 97
 98GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6
 99DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E
100DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F
101POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80
102
103VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89
104
105ALGO_PHASECAL_LIM = 0x30
106ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30
107
108
109class TimeoutError(RuntimeError):
110    pass
111
112
113class VL53L0X:
114    def __init__(self, i2c, address=0x29):
115        self.i2c = i2c
116        self.address = address
117        self.init()
118        self._started = False
119        self.measurement_timing_budget_us = 0
120        self.set_measurement_timing_budget(self.measurement_timing_budget_us)
121        self.enables = {"tcc": 0,
122                        "dss": 0,
123                        "msrc": 0,
124                        "pre_range": 0,
125                        "final_range": 0}
126        self.timeouts = {"pre_range_vcsel_period_pclks": 0,
127                         "msrc_dss_tcc_mclks": 0,
128                         "msrc_dss_tcc_us": 0,
129                         "pre_range_mclks": 0,
130                         "pre_range_us": 0,
131                         "final_range_vcsel_period_pclks": 0,
132                         "final_range_mclks": 0,
133                         "final_range_us": 0
134                         }
135        self.vcsel_period_type = ["VcselPeriodPreRange", "VcselPeriodFinalRange"]
136
137    def _registers(self, register, values=None, struct='B'):
138        if values is None:
139            size = ustruct.calcsize(struct)
140            data = self.i2c.readfrom_mem(self.address, register, size)
141            values = ustruct.unpack(struct, data)
142            return values
143        data = ustruct.pack(struct, *values)
144        self.i2c.writeto_mem(self.address, register, data)
145
146    def _register(self, register, value=None, struct='B'):
147        if value is None:
148            return self._registers(register, struct=struct)[0]
149        self._registers(register, (value,), struct=struct)
150
151    def _flag(self, register=0x00, bit=0, value=None):
152        data = self._register(register)
153        mask = 1 << bit
154        if value is None:
155            return bool(data & mask)
156        elif value:
157            data |= mask
158        else:
159            data &= ~mask
160        self._register(register, data)
161
162    def _config(self, *config):
163        for register, value in config:
164            self._register(register, value)
165
166    def init(self, power2v8=True):
167        self._flag(_EXTSUP_HV, 0, power2v8)
168
169        # I2C standard mode
170        self._config(
171            (0x88, 0x00),
172
173            (0x80, 0x01),
174            (0xff, 0x01),
175            (0x00, 0x00),
176        )
177        self._stop_variable = self._register(0x91)
178        self._config(
179            (0x00, 0x01),
180            (0xff, 0x00),
181            (0x80, 0x00),
182        )
183
184        # disable signal_rate_msrc and signal_rate_pre_range limit checks
185        self._flag(_MSRC_CONFIG, 1, True)
186        self._flag(_MSRC_CONFIG, 4, True)
187
188        # rate_limit = 0.25
189        self._register(_FINAL_RATE_RTN_LIMIT, int(0.1 * (1 << 7)),
190                       struct='>H')
191
192        self._register(_SYSTEM_SEQUENCE, 0xff)
193
194        spad_count, is_aperture = self._spad_info()
195        spad_map = bytearray(self._registers(_SPAD_ENABLES, struct='6B'))
196
197        # set reference spads
198        self._config(
199            (0xff, 0x01),
200            (_SPAD_REF_START, 0x00),
201            (_SPAD_NUM_REQUESTED, 0x2c),
202            (0xff, 0x00),
203            (_REF_EN_START_SELECT, 0xb4),
204        )
205
206        spads_enabled = 0
207        for i in range(48):
208            if i < 12 and is_aperture or spads_enabled >= spad_count:
209                spad_map[i // 8] &= ~(1 << (i >> 2))
210            elif spad_map[i // 8] & (1 << (i >> 2)):
211                spads_enabled += 1
212
213        self._registers(_SPAD_ENABLES, spad_map, struct='6B')
214
215        self._config(
216            (0xff, 0x01),
217            (0x00, 0x00),
218
219            (0xff, 0x00),
220            (0x09, 0x00),
221            (0x10, 0x00),
222            (0x11, 0x00),
223
224            (0x24, 0x01),
225            (0x25, 0xFF),
226            (0x75, 0x00),
227
228            (0xFF, 0x01),
229            (0x4E, 0x2C),
230            (0x48, 0x00),
231            (0x30, 0x20),
232
233            (0xFF, 0x00),
234            (0x30, 0x09),
235            (0x54, 0x00),
236            (0x31, 0x04),
237            (0x32, 0x03),
238            (0x40, 0x83),
239            (0x46, 0x25),
240            (0x60, 0x00),
241            (0x27, 0x00),
242            (0x50, 0x06),
243            (0x51, 0x00),
244            (0x52, 0x96),
245            (0x56, 0x08),
246            (0x57, 0x30),
247            (0x61, 0x00),
248            (0x62, 0x00),
249            (0x64, 0x00),
250            (0x65, 0x00),
251            (0x66, 0xA0),
252
253            (0xFF, 0x01),
254            (0x22, 0x32),
255            (0x47, 0x14),
256            (0x49, 0xFF),
257            (0x4A, 0x00),
258
259            (0xFF, 0x00),
260            (0x7A, 0x0A),
261            (0x7B, 0x00),
262            (0x78, 0x21),
263
264            (0xFF, 0x01),
265            (0x23, 0x34),
266            (0x42, 0x00),
267            (0x44, 0xFF),
268            (0x45, 0x26),
269            (0x46, 0x05),
270            (0x40, 0x40),
271            (0x0E, 0x06),
272            (0x20, 0x1A),
273            (0x43, 0x40),
274
275            (0xFF, 0x00),
276            (0x34, 0x03),
277            (0x35, 0x44),
278
279            (0xFF, 0x01),
280            (0x31, 0x04),
281            (0x4B, 0x09),
282            (0x4C, 0x05),
283            (0x4D, 0x04),
284
285            (0xFF, 0x00),
286            (0x44, 0x00),
287            (0x45, 0x20),
288            (0x47, 0x08),
289            (0x48, 0x28),
290            (0x67, 0x00),
291            (0x70, 0x04),
292            (0x71, 0x01),
293            (0x72, 0xFE),
294            (0x76, 0x00),
295            (0x77, 0x00),
296
297            (0xFF, 0x01),
298            (0x0D, 0x01),
299
300            (0xFF, 0x00),
301            (0x80, 0x01),
302            (0x01, 0xF8),
303
304            (0xFF, 0x01),
305            (0x8E, 0x01),
306            (0x00, 0x01),
307            (0xFF, 0x00),
308            (0x80, 0x00),
309        )
310
311        self._register(_INTERRUPT_GPIO, 0x04)
312        self._flag(_GPIO_MUX_ACTIVE_HIGH, 4, False)
313        self._register(_INTERRUPT_CLEAR, 0x01)
314
315        # XXX Need to implement this.
316        # budget = self._timing_budget()
317        # self._register(_SYSTEM_SEQUENCE, 0xe8)
318        # self._timing_budget(budget)
319
320        self._register(_SYSTEM_SEQUENCE, 0x01)
321        self._calibrate(0x40)
322        self._register(_SYSTEM_SEQUENCE, 0x02)
323        self._calibrate(0x00)
324
325        self._register(_SYSTEM_SEQUENCE, 0xe8)
326
327    def _spad_info(self):
328        self._config(
329            (0x80, 0x01),
330            (0xff, 0x01),
331            (0x00, 0x00),
332
333            (0xff, 0x06),
334        )
335        self._flag(0x83, 3, True)
336        self._config(
337            (0xff, 0x07),
338            (0x81, 0x01),
339
340            (0x80, 0x01),
341
342            (0x94, 0x6b),
343            (0x83, 0x00),
344        )
345        for timeout in range(_IO_TIMEOUT):
346            if self._register(0x83):
347                break
348            utime.sleep_ms(1)
349        else:
350            raise TimeoutError()
351        self._config(
352            (0x83, 0x01),
353        )
354        value = self._register(0x92)
355        self._config(
356            (0x81, 0x00),
357            (0xff, 0x06),
358        )
359        self._flag(0x83, 3, False)
360        self._config(
361            (0xff, 0x01),
362            (0x00, 0x01),
363
364            (0xff, 0x00),
365            (0x80, 0x00),
366        )
367        count = value & 0x7f
368        is_aperture = bool(value & 0b10000000)
369        return count, is_aperture
370
371    def _calibrate(self, vhv_init_byte):
372        self._register(_SYSRANGE_START, 0x01 | vhv_init_byte)
373        for timeout in range(_IO_TIMEOUT):
374            if self._register(_RESULT_INTERRUPT_STATUS) & 0x07:
375                break
376            utime.sleep_ms(1)
377        else:
378            raise TimeoutError()
379        self._register(_INTERRUPT_CLEAR, 0x01)
380        self._register(_SYSRANGE_START, 0x00)
381
382    def start(self, period=0):
383        self._config(
384            (0x80, 0x01),
385            (0xFF, 0x01),
386            (0x00, 0x00),
387            (0x91, self._stop_variable),
388            (0x00, 0x01),
389            (0xFF, 0x00),
390            (0x80, 0x00),
391        )
392        if period:
393            oscilator = self._register(_OSC_CALIBRATE, struct='>H')
394            if oscilator:
395                period *= oscilator
396            self._register(_MEASURE_PERIOD, period, struct='>H')
397            self._register(_SYSRANGE_START, 0x04)
398        else:
399            self._register(_SYSRANGE_START, 0x02)
400        self._started = True
401
402    def stop(self):
403        self._register(_SYSRANGE_START, 0x01)
404        self._config(
405            (0xFF, 0x01),
406            (0x00, 0x00),
407            (0x91, self._stop_variable),
408            (0x00, 0x01),
409            (0xFF, 0x00),
410        )
411        self._started = False
412
413    def read(self):
414        if not self._started:
415            self._config(
416                (0x80, 0x01),
417                (0xFF, 0x01),
418                (0x00, 0x00),
419                (0x91, self._stop_variable),
420                (0x00, 0x01),
421                (0xFF, 0x00),
422                (0x80, 0x00),
423                (_SYSRANGE_START, 0x01),
424            )
425            for timeout in range(_IO_TIMEOUT):
426                if not self._register(_SYSRANGE_START) & 0x01:
427                    break
428                utime.sleep_ms(1)
429            else:
430                raise TimeoutError()
431        for timeout in range(_IO_TIMEOUT):
432            if self._register(_RESULT_INTERRUPT_STATUS) & 0x07:
433                break
434            utime.sleep_ms(1)
435        else:
436            raise TimeoutError()
437        value = self._register(_RESULT_RANGE_STATUS + 10, struct='>H')
438        self._register(_INTERRUPT_CLEAR, 0x01)
439        return value
440
441    def set_signal_rate_limit(self, limit_Mcps):
442        if limit_Mcps < 0 or limit_Mcps > 511.99:
443            return False
444        self._register(0x44, limit_Mcps * (1 << 7))
445        return True
446
447    def decode_Vcsel_period(self, reg_val):
448        return (((reg_val) + 1) << 1)
449
450    def encode_Vcsel_period(self, period_pclks):
451        return (((period_pclks) >> 1) - 1)
452
453    def set_Vcsel_pulse_period(self, type, period_pclks):
454        vcsel_period_reg = self.encode_Vcsel_period(period_pclks)
455
456        self.get_sequence_step_enables()
457        self.get_sequence_step_timeouts()
458
459        if type == self.vcsel_period_type[0]:
460            if period_pclks == 12:
461                self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18)
462            elif period_pclks == 14:
463                self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30)
464            elif period_pclks == 16:
465                self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40)
466            elif period_pclks == 18:
467                self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50)
468            else:
469                return False
470
471            self._register(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
472            self._register(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg)
473
474            new_pre_range_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["pre_range_us"],
475                                                                             period_pclks)
476            self._register(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(new_pre_range_timeout_mclks))
477
478            new_msrc_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["msrc_dss_tcc_us"],
479                                                                        period_pclks)
480            self._register(MSRC_CONFIG_TIMEOUT_MACROP, 255 if new_msrc_timeout_mclks > 256 else (new_msrc_timeout_mclks - 1))
481        elif type == self.vcsel_period_type[1]:
482            if period_pclks == 8:
483                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10)
484                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
485                self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02)
486                self._(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C)
487                self._register(0xFF, 0x01)
488                self._register(ALGO_PHASECAL_LIM, 0x30)
489                self._register(0xFF, 0x00)
490            elif period_pclks == 10:
491                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28)
492                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
493                self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
494                self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09)
495                self._register(0xFF, 0x01)
496                self._register(ALGO_PHASECAL_LIM, 0x20)
497                self._register(0xFF, 0x00)
498            elif period_pclks == 12:
499                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38)
500                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
501                self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
502                self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08)
503                self._register(0xFF, 0x01)
504                self._register(ALGO_PHASECAL_LIM, 0x20)
505                self._register(0xFF, 0x00)
506            elif period_pclks == 14:
507                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48)
508                self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
509                self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
510                self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07)
511                self._register(0xFF, 0x01)
512                self._register(ALGO_PHASECAL_LIM, 0x20)
513                self._register(0xFF, 0x00)
514            else:
515                return False
516
517            self._register(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg)
518
519            new_final_range_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["final_range_us"], period_pclks)
520
521            if self.enables["pre_range"]:
522                new_final_range_timeout_mclks += 1
523            self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(new_final_range_timeout_mclks))
524        else:
525            return False
526        self.set_measurement_timing_budget(self.measurement_timing_budget_us)
527        sequence_config = self._register(SYSTEM_SEQUENCE_CONFIG)
528        self._register(SYSTEM_SEQUENCE_CONFIG, 0x02)
529        self.perform_single_ref_calibration(0x0)
530        self._register(SYSTEM_SEQUENCE_CONFIG, sequence_config)
531
532        return True
533
534    def get_sequence_step_enables(self):
535        sequence_config = self._register(0x01)
536
537        self.enables["tcc"] = (sequence_config >> 4) & 0x1
538        self.enables["dss"] = (sequence_config >> 3) & 0x1
539        self.enables["msrc"] = (sequence_config >> 2) & 0x1
540        self.enables["pre_range"] = (sequence_config >> 6) & 0x1
541        self.enables["final_range"] = (sequence_config >> 7) & 0x1
542
543    def get_vcsel_pulse_period(self, type):
544        if type == self.vcsel_period_type[0]:
545            return self.decode_Vcsel_period(0x50)
546        elif type == self.vcsel_period_type[1]:
547            return self.decode_Vcsel_period(0x70)
548        else:
549            return 255
550
551    def get_sequence_step_timeouts(self):
552        self.timeouts["pre_range_vcsel_period_pclks"] = self.get_vcsel_pulse_period(self.vcsel_period_type[0])
553        self.timeouts["msrc_dss_tcc_mclks"] = int(self._register(MSRC_CONFIG_TIMEOUT_MACROP)) + 1
554        self.timeouts["msrc_dss_tcc_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["msrc_dss_tcc_mclks"],
555                                                                              self.timeouts[
556                                                                                  "pre_range_vcsel_period_pclks"])
557        self.timeouts["pre_range_mclks"] = self.decode_timeout(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)
558        self.timeouts["pre_range_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["pre_range_mclks"],
559                                                                           self.timeouts[
560                                                                               "pre_range_vcsel_period_pclks"])
561        self.timeouts["final_range_vcsel_period_pclks"] = self.get_vcsel_pulse_period(self.vcsel_period_type[1])
562        self.timeouts["final_range_mclks"] = self.decode_timeout(self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI))
563
564        if self.enables["pre_range"]:
565            self.timeouts["final_range_mclks"] -= self.timeouts["pre_range_mclks"]
566        self.timeouts["final_range_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["final_range_mclks"],
567                                                                             self.timeouts[
568                                                                                 "final_range_vcsel_period_pclks"])
569
570    def timeout_Mclks_to_microseconds(self, timeout_period_mclks, vcsel_period_pclks):
571        macro_period_ns = self.calc_macro_period(vcsel_period_pclks)
572        return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000
573
574    def timeout_microseconds_to_Mclks(self, timeout_period_us, vcsel_period_pclks):
575        macro_period_ns = self.calc_macro_period(vcsel_period_pclks)
576        return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns)
577
578    def calc_macro_period(self, vcsel_period_pclks):
579        return (((2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
580
581    def decode_timeout(self, reg_val):
582        return ((reg_val & 0x00FF) << ((reg_val & 0xFF00) >> 8)) + 1
583
584    def encode_timeout(self, timeout_mclks):
585        timeout_mclks = int(timeout_mclks)
586        ls_byte = 0
587        ms_byte = 0
588
589        if timeout_mclks > 0:
590            ls_byte = timeout_mclks - 1
591
592            while (ls_byte & 0xFFFFFF00) > 0:
593                ls_byte >>= 1
594                ms_byte += 1
595            return (ms_byte << 8) or (ls_byte & 0xFF)
596        else:
597            return 0
598
599    def set_measurement_timing_budget(self, budget_us):
600        start_overhead = 1320
601        end_overhead = 960
602        msrc_overhead = 660
603        tcc_overhead = 590
604        dss_overhead = 690
605        pre_range_overhead = 660
606        final_range_overhead = 550
607
608        min_timing_budget = 20000
609
610        if budget_us < min_timing_budget:
611            return False
612        used_budget_us = start_overhead + end_overhead
613
614        self.get_sequence_step_enables()
615        self.get_sequence_step_timeouts()
616
617        if self.enables["tcc"]:
618            used_budget_us += self.timeouts["msrc_dss_tcc_us"] + tcc_overhead
619        if self.enables["dss"]:
620            used_budget_us += 2* self.timeouts["msrc_dss_tcc_us"] + dss_overhead
621        if self.enables["msrc"]:
622            used_budget_us += self.timeouts["msrc_dss_tcc_us"] + msrc_overhead
623        if self.enables["pre_range"]:
624            used_budget_us += self.timeouts["pre_range_us"] + pre_range_overhead
625        if self.enables["final_range"]:
626            used_budget_us += final_range_overhead
627
628            if used_budget_us > budget_us:
629                return False
630            final_range_timeout_us = budget_us - used_budget_us
631            final_range_timeout_mclks = self.timeout_microseconds_to_Mclks(final_range_timeout_us, self.timeouts["final_range_vcsel_period_pclks"])
632
633            if self.enables["pre_range"]:
634                final_range_timeout_mclks += self.timeouts["pre_range_mclks"]
635            self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(final_range_timeout_mclks))
636            self.measurement_timing_budget_us = budget_us
637        return True
638
639    def perform_single_ref_calibration(self, vhv_init_byte):
640        chrono = Timer.Chrono()
641        self._register(SYSRANGE_START, 0x01|vhv_init_byte)
642        chrono.start()
643        while self._register((RESULT_INTERRUPT_STATUS & 0x07) == 0):
644            time_elapsed = chrono.read_ms()
645            if time_elapsed > _IO_TIMEOUT:
646                return False
647        self._register(SYSTEM_INTERRUPT_CLEAR, 0x01)
648        self._register(SYSRANGE_START, 0x00)
649        return True