Basic support for VL53L1X time-of-flight distance sensor (#20)

* Basic support for VL53L1X time-of-flight distance sensor
2026-08-02 22:17:49 +00:00 · 2019-03-06 17:42:17 +01:00
parent 8e1df6d63a
commit f144bfed79
3 changed files with 665 additions and 0 deletions
@@ -0,0 +1,34 @@
+package main
+
+import (
+	"machine"
+
+	"time"
+
+	"github.com/tinygo-org/drivers/vl53v1x"
+)
+
+func main() {
+	machine.I2C0.Configure(machine.I2CConfig{
+		Frequency: 400000,
+	})
+	sensor := vl53l1x.New(machine.I2C0)
+	connected := sensor.Connected()
+	if !connected {
+		println("VL53L1X device not found")
+		return
+	}
+	println("VL53L1X device found")
+	sensor.Configure(true)
+	sensor.SetMeasurementTimingBudget(50000)
+	sensor.StartContinuous(50)
+	for {
+		sensor.Read(true)
+		println("Distance (mm):", sensor.Distance())
+		println("Status:", sensor.Status())
+		println("Peak signal rate (cps):", sensor.SignalRate())
+		println("Ambient rate (cps):", sensor.AmbientRate())
+		println("---")
+		time.Sleep(100 * time.Millisecond)
+	}
+}
@@ -0,0 +1,88 @@
+package vl53l1x
+
+// The I2C address which this device listens to.
+const Address = 0x29 //0x52
+
+// Registers
+const (
+	CHIP_ID                                                 = 0xEACC
+	SOFT_RESET                                              = 0x0000
+	OSC_MEASURED_FAST_OSC_FREQUENCY                         = 0x0006
+	VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND                    = 0x0008
+	VHV_CONFIG_INIT                                         = 0x000B
+	ALGO_PART_TO_PART_RANGE_OFFSET_MM                       = 0x001E
+	MM_CONFIG_OUTER_OFFSET_MM                               = 0x0022
+	DSS_CONFIG_TARGET_TOTAL_RATE_MCPS                       = 0x0024
+	PAD_I2C_HV_EXTSUP_CONFIG                                = 0x002E
+	GPIO_TIO_HV_STATUS                                      = 0x0031
+	SIGMA_ESTIMATOR_EFFECTIVE_PULSE_WIDTH_NS                = 0x0036
+	SIGMA_ESTIMATOR_EFFECTIVE_AMBIENT_WIDTH_NS              = 0x0037
+	ALGO_CROSSTALK_COMPENSATION_VALID_HEIGHT_MM             = 0x0039
+	ALGO_RANGE_MIN_CLIP                                     = 0x003F
+	ALGO_CONSISTENCY_CHECK_TOLERANCE                        = 0x0040
+	CAL_CONFIG_VCSEL_START                                  = 0x0047
+	PHASECAL_CONFIG_TIMEOUT_MACROP                          = 0x004B
+	PHASECAL_CONFIG_OVERRIDE                                = 0x004D
+	DSS_CONFIG_ROI_MODE_CONTROL                             = 0x004F
+	SYSTEM_THRESH_RATE_HIGH                                 = 0x0050
+	SYSTEM_THRESH_RATE_LOW                                  = 0x0052
+	DSS_CONFIG_MANUAL_EFFECTIVE_SPADS_SELECT                = 0x0054
+	DSS_CONFIG_APERTURE_ATTENUATION                         = 0x0057
+	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_SIGMA_THRESH                               = 0x0064
+	RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_MCPS              = 0x0066
+	RANGE_CONFIG_VALID_PHASE_HIGH                           = 0x0069
+	SYSTEM_INTERMEASUREMENT_PERIOD                          = 0x006C
+	SYSTEM_GROUPED_PARAMETER_HOLD_0                         = 0x0071
+	SYSTEM_SEED_CONFIG                                      = 0x0077
+	SD_CONFIG_WOI_SD0                                       = 0x0078
+	SD_CONFIG_WOI_SD1                                       = 0x0079
+	SD_CONFIG_INITIAL_PHASE_SD0                             = 0x007A
+	SD_CONFIG_INITIAL_PHASE_SD1                             = 0x007B
+	SYSTEM_GROUPED_PARAMETER_HOLD_1                         = 0x007C
+	SD_CONFIG_QUANTIFIER                                    = 0x007E
+	SYSTEM_SEQUENCE_CONFIG                                  = 0x0081
+	SYSTEM_GROUPED_PARAMETER_HOLD                           = 0x0082
+	SYSTEM_INTERRUPT_CLEAR                                  = 0x0086
+	SYSTEM_MODE_START                                       = 0x0087
+	RESULT_RANGE_STATUS                                     = 0x0089
+	PHASECAL_RESULT_VCSEL_START                             = 0x00D8
+	RESULT_OSC_CALIBRATE_VAL                                = 0x00DE
+	FIRMWARE_SYSTEM_STATUS                                  = 0x00E5
+	WHO_AM_I                                                = 0x010F
+	SHADOW_RESULT_FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0_HI = 0x0FBE
+
+	TIMING_GUARD = 4528
+	TARGETRATE   = 0x0A00
+)
+
+const (
+	SHORT DistanceMode = iota
+	MEDIUM
+	LONG
+)
+
+const (
+	RangeValid RangeStatus = iota
+	SigmaFail
+	SignalFail
+	RangeValidMinRangeClipped
+	OutOfBoundsFail
+	HardwareFail
+	RangeValidNoWrapCheckFail
+	WrapTargetFail
+	ProcessingFail
+	XtalkSignalFail
+	SynchronizationInt
+	MergedPulse
+	TargetPresentLackOfSignal
+	MinRangeFail
+	RangeInvalid
+
+	None RangeStatus = 255
+)
@@ -0,0 +1,543 @@
+// Package vl53l1x provides a driver for the VL53L1X time-of-flight
+// distance sensor
+//
+// Datasheet:
+// https://www.st.com/resource/en/datasheet/vl53l1x.pdf
+// This driver was based on the library https://github.com/pololu/vl53l1x-arduino
+// and ST's VL53L1X API (STSW-IMG007)
+// https://www.st.com/content/st_com/en/products/embedded-software/proximity-sensors-software/stsw-img007.html
+package vl53l1x
+
+import (
+	"machine"
+	"time"
+)
+
+type DistanceMode uint8
+type RangeStatus uint8
+
+type rangingData struct {
+	mm              uint16
+	status          RangeStatus
+	signalRateMCPS  int32 //MCPS : Mega Count Per Second
+	ambientRateMCPS int32
+}
+
+type resultBuffer struct {
+	status                     uint8
+	streamCount                uint8
+	effectiveSPADCount         uint16
+	ambientRateMCPSSD0         uint16
+	mmCrosstalkSD0             uint16
+	signalRateCrosstalkMCPSSD0 uint16
+}
+
+// Device wraps an I2C connection to a VL53L1X device.
+type Device struct {
+	bus                machine.I2C
+	mode               DistanceMode
+	timeout            uint32
+	fastOscillatorFreq uint16
+	oscillatorOffset   uint16
+	calibrated         bool
+	VHVInit            uint8
+	VHVTimeout         uint8
+	rangingData        rangingData
+	results            resultBuffer
+}
+
+// New creates a new VL53L1X connection. The I2C bus must already be
+// configured.
+//
+// This function only creates the Device object, it does not touch the device.
+func New(bus machine.I2C) Device {
+	return Device{
+		bus:     bus,
+		mode:    LONG,
+		timeout: 500,
+	}
+}
+
+// Connected returns whether a VL53L1X has been found.
+// It does a "who am I" request and checks the response.
+func (d *Device) Connected() bool {
+	return d.readReg16Bit(WHO_AM_I) == CHIP_ID
+}
+
+// Configure sets up the device for communication
+func (d *Device) Configure(use2v8Mode bool) bool {
+	if !d.Connected() {
+		return false
+	}
+	d.writeReg(SOFT_RESET, 0x00)
+	time.Sleep(100 * time.Microsecond)
+	d.writeReg(SOFT_RESET, 0x01)
+	time.Sleep(1 * time.Millisecond)
+
+	start := time.Now()
+	for (d.readReg(FIRMWARE_SYSTEM_STATUS) & 0x01) == 0 {
+		elapsed := time.Since(start)
+		if d.timeout > 0 && uint32(elapsed.Seconds()*1000) > d.timeout {
+			return false
+		}
+	}
+
+	if use2v8Mode {
+		d.writeReg(PAD_I2C_HV_EXTSUP_CONFIG, d.readReg(PAD_I2C_HV_EXTSUP_CONFIG)|0x01)
+	}
+
+	d.fastOscillatorFreq = d.readReg16Bit(OSC_MEASURED_FAST_OSC_FREQUENCY)
+	d.oscillatorOffset = d.readReg16Bit(RESULT_OSC_CALIBRATE_VAL)
+
+	// static config
+	d.writeReg16Bit(DSS_CONFIG_TARGET_TOTAL_RATE_MCPS, TARGETRATE)
+	d.writeReg(GPIO_TIO_HV_STATUS, 0x02)
+	d.writeReg(SIGMA_ESTIMATOR_EFFECTIVE_PULSE_WIDTH_NS, 8)
+	d.writeReg(SIGMA_ESTIMATOR_EFFECTIVE_AMBIENT_WIDTH_NS, 16)
+	d.writeReg(ALGO_CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0xFF)
+	d.writeReg(ALGO_RANGE_MIN_CLIP, 0)
+	d.writeReg(ALGO_CONSISTENCY_CHECK_TOLERANCE, 2)
+
+	// general config
+	d.writeReg16Bit(SYSTEM_THRESH_RATE_HIGH, 0x0000)
+	d.writeReg16Bit(SYSTEM_THRESH_RATE_LOW, 0x0000)
+	d.writeReg(DSS_CONFIG_APERTURE_ATTENUATION, 0x38)
+
+	// timing config
+	d.writeReg16Bit(RANGE_CONFIG_SIGMA_THRESH, 360)
+	d.writeReg16Bit(RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192)
+
+	// dynamic config
+	d.writeReg(SYSTEM_GROUPED_PARAMETER_HOLD_0, 0x01)
+	d.writeReg(SYSTEM_GROUPED_PARAMETER_HOLD_1, 0x01)
+	d.writeReg(SD_CONFIG_QUANTIFIER, 2)
+
+	d.writeReg(SYSTEM_GROUPED_PARAMETER_HOLD, 0x00)
+	d.writeReg(SYSTEM_SEED_CONFIG, 1)
+
+	// Low power auto mode
+	d.writeReg(SYSTEM_SEQUENCE_CONFIG, 0x8B) // VHV, PHASECAL, DSS1, RANGE
+	d.writeReg16Bit(DSS_CONFIG_MANUAL_EFFECTIVE_SPADS_SELECT, 200<<8)
+	d.writeReg(DSS_CONFIG_ROI_MODE_CONTROL, 2) // REQUESTED_EFFFECTIVE_SPADS
+
+	d.SetDistanceMode(d.mode)
+	d.SetMeasurementTimingBudget(50000)
+
+	d.writeReg16Bit(ALGO_PART_TO_PART_RANGE_OFFSET_MM, d.readReg16Bit(MM_CONFIG_OUTER_OFFSET_MM)*4)
+
+	return true
+}
+
+// SetTimeout configures the timeout
+func (d *Device) SetTimeout(timeout uint32) {
+	d.timeout = timeout
+}
+
+// SetDistanceMode sets the mode for calculating the distance.
+// Distance mode vs. max. distance
+// SHORT: 136cm (dark) - 135cm (strong ambient light)
+// MEDIUM: 290cm (dark) - 76cm (strong ambient light)
+// LONG: 360cm (dark) - 73cm (strong ambient light)
+// It returns false if an invalid mode is provided
+func (d *Device) SetDistanceMode(mode DistanceMode) bool {
+	budgetMicroseconds := d.GetMeasurementTimingBudget()
+	switch mode {
+	case SHORT:
+		// timing config
+		d.writeReg(RANGE_CONFIG_VCSEL_PERIOD_A, 0x07)
+		d.writeReg(RANGE_CONFIG_VCSEL_PERIOD_B, 0x05)
+		d.writeReg(RANGE_CONFIG_VALID_PHASE_HIGH, 0x38)
+
+		// dynamic config
+		d.writeReg(SD_CONFIG_WOI_SD0, 0x07)
+		d.writeReg(SD_CONFIG_WOI_SD1, 0x05)
+		d.writeReg(SD_CONFIG_INITIAL_PHASE_SD0, 6)
+		d.writeReg(SD_CONFIG_INITIAL_PHASE_SD1, 6)
+		break
+	case MEDIUM:
+		// timing config
+		d.writeReg(RANGE_CONFIG_VCSEL_PERIOD_A, 0x0B)
+		d.writeReg(RANGE_CONFIG_VCSEL_PERIOD_B, 0x09)
+		d.writeReg(RANGE_CONFIG_VALID_PHASE_HIGH, 0x78)
+
+		// dynamic config
+		d.writeReg(SD_CONFIG_WOI_SD0, 0x0B)
+		d.writeReg(SD_CONFIG_WOI_SD1, 0x09)
+		d.writeReg(SD_CONFIG_INITIAL_PHASE_SD0, 10)
+		d.writeReg(SD_CONFIG_INITIAL_PHASE_SD1, 10)
+		break
+	case LONG:
+		// timing config
+		d.writeReg(RANGE_CONFIG_VCSEL_PERIOD_A, 0x0F)
+		d.writeReg(RANGE_CONFIG_VCSEL_PERIOD_B, 0x0D)
+		d.writeReg(RANGE_CONFIG_VALID_PHASE_HIGH, 0xB8)
+
+		// dynamic config
+		d.writeReg(SD_CONFIG_WOI_SD0, 0x0F)
+		d.writeReg(SD_CONFIG_WOI_SD1, 0x0D)
+		d.writeReg(SD_CONFIG_INITIAL_PHASE_SD0, 14)
+		d.writeReg(SD_CONFIG_INITIAL_PHASE_SD1, 14)
+		break
+	default:
+		return false
+	}
+
+	d.SetMeasurementTimingBudget(budgetMicroseconds)
+	d.mode = mode
+	return true
+}
+
+// GetMeasurementTimingBudget returns the timing budget in microseconds
+func (d *Device) GetMeasurementTimingBudget() uint32 {
+	macroPeriod := d.calculateMacroPeriod(uint32(d.readReg(RANGE_CONFIG_VCSEL_PERIOD_A)))
+	rangeConfigTimeout := timeoutMclksToMicroseconds(decodeTimeout(d.readReg16Bit(RANGE_CONFIG_TIMEOUT_MACROP_A)), macroPeriod)
+	return 2 * uint32(rangeConfigTimeout) * TIMING_GUARD
+}
+
+// SetMeasurementTimingBudget configures the timing budget in microseconds
+// It returns false if an invalid timing budget is provided
+func (d *Device) SetMeasurementTimingBudget(budgetMicroseconds uint32) bool {
+	if budgetMicroseconds <= TIMING_GUARD {
+		return false
+	}
+	budgetMicroseconds -= TIMING_GUARD
+	if budgetMicroseconds > 1100000 {
+		return false
+	}
+	rangeConfigTimeout := budgetMicroseconds / 2
+	// Update Macro Period for Range A VCSEL Period
+	macroPeriod := d.calculateMacroPeriod(uint32(d.readReg(RANGE_CONFIG_VCSEL_PERIOD_A)))
+
+	// Update Phase timeout - uses Timing A
+	phasecalTimeoutMclks := timeoutMicrosecondsToMclks(1000, macroPeriod)
+	if phasecalTimeoutMclks > 0xFF {
+		phasecalTimeoutMclks = 0xFF
+	}
+	d.writeReg(PHASECAL_CONFIG_TIMEOUT_MACROP, uint8(phasecalTimeoutMclks))
+
+	// Update MM Timing A timeout
+	d.writeReg16Bit(MM_CONFIG_TIMEOUT_MACROP_A, encodeTimeout(timeoutMicrosecondsToMclks(1, macroPeriod)))
+	// Update Range Timing A timeout
+	d.writeReg16Bit(RANGE_CONFIG_TIMEOUT_MACROP_A, encodeTimeout(timeoutMicrosecondsToMclks(rangeConfigTimeout, macroPeriod)))
+
+	macroPeriod = d.calculateMacroPeriod(uint32(d.readReg(RANGE_CONFIG_VCSEL_PERIOD_B)))
+	// Update MM Timing B timeout
+	d.writeReg16Bit(MM_CONFIG_TIMEOUT_MACROP_B, encodeTimeout(timeoutMicrosecondsToMclks(1, macroPeriod)))
+	// Update Range Timing B timeout
+	d.writeReg16Bit(RANGE_CONFIG_TIMEOUT_MACROP_B, encodeTimeout(timeoutMicrosecondsToMclks(rangeConfigTimeout, macroPeriod)))
+
+	return true
+}
+
+// Read stores in the buffer the values of the sensor and returns
+// the current distance in mm
+func (d *Device) Read(blocking bool) uint16 {
+	if blocking {
+		start := time.Now()
+
+		for !d.dataReady() {
+			elapsed := time.Since(start)
+			if d.timeout > 0 && uint32(elapsed.Seconds()*1000) > d.timeout {
+				d.rangingData.status = None
+				d.rangingData.mm = 0
+				d.rangingData.signalRateMCPS = 0
+				d.rangingData.ambientRateMCPS = 0
+				return d.rangingData.mm
+			}
+		}
+	}
+	d.readResults()
+
+	if !d.calibrated {
+		d.setupManualCalibration()
+		d.calibrated = true
+	}
+
+	d.updateDSS()
+	d.getRangingData()
+	d.writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01) //sys_interrupt_clear_range
+
+	return d.rangingData.mm
+}
+
+// updateDSS updates the DSS
+func (d *Device) updateDSS() {
+	spadCount := d.results.effectiveSPADCount
+	if spadCount != 0 {
+		totalRatePerSpad := uint32(d.results.signalRateCrosstalkMCPSSD0) + uint32(d.results.ambientRateMCPSSD0)
+		if totalRatePerSpad > 0xFFFF {
+			totalRatePerSpad = 0xFFFF
+		}
+		totalRatePerSpad <<= 16
+		totalRatePerSpad /= uint32(spadCount)
+		if totalRatePerSpad != 0 {
+			requireSpads := (uint32(TARGETRATE) << 16) / totalRatePerSpad
+			if requireSpads > 0xFFFF {
+				requireSpads = 0xFFFF
+			}
+			d.writeReg16Bit(DSS_CONFIG_MANUAL_EFFECTIVE_SPADS_SELECT, uint16(requireSpads))
+			return
+		}
+	}
+	d.writeReg16Bit(DSS_CONFIG_MANUAL_EFFECTIVE_SPADS_SELECT, 0x8000)
+}
+
+// readResults read the register and stores the data in the results buffer
+func (d *Device) readResults() {
+	data := make([]byte, 17)
+	msb := byte((RESULT_RANGE_STATUS >> 8) & 0xFF)
+	lsb := byte(RESULT_RANGE_STATUS & 0xFF)
+	d.bus.Tx(Address, []byte{msb, lsb}, data)
+	d.results.status = data[0]
+	// data[1] report_status : not used
+	d.results.streamCount = data[2]
+	d.results.effectiveSPADCount = readUint(data[3], data[4])
+	// data[5] , data[6] peak signal count rate mcps sd0 : not used
+	d.results.ambientRateMCPSSD0 = readUint(data[7], data[8])
+	// data[9] , data[10] sigma_sd0 : not used
+	// data[11] , data[12] phase_sd0 : not used
+	d.results.mmCrosstalkSD0 = readUint(data[13], data[14])
+	d.results.signalRateCrosstalkMCPSSD0 = readUint(data[15], data[16])
+}
+
+// dataReady returns true when the data is ready to be read
+func (d *Device) dataReady() bool {
+	return (d.readReg(GPIO_TIO_HV_STATUS) & 0x01) == 0
+}
+
+// Distance returns the distance in mm
+func (d *Device) Distance() int32 {
+	return int32(d.rangingData.mm)
+}
+
+// Status returns the status of the sensor
+func (d *Device) Status() RangeStatus {
+	return d.rangingData.status
+}
+
+// SignalRate returns the peak signal rate in count per second (cps)
+func (d *Device) SignalRate() int32 {
+	return d.rangingData.signalRateMCPS
+}
+
+// AmbientRate returns the ambient rate in count per second (cps)
+func (d *Device) AmbientRate() int32 {
+	return d.rangingData.ambientRateMCPS
+}
+
+// getRangingData stores in the buffer the ranging data
+func (d *Device) getRangingData() {
+	d.rangingData.mm = uint16((uint32(d.results.mmCrosstalkSD0)*2011 + 0x0400) / 0x0800)
+	switch d.results.status {
+	case 17: // MULTCLIPFAIL
+	case 2: // VCSELWATCHDOGTESTFAILURE
+	case 1: // VCSELCONTINUITYTESTFAILURE
+	case 3: // NOVHVVALUEFOUND
+		d.rangingData.status = HardwareFail
+		break
+
+	case 13: // USERROICLIP
+		d.rangingData.status = MinRangeFail
+		break
+
+	case 18: // GPHSTREAMCOUNT0READY
+		d.rangingData.status = SynchronizationInt
+		break
+
+	case 5: // RANGEPHASECHECK
+		d.rangingData.status = OutOfBoundsFail
+		break
+
+	case 4: // MSRCNOTARGET
+		d.rangingData.status = SignalFail
+		break
+
+	case 6: // SIGMATHRESHOLDCHECK
+		d.rangingData.status = SignalFail
+		break
+
+	case 7: // PHASECONSISTENCY
+		d.rangingData.status = WrapTargetFail
+		break
+
+	case 12: // RANGEIGNORETHRESHOLD
+		d.rangingData.status = XtalkSignalFail
+		break
+
+	case 8: // MINCLIP
+		d.rangingData.status = RangeValidMinRangeClipped
+		break
+
+	case 9: // RANGECOMPLETE
+		if d.results.streamCount == 0 {
+			d.rangingData.status = RangeValidNoWrapCheckFail
+		} else {
+			d.rangingData.status = RangeValid
+		}
+		break
+
+	default:
+		d.rangingData.status = None
+	}
+
+	d.rangingData.signalRateMCPS = 1000000 * int32(d.results.signalRateCrosstalkMCPSSD0) / (1 << 7)
+	d.rangingData.ambientRateMCPS = 1000000 * int32(d.results.ambientRateMCPSSD0) / (1 << 7)
+}
+
+// setupManualCalibration configures the manual calibration
+func (d *Device) setupManualCalibration() {
+	// save original VHV configs
+	d.VHVInit = d.readReg(VHV_CONFIG_INIT)
+	d.VHVTimeout = d.readReg(VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND)
+
+	// disable VHV init
+	d.writeReg(VHV_CONFIG_INIT, d.VHVInit&0x7F)
+
+	// set loop bound to tuning param
+	d.writeReg(VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND, (d.VHVTimeout&0x03)+(3<<2))
+
+	// override phasecal
+	d.writeReg(PHASECAL_CONFIG_OVERRIDE, 0x01)
+	d.writeReg(CAL_CONFIG_VCSEL_START, d.readReg(PHASECAL_RESULT_VCSEL_START))
+}
+
+// StartContinuous starts the continuous sensing mode
+func (d *Device) StartContinuous(periodMs uint32) {
+	d.writeReg32Bit(SYSTEM_INTERMEASUREMENT_PERIOD, periodMs*uint32(d.oscillatorOffset))
+	d.writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01) // sys_interrupt_clear_range
+	d.writeReg(SYSTEM_MODE_START, 0x40)      // mode_range_timed
+}
+
+// StopContinuous stops the continuous sensing mode
+func (d *Device) StopContinuous() {
+	d.writeReg(SYSTEM_MODE_START, 0x80) // mode_range_abort
+
+	d.calibrated = false
+
+	// restore vhv configs
+	if d.VHVInit != 0 {
+		d.writeReg(VHV_CONFIG_INIT, d.VHVInit)
+	}
+	if d.VHVTimeout != 0 {
+		d.writeReg(VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND, d.VHVTimeout)
+	}
+
+	// remove phasecal override
+	d.writeReg(PHASECAL_CONFIG_OVERRIDE, 0x00)
+}
+
+// writeReg sends a single byte to the specified register address
+func (d *Device) writeReg(reg uint16, value uint8) {
+	msb := byte((reg >> 8) & 0xFF)
+	lsb := byte(reg & 0xFF)
+	d.bus.Tx(Address, []byte{msb, lsb, value}, nil)
+}
+
+// writeReg16Bit sends two bytes to the specified register address
+func (d *Device) writeReg16Bit(reg uint16, value uint16) {
+	data := make([]byte, 4)
+	data[0] = byte((reg >> 8) & 0xFF)
+	data[1] = byte(reg & 0xFF)
+	data[2] = byte((value >> 8) & 0xFF)
+	data[3] = byte(value & 0xFF)
+	d.bus.Tx(Address, data, nil)
+}
+
+// writeReg32Bit sends four bytes to the specified register address
+func (d *Device) writeReg32Bit(reg uint16, value uint32) {
+	data := make([]byte, 6)
+	data[0] = byte((reg >> 8) & 0xFF)
+	data[1] = byte(reg & 0xFF)
+	data[2] = byte((value >> 24) & 0xFF)
+	data[3] = byte((value >> 16) & 0xFF)
+	data[4] = byte((value >> 8) & 0xFF)
+	data[5] = byte(value & 0xFF)
+	d.bus.Tx(Address, data, nil)
+}
+
+// readReg reads a single byte from the specified address
+func (d *Device) readReg(reg uint16) uint8 {
+	data := []byte{0}
+	msb := byte((reg >> 8) & 0xFF)
+	lsb := byte(reg & 0xFF)
+	d.bus.Tx(Address, []byte{msb, lsb}, data)
+	return data[0]
+}
+
+// readReg16Bit reads two bytes from the specified address
+// and returns it as a uint16
+func (d *Device) readReg16Bit(reg uint16) uint16 {
+	data := []byte{0, 0}
+	msb := byte((reg >> 8) & 0xFF)
+	lsb := byte(reg & 0xFF)
+	d.bus.Tx(Address, []byte{msb, lsb}, data)
+	return readUint(data[0], data[1])
+}
+
+// readReg32Bit reads four bytes from the specified address
+// and returns it as a uint32
+func (d *Device) readReg32Bit(reg uint16) uint32 {
+	data := make([]byte, 4)
+	msb := byte((reg >> 8) & 0xFF)
+	lsb := byte(reg & 0xFF)
+	d.bus.Tx(Address, []byte{msb, lsb}, data)
+	return readUint32(data)
+}
+
+// readUint converts two bytes to uint16
+func readUint(msb byte, lsb byte) uint16 {
+	return (uint16(msb) << 8) | uint16(lsb)
+}
+
+// readUint converts four bytes to uint32
+func readUint32(data []byte) uint32 {
+	if len(data) != 4 {
+		return 0
+	}
+	var value uint32
+	value = uint32(data[0]) << 24
+	value |= uint32(data[1]) << 16
+	value |= uint32(data[2]) << 8
+	value |= uint32(data[3])
+	return value
+}
+
+// encodeTimeout encodes the timeout in the correct format: (LSByte * 2^MSByte) + 1
+func encodeTimeout(timeoutMclks uint32) uint16 {
+	if timeoutMclks == 0 {
+		return 0
+	}
+	msb := 0
+	lsb := timeoutMclks - 1
+	for (lsb & 0xFFFFFF00) > 0 {
+		lsb >>= 1
+		msb++
+	}
+	return uint16(msb<<8) | uint16(lsb&0xFF)
+}
+
+// decodeTimeout decodes the timeout from the format: (LSByte * 2^MSByte) + 1
+func decodeTimeout(regVal uint16) uint32 {
+	return (uint32(regVal&0xFF) << (regVal >> 8)) + 1
+}
+
+// timeoutMclksToMicroseconds transform from mclks to microseconds
+func timeoutMclksToMicroseconds(timeoutMclks uint32, macroPeriodMicroseconds uint32) uint32 {
+	return uint32((uint64(timeoutMclks)*uint64(macroPeriodMicroseconds) + 0x800) >> 12)
+}
+
+// timeoutMicrosecondsToMclks transform from microseconds to mclks
+func timeoutMicrosecondsToMclks(timeoutMicroseconds uint32, macroPeriodMicroseconds uint32) uint32 {
+	return ((timeoutMicroseconds << 12) + (macroPeriodMicroseconds >> 1)) / macroPeriodMicroseconds
+}
+
+// calculateMacroPerios calculates the macro period in microsendos from the vcsel period
+func (d *Device) calculateMacroPeriod(vcselPeriod uint32) uint32 {
+	pplPeriodMicroseconds := (uint32(1) << 30) / uint32(d.fastOscillatorFreq)
+	vcselPeriodPclks := (vcselPeriod + 1) << 1
+	macroPeriodMicroseconds := 2304 * pplPeriodMicroseconds
+	macroPeriodMicroseconds >>= 6
+	macroPeriodMicroseconds *= vcselPeriodPclks
+	macroPeriodMicroseconds >>= 6
+	return macroPeriodMicroseconds
+}