/** @file * @defgroup VL53L1X.c * @{ * */ #include #include #include // add #include // add #include #include #include "VL53L1X.h" // Da decidere uint8_t address = AddressDefault ; uint16_t range_mm = 0 ; uint16_t final_crosstalk_corrected_range_mm_sd0 = 0 ; bool calibrated ; uint16_t fast_osc_frequency ; uint16_t osc_calibrate_val ; // Results uint8_t range_status ; // uint8_t report_status: not used uint8_t stream_count ; uint16_t dss_actual_effective_spads_sd0 ; // uint16_t peak_signal_count_rate_mcps_sd0: not used uint16_t ambient_count_rate_mcps_sd0 ; // uint16_t sigma_sd0: not used // uint16_t phase_sd0: not used uint16_t final_crosstalk_corrected_range_mm_sd0 ; uint16_t peak_signal_count_rate_crosstalk_corrected_mcps_sd0 ; // value used in measurement timing budget calculations // assumes PresetMode is LOWPOWER_AUTONOMOUS // // vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US + LOWPOWERAUTO_VHV_LOOP_BOUND // (tuning parm default) * LOWPOWER_AUTO_VHV_LOOP_DURATION_US // = 245 + 3 * 245 = 980 // TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING + // LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING + vhv // = 1448 + 2100 + 980 = 4528 static const uint32_t TimingGuard = 4528 ; // value in DSS_CONFIG__TARGET_TOTAL_RATE_MCPS register, used in DSS // calculations static const uint16_t TargetRate = 0x0A00 ; // check if sensor has new reading available // assumes interrupt is active low (GPIO_HV_MUX__CTRL bit 4 is 1) bool dataReady() { return (readReg(GPIO__TIO_HV_STATUS) & 0x01) == 0; } //uint16_t read(bool blocking = true); //uint16_t readRangeContinuousMillimeters(bool blocking = true) { return read(blocking); } // alias of read() /* EXTERN */ extern const nrf_drv_twi_t m_twi ; int main ( void ) { ... nrf_delay_ms ( 1000 ) ; VL53L1X_init ( true , ADDRESS_VL1 ) ; while ( 1 ) { readDistance ( true , ADDRESS_VL1 ) ; } } // Initialize sensor using settings taken mostly from VL53L1_DataInit() and // VL53L1_StaticInit(). // If io_2v8 (optional) is true or not given, the sensor is configured for 2V8 // mode. bool VL53L1X_init ( bool io_2v8 , uint8_t tmp_Address ) { nrf_delay_ms ( 100 ) ; // check model ID and module type registers (values specified in datasheet) if (readReg16Bit(IDENTIFICATION__MODEL_ID) != 0xEACC) return false; // VL53L1_software_reset() begin writeReg(SOFT_RESET, 0x00); nrf_delay_ms ( 1 ) ; writeReg(SOFT_RESET, 0x01); // give it some time to boot; otherwise the sensor NACKs during the readReg() // call below and the Arduino 101 doesn't seem to handle that well nrf_delay_ms ( 1 ) ; // VL53L1_poll_for_boot_completion() begin // startTimeout(); // check last_status in case we still get a NACK to try to deal with it correctly if ( (readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0 ) return false; // VL53L1_poll_for_boot_completion() end // VL53L1_software_reset() end // VL53L1_DataInit() begin // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary if (io_2v8) { writeReg(PAD_I2C_HV__EXTSUP_CONFIG, readReg(PAD_I2C_HV__EXTSUP_CONFIG) | 0x01); } // store oscillator info for later use fast_osc_frequency = readReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY); osc_calibrate_val = readReg16Bit(RESULT__OSC_CALIBRATE_VAL); // VL53L1_DataInit() end // VL53L1_StaticInit() begin // Note that the API does not actually apply the configuration settings below // when VL53L1_StaticInit() is called: it keeps a copy of the sensor's // register contents in memory and doesn't actually write them until a // measurement is started. Writing the configuration here means we don't have // to keep it all in memory and avoids a lot of redundant writes later. // the API sets the preset mode to LOWPOWER_AUTONOMOUS here: // VL53L1_set_preset_mode() begin // VL53L1_preset_mode_standard_ranging() begin // values labeled "tuning parm default" are from vl53l1_tuning_parm_defaults.h // (API uses these in VL53L1_init_tuning_parm_storage_struct()) // static config // API resets PAD_I2C_HV__EXTSUP_CONFIG here, but maybe we don't want to do // that? (seems like it would disable 2V8 mode) writeReg16Bit(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate); // should already be this value after reset writeReg(GPIO__TIO_HV_STATUS, 0x02); writeReg(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8); // tuning parm default writeReg(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16); // tuning parm default writeReg(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01); writeReg(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF); writeReg(ALGO__RANGE_MIN_CLIP, 0); // tuning parm default writeReg(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2); // tuning parm default // general config writeReg16Bit(SYSTEM__THRESH_RATE_HIGH, 0x0000); writeReg16Bit(SYSTEM__THRESH_RATE_LOW, 0x0000); writeReg(DSS_CONFIG__APERTURE_ATTENUATION, 0x38); // timing config // most of these settings will be determined later by distance and timing // budget configuration writeReg16Bit(RANGE_CONFIG__SIGMA_THRESH, 360); // tuning parm default writeReg16Bit(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192); // tuning parm default // dynamic config writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01); writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01); writeReg(SD_CONFIG__QUANTIFIER, 2); // tuning parm default // VL53L1_preset_mode_standard_ranging() end // from VL53L1_preset_mode_timed_ranging_* // GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1, // and things don't seem to work if we don't set GPH back to 0 (which the API // does here). writeReg(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00); writeReg(SYSTEM__SEED_CONFIG, 1); // tuning parm default // from VL53L1_config_low_power_auto_mode writeReg(SYSTEM__SEQUENCE_CONFIG, 0x8B); // VHV, PHASECAL, DSS1, RANGE writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8); writeReg(DSS_CONFIG__ROI_MODE_CONTROL, 2); // REQUESTED_EFFFECTIVE_SPADS // VL53L1_set_preset_mode() end // default to long range, 50 ms timing budget // note that this is different than what the API defaults to setDistanceMode(Long); setMeasurementTimingBudget(50000); // VL53L1_StaticInit() end // the API triggers this change in VL53L1_init_and_start_range() once a // measurement is started; assumes MM1 and MM2 are disabled writeReg16Bit(ALGO__PART_TO_PART_RANGE_OFFSET_MM, readReg16Bit(MM_CONFIG__OUTER_OFFSET_MM) * 4); return true; } // set distance mode to Short, Medium, or Long // based on VL53L1_SetDistanceMode() bool setDistanceMode ( uint8_t mode ) { // save existing timing budget // uint32_t budget_us = getMeasurementTimingBudget( ) ; switch ( mode ) { case Short : { // from VL53L1_preset_mode_standard_ranging_short_range() // timing config writeReg ( RANGE_CONFIG__VCSEL_PERIOD_A , 0x07 ) ; writeReg ( RANGE_CONFIG__VCSEL_PERIOD_B , 0x05 ) ; writeReg ( RANGE_CONFIG__VALID_PHASE_HIGH , 0x38 ) ; // dynamic config writeReg ( SD_CONFIG__WOI_SD0 , 0x07 ) ; writeReg ( SD_CONFIG__WOI_SD1 , 0x05 ) ; writeReg ( SD_CONFIG__INITIAL_PHASE_SD0 , 6 ) ; // tuning parm default writeReg ( SD_CONFIG__INITIAL_PHASE_SD1 , 6 ) ; // tuning parm default break ; } case Medium : { // from VL53L1_preset_mode_standard_ranging() // timing config writeReg ( RANGE_CONFIG__VCSEL_PERIOD_A , 0x0B ) ; writeReg ( RANGE_CONFIG__VCSEL_PERIOD_B , 0x09 ) ; writeReg ( RANGE_CONFIG__VALID_PHASE_HIGH , 0x78 ) ; // dynamic config writeReg ( SD_CONFIG__WOI_SD0 , 0x0B ) ; writeReg ( SD_CONFIG__WOI_SD1 , 0x09 ) ; writeReg ( SD_CONFIG__INITIAL_PHASE_SD0 , 10 ) ; // tuning parm default writeReg ( SD_CONFIG__INITIAL_PHASE_SD1 , 10 ) ; // tuning parm default break ; } case Long : // long { // from VL53L1_preset_mode_standard_ranging_long_range ( ) // timing config writeReg ( RANGE_CONFIG__VCSEL_PERIOD_A , 0x0F ) ; writeReg ( RANGE_CONFIG__VCSEL_PERIOD_B , 0x0D ) ; writeReg ( RANGE_CONFIG__VALID_PHASE_HIGH , 0xB8 ) ; // dynamic config writeReg ( SD_CONFIG__WOI_SD0 , 0x0F ) ; writeReg ( SD_CONFIG__WOI_SD1 , 0x0D ) ; writeReg ( SD_CONFIG__INITIAL_PHASE_SD0 , 14 ) ; // tuning parm default writeReg ( SD_CONFIG__INITIAL_PHASE_SD1 , 14 ) ; // tuning parm default break ; } default : // unrecognized mode - do nothing return false ; } // reapply timing budget // setMeasurementTimingBudget ( budget_us ) ; // save mode so it can be returned by getDistanceMode() // distance_mode = mode ; return true ; } // Start continuous ranging measurements, with the given inter-measurement // period in milliseconds determining how often the sensor takes a measurement. void startContinuous ( uint32_t period_ms ) { uint16_t osc_calibrate_val = readReg16Bit(RESULT__OSC_CALIBRATE_VAL); // from VL53L1_set_inter_measurement_period_ms() writeReg32Bit ( SYSTEM__INTERMEASUREMENT_PERIOD , period_ms * osc_calibrate_val ) ; writeReg ( SYSTEM__INTERRUPT_CLEAR , 0x01 ) ; // sys_interrupt_clear_range writeReg ( SYSTEM__MODE_START , 0x40 ); // mode_range__timed } // Returns a range reading in millimeters when continuous mode is active // (readRangeSingleMillimeters ( ) also calls this function after starting a // single-shot range measurement) uint16_t readDistance ( bool blocking , uint8_t tmp_Address ) { // Utilizzare l'indirizzo del sensore che vado a leggere address = tmp_Address ; // if ( blocking ) // { // // startTimeout ( ) ; // while ( !dataReady ( ) ) // { // // if (checkTimeoutExpired ( ) ) // // { // // did_timeout = true ; // range_status = None ; // range_mm = 0 ; // // peak_signal_count_rate_MCPS = 0 ; // // ambient_count_rate_MCPS = 0 ; // // return range_mm ; // // // } // // } // } while ( !dataReady ( ) ) return 0 ; readResults ( ) ; if ( !calibrated ) { setupManualCalibration ( ) ; calibrated = true ; } updateDSS ( ) ; getRangingData ( ) ; writeReg ( SYSTEM__INTERRUPT_CLEAR, 0x01 ) ; // sys_interrupt_clear_range return range_mm ; } // read measurement results into buffer void readResults ( void ) { uint8_t buff_reg [ 2 ] = "" ; buff_reg [ 0 ] = ( RESULT__RANGE_STATUS >> 8 ) & 0xFF ; // reg high byte buff_reg [ 1 ] = RESULT__RANGE_STATUS & 0xFF ; // reg low byte nrf_drv_twi_tx ( &m_twi , address , buff_reg , 2 , false ) ; uint8_t buff_resp [ 20 ] = "" ; nrf_drv_twi_rx ( &m_twi , address , buff_resp , 17 ) ; // Wire.requestFrom(address, (uint8_t)17); if ( ( check_resp_buffer_isEmpty ( buff_resp , 17 ) ) == false ) { range_status = buff_resp [ 0 ] ; // buff_resp [ 1 ] // report_status: not used stream_count = buff_resp [ 2 ] ; dss_actual_effective_spads_sd0 = ( uint16_t ) buff_resp [ 3 ] << 8 ; // high byte dss_actual_effective_spads_sd0 |= buff_resp [ 4 ] ; // low byte // buff_resp [ 5 ] ; // peak_signal_count_rate_mcps_sd0: not used // buff_resp [ 6 ] ; ambient_count_rate_mcps_sd0 = ( uint16_t ) buff_resp [ 7 ] << 8 ; // high byte ambient_count_rate_mcps_sd0 |= buff_resp [ 8 ] ; // low byte // buff_resp [ 9 ] ; // sigma_sd0: not used // buff_resp [ 10 ] ; // buff_resp [ 11 ] ; // phase_sd0: not used // buff_resp [ 12 ] ; final_crosstalk_corrected_range_mm_sd0 = ( uint16_t ) buff_resp [ 13 ] << 8 ; // high byte final_crosstalk_corrected_range_mm_sd0 |= buff_resp [ 14 ] ; // low byte if ( final_crosstalk_corrected_range_mm_sd0 != 0 ) { uint8_t test ; test = 0 ; } peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = (uint16_t) buff_resp [ 15 ] << 8 ; // high byte peak_signal_count_rate_crosstalk_corrected_mcps_sd0 |= buff_resp [ 16 ] ; // low byte } } // perform Dynamic SPAD Selection calculation/update // based on VL53L1_low_power_auto_update_DSS() void updateDSS ( void ) { uint16_t spadCount = dss_actual_effective_spads_sd0; if (spadCount != 0) { // "Calc total rate per spad" uint32_t totalRatePerSpad = (uint32_t)peak_signal_count_rate_crosstalk_corrected_mcps_sd0 + ambient_count_rate_mcps_sd0; // "clip to 16 bits" if (totalRatePerSpad > 0xFFFF) { totalRatePerSpad = 0xFFFF; } // "shift up to take advantage of 32 bits" totalRatePerSpad <<= 16; totalRatePerSpad /= spadCount; if (totalRatePerSpad != 0) { // "get the target rate and shift up by 16" uint32_t requiredSpads = ((uint32_t)TargetRate << 16) / totalRatePerSpad; // "clip to 16 bit" if (requiredSpads > 0xFFFF) { requiredSpads = 0xFFFF; } // "override DSS config" writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, requiredSpads); // DSS_CONFIG__ROI_MODE_CONTROL should already be set to REQUESTED_EFFFECTIVE_SPADS return; } } // If we reached this point, it means something above would have resulted in a // divide by zero. // "We want to gracefully set a spad target, not just exit with an error" // "set target to mid point" writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 0x8000); } // "Setup ranges after the first one in low power auto mode by turning off // FW calibration steps and programming static values" // based on VL53L1_low_power_auto_setup_manual_calibration() void setupManualCalibration ( void ) { uint8_t saved_vhv_init ; uint8_t saved_vhv_timeout ; // "save original vhv configs" saved_vhv_init = readReg ( VHV_CONFIG__INIT ) ; saved_vhv_timeout = readReg ( VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND ) ; // "disable VHV init" writeReg ( VHV_CONFIG__INIT, saved_vhv_init & 0x7F ) ; // "set loop bound to tuning param" writeReg ( VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND , ( saved_vhv_timeout & 0x03 ) + ( 3 << 2 ) ) ; // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT) // "override phasecal" writeReg ( PHASECAL_CONFIG__OVERRIDE , 0x01 ) ; writeReg ( CAL_CONFIG__VCSEL_START , readReg ( PHASECAL_RESULT__VCSEL_START ) ) ; } // get range, status, rates from results buffer // based on VL53L1_GetRangingMeasurementData ( ) void getRangingData ( void ) { // VL53L1_copy_sys_and_core_results_to_range_results ( ) begin uint16_t range = final_crosstalk_corrected_range_mm_sd0 ; // "apply correction gain" // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT) // Basically, this appears to scale the result by 2011/2048, or about 98% // (with the 1024 added for proper rounding). range_mm = ( ( uint32_t ) range * 2011 + 0x0400 ) / 0x0800 ; // VL53L1_copy_sys_and_core_results_to_range_results ( ) end // set range_status in ranging_data based on value of RESULT__RANGE_STATUS register // mostly based on ConvertStatusLite ( ) //// switch ( results.range_status ) //// { // case 17 : // MULTCLIPFAIL // case 2 : // VCSELWATCHDOGTESTFAILURE // case 1 : // VCSELCONTINUITYTESTFAILURE // case 3 : // NOVHVVALUEFOUND // { // // from SetSimpleData ( ) // // ranging_data.range_status = HardwareFail ; // // break ; // // } // // case 13 : // USERROICLIP // { // // from SetSimpleData ( ) // // ranging_data.range_status = MinRangeFail ; // // break ; // // } // // case 18 : // GPHSTREAMCOUNT0READY // { // // ranging_data.range_status = SynchronizationInt ; // // break ; // // } // // case 5 : // RANGEPHASECHECK // { // // ranging_data.range_status = OutOfBoundsFail ; // // break ; // // } // case 4 : // MSRCNOTARGET // { // // ranging_data.range_status = SignalFail ; // // break ; // // } // case 6 : // SIGMATHRESHOLDCHECK // { // // ranging_data.range_status = SignalFail ; // // break ; // // } // // case 7 : // PHASECONSISTENCY // { // // ranging_data.range_status = WrapTargetFail ; // // break ; // // } // // case 12 : // RANGEIGNORETHRESHOLD // { // // ranging_data.range_status = XtalkSignalFail ; // // break ; // // } // // case 8 : // MINCLIP // { // // ranging_data.range_status = RangeValidMinRangeClipped ; // // break ; // // } // // case 9 : // RANGECOMPLETE // { // // from VL53L1_copy_sys_and_core_results_to_range_results ( ) //// if ( results.stream_count == 0 ) //// ranging_data.range_status = RangeValidNoWrapCheckFail ; //// //// else //// ranging_data.range_status = RangeValid ; // // break ; // // } // default : // ranging_data.range_status = None ; // } // from SetSimpleData ( ) // ranging_data.peak_signal_count_rate_MCPS = countRateFixedToFloat ( results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 ) ; // ranging_data.ambient_count_rate_MCPS = countRateFixedToFloat ( results.ambient_count_rate_mcps_sd0 ) ; } // Setto il nuovo indirizzo void setAddress ( uint8_t new_addr ) { writeReg ( I2C_SLAVE__DEVICE_ADDRESS , new_addr & 0x7F ) ; address = new_addr ; } //**************************************************** // Verify if the buffer is empty. bool check_resp_buffer_isEmpty ( uint8_t * tmp_gen_buff , uint8_t tmp_len ) { uint8_t index_gen_buff = 0 ; for ( index_gen_buff = 0 ; index_gen_buff < tmp_len ; index_gen_buff ++ ) { if ( tmp_gen_buff [ index_gen_buff ] != 0 ) return false ; } return true ; } // ********************** REGISTER FUNCTIONS ********************** // Read a 16-bit register uint16_t readReg16Bit ( uint16_t reg ) { uint16_t value ; uint8_t buff_reg [ 2 ] = "" ; buff_reg [ 0 ] = ( reg >> 8 ) & 0xFF ; buff_reg [ 1 ] = reg & 0xFF ; nrf_drv_twi_tx ( &m_twi , address , buff_reg , 2 , false ) ; // Leggo la risposta uint8_t buff_res [ 2 ] = "" ; nrf_drv_twi_rx ( &m_twi , address , buff_res , 2 ) ; value = ( uint16_t ) buff_res [ 0 ] << 8 ; // value high byte value |= buff_res [ 1 ] ; // value low byte return value ; } // Write an 8-bit register void writeReg ( uint16_t reg , uint8_t value ) { uint8_t buff_reg [ 2 ] = "" ; buff_reg [ 0 ] = ( reg >> 8 ) & 0xFF ; // reg high byte buff_reg [ 1 ] = reg & 0xFF ; // reg low byte nrf_drv_twi_tx ( &m_twi , address , buff_reg , 2 , false ) ; } // Read an 8-bit register uint8_t readReg ( uint16_t reg ) { uint8_t value ; uint8_t buff_reg [ 2 ] = "" ; buff_reg [ 0 ] = ( reg >> 8 ) & 0xFF ; // reg high byte buff_reg [ 1 ] = reg & 0xFF ; // reg low byte nrf_drv_twi_tx ( &m_twi , address , buff_reg , 2 , false ) ; nrf_drv_twi_rx ( &m_twi , address , &value , 1 ) ; return value ; } // Write a 16-bit register void writeReg16Bit ( uint16_t reg , uint16_t value ) { uint8_t buff_reg [ 4 ] = "" ; buff_reg [ 0 ] = ( reg >> 8 ) & 0xFF ; // reg high byte buff_reg [ 1 ] = reg & 0xFF ; // reg low byte buff_reg [ 3 ] = ( value >> 8 ) & 0xFF ; // reg high byte buff_reg [ 4 ] = value & 0xFF ; // reg low byte nrf_drv_twi_tx ( &m_twi , address , buff_reg , 4 , false ) ; } // Write a 32-bit register void writeReg32Bit ( uint16_t reg , uint32_t value ) { uint8_t buff_send [ 6 ] = "" ; buff_send [ 0 ] = ( reg >> 8 ) & 0xFF ; buff_send [ 1 ] = reg & 0xFF ; buff_send [ 2 ] = ( value >> 24 ) & 0xFF ; buff_send [ 3 ] = ( value >> 16 ) & 0xFF ; buff_send [ 4 ] = ( value >> 8 ) & 0xFF ; buff_send [ 5 ] = value & 0xFF ; nrf_drv_twi_tx ( &m_twi , address , buff_send , 6 , false ) ; } // Set the measurement timing budget in microseconds, which is the time allowed // for one measurement. A longer timing budget allows for more accurate // measurements. // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds() bool setMeasurementTimingBudget(uint32_t budget_us) { // assumes PresetMode is LOWPOWER_AUTONOMOUS if (budget_us <= TimingGuard) { return false; } uint32_t range_config_timeout_us = budget_us -= TimingGuard; if (range_config_timeout_us > 1100000) { return false; } // FDA_MAX_TIMING_BUDGET_US * 2 range_config_timeout_us /= 2; // VL53L1_calc_timeout_register_values() begin uint32_t macro_period_us; // "Update Macro Period for Range A VCSEL Period" macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A)); // "Update Phase timeout - uses Timing A" // Timeout of 1000 is tuning parm default (TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT) // via VL53L1_get_preset_mode_timing_cfg(). uint32_t phasecal_timeout_mclks = timeoutMicrosecondsToMclks(1000, macro_period_us); if (phasecal_timeout_mclks > 0xFF) { phasecal_timeout_mclks = 0xFF; } writeReg(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks); // "Update MM Timing A timeout" // Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT) // via VL53L1_get_preset_mode_timing_cfg(). With the API, the register // actually ends up with a slightly different value because it gets assigned, // retrieved, recalculated with a different macro period, and reassigned, // but it probably doesn't matter because it seems like the MM ("mode // mitigation"?) sequence steps are disabled in low power auto mode anyway. writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout( timeoutMicrosecondsToMclks(1, macro_period_us))); // "Update Range Timing A timeout" writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout( timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))); // "Update Macro Period for Range B VCSEL Period" macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_B)); // "Update MM Timing B timeout" // (See earlier comment about MM Timing A timeout.) writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout( timeoutMicrosecondsToMclks(1, macro_period_us))); // "Update Range Timing B timeout" writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout( timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))); // VL53L1_calc_timeout_register_values() end return true; } // Calculate macro period in microseconds (12.12 format) with given VCSEL period // assumes fast_osc_frequency has been read and stored // based on VL53L1_calc_macro_period_us() uint32_t calcMacroPeriod ( uint8_t vcsel_period ) { // from VL53L1_calc_pll_period_us() // fast osc frequency in 4.12 format; PLL period in 0.24 format uint32_t pll_period_us = ((uint32_t)0x01 << 30) / fast_osc_frequency ; // from VL53L1_decode_vcsel_period() uint8_t vcsel_period_pclks = (vcsel_period + 1) << 1; // VL53L1_MACRO_PERIOD_VCSEL_PERIODS = 2304 uint32_t macro_period_us = (uint32_t)2304 * pll_period_us; macro_period_us >>= 6; macro_period_us *= vcsel_period_pclks; macro_period_us >>= 6; return macro_period_us; } // Convert sequence step timeout from macro periods to microseconds with given // macro period in microseconds (12.12 format) // based on VL53L1_calc_timeout_us() uint32_t timeoutMclksToMicroseconds(uint32_t timeout_mclks, uint32_t macro_period_us) { return ((uint64_t)timeout_mclks * macro_period_us + 0x800) >> 12; } // Convert sequence step timeout from microseconds to macro periods with given // macro period in microseconds (12.12 format) // based on VL53L1_calc_timeout_mclks() uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_us, uint32_t macro_period_us) { return (((uint32_t)timeout_us << 12) + (macro_period_us >> 1)) / macro_period_us; } // Encode sequence step timeout register value from timeout in MCLKs // based on VL53L1_encode_timeout() uint16_t encodeTimeout(uint32_t timeout_mclks) { // encoded format: "(LSByte * 2^MSByte) + 1" uint32_t ls_byte = 0; uint16_t ms_byte = 0; if (timeout_mclks > 0) { ls_byte = timeout_mclks - 1; while ((ls_byte & 0xFFFFFF00) > 0) { ls_byte >>= 1; ms_byte++; } return (ms_byte << 8) | (ls_byte & 0xFF); } else { return 0; } } /** * @} */