Condensed Version of Public VL53L0X
Diff: VL53L0X.cpp
- Revision:
- 11:c6f95a42d4d7
- Parent:
- 10:cd251e0fc2fd
- Child:
- 12:aa177f0e4c10
diff -r cd251e0fc2fd -r c6f95a42d4d7 VL53L0X.cpp --- a/VL53L0X.cpp Sun Mar 24 22:24:16 2019 +0000 +++ b/VL53L0X.cpp Mon Apr 08 16:26:19 2019 +0000 @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @file VL53L0X_class.cpp + * @file Class.cpp * @author IMG * @version V0.0.1 * @date 28-June-2016 @@ -36,955 +36,525 @@ */ // Some example regex that were used to replace useless macros -// \QVL53L0X_SETDEVICESPECIFICPARAMETER(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E -// _device->DevSpecParams.\1 = \3; - -// \QVL53L0X_GETDEVICESPECIFICPARAMETER(\E([A-Z\d]+)\Q);\E -// _device->DevSpecParams.\1; - -// \QVL53L0X_SETPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E -// _device->CurrentParameters.\1 = \3; - -// \QVL53L0X_GETPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E -// \3 = _device->CurrentParameters.\1 - // \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E -// _device->CurrentParameters.\1[\3] = \5; -// is used to replace following macro for ex: -// #define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value) \ -//_device->CurrentParameters.field[index] = value - -// \QVL53L0X_GETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E -// \5 = _device->CurrentParameters.\1[\3] -// is used to replace following macro for ex: -// #define VL53L0X_GETARRAYPARAMETERFIELD(field, index, variable) \ -// variable = _device-> CurrentParameters.field[index] - -// \QPALDevDataSet(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E -// _device->\1 = \3; -// is used to replace: -// #define PALDevDataSet(field, data) (Dev->field)=(data) +// _device->CurrParams.\1[\3] = \5; +// to replace this "#define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value)" by "_device->CurrParams.field[index] = value" + +// to replace "Read_Byte(0x90,&module_id);" by "module_id = Read_Byte(0x90);" search and replace +// \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E +// \3 = Read_Byte\(\1\); /* Includes */ #include <stdlib.h> #include "VL53L0X.h" #include "VL53L0X_tuning.h" -#define REF_ARRAY_SPAD_0 0 -#define REF_ARRAY_SPAD_5 5 -#define REF_ARRAY_SPAD_10 10 - -uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10,REF_ARRAY_SPAD_5, - REF_ARRAY_SPAD_0,REF_ARRAY_SPAD_5 }; - -VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t strobe; - uint32_t loop_nb; - - status |= VL53L0X_write_byte(0x83,0x00); - - /* polling - * use timeout to avoid deadlock*/ - if (status == VL53L0X_ERROR_NONE) { - loop_nb = 0; - do {status = VL53L0X_read_byte(0x83,&strobe); - if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE) {break;} - loop_nb = loop_nb + 1; - } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); - - if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { - status = VL53L0X_ERROR_TIME_OUT; - } - } - - status |= VL53L0X_write_byte(0x83,0x01); - return status; +// Function Data_init and Init_Sensor is united into Start_Sensor +VL53L0X_Error VL53L0X::Start_Sensor(uint8_t new_addr) +{ ErrState = VL53L0X_OK; + + if (_gpio0) { // Can the shutdown pin be controlled? + *_gpio0 = 0; wait_ms(1); // quick shutdown + *_gpio0 = 1; wait_ms(10); // and back ON again + } + + /* Setup the I2C bus. By default the I2C is running at 1V8 if you + * want to change it you need to include this define at compilation level. */ +#ifdef USE_I2C_2V8 + VL53L0X_UpdateByte(REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01); +#endif + /* Set I2C standard mode */ + Write_Byte(0x88,0x00); + + // read and check the device ID from the ID register + Device_Info.ProductType = Read_Byte(REG_IDENTIFICATION_MODEL_ID); + if ( (ErrState == VL53L0X_OK) && (Device_Info.ProductType != 0xEEAA) ) + {return VL53L0X_ERROR_I2C_WRONG_DEV_ID; } + + // reconfigure the address with a new address if requested + if ( (ErrState == VL53L0X_OK) && (new_addr != VL53L0X_DEFAULT_ADDRESS) ) + { Write_Byte(REG_I2C_SLAVE_DEVICE_ADDRESS, new_addr / 2); + I2cDevAddr = new_addr; + } + // quite if an error was raised + if (ErrState != VL53L0X_OK) {return ErrState; } + + /* Set Default static parameters + *set first temporary values 9.44MHz * 65536 = 618660 */ + DevSpecParams.OscFrequencyMHz = 618660; + DevSpecParams.RefSPADSInitialised = 0; + DevSpecParams.ReadDataFromDeviceDone = 0; + +#ifdef USE_IQC_STATION + VL53L0X_Apply_Offset_Cal(); +#endif + + /* Default value is 1000 for Linearity Corrective Gain */ + LinearityCorrectiveGain = 1000; + + /* Dmax default Parameter */ + DmaxCalRangeMilliMeter = 400; + DmaxCalSignalRateRtnMHz = (TFP1616)((0x00016B85)); /* 1.42 No Cover Glass*/ + + /* Get default parameters */ + CurrParams = Get_device_parameters(); + + /* Set Default Xtalk_CompRate_MHz to 0 */ + CurrParams.Xtalk_CompRate_MHz = 0; + + /* initialize CurrParams values */ + CurrParams.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; + CurrParams.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED; + + /* Sigma estimator variable */ + SigmaEstRefArray = 100; + SigmaEstEffPulseWidth = 900; + SigmaEstEffAmbWidth = 500; + targetRefRate = 0x0A00; /* 20 MHz in 9:7 format */ + + /* Use internal default settings */ + UseInternalTuningSettings = 1; + Write_Byte(0x80,0x01); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x00); + StopVariable = Read_Byte(0x91); + Write_Byte(0x00,0x01); + Write_Byte(0xFF,0x00); + Write_Byte(0x80,0x00); + + // quite if an error was raised + if (ErrState != VL53L0X_OK) {return ErrState; } + + /* Disable the following SW-internal checks plaus set some values */ + CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_REF_CLIP] = 0; + CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIG_REF_CLIP] = (35 * 65536); + CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0; + CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0; + + /* Disable the following Device-Internal Checks: */ + CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_MSRC] = 0; + CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE] = 0; + Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEE, 0); + + /* Only enable this internal Check : */ + CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = 1; + CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = (18 * 65536); + + /* Plus Enable VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE check */ + Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); + + if (ErrState == VL53L0X_OK) { /* 0.25 in FP1616 notation 65536 */ + Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE, + (TFP1616)(25 * 65536 / 100)); } + + // quit if an error was raised + if (ErrState != VL53L0X_OK) {return ErrState; } + + // Preset the Config States + SequenceConfig = 0xFF ; + + /* Set Device state to tell that we are waiting for call to VL53L0X_StaticInit */ + Current_State = VL53L0X_STATE_WAIT_STATICINIT ; + + Fill_device_info(); // Retrieve Silicon version, stored in Device_Info + + uint32_t ref_SPAD_count; + uint8_t is_aperture_SPADS; + uint8_t vhv_settings; + uint8_t phase_cal; + + if (ErrState == VL53L0X_OK) { Static_init(); } // Device Initialization + + if (ErrState == VL53L0X_OK) { // Device Calibration + Perf_Ref_calibration( &vhv_settings, &phase_cal, 1); } + + if (ErrState == VL53L0X_OK) { // SPAD Configuration + Perf_Ref_SPAD_management( &ref_SPAD_count, &is_aperture_SPADS); } + + return ErrState; +} + +void VL53L0X::Fill_device_info() +{ uint8_t revision; + + Get_info_from_device(2); + + if (ErrState == VL53L0X_OK) + { if (DevSpecParams.ModuleId == 0) + { revision = 0; + strcpy(Device_Info.ProductId,""); } + else + { revision = DevSpecParams.Revision; + strcpy(Device_Info.ProductId,DevSpecParams.ProductId); + } + if (revision == 0) + { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS0); } + else if ((revision <= 34) && (revision != 32)) + { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS1); } + else if (revision < 39) + { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS2); } + else { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_ES1); } + strcpy(Device_Info.Type,VL53L0X_STRING_DEVICE_INFO_TYPE); + } + + Device_Info.ProductRevisionMajor = 1; + Device_Info.ProductRevisionMinor = + (Read_Byte(REG_IDENTIFICATION_REVISION_ID) & 0xF0) >> 4; } - -VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(uint8_t option) -{ VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t byte; - uint32_t tmp_dword; - uint8_t module_id; - uint8_t revision; - uint8_t reference_spad_count = 0; - uint8_t reference_spad_type = 0; - uint32_t part_uid_upper = 0; - uint32_t part_uid_lower = 0; - uint32_t offset_fixed1104_mm = 0; - int16_t offset_um = 0; - uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4; - uint32_t dist_meas_fixed1104_400_mm = 0; - uint32_t signal_rate_meas_fixed1104_400_mm = 0; - char product_id[19]; - char *product_id_tmp; - uint8_t read_data_from_device_done; - FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0; - uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE]; - int i; - - read_data_from_device_done = DevSpecParams.ReadDataFromDeviceDone; - - /* This access is done only once after that a GetDeviceInfo or - * datainit is done*/ - if (read_data_from_device_done != 7) { - - status |= VL53L0X_write_byte(0x80,0x01); - status |= VL53L0X_write_byte(0xFF,0x01); - status |= VL53L0X_write_byte(0x00,0x00); - status |= VL53L0X_write_byte(0xFF,0x06); - status |= VL53L0X_read_byte(0x83,&byte); - status |= VL53L0X_write_byte(0x83,byte | 4); - status |= VL53L0X_write_byte(0xFF,0x07); - status |= VL53L0X_write_byte(0x81,0x01); - status |= VL53L0X_polling_delay(); - status |= VL53L0X_write_byte(0x80,0x01); - - if (((option & 1) == 1) && - ((read_data_from_device_done & 1) == 0)) { - status |= VL53L0X_write_byte(0x94,0x6b); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x7f); - reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01); - - status |= VL53L0X_write_byte(0x94,0x24); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - - nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24)& 0xff); - nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16)& 0xff); - nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8)& 0xff); - nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff); - - status |= VL53L0X_write_byte(0x94,0x25); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24)& 0xff); - nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16)& 0xff); - } - - if (((option & 2) == 2) && - ((read_data_from_device_done & 2) == 0)) { - - status |= VL53L0X_write_byte(0x94,0x02); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_byte(0x90,&module_id); - - status |= VL53L0X_write_byte(0x94,0x7B); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_byte(0x90,&revision); - - status |= VL53L0X_write_byte(0x94,0x77); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - product_id[0] = (char)((tmp_dword >> 25) & 0x07f); - product_id[1] = (char)((tmp_dword >> 18) & 0x07f); - product_id[2] = (char)((tmp_dword >> 11) & 0x07f); - product_id[3] = (char)((tmp_dword >> 4) & 0x07f); - - byte = (uint8_t)((tmp_dword & 0x00f) << 3); - - status |= VL53L0X_write_byte(0x94,0x78); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - product_id[4] = (char)(byte +((tmp_dword >> 29) & 0x07f)); - product_id[5] = (char)((tmp_dword >> 22) & 0x07f); - product_id[6] = (char)((tmp_dword >> 15) & 0x07f); - product_id[7] = (char)((tmp_dword >> 8) & 0x07f); - product_id[8] = (char)((tmp_dword >> 1) & 0x07f); - - byte = (uint8_t)((tmp_dword & 0x001) << 6); - - status |= VL53L0X_write_byte(0x94,0x79); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - product_id[9] = (char)(byte + - ((tmp_dword >> 26) & 0x07f)); - product_id[10] = (char)((tmp_dword >> 19) & 0x07f); - product_id[11] = (char)((tmp_dword >> 12) & 0x07f); - product_id[12] = (char)((tmp_dword >> 5) & 0x07f); - - byte = (uint8_t)((tmp_dword & 0x01f) << 2); - - status |= VL53L0X_write_byte(0x94,0x7A); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - product_id[13] = (char)(byte + - ((tmp_dword >> 30) & 0x07f)); - product_id[14] = (char)((tmp_dword >> 23) & 0x07f); - product_id[15] = (char)((tmp_dword >> 16) & 0x07f); - product_id[16] = (char)((tmp_dword >> 9) & 0x07f); - product_id[17] = (char)((tmp_dword >> 2) & 0x07f); - product_id[18] = '\0'; - - } - - if (((option & 4) == 4) && ((read_data_from_device_done & 4) == 0)) - { - status |= VL53L0X_write_byte(0x94,0x7B); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&part_uid_upper); - - status |= VL53L0X_write_byte(0x94,0x7C); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&part_uid_lower); - - status |= VL53L0X_write_byte(0x94,0x73); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - signal_rate_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff) << 8; - - status |= VL53L0X_write_byte(0x94,0x74); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - signal_rate_meas_fixed1104_400_mm |= ((tmp_dword & - 0xff000000) >> 24); - - status |= VL53L0X_write_byte(0x94,0x75); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)<< 8; - - status |= VL53L0X_write_byte(0x94,0x76); - status |= VL53L0X_device_read_strobe(); - status |= VL53L0X_read_dword(0x90,&tmp_dword); - - dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000) >> 24); - } - - status |= VL53L0X_write_byte(0x81,0x00); - status |= VL53L0X_write_byte(0xFF,0x06); - status |= VL53L0X_read_byte(0x83,&byte); - status |= VL53L0X_write_byte(0x83,byte & 0xfb); - status |= VL53L0X_write_byte(0xFF,0x01); - status |= VL53L0X_write_byte(0x00,0x01); - - status |= VL53L0X_write_byte(0xFF,0x00); - status |= VL53L0X_write_byte(0x80,0x00); - } - - if ((status == VL53L0X_ERROR_NONE) && - (read_data_from_device_done != 7)) { - /* Assign to variable if status is ok */ - if (((option & 1) == 1) && - ((read_data_from_device_done & 1) == 0)) { - DevSpecParams.ReferenceSpadCount=reference_spad_count; - DevSpecParams.ReferenceSpadType =reference_spad_type; - for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) { - SpadData.RefGoodSpadMap[i] = - nvm_ref_good_spad_map[i]; - } - } - - if (((option & 2) == 2) && - ((read_data_from_device_done & 2) == 0)) { - DevSpecParams.ModuleId = module_id; - DevSpecParams.Revision = revision; - product_id_tmp = DevSpecParams.ProductId; - strcpy(product_id_tmp,product_id); - - } - - if (((option & 4) == 4) && - ((read_data_from_device_done & 4) == 0)) { - DevSpecParams.PartUIDUpper = part_uid_upper; - DevSpecParams.PartUIDLower = part_uid_lower; - signal_rate_meas_fixed400_mm_fix = - VL53L0X_FIXPOINT97TOFIXPOINT1616(signal_rate_meas_fixed1104_400_mm); - DevSpecParams.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix; - DevSpecParams.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix; - - offset_um = 0; - if (dist_meas_fixed1104_400_mm != 0) { - offset_fixed1104_mm = - dist_meas_fixed1104_400_mm - - dist_meas_tgt_fixed1104_mm; - offset_um = (offset_fixed1104_mm * 1000) >> 4; - offset_um *= -1; - } - Part2PartOffsetAdjustmentNVMMicroMeter = offset_um; - } - byte = (uint8_t)(read_data_from_device_done | option); - DevSpecParams.ReadDataFromDeviceDone = byte; - } - - - return status; + + +uint32_t VL53L0X::Get_distance() +{ ErrState = VL53L0X_OK; + TRangeResults p_ranging_results; + + Start_Measurement(op_single_shot_poll, NULL); + if (ErrState==VL53L0X_OK) + { p_ranging_results = Get_Measurement(op_single_shot_poll); } + + Stop_Measurement(op_single_shot_poll); + + if (p_ranging_results.RangeStatus == 0) // we have a valid range ? + { return p_ranging_results.RangeMilliMeter; } + else + { ErrState = VL53L0X_ERROR_RANGE_ERROR; return 0;} } -VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_offsetCallDat_um(int32_t *p_offsetCallDat_um) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint16_t range_offset_register; +TRangeResults VL53L0X::Get_Measurement(TOperatingMode operating_mode) +{ TRangeResults p_data; + + switch (operating_mode) { + case op_single_shot_poll: + Perf_single_ranging_measurement(&p_data); + break; + case op_poll: + Poll_Measure_Completion(); + Get_ranging_results(&p_data); + if (ErrState == VL53L0X_OK) { // Clear the interrupt + Clear_interrupt_mask(REG_SYSINT_GPIO_NEW_SAMPLE_READY); + Polling_delay(); + } + break; + case op_INT: + Get_ranging_results(&p_data); + Clear_interrupt_mask(REG_SYSINT_CLEAR | REG_RESULT_INTERRUPT_STATUS); + } // switch + return p_data; +} + +/** Get part to part calibration offset; Should only be used after a + successful call to @a VL53L0X_DataInit to backup device NVM value **/ +int32_t VL53L0X::Get_Offset_Cal_um() +{ uint16_t range_offset_register; int16_t c_max_offset = 2047; int16_t c_offset_range = 4096; /* Note that offset has 10.2 format */ - - status = VL53L0X_read_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, - &range_offset_register); - - if (status == VL53L0X_ERROR_NONE) { + range_offset_register = Read_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM); + + if (ErrState == VL53L0X_OK) { range_offset_register = (range_offset_register & 0x0fff); - /* Apply 12 bit 2's compliment conversion */ - if (range_offset_register > c_max_offset) { - *p_offsetCallDat_um = - (int16_t)(range_offset_register - c_offset_range) - * 250; - } else { - *p_offsetCallDat_um = - (int16_t)range_offset_register * 250; - } - + /* Apply 12 bit 2's complement conversion */ + if (range_offset_register > c_max_offset) + { return (int16_t)(range_offset_register - c_offset_range) * 250; } + else + { return (int16_t)range_offset_register * 250;} } - - return status; + else return 0; } -VL53L0X_Error VL53L0X::VL53L0X_get_offsetCallDat_um(int32_t *p_offsetCallDat_um) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - - status = wrapped_VL53L0X_get_offsetCallDat_um(p_offsetCallDat_um); - - - return status; -} - -VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_offsetCallDat_um(int32_t offsetCallDat_um) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - int32_t c_max_offset_um = 511000; +void VL53L0X::Set_Offset_Cal_um(int32_t Offset_Cal_um) +{ int32_t c_max_offset_um = 511000; int32_t c_min_offset_um = -512000; - int16_t c_offset_range = 4096; + int16_t c_offset_range = 4096; uint32_t encoded_offset_val; - - - if (offsetCallDat_um > c_max_offset_um) { - offsetCallDat_um = c_max_offset_um; - } else { - if (offsetCallDat_um < c_min_offset_um) { - offsetCallDat_um = c_min_offset_um; - } - } + if (Offset_Cal_um > c_max_offset_um) { Offset_Cal_um = c_max_offset_um; } + else + if (Offset_Cal_um < c_min_offset_um) { Offset_Cal_um = c_min_offset_um; } /* The offset register is 10.2 format and units are mm * therefore conversion is applied by a division of 250. */ - if (offsetCallDat_um >= 0) { - encoded_offset_val = - offsetCallDat_um / 250; - } else { - encoded_offset_val = - c_offset_range + - offsetCallDat_um / 250; - } - - status = VL53L0X_write_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, - encoded_offset_val); - - - return status; + if (Offset_Cal_um >= 0) { encoded_offset_val = Offset_Cal_um / 250; } + else { encoded_offset_val = c_offset_range + Offset_Cal_um / 250; } + + Write_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, encoded_offset_val); } -VL53L0X_Error VL53L0X::VL53L0X_set_offsetCallDat_um(int32_t offsetCallDat_um) -{ VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = wrapped_VL53L0X_set_offsetCallDat_um(offsetCallDat_um); - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - int32_t corrected_offset_um; - int32_t current_offset_um; - - /* if we run on this function we can read all the NVM info - * used by the API */ - status = VL53L0X_get_info_from_device(7); - - /* Read back current device offset */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_offsetCallDat_um(¤t_offset_um); - } +void VL53L0X::VL53L0X_Apply_Offset_Cal() +{ int32_t Summed_Offset_Cal_um; + + /* read all NVM info used by the API */ + Get_info_from_device(7); + + /* Read back current device offset, and remember in case later someone wants to use it */ + if (ErrState == VL53L0X_OK) { Last_Offset_Cal_um = Get_Offset_Cal_um(); } /* Apply Offset Adjustment derived from 400mm measurements */ - if (status == VL53L0X_ERROR_NONE) { - - /* Store initial device offset */ - Part2PartOffsetNVMMicroMeter = current_offset_um; - - corrected_offset_um = current_offset_um + - (int32_t) Part2PartOffsetAdjustmentNVMMicroMeter; - - status = VL53L0X_set_offsetCallDat_um(corrected_offset_um); - - /* store current,adjusted offset */ - if (status == VL53L0X_ERROR_NONE) { - CurrentParameters.RangeOffsetMicroMeters = corrected_offset_um; + if (ErrState == VL53L0X_OK) + { Summed_Offset_Cal_um = Last_Offset_Cal_um + (int32_t) NVM_Offset_Cal_um; + Set_Offset_Cal_um(Summed_Offset_Cal_um); + /* remember current,adjusted offset */ + if (ErrState == VL53L0X_OK) { CurrParams.Offset_Cal_um = Summed_Offset_Cal_um; } + } +} + +void VL53L0X::Get_measure_period_ms(uint32_t *p_measure_period_ms) +{ uint16_t osc_calibrate_val; + uint32_t im_period_ms; + + osc_calibrate_val = Read_Word(REG_OSC_CALIBRATE_VAL); + + if (ErrState == VL53L0X_OK) { im_period_ms = Read_DWord(REG_SYSTEM_MEASURE_PERIOD); } + + if (ErrState == VL53L0X_OK) { + if (osc_calibrate_val != 0) + {*p_measure_period_ms = im_period_ms / osc_calibrate_val; } + CurrParams.Measure_Period_ms = *p_measure_period_ms; + } +} + +void VL53L0X::Get_Xtalk_CompRate_MHz( TFP1616 *p_Xtalk_CompRate_MHz) +{ uint16_t value; + TFP1616 temp_fix1616; + + value = Read_Word(REG_XTALK_COMPENS_RATE_MHz); + + if (ErrState == VL53L0X_OK) { + if (value == 0) { + /* the Xtalk is disabled return value from memory */ + temp_fix1616 = CurrParams.Xtalk_CompRate_MHz; + *p_Xtalk_CompRate_MHz = temp_fix1616; + CurrParams.XTalk_Compens_En = 0; + } else { + temp_fix1616 = FP313_TO_FP1616(value); + *p_Xtalk_CompRate_MHz = temp_fix1616; + CurrParams.Xtalk_CompRate_MHz = temp_fix1616; + CurrParams.XTalk_Compens_En = 1; } } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_get_device_mode(VL53L0X_DeviceModes *p_device_mode) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - *p_device_mode = CurrentParameters.DeviceMode; - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_ms(uint32_t *p_inter_measurement_period_ms) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint16_t osc_calibrate_val; - uint32_t im_period_ms; - - - - status = VL53L0X_read_word(VL53L0X_REG_OSC_CALIBRATE_VAL, - &osc_calibrate_val); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_dword(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, - &im_period_ms); - } - - if (status == VL53L0X_ERROR_NONE) { - if (osc_calibrate_val != 0) { - *p_inter_measurement_period_ms = - im_period_ms / osc_calibrate_val; - } - CurrentParameters.InterMeasurementPeriod_ms= - *p_inter_measurement_period_ms; - } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_mega_cps( - FixPoint1616_t *p_xtalk_compensation_rate_mega_cps) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint16_t value; - FixPoint1616_t temp_fix1616; - - status = VL53L0X_read_word(VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS,(uint16_t *)&value); - if (status == VL53L0X_ERROR_NONE) { - if (value == 0) { - /* the Xtalk is disabled return value from memory */ - temp_fix1616 = CurrentParameters.XTalkCompensationRateMegaCps; - *p_xtalk_compensation_rate_mega_cps = temp_fix1616; - CurrentParameters.XTalkCompensationEnable = 0; - } else { - temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value); - *p_xtalk_compensation_rate_mega_cps = temp_fix1616; - CurrentParameters.XTalkCompensationRateMegaCps = temp_fix1616; - CurrentParameters.XTalkCompensationEnable = 1; - } - } - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value(uint16_t limit_check_id, - FixPoint1616_t *p_limit_check_value) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t enable_zero_value = 0; - uint16_t temp16; - FixPoint1616_t temp_fix1616; +TFP1616 VL53L0X::Get_limit_chk_val( uint16_t limit_check_id ) +{ uint16_t temp16; + TFP1616 temp_fix1616; switch (limit_check_id) { - - case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: - /* internal computation: */ - temp_fix1616 = CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE]; - enable_zero_value = 0; - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: - status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, - &temp16); - if (status == VL53L0X_ERROR_NONE) { - temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16); - } - - - enable_zero_value = 1; - break; - - case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: - /* internal computation: */ - temp_fix1616 = CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP]; - enable_zero_value = 0; - break; - - case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: - /* internal computation: */ - temp_fix1616 = CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD]; - enable_zero_value = 0; - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: - case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: - status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, - &temp16); - if (status == VL53L0X_ERROR_NONE) { - temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16); - } - - enable_zero_value = 0; - break; + case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* only internal computations: */ + case VL53L0X_CHECKEN_SIG_REF_CLIP: + case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: + return CurrParams.Limit_Chk_Val[limit_check_id];// need no more 'break'; + + case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE: + temp16 = Read_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT); + temp_fix1616 = FP97_TO_FP1616(temp16); + if (temp_fix1616 == 0) /* disabled: return value from memory instead*/ + { temp_fix1616 = CurrParams.Limit_Chk_Val[limit_check_id]; + CurrParams.Limit_Chk_En[limit_check_id] = 0; } + else + { CurrParams.Limit_Chk_Val[limit_check_id] = temp_fix1616; + CurrParams.Limit_Chk_En[limit_check_id] = 1; } + return temp_fix1616; // need no more 'break'; + + case VL53L0X_CHECKEN_SIG_RATE_MSRC: + case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE: + temp16 = Read_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT); + return FP97_TO_FP1616(temp16); // need no more break; default: - status = VL53L0X_ERROR_INVALID_PARAMS; + ErrState = VL53L0X_ERROR_INVALID_PARAMS; + return 0; } - - if (status == VL53L0X_ERROR_NONE) { - - if (enable_zero_value == 1) { - - if (temp_fix1616 == 0) { - /* disabled: return value from memory */ - temp_fix1616 = CurrentParameters.LimitChecksValue[limit_check_id]; - *p_limit_check_value = temp_fix1616; - CurrentParameters.LimitChecksEnable[limit_check_id] = 0; - CurrentParameters.LimitChecksEnable[limit_check_id] = 0; - } else { - *p_limit_check_value = temp_fix1616; - CurrentParameters.LimitChecksValue[limit_check_id] = temp_fix1616; - CurrentParameters.LimitChecksEnable[limit_check_id] = 1; - } - } else { - *p_limit_check_value = temp_fix1616; - } - } - - - return status; - } -VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable(uint16_t limit_check_id, - uint8_t *p_limit_check_enable) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t temp8; - - if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { - status = VL53L0X_ERROR_INVALID_PARAMS; - *p_limit_check_enable = 0; - } else { - temp8 = CurrentParameters.LimitChecksEnable[limit_check_id]; - *p_limit_check_enable = temp8; - } - - return status; +uint8_t VL53L0X::Get_limit_chk_en(uint16_t limit_check_id ) +{ if (limit_check_id >= VL53L0X_CHECKEN_NUMBER_OF_CHECKS) + { ErrState = VL53L0X_ERROR_INVALID_PARAMS; + return 0; } + else { return CurrParams.Limit_Chk_En[limit_check_id]; } } -VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(uint8_t *p_wrap_around_check_enable) -{ VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t data; - - status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,&data); - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = data; - if (data & (0x01 << 7)) { *p_wrap_around_check_enable = 0x01; } - else { *p_wrap_around_check_enable = 0x00; } - } - if (status == VL53L0X_ERROR_NONE) { - CurrentParameters.WrapAroundCheckEnable= - *p_wrap_around_check_enable; } - - return status; -} - -VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_SequenceStepId sequence_step_id,uint8_t sequence_config, - uint8_t *p_sequence_step_enabled) -{ - VL53L0X_Error Status = VL53L0X_ERROR_NONE; - *p_sequence_step_enabled = 0; +uint8_t VL53L0X::Get_Wrap_Around_Chk_En() +{ /* Now using the private state field SequenceConfig instead of reading from device: + uint8_t SequenceConfig; + SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); + Set_SequenceConfig( SequenceConfig ); // checks for ErrState - switch (sequence_step_id) { - case VL53L0X_SEQUENCESTEP_TCC: - *p_sequence_step_enabled = (sequence_config & 0x10) >> 4; - break; - case VL53L0X_SEQUENCESTEP_DSS: - *p_sequence_step_enabled = (sequence_config & 0x08) >> 3; - break; - case VL53L0X_SEQUENCESTEP_MSRC: - *p_sequence_step_enabled = (sequence_config & 0x04) >> 2; - break; - case VL53L0X_SEQUENCESTEP_PRE_RANGE: - *p_sequence_step_enabled = (sequence_config & 0x40) >> 6; - break; - case VL53L0X_SEQUENCESTEP_FINAL_RANGE: - *p_sequence_step_enabled = (sequence_config & 0x80) >> 7; - break; - default: - Status = VL53L0X_ERROR_INVALID_PARAMS; - } - - return Status; + if (ErrState == VL53L0X_OK) { + */ + CurrParams.Wrap_Around_Chk_En = (SequenceConfig >> 7) & 0x01; + return CurrParams.Wrap_Around_Chk_En; + // } else return 0; } -VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t sequence_config = 0; - - status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, - &sequence_config); - - if (status == VL53L0X_ERROR_NONE) { - status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_TCC,sequence_config, - &p_scheduler_sequence_steps->TccOn);} - if (status == VL53L0X_ERROR_NONE) { - status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_DSS,sequence_config, - &p_scheduler_sequence_steps->DssOn);} - if (status == VL53L0X_ERROR_NONE) { - status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_MSRC,sequence_config, - &p_scheduler_sequence_steps->MsrcOn);} - if (status == VL53L0X_ERROR_NONE) { - status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_PRE_RANGE,sequence_config, - &p_scheduler_sequence_steps->PreRangeOn);} - if (status == VL53L0X_ERROR_NONE) { - status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_FINAL_RANGE,sequence_config, - &p_scheduler_sequence_steps->FinalRangeOn);} - return status; +VL53L0X_Sequence_Steps_t VL53L0X::Get_sequence_step_enables() +{ VL53L0X_Sequence_Steps_t p_sequence_steps; + /* Now using the private state field SequenceConfig instead of reading from device: + uint8_t SequenceConfig; + + SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); + + if (ErrState == VL53L0X_OK) { + */ + p_sequence_steps.TccOn = (SequenceConfig & 0x10) >> 4; + p_sequence_steps.DssOn = (SequenceConfig & 0x08) >> 3; + p_sequence_steps.MsrcOn = (SequenceConfig & 0x04) >> 2; + p_sequence_steps.PreRangeOn = (SequenceConfig & 0x40) >> 6; + p_sequence_steps.FinalRangeOn = (SequenceConfig & 0x80) >> 7; + // } + return p_sequence_steps; } -uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg) -{ - /*! - * Converts the encoded VCSEL period register value into the real - * period in PLL clocks - */ - - uint8_t vcsel_period_pclks = 0; - vcsel_period_pclks = (vcsel_period_reg + 1) << 1; - return vcsel_period_pclks; -} - -uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks) -{ - /*! - * Converts the encoded VCSEL period register value into the real period - * in PLL clocks - */ - uint8_t vcsel_period_reg = 0; - vcsel_period_reg = (vcsel_period_pclks >> 1) - 1; - return vcsel_period_reg; -} - - -VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t vcsel_pulse_period_pclk) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t vcsel_period_reg; +void VL53L0X::Set_vcsel_PPeriod(VL53L0X_Range_Phase Vcsel_Range_Phase, uint8_t vcsel_PPeriod_pclk) +{ uint8_t vcsel_period_reg; uint8_t min_pre_vcsel_period_pclk = 12; uint8_t max_pre_vcsel_period_pclk = 18; uint8_t min_final_vcsel_period_pclk = 8; uint8_t max_final_vcsel_period_pclk = 14; - uint32_t measurement_timing_budget_us; uint32_t final_range_timeout_us; uint32_t pre_range_timeout_us; uint32_t msrc_timeout_us; uint8_t phase_cal_int = 0; /* Check if valid clock period requested */ - - if ((vcsel_pulse_period_pclk % 2) != 0) { - /* Value must be an even number */ - status = VL53L0X_ERROR_INVALID_PARAMS; - } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE && - (vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk || - vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) { - status = VL53L0X_ERROR_INVALID_PARAMS; - } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE && - (vcsel_pulse_period_pclk < min_final_vcsel_period_pclk || - vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) { - - status = VL53L0X_ERROR_INVALID_PARAMS; - } + if ( ((vcsel_PPeriod_pclk % 2) != 0 ) /* Value must be an even number */ + || + ( Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE && + ((vcsel_PPeriod_pclk < min_pre_vcsel_period_pclk)|| + (vcsel_PPeriod_pclk > max_pre_vcsel_period_pclk) ) ) + || + ( Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE && + (vcsel_PPeriod_pclk < min_final_vcsel_period_pclk || + vcsel_PPeriod_pclk > max_final_vcsel_period_pclk) ) ) + { ErrState = VL53L0X_ERROR_INVALID_PARAMS; + return;} /* Apply specific settings for the requested clock period */ - if (status != VL53L0X_ERROR_NONE) { return status; } - - if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) { - - /* Set phase check limits */ - if (vcsel_pulse_period_pclk == 12) { - - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x18); - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - } else if (vcsel_pulse_period_pclk == 14) { - - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x30); - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - } else if (vcsel_pulse_period_pclk == 16) { - - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x40); - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - } else if (vcsel_pulse_period_pclk == 18) { - - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x50); - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); + if (Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE) { + /* Set phase check limits for pre-ranging*/ + if (vcsel_PPeriod_pclk == 12) { + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x18); + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); + } else if (vcsel_PPeriod_pclk == 14) { + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x30); + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); + } else if (vcsel_PPeriod_pclk == 16) { + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x40); + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); + } else if (vcsel_PPeriod_pclk == 18) { + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x50); + Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); } - } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) { - - if (vcsel_pulse_period_pclk == 8) { - - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x10); - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - - status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x02); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C); - - status |= VL53L0X_write_byte(0xff,0x01); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM, - 0x30); - status |= VL53L0X_write_byte(0xff,0x00); - } else if (vcsel_pulse_period_pclk == 10) { - - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x28); - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - - status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09); - - status |= VL53L0X_write_byte(0xff,0x01); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,0x20); - status |= VL53L0X_write_byte(0xff,0x00); - } else if (vcsel_pulse_period_pclk == 12) { - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x38); - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - - status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08); - status |= VL53L0X_write_byte(0xff,0x01); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,0x20); - status |= VL53L0X_write_byte(0xff,0x00); - } else if (vcsel_pulse_period_pclk == 14) { - - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, - 0x048); - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, - 0x08); - - status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07); - status |= VL53L0X_write_byte(0xff,0x01); - status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20); - status |= VL53L0X_write_byte(0xff,0x00); + } else if (Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE) { + if (vcsel_PPeriod_pclk == 8) { + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10); + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); + Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH ,0x02); + Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C); + Write_Byte(0xff,0x01); + Write_Byte(REG_ALGO_PHASECAL_LIM,0x30); + Write_Byte(0xff,0x00); + } else if (vcsel_PPeriod_pclk == 10) { + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28); + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); + Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); + Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09); + Write_Byte(0xff,0x01); + Write_Byte(REG_ALGO_PHASECAL_LIM,0x20); + Write_Byte(0xff,0x00); + } else if (vcsel_PPeriod_pclk == 12) { + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x38); + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); + Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); + Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08); + Write_Byte(0xff,0x01); + Write_Byte(REG_ALGO_PHASECAL_LIM,0x20); + Write_Byte(0xff,0x00); + } else if (vcsel_PPeriod_pclk == 14) { + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048); + Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); + Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); + Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07); + Write_Byte(0xff,0x01); + Write_Byte(REG_ALGO_PHASECAL_LIM, 0x20); + Write_Byte(0xff,0x00); } } - /* Re-calculate and apply timeouts,in macro periods */ - - if (status == VL53L0X_ERROR_NONE) { - vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t) - vcsel_pulse_period_pclk); - + if (ErrState == VL53L0X_OK) { + /* Converts the encoded VCSEL period register value into the real period in PLL clocks */ + /* Flattened from procedure called Encode_vcsel_period */ + vcsel_period_reg = (vcsel_PPeriod_pclk >> 1) - 1; + /* When the VCSEL period for the pre or final range is changed, * the corresponding timeout must be read from the device using * the current VCSEL period,then the new VCSEL period can be * applied. The timeout then must be written back to the device * using the new VCSEL period. - * * For the MSRC timeout,the same applies - this timeout being * dependant on the pre-range vcsel period. */ - switch (vcsel_period_type) { - case VL53L0X_VCSEL_PERIOD_PRE_RANGE: - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, - &pre_range_timeout_us); - - if (status == VL53L0X_ERROR_NONE) - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, - &msrc_timeout_us); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, - vcsel_period_reg); - - - if (status == VL53L0X_ERROR_NONE) - status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, - pre_range_timeout_us); - - - if (status == VL53L0X_ERROR_NONE) - status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, - msrc_timeout_us); - - DevSpecParams.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk; - break; - case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, - &final_range_timeout_us); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, - vcsel_period_reg); - - if (status == VL53L0X_ERROR_NONE) - status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, - final_range_timeout_us); - - DevSpecParams.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk; - break; - default: - status = VL53L0X_ERROR_INVALID_PARAMS; + switch (Vcsel_Range_Phase) { + case VL53L0X_VCSEL_PRE_RANGE: + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&pre_range_timeout_us); + + if (ErrState == VL53L0X_OK) + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,&msrc_timeout_us); + + Write_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg); + + if (ErrState == VL53L0X_OK) + Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,pre_range_timeout_us); + + if (ErrState == VL53L0X_OK) + Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,msrc_timeout_us); + + DevSpecParams.PreRangeVcselPPeriod = vcsel_PPeriod_pclk; + break; + + case VL53L0X_VCSEL_FINAL_RANGE: + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&final_range_timeout_us); + + Write_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg); + + if (ErrState == VL53L0X_OK) + Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,final_range_timeout_us); + + DevSpecParams.FinalRangeVcselPPeriod = vcsel_PPeriod_pclk; + break; + default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; } } - /* Finally,the timing budget must be re-applied */ - if (status == VL53L0X_ERROR_NONE) { - measurement_timing_budget_us = CurrentParameters.MeasurementTimingBudget_us; - - status = VL53L0X_set_measurement_timing_budget_us(measurement_timing_budget_us); - } - - /* Perform the phase calibration. This is needed after changing on - * vcsel period. + /* Finally,the timing budget is re-applied */ + if (ErrState == VL53L0X_OK) + { Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); } + + /* Perform the phase calibration. This is needed after changing on vcsel period. * get_data_enable = 0,restore_config = 1 */ - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_perform_phase_calibration(&phase_cal_int,0,1); - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, - uint8_t vcsel_pulse_period) -{ VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = wrapped_VL53L0X_set_vcsel_pulse_period(vcsel_period_type, - vcsel_pulse_period); - - return status; + Perf_phase_calibration(&phase_cal_int,0,1); } -VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_vcsel_pulse_period( - VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t vcsel_period_reg; - - switch (vcsel_period_type) { - case VL53L0X_VCSEL_PERIOD_PRE_RANGE: - status = VL53L0X_read_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, - &vcsel_period_reg); - break; - case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: - status = VL53L0X_read_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, - &vcsel_period_reg); - break; - default: - status = VL53L0X_ERROR_INVALID_PARAMS; - } - - if (status == VL53L0X_ERROR_NONE) - *p_vcsel_pulse_period_pclk = VL53L0X_decode_vcsel_period(vcsel_period_reg); - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t *p_vcsel_pulse_period_pclk) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = wrapped_VL53L0X_get_vcsel_pulse_period(vcsel_period_type, - p_vcsel_pulse_period_pclk); - - return status; -} - -uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout) -{ - /*! - * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 - */ - - uint32_t timeout_macro_clks = 0; - - timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF) - << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1; - return timeout_macro_clks; -} - -uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(uint8_t vcsel_period_pclks) -{ - uint64_t pll_period_ps; - uint32_t macro_period_vclks; - uint32_t macro_period_ps; - - /* The above calculation will produce rounding errors, - therefore set fixed value */ - pll_period_ps = 1655; - macro_period_vclks = 2304; - macro_period_ps = (uint32_t)(macro_period_vclks - * vcsel_period_pclks * pll_period_ps); - return macro_period_ps; -} +#define VL53L0X_MACRO_PERIOD_NS 3813; // = ( VL53L0X_PLL_PERIOD_PS * VL53L0X_MACRO_PERIOD_VCLKS / 1000 ) /* To convert register value into us */ -uint32_t VL53L0X::VL53L0X_calc_timeout_us(uint16_t timeout_period_mclks, +uint32_t VL53L0X::Calc_timeout_us(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) { - uint32_t macro_period_ps; uint32_t macro_period_ns; uint32_t actual_timeout_period_us = 0; - macro_period_ps = VL53L0X_calc_macro_period_ps(vcsel_period_pclks); - macro_period_ns = (macro_period_ps + 500) / 1000; - - actual_timeout_period_us = - ((timeout_period_mclks * macro_period_ns) + 500) / 1000; + macro_period_ns = (uint32_t) (vcsel_period_pclks ) * VL53L0X_MACRO_PERIOD_NS; + + actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000; return actual_timeout_period_us; } -VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id, +void VL53L0X::Get_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id, uint32_t *p_time_out_micro_secs) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t current_vcsel_pulse_period_p_clk; +{ uint8_t current_vcsel_PPeriod_p_clk; uint8_t encoded_time_out_byte = 0; uint32_t timeout_us = 0; uint16_t pre_range_encoded_time_out = 0; @@ -992,779 +562,389 @@ uint16_t pre_range_time_out_m_clks; uint16_t final_range_time_out_m_clks = 0; uint16_t final_range_encoded_time_out; - VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; - - if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) || - (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) || - (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) { - - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, - &encoded_time_out_byte); + VL53L0X_Sequence_Steps_t sequence_steps; + + if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) || + (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) || + (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC) ) { + + current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; + + if (ErrState == VL53L0X_OK) { + encoded_time_out_byte = Read_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP); } - msrc_time_out_m_clks = VL53L0X_decode_timeout(encoded_time_out_byte); - - timeout_us = VL53L0X_calc_timeout_us(msrc_time_out_m_clks, - current_vcsel_pulse_period_p_clk); + msrc_time_out_m_clks = Decode_timeout(encoded_time_out_byte); + + timeout_us = Calc_timeout_us(msrc_time_out_m_clks, + current_vcsel_PPeriod_p_clk); } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { - /* Retrieve PRE-RANGE VCSEL Period */ - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); + + current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ - if (status == VL53L0X_ERROR_NONE) { - - /* Retrieve PRE-RANGE VCSEL Period */ - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, - &pre_range_encoded_time_out); - } - - pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out); - - timeout_us = VL53L0X_calc_timeout_us(pre_range_time_out_m_clks, - current_vcsel_pulse_period_p_clk); + if (ErrState == VL53L0X_OK) { + + pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI); + + pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out); + + timeout_us = Calc_timeout_us(pre_range_time_out_m_clks, + current_vcsel_PPeriod_p_clk); } } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { - VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps); + sequence_steps = Get_sequence_step_enables(); pre_range_time_out_m_clks = 0; - if (scheduler_sequence_steps.PreRangeOn) { - /* Retrieve PRE-RANGE VCSEL Period */ - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); - - /* Retrieve PRE-RANGE Timeout in Macro periods - * (MCLKS) */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, - &pre_range_encoded_time_out); - pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out); + if (sequence_steps.PreRangeOn) { + current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; + + /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ + if (ErrState == VL53L0X_OK) { + pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI); + pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out); } } - if (status == VL53L0X_ERROR_NONE) { - /* Retrieve FINAL-RANGE VCSEL Period */ - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE, - ¤t_vcsel_pulse_period_p_clk); + if (ErrState == VL53L0X_OK) { + current_vcsel_PPeriod_p_clk = /* Get and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; + } /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, - &final_range_encoded_time_out); - final_range_time_out_m_clks = VL53L0X_decode_timeout(final_range_encoded_time_out); + if (ErrState == VL53L0X_OK) { + final_range_encoded_time_out = Read_Word(REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI); + final_range_time_out_m_clks = Decode_timeout(final_range_encoded_time_out); } final_range_time_out_m_clks -= pre_range_time_out_m_clks; - timeout_us = VL53L0X_calc_timeout_us(final_range_time_out_m_clks, - current_vcsel_pulse_period_p_clk); + timeout_us = Calc_timeout_us(final_range_time_out_m_clks,current_vcsel_PPeriod_p_clk); } *p_time_out_micro_secs = timeout_us; - - return status; } -VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_measurement_timing_budget_us(uint32_t *p_measurement_timing_budget_us) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; +uint32_t VL53L0X::Get_Measure_Time_Budget_us() +{ VL53L0X_Sequence_Steps_t sequence_steps; + uint32_t p_Measure_Time_Budget_us; uint32_t final_range_timeout_us; - uint32_t msrc_dcc_tcc_timeout_us = 2000; + uint32_t msrc_dcc_tcc_timeout_us= 2000; uint32_t start_overhead_us = 1910; - uint32_t end_overhead_us = 960; + uint32_t end_overhead_us = 960; uint32_t msrc_overhead_us = 660; - uint32_t tcc_overhead_us = 590; - uint32_t dss_overhead_us = 690; + uint32_t tcc_overhead_us = 590; + uint32_t dss_overhead_us = 690; uint32_t pre_range_overhead_us = 660; - uint32_t final_range_overhead_us = 550; + uint32_t final_range_overhead_us= 550; uint32_t pre_range_timeout_us = 0; + if (ErrState != VL53L0X_OK) {return 0; } // do nothing while in Error State!!!! + /* Start and end overhead times always present */ - *p_measurement_timing_budget_us - = start_overhead_us + end_overhead_us; - - status = VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps); - - if (status != VL53L0X_ERROR_NONE) { return status; } - - if (scheduler_sequence_steps.TccOn || scheduler_sequence_steps.MsrcOn || - scheduler_sequence_steps.DssOn) { - - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, - &msrc_dcc_tcc_timeout_us); - - if (status == VL53L0X_ERROR_NONE) { - if (scheduler_sequence_steps.TccOn) { - *p_measurement_timing_budget_us += - msrc_dcc_tcc_timeout_us + - tcc_overhead_us; - } - - if (scheduler_sequence_steps.DssOn) { - *p_measurement_timing_budget_us += - 2 * (msrc_dcc_tcc_timeout_us + - dss_overhead_us); - } else if (scheduler_sequence_steps.MsrcOn) { - *p_measurement_timing_budget_us += - msrc_dcc_tcc_timeout_us + - msrc_overhead_us; + p_Measure_Time_Budget_us = start_overhead_us + end_overhead_us; + + sequence_steps = Get_sequence_step_enables(); + + if (sequence_steps.TccOn || sequence_steps.MsrcOn || sequence_steps.DssOn) + { Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us); + + if (ErrState == VL53L0X_OK) { + if (sequence_steps.TccOn) + { p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + tcc_overhead_us; } + + if (sequence_steps.DssOn) { + p_Measure_Time_Budget_us += 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us); + } else if (sequence_steps.MsrcOn) { + p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + msrc_overhead_us; } } } - if (status == VL53L0X_ERROR_NONE) { - if (scheduler_sequence_steps.PreRangeOn) { - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, - &pre_range_timeout_us); - *p_measurement_timing_budget_us += - pre_range_timeout_us + - pre_range_overhead_us; - } - } - - if (status == VL53L0X_ERROR_NONE) { - if (scheduler_sequence_steps.FinalRangeOn) { - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, - &final_range_timeout_us); - *p_measurement_timing_budget_us += - (final_range_timeout_us + - final_range_overhead_us); + if ( (ErrState == VL53L0X_OK) && sequence_steps.PreRangeOn) { + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us); + p_Measure_Time_Budget_us += pre_range_timeout_us + pre_range_overhead_us; + } + + if (ErrState == VL53L0X_OK) { + if (sequence_steps.FinalRangeOn) { + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, &final_range_timeout_us); + p_Measure_Time_Budget_us += (final_range_timeout_us + final_range_overhead_us); } } - if (status == VL53L0X_ERROR_NONE) { - CurrentParameters.MeasurementTimingBudget_us = - *p_measurement_timing_budget_us; - } - - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_us( - uint32_t *p_measurement_timing_budget_us) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = wrapped_VL53L0X_get_measurement_timing_budget_us( - p_measurement_timing_budget_us); - - return status; + if (ErrState == VL53L0X_OK) + { CurrParams.Measure_Time_Budget_us = p_Measure_Time_Budget_us; } + + return p_Measure_Time_Budget_us; } -VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters( - VL53L0X_DeviceParameters_t *p_device_parameters) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - int i; - - status = VL53L0X_get_device_mode(&(p_device_parameters->DeviceMode)); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_get_inter_measurement_period_ms(&(p_device_parameters->InterMeasurementPeriod_ms)); - - if (status == VL53L0X_ERROR_NONE) { - p_device_parameters->XTalkCompensationEnable = 0; } - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_get_x_talk_compensation_rate_mega_cps(&(p_device_parameters->XTalkCompensationRateMegaCps)); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_get_offsetCallDat_um(&(p_device_parameters->RangeOffsetMicroMeters)); - - if (status == VL53L0X_ERROR_NONE) { - for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { - /* get first the values,then the enables. - * VL53L0X_GetLimitCheckValue will modify the enable - * flags - */ - if (status == VL53L0X_ERROR_NONE) { - status |= VL53L0X_get_limit_check_value(i,&(p_device_parameters->LimitChecksValue[i])); - } else { - break; - } - if (status == VL53L0X_ERROR_NONE) { - status |= VL53L0X_get_limit_check_enable(i, - &(p_device_parameters->LimitChecksEnable[i])); } +VL53L0X_DeviceParams_t VL53L0X::Get_device_parameters() +{ VL53L0X_DeviceParams_t device_params = {0}; + int i; + + if (ErrState != VL53L0X_OK) {return device_params; } // do nothing while in Error State!!!! + + device_params.DeviceMode = CurrParams.DeviceMode; + device_params.XTalk_Compens_En = 0; + device_params.Offset_Cal_um = Get_Offset_Cal_um(); + + Get_measure_period_ms(&(device_params.Measure_Period_ms)); + + if (ErrState == VL53L0X_OK) + Get_Xtalk_CompRate_MHz(&(device_params.Xtalk_CompRate_MHz)); + + if (ErrState == VL53L0X_OK) { + for (i = 0; i < VL53L0X_CHECKEN_NUMBER_OF_CHECKS; i++) + {/* get first the values,then the enables. GetLimitCheckValue will + modify the enable flags */ + if (ErrState == VL53L0X_OK) + { device_params.Limit_Chk_Val[i] = Get_limit_chk_val(i); } + else { break; } + if (ErrState == VL53L0X_OK) + { device_params.Limit_Chk_En[i]= Get_limit_chk_en(i);} else { break; } } } - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_wrap_around_check_enable(&(p_device_parameters->WrapAroundCheckEnable)); - } - - /* Need to be done at the end as it uses VCSELPulsePeriod */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_measurement_timing_budget_us( - &(p_device_parameters->MeasurementTimingBudget_us)); - } - - return status; + if (ErrState == VL53L0X_OK) { + device_params.Wrap_Around_Chk_En = Get_Wrap_Around_Chk_En();} + + /* Need to be done at the end as it uses VCSELPPeriod */ + if (ErrState == VL53L0X_OK) { + device_params.Measure_Time_Budget_us = Get_Measure_Time_Budget_us(); } + + return device_params; } -VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value(uint16_t limit_check_id, - FixPoint1616_t limit_check_value) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t temp8; - - temp8 = CurrentParameters.LimitChecksEnable[limit_check_id]; - - if (temp8 == 0) { /* disabled write only internal value */ - CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value; - } else { - +void VL53L0X::Set_limit_chk_val(uint16_t limit_check_id, TFP1616 limit_chk_val) +{ /* first verify that the ID is within bounds .. */ + if (limit_check_id>=VL53L0X_CHECKEN_NUMBER_OF_CHECKS) + { ErrState = VL53L0X_ERROR_INVALID_PARAMS; return; } + + /* Under all other circumstances store value in local array: */ + CurrParams.Limit_Chk_Val[limit_check_id] = limit_chk_val; + + /* in addition, if enabled, then write the external ones also to the Registers */ + if (CurrParams.Limit_Chk_En[ limit_check_id ]) switch (limit_check_id) { - - case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: - /* internal computation: */ - CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_value; - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: - status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, - VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value)); - break; - - case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: - /* internal computation: */ - CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_value; - break; - - case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: - /* internal computation: */ - CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_value; - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: - case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: - status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, - VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value)); + case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE: + Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, + FP1616_TO_FP97(limit_chk_val)); break; - - default: - status = VL53L0X_ERROR_INVALID_PARAMS; - } - - if (status == VL53L0X_ERROR_NONE) { - CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value; - } - } - - return status; + case VL53L0X_CHECKEN_SIG_RATE_MSRC: + case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE: + Write_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, + FP1616_TO_FP97(limit_chk_val)); + break; + } // switch } -VL53L0X_Error VL53L0X::VL53L0X_data_init() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - VL53L0X_DeviceParameters_t Updated_Curr_Params; - int i; - - /* by default the I2C is running at 1V8 if you want to change it you - * need to include this define at compilation level. */ -#ifdef USE_I2C_2V8 - Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, - 0xFE, 0x01); -#endif - - /* Set I2C standard mode */ - if (status == VL53L0X_ERROR_NONE) {status = VL53L0X_write_byte(0x88,0x00);} - DevSpecParams.ReadDataFromDeviceDone = 0; - DevSpecParams.ReadDataFromDeviceDone = 0; - -#ifdef USE_IQC_STATION - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_apply_offset_adjustment(); } -#endif - - /* Default value is 1000 for Linearity Corrective Gain */ - LinearityCorrectiveGain = 1000; - - /* Dmax default Parameter */ - DmaxCalRangeMilliMeter = 400; - DmaxCalSignalRateRtnMegaCps = (FixPoint1616_t)((0x00016B85)); /* 1.42 No Cover Glass*/ - - /* Set Default static parameters - *set first temporary values 9.44MHz * 65536 = 618660 */ - DevSpecParams.OscFrequencyMHz = 618660; - - /* Set Default XTalkCompensationRateMegaCps to 0 */ - CurrentParameters.XTalkCompensationRateMegaCps = 0; - - /* Get default parameters */ - status = VL53L0X_get_device_parameters(&Updated_Curr_Params); - if (status == VL53L0X_ERROR_NONE) { - /* initialize PAL values */ - Updated_Curr_Params.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; - Updated_Curr_Params.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED; - CurrentParameters = Updated_Curr_Params; + + + +void VL53L0X::Get_interrupt_mask_status(uint32_t *p_interrupt_mask_status) +{ uint8_t intStat; + + intStat = Read_Byte(REG_RESULT_INTERRUPT_STATUS); + *p_interrupt_mask_status = intStat & 0x07; + if (intStat & 0x18) { ErrState = VL53L0X_ERROR_RANGE_ERROR; } +} + +uint8_t VL53L0X::Get_Measurement_Ready() +{ uint8_t sys_range_status_register; + uint32_t interrupt_mask; + + if (DevSpecParams.GpioFunctionality == REG_SYSINT_GPIO_NEW_SAMPLE_READY) + { Get_interrupt_mask_status(&interrupt_mask); + if (interrupt_mask == REG_SYSINT_GPIO_NEW_SAMPLE_READY) + { return 1; } else { return 0; } + } + else + { sys_range_status_register = Read_Byte(REG_RESULT_RANGE_STATUS); + if ( ( ErrState == VL53L0X_OK ) & (sys_range_status_register & 0x01) ) + { return 1; } else { return 0; } } - - /* Sigma estimator variable */ - SigmaEstRefArray = 100; - SigmaEstEffPulseWidth = 900; - SigmaEstEffAmbWidth = 500; - targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */ - - /* Use internal default settings */ - UseInternalTuningSettings = 1; - - status |= VL53L0X_write_byte(0x80,0x01); - status |= VL53L0X_write_byte(0xFF,0x01); - status |= VL53L0X_write_byte(0x00,0x00); - status |= VL53L0X_read_byte (0x91,&StopVariable); - status |= VL53L0X_write_byte(0x00,0x01); - status |= VL53L0X_write_byte(0xFF,0x00); - status |= VL53L0X_write_byte(0x80,0x00); - - /* Enable all check */ - for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { - if (status == VL53L0X_ERROR_NONE) { - status |= VL53L0X_set_limit_check_enable(i,1); - } else { - break; - } - - } - - /* Disable the following checks */ - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,0); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,0); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC,0); - - if (status == VL53L0X_ERROR_NONE) - status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE,0); - - /* Limit default values */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, - (FixPoint1616_t)(18 * 65536)); - } - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, - (FixPoint1616_t)(25 * 65536 / 100)); - /* 0.25 * 65536 */ - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, - (FixPoint1616_t)(35 * 65536)); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, - (FixPoint1616_t)(0 * 65536)); - } - - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = 0xFF; - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xFF); - /* Set PAL state to tell that we are waiting for call to - * VL53L0X_StaticInit */ - PalState = VL53L0X_STATE_WAIT_STATICINIT; - } - - if (status == VL53L0X_ERROR_NONE) { DevSpecParams.RefSpadsInitialised = 0; } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_check_part_used(uint8_t *revision, - VL53L0X_DeviceInfo_t *p_VL53L0X_device_info) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t module_id_int; - char *product_id_tmp; - - status = VL53L0X_get_info_from_device(2); - - if (status == VL53L0X_ERROR_NONE) { - module_id_int = DevSpecParams.ModuleId; - - if (module_id_int == 0) { - *revision = 0; - strcpy(p_VL53L0X_device_info->ProductId,""); - } else { - *revision = DevSpecParams.Revision; - product_id_tmp = DevSpecParams.ProductId; - strcpy(p_VL53L0X_device_info->ProductId,product_id_tmp); - } - } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DeviceInfo_t *p_VL53L0X_device_info) +void VL53L0X::Polling_delay() { - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t revision_id; - uint8_t revision; - - status = VL53L0X_check_part_used(&revision,p_VL53L0X_device_info); - - if (status == VL53L0X_ERROR_NONE) { - if (revision == 0) { - strcpy(p_VL53L0X_device_info->Name, - VL53L0X_STRING_DEVICE_INFO_NAME_TS0); - } else if ((revision <= 34) && (revision != 32)) { - strcpy(p_VL53L0X_device_info->Name, - VL53L0X_STRING_DEVICE_INFO_NAME_TS1); - } else if (revision < 39) { - strcpy(p_VL53L0X_device_info->Name, - VL53L0X_STRING_DEVICE_INFO_NAME_TS2); - } else { - strcpy(p_VL53L0X_device_info->Name, - VL53L0X_STRING_DEVICE_INFO_NAME_ES1); - } - - strcpy(p_VL53L0X_device_info->Type,VL53L0X_STRING_DEVICE_INFO_TYPE); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_MODEL_ID, - &p_VL53L0X_device_info->ProductType); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_REVISION_ID, - &revision_id); - p_VL53L0X_device_info->ProductRevisionMajor = 1; - p_VL53L0X_device_info->ProductRevisionMinor = - (revision_id & 0xF0) >> 4; - } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(uint32_t *p_interrupt_mask_status) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t intStat; - - status = VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS,&intStat); - *p_interrupt_mask_status = intStat & 0x07; - - if (intStat & 0x18) { status = VL53L0X_ERROR_RANGE_ERROR; } - - return status; + // do nothing VL53L0X_OsDelay(); } -VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(uint8_t *p_measurement_data_ready) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t sys_range_status_register; - uint8_t interrupt_config; - uint32_t interrupt_mask; - - interrupt_config = DevSpecParams.Pin0GpioFunctionality; - - if (interrupt_config == - VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { - status = VL53L0X_get_interrupt_mask_status(&interrupt_mask); - if (interrupt_mask == - VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { - *p_measurement_data_ready = 1; - } else { - *p_measurement_data_ready = 0; - } - } else { - status = VL53L0X_read_byte(VL53L0X_REG_RESULT_RANGE_STATUS, - &sys_range_status_register); - if (status == VL53L0X_ERROR_NONE) { - if (sys_range_status_register & 0x01) { - *p_measurement_data_ready = 1; - } else { - *p_measurement_data_ready = 0; - } - } - } - return status; +void VL53L0X::Poll_Measure_Completion() +{ uint8_t new_data_ready; + uint32_t loop_nb = 0; + + if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error + + new_data_ready = Get_Measurement_Ready(); + + while ((ErrState==0) && (new_data_ready != 1) ) + { Polling_delay(); + new_data_ready = Get_Measurement_Ready(); + if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) ErrState=VL53L0X_ERROR_TIME_OUT; + } // while ; } -VL53L0X_Error VL53L0X::VL53L0X_polling_delay() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - // do nothing VL53L0X_OsDelay(); - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t new_data_ready = 0; - uint32_t loop_nb; - - loop_nb = 0; - - status = VL53L0X_get_measurement_data_ready(&new_data_ready); - - while ((status==0) && (new_data_ready != 1) && - (loop_nb < VL53L0X_DEFAULT_MAX_LOOP) ) - { - VL53L0X_polling_delay(); - status = VL53L0X_get_measurement_data_ready(&new_data_ready); - loop_nb++; - } // while ; - - if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) status = VL53L0X_ERROR_TIME_OUT; - - return status; -} - -/* Group PAL Interrupt Functions */ -VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask(uint32_t interrupt_mask) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t loop_count; +/* Group Device Interrupt Functions */ +void VL53L0X::Clear_interrupt_mask(uint32_t interrupt_mask) +{ uint8_t loop_count = 0; uint8_t byte; + if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error + /* clear bit 0 range interrupt,bit 1 error interrupt */ - loop_count = 0; do { - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR,0x01); - status |= VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR,0x00); - status |= VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS,&byte); - loop_count++; - } while (((byte & 0x07) != 0x00) - && (loop_count < 3) - && (status == VL53L0X_ERROR_NONE)); - - if (loop_count >= 3) { - status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED; - } - - return status; + Write_Byte(REG_SYSINT_CLEAR,0x01); + Write_Byte(REG_SYSINT_CLEAR,0x00); + byte = Read_Byte(REG_RESULT_INTERRUPT_STATUS); + if (loop_count++ > 3) {ErrState =VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;} + } while (((byte & 0x07) != 0x00) && (ErrState == VL53L0X_OK)); + } -VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(uint8_t vhv_init_byte) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START, - VL53L0X_REG_SYSRANGE_MODE_START_STOP | - vhv_init_byte); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_measurement_poll_for_completion(); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_clear_interrupt_mask(0); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,0x00); - } - - return status; +void VL53L0X::Perf_single_Ref_calibration(uint8_t vhv_init_byte) +{ if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! + Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_START_STOP | vhv_init_byte); + Poll_Measure_Completion(); + Clear_interrupt_mask(0); + Write_Byte(REG_SYSRANGE_START,0x00); } -VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(uint8_t read_not_write, +void VL53L0X::Ref_calibration_io(uint8_t read_not_write, uint8_t vhv_settings,uint8_t phase_cal, uint8_t *p_vhv_settings,uint8_t *p_phase_cal, const uint8_t vhv_enable,const uint8_t phase_enable) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t phase_calint = 0; +{ uint8_t phase_calint = 0; /* Read VHV from device */ - status |= VL53L0X_write_byte(0xFF,0x01); - status |= VL53L0X_write_byte(0x00,0x00); - status |= VL53L0X_write_byte(0xFF,0x00); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x00); + Write_Byte(0xFF,0x00); if (read_not_write) { - if (vhv_enable) { - status |= VL53L0X_read_byte(0xCB,p_vhv_settings); - } - if (phase_enable) { - status |= VL53L0X_read_byte(0xEE,&phase_calint); - } - } else { - if (vhv_enable) { - status |= VL53L0X_write_byte(0xCB,vhv_settings); - } - if (phase_enable) { - status |= VL53L0X_update_byte(0xEE,0x80,phase_cal); - } - } - - status |= VL53L0X_write_byte(0xFF,0x01); - status |= VL53L0X_write_byte(0x00,0x01); - status |= VL53L0X_write_byte(0xFF,0x00); + if (vhv_enable ) { *p_vhv_settings = Read_Byte(0xCB); } + if (phase_enable) { phase_calint = Read_Byte(0xEE); } + } + else { + if (vhv_enable ) { Write_Byte(0xCB,vhv_settings); } + if (phase_enable) { Register_BitMask(0xEE,0x80,phase_cal); } + } + + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x01); + Write_Byte(0xFF,0x00); *p_phase_cal = (uint8_t)(phase_calint & 0xEF); - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(uint8_t *p_vhv_settings,const uint8_t get_data_enable, - const uint8_t restore_config) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t sequence_config = 0; +void VL53L0X::Perf_vhv_calibration(uint8_t *p_vhv_settings, + const uint8_t get_data_enable, const uint8_t restore_config) +{ uint8_t orig_sequence_config = 0; uint8_t vhv_settings = 0; uint8_t phase_cal = 0; uint8_t phase_cal_int = 0; /* store the value of the sequence config, - * this will be reset before the end of the function - */ - if (restore_config) {sequence_config = SequenceConfig;} + * this will be reset before the end of the function */ + orig_sequence_config = SequenceConfig; /* Run VHV */ - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0x01); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_perform_single_ref_calibration(0x40); - } + Set_SequenceConfig( 0x01 ); + Perf_single_Ref_calibration(0x40); /* Read VHV from device */ - if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { - status = VL53L0X_ref_calibration_io(1, - vhv_settings,phase_cal,/* Not used here */ - p_vhv_settings,&phase_cal_int, - 1,0); - } else { - *p_vhv_settings = 0; - } - - if ((status == VL53L0X_ERROR_NONE) && restore_config) { - /* restore the previous Sequence Config */ - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, - sequence_config); - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = sequence_config; - } - } - - return status; + if ((ErrState == VL53L0X_OK) && (get_data_enable == 1)) + { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */ + p_vhv_settings,&phase_cal_int, 1,0); } + else { *p_vhv_settings = 0; } + + if (restore_config) { /* restore the previous Sequence Config */ + Set_SequenceConfig( orig_sequence_config ); } // checks for ErrState } -VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable, +void VL53L0X::Perf_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable, const uint8_t restore_config) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t sequence_config = 0; +{ uint8_t orig_sequence_config; uint8_t vhv_settings = 0; uint8_t phase_cal = 0; uint8_t vhv_settingsint; - /* store the value of the sequence config, - * this will be reset before the end of the function - */ - if (restore_config) { sequence_config = SequenceConfig; } - - /* Run PhaseCal */ - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0x02); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_perform_single_ref_calibration(0x0); - } + if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error + + /* store the value of the sequence config, this will be reset before the end of the function */ + orig_sequence_config = SequenceConfig; + + /* Run PhaseCal: */ + Set_SequenceConfig( 0x02 ); // sets REG_SYSTEM_SEQUENCE_CONFIG + Perf_single_Ref_calibration(0x0); /* Read PhaseCal from device */ - if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { - status = VL53L0X_ref_calibration_io(1, - vhv_settings,phase_cal,/* Not used here */ - &vhv_settingsint,p_phase_cal, - 0,1); - } else { - *p_phase_cal = 0; - } - - if ((status == VL53L0X_ERROR_NONE) && restore_config) { - /* restore the previous Sequence Config */ - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, - sequence_config); - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = sequence_config; - } - } - - return status; + if ((ErrState == VL53L0X_OK) && (get_data_enable == 1)) + { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */ + &vhv_settingsint,p_phase_cal, 0,1); } + else { *p_phase_cal = 0; } + + if (restore_config) { /* restore the previous Sequence Config */ + Set_SequenceConfig( orig_sequence_config ); } } -VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t *p_vhv_settings, +void VL53L0X::Perf_Ref_calibration(uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t sequence_config = 0; +{ uint8_t orig_sequence_config; /* store the value of the sequence config, * this will be reset before the end of the function */ - sequence_config = SequenceConfig; + orig_sequence_config = SequenceConfig; /* In the following function we don't save the config to optimize * writes on device. Config is saved and restored only once. */ - status = VL53L0X_perform_vhv_calibration(p_vhv_settings,get_data_enable,0); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_perform_phase_calibration(p_phase_cal,get_data_enable,0); - } - - if (status == VL53L0X_ERROR_NONE) { - /* restore the previous Sequence Config */ - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, - sequence_config); - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = sequence_config; } - } - return status; + Perf_vhv_calibration(p_vhv_settings,get_data_enable,0); + Perf_phase_calibration(p_phase_cal,get_data_enable,0); + + /* restore the previous Sequence Config */ + Set_SequenceConfig( orig_sequence_config ); // sets REG_SYSTEM_SEQUENCE_CONFIG } -void VL53L0X::get_next_good_spad(uint8_t good_spad_array[],uint32_t size, +void VL53L0X::Get_Next_Good_SPAD(uint8_t good_SPAD_array[],uint32_t size, uint32_t curr,int32_t *p_next) { uint32_t start_index; uint32_t fine_offset; - uint32_t c_spads_per_byte = 8; + uint32_t c_SPADS_per_byte = 8; uint32_t coarse_index; uint32_t fine_index; uint8_t data_byte; uint8_t success = 0; - /* Starting with the current good spad,loop through the array to find + /* Starting with the current good SPAD,loop through the array to find * the next. i.e. the next bit set in the sequence. - * * The coarse index is the byte index of the array and the fine index is - * the index of the bit within each byte. - */ - + * the index of the bit within each byte. */ *p_next = -1; - start_index = curr / c_spads_per_byte; - fine_offset = curr % c_spads_per_byte; + start_index = curr / c_SPADS_per_byte; + fine_offset = curr % c_SPADS_per_byte; for (coarse_index = start_index; ((coarse_index < size) && !success); coarse_index++) { fine_index = 0; - data_byte = good_spad_array[coarse_index]; + data_byte = good_SPAD_array[coarse_index]; if (coarse_index == start_index) { /* locate the bit position of the provided current - * spad bit before iterating */ + * SPAD bit before iterating */ data_byte >>= fine_offset; fine_index = fine_offset; } - while (fine_index < c_spads_per_byte) { + while (fine_index < c_SPADS_per_byte) { if ((data_byte & 0x1) == 1) { success = 1; - *p_next = coarse_index * c_spads_per_byte + fine_index; + *p_next = coarse_index * c_SPADS_per_byte + fine_index; break; } data_byte >>= 1; @@ -1773,192 +953,124 @@ } } -uint8_t VL53L0X::is_aperture(uint32_t spad_index) -{ - /* - * This function reports if a given spad index is an aperture SPAD by - * deriving the quadrant. - */ - uint32_t quadrant; - uint8_t is_aperture = 1; - quadrant = spad_index >> 6; - if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) { - is_aperture = 0; - } - return is_aperture; -} - -VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[],uint32_t size, - uint32_t spad_index) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint32_t c_spads_per_byte = 8; +void VL53L0X::Enable_SPAD_bit(uint8_t SPAD_array[],uint32_t size,uint32_t SPAD_index) +{ uint32_t c_SPADS_per_byte = 8; uint32_t coarse_index; uint32_t fine_index; - coarse_index = spad_index / c_spads_per_byte; - fine_index = spad_index % c_spads_per_byte; - if (coarse_index >= size) { - status = VL53L0X_ERROR_REF_SPAD_INIT; - } else { - spad_array[coarse_index] |= (1 << fine_index); - } - - return status; -} - -VL53L0X_Error VL53L0X::set_ref_spad_map(uint8_t *p_ref_spad_array) -{ - VL53L0X_Error status = VL53L0X_write_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, - p_ref_spad_array,6); - return status; -} - -VL53L0X_Error VL53L0X::get_ref_spad_map(uint8_t *p_ref_spad_array) -{ - VL53L0X_Error status = VL53L0X_read_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, - p_ref_spad_array, - 6); - return status; + coarse_index = SPAD_index / c_SPADS_per_byte; + fine_index = SPAD_index % c_SPADS_per_byte; + if (coarse_index >= size) { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; } + else { SPAD_array[coarse_index] |= (1 << fine_index); } } -VL53L0X_Error VL53L0X::enable_ref_spads(uint8_t aperture_spads, - uint8_t good_spad_array[], - uint8_t spad_array[], - uint32_t size, - uint32_t start, - uint32_t offset, - uint32_t spad_count, - uint32_t *p_last_spad) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint32_t index; +void VL53L0X::Enable_Ref_SPADS( uint8_t aperture_SPADS, uint8_t good_SPAD_array[], + uint8_t SPAD_array[], uint32_t size, uint32_t start, uint32_t offset, + uint32_t SPAD_count, uint32_t *p_last_SPAD ) +{ uint32_t index; uint32_t i; - int32_t next_good_spad = offset; - uint32_t current_spad; - uint8_t check_spad_array[6]; - - /* - * This function takes in a spad array which may or may not have SPADS + int32_t next_good_SPAD = offset; + uint32_t current_SPAD; + uint8_t check_SPAD_array[6]; + + /* This function takes in a SPAD array which may or may not have SPADS * already enabled and appends from a given offset a requested number - * of new SPAD enables. The 'good spad map' is applied to - * determine the next SPADs to enable. + * of new SPAD enables. The 'good SPAD map' is applied to + * determine the next SPADS to enable. * - * This function applies to only aperture or only non-aperture spads. + * This function applies to only aperture or only non-aperture SPADS. * Checks are performed to ensure this. */ - current_spad = offset; - for (index = 0; index < spad_count; index++) { - get_next_good_spad(good_spad_array,size,current_spad, - &next_good_spad); - - if (next_good_spad == -1) { - status = VL53L0X_ERROR_REF_SPAD_INIT; - break; - } + current_SPAD = offset; + for (index = 0; index < SPAD_count; index++) { + Get_Next_Good_SPAD(good_SPAD_array,size,current_SPAD, &next_good_SPAD); + + if (next_good_SPAD == -1) + { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; + break; } + /* Confirm that the next good SPAD is non-aperture */ - if (is_aperture(start + next_good_spad) != aperture_spads) { + if (Is_ApertureSPAD(start + next_good_SPAD) != aperture_SPADS) { /* if we can't get the required number of good aperture - * spads from the current quadrant then this is an error - */ - status = VL53L0X_ERROR_REF_SPAD_INIT; - break; - } - current_spad = (uint32_t)next_good_spad; - enable_spad_bit(spad_array,size,current_spad); - current_spad++; + * SPADS from the current quadrant then this is an error */ + ErrState = VL53L0X_ERROR_REF_SPAD_INIT; + break;} + + current_SPAD = (uint32_t)next_good_SPAD; + Enable_SPAD_bit(SPAD_array,size,current_SPAD); + current_SPAD++; } - *p_last_spad = current_spad; - - if (status == VL53L0X_ERROR_NONE) { - status = set_ref_spad_map(spad_array); } - - if (status == VL53L0X_ERROR_NONE) { - status = get_ref_spad_map(check_spad_array); + *p_last_SPAD = current_SPAD; + + if (ErrState == VL53L0X_OK) + { I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPAD_array,6); } // set_Ref_SPAD_map() + + if (ErrState == VL53L0X_OK) { + // Get the ref_SPAD_map from the device + I2c_Read(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,check_SPAD_array,6); + + /* Compare SPAD maps. If not equal report error. */ i = 0; - - /* Compare spad maps. If not equal report error. */ while (i < size) { - if (spad_array[i] != check_spad_array[i]) { - status = VL53L0X_ERROR_REF_SPAD_INIT; + if (SPAD_array[i] != check_SPAD_array[i]) { + ErrState = VL53L0X_ERROR_REF_SPAD_INIT; break; } i++; } } - return status; } -VL53L0X_Error VL53L0X::VL53L0X_set_device_mode(VL53L0X_DeviceModes device_mode) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; +void VL53L0X::Set_device_mode(VL53L0X_DeviceModes device_mode) +{ if (ErrState != VL53L0X_OK) {return; } // no reaction while in Error State!!!! switch (device_mode) { case VL53L0X_DEVICEMODE_SINGLE_RANGING: case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: case VL53L0X_DEVICEMODE_GPIO_DRIVE: - case VL53L0X_DEVICEMODE_GPIO_OSC: - /* Supported modes */ - CurrentParameters.DeviceMode = device_mode; + case VL53L0X_DEVICEMODE_GPIO_OSC: /* Supported modes */ + CurrParams.DeviceMode = device_mode; break; - default: - /* Unsupported mode */ - status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; + default: /* Unsupported mode */ + ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED; } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,FixPoint1616_t threshold_low, - FixPoint1616_t threshold_high) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint16_t threshold16; - +void VL53L0X::Set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 threshold_low, + TFP1616 threshold_high) +{ uint16_t threshold16; /* no dependency on DeviceMode for FlightSense */ /* Need to divide by 2 because the FW will apply a x2 */ threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff); - status = VL53L0X_write_word(VL53L0X_REG_SYSTEM_THRESH_LOW,threshold16); - - if (status == VL53L0X_ERROR_NONE) { - /* Need to divide by 2 because the FW will apply a x2 */ - threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff); - status = VL53L0X_write_word(VL53L0X_REG_SYSTEM_THRESH_HIGH,threshold16); - } - - return status; + Write_Word(REG_SYSTEM_THRESH_LOW,threshold16); + + /* Need to divide by 2 because the FW will apply a x2 */ + threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff); + Write_Word(REG_SYSTEM_THRESH_HIGH,threshold16); } -VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,FixPoint1616_t *p_threshold_low, - FixPoint1616_t *p_threshold_high) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint16_t threshold16; +void VL53L0X::Get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 *p_threshold_low, + TFP1616 *p_threshold_high) +{ uint16_t threshold16; /* no dependency on DeviceMode for FlightSense */ - status = VL53L0X_read_word(VL53L0X_REG_SYSTEM_THRESH_LOW,&threshold16); + threshold16 = Read_Word(REG_SYSTEM_THRESH_LOW); /* Need to multiply by 2 because the FW will apply a x2 */ - *p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(VL53L0X_REG_SYSTEM_THRESH_HIGH, - &threshold16); + *p_threshold_low = (TFP1616)((0x00fff & threshold16) << 17); + + if (ErrState == VL53L0X_OK) { + threshold16 = Read_Word(REG_SYSTEM_THRESH_HIGH); /* Need to multiply by 2 because the FW will apply a x2 */ - *p_threshold_high = - (FixPoint1616_t)((0x00fff & threshold16) << 17); + *p_threshold_high = (TFP1616)((0x00fff & threshold16) << 17); } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(uint8_t *p_tuning_setting_buffer) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - int i; +void VL53L0X::Load_tuning_settings(uint8_t *p_tuning_setting_buffer) +{ int i; int index; uint8_t msb; uint8_t lsb; @@ -1971,7 +1083,7 @@ index = 0; while ((*(p_tuning_setting_buffer + index) != 0) && - (status == VL53L0X_ERROR_NONE)) { + (ErrState == VL53L0X_OK)) { number_of_writes = *(p_tuning_setting_buffer + index); index++; if (number_of_writes == 0xFF) { @@ -2012,263 +1124,172 @@ targetRefRate = temp16; break; default: /* invalid parameter */ - status = VL53L0X_ERROR_INVALID_PARAMS; + ErrState = VL53L0X_ERROR_INVALID_PARAMS; } } else if (number_of_writes <= 4) { address = *(p_tuning_setting_buffer + index); index++; - for (i = 0; i < number_of_writes; i++) { - local_buffer[i] = *(p_tuning_setting_buffer + - index); + local_buffer[i] = *(p_tuning_setting_buffer + index); index++; } - - status = VL53L0X_write_multi(address,local_buffer, - number_of_writes); - + I2c_Write(address,local_buffer,number_of_writes); } else { - status = VL53L0X_ERROR_INVALID_PARAMS; + ErrState = VL53L0X_ERROR_INVALID_PARAMS; } } +} + +void VL53L0X::Check_and_load_interrupt_settings(uint8_t start_not_stopflag) +{ uint8_t interrupt_config; + TFP1616 threshold_low; + TFP1616 threshold_high; - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(uint8_t start_not_stopflag) -{ - uint8_t interrupt_config; - FixPoint1616_t threshold_low; - FixPoint1616_t threshold_high; - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - interrupt_config = DevSpecParams.Pin0GpioFunctionality; - - if ((interrupt_config == - VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) || - (interrupt_config == - VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) || - (interrupt_config == - VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) { - - status = VL53L0X_get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, + if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error + + interrupt_config = DevSpecParams.GpioFunctionality; + + if ((interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_LOW ) || + (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_HIGH) || + (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_OUT )) { + Get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, &threshold_low,&threshold_high); - if (((threshold_low > 255 * 65536) || - (threshold_high > 255 * 65536)) && - (status == VL53L0X_ERROR_NONE)) { - - if (start_not_stopflag != 0) { - status = VL53L0X_load_tuning_settings(InterruptThresholdSettings); - } else { - status |= VL53L0X_write_byte(0xFF,0x04); - status |= VL53L0X_write_byte(0x70,0x00); - status |= VL53L0X_write_byte(0xFF,0x00); - status |= VL53L0X_write_byte(0x80,0x00); - } + if (((threshold_low > 255 * 65536) || (threshold_high > 255 * 65536)) && + (ErrState == VL53L0X_OK)) + { if (start_not_stopflag != 0) + {Load_tuning_settings(InterruptThresholdSettings); } + else + {Write_Byte(0xFF,0x04); + Write_Byte(0x70,0x00); + Write_Byte(0xFF,0x00); + Write_Byte(0x80,0x00); + } } } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_start_measurement() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - VL53L0X_DeviceModes device_mode; +void VL53L0X::Start_Measurement() +{ VL53L0X_DeviceModes device_mode; uint8_t byte; - uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP; + uint8_t start_stop_byte = REG_SYSRANGE_MODE_START_STOP; uint32_t loop_nb; + if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! + /* Get Current DeviceMode */ - VL53L0X_get_device_mode(&device_mode); - - status = VL53L0X_write_byte(0x80,0x01); - status = VL53L0X_write_byte(0xFF,0x01); - status = VL53L0X_write_byte(0x00,0x00); - status = VL53L0X_write_byte(0x91,StopVariable); - status = VL53L0X_write_byte(0x00,0x01); - status = VL53L0X_write_byte(0xFF,0x00); - status = VL53L0X_write_byte(0x80,0x00); + device_mode = CurrParams.DeviceMode; + + Write_Byte(0x80,0x01); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x00); + Write_Byte(0x91,StopVariable); + Write_Byte(0x00,0x01); + Write_Byte(0xFF,0x00); + Write_Byte(0x80,0x00); switch (device_mode) { case VL53L0X_DEVICEMODE_SINGLE_RANGING: - status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,0x01); + Write_Byte(REG_SYSRANGE_START,0x01); byte = start_stop_byte; - if (status == VL53L0X_ERROR_NONE) { + if (ErrState == VL53L0X_OK) { /* Wait until start bit has been cleared */ loop_nb = 0; do { if (loop_nb > 0) - status = VL53L0X_read_byte(VL53L0X_REG_SYSRANGE_START,&byte); + byte = Read_Byte(REG_SYSRANGE_START); loop_nb = loop_nb + 1; } while (((byte & start_stop_byte) == start_stop_byte) - && (status == VL53L0X_ERROR_NONE) + && (ErrState == VL53L0X_OK) && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP)); if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { - status = VL53L0X_ERROR_TIME_OUT; + ErrState = VL53L0X_ERROR_TIME_OUT; } - - } - - break; - case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: - /* Back-to-back mode */ - /* Check if need to apply interrupt settings */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_check_and_load_interrupt_settings(1); - } - - status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START, - VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK); - if (status == VL53L0X_ERROR_NONE) { - /* Set PAL State to Running */ - PalState = VL53L0X_STATE_RUNNING; } break; + + case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: + /* Back-to-back mode, Check if need to apply interrupt settings */ + Check_and_load_interrupt_settings(1); + Write_Byte(REG_SYSRANGE_START,REG_SYSRANGE_MODE_BACKTOBACK); + Set_Current_State( VL53L0X_STATE_RUNNING ); + break; case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: - /* Continuous mode */ - /* Check if need to apply interrupt settings */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_check_and_load_interrupt_settings(1); - } - - status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START, - VL53L0X_REG_SYSRANGE_MODE_TIMED); - - if (status == VL53L0X_ERROR_NONE) { - /* Set PAL State to Running */ - PalState = VL53L0X_STATE_RUNNING; - } + /* Continuous mode; Check if need to apply interrupt settings */ + Check_and_load_interrupt_settings(1); + Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_TIMED); + Set_Current_State( VL53L0X_STATE_RUNNING ); break; default: /* Selected mode not supported */ - status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; + ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED; } - - return status; } -/* Group PAL Measurement Functions */ -VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - VL53L0X_DeviceModes device_mode; +/* Group Device Measurement Functions */ +void VL53L0X::Perf_single_measurement() +{ VL53L0X_DeviceModes device_mode; + + if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! /* Get Current DeviceMode */ - status = VL53L0X_get_device_mode(&device_mode); + device_mode = CurrParams.DeviceMode; /* Start immediately to run a single ranging measurement in case of * single ranging or single histogram */ - if (status == VL53L0X_ERROR_NONE - && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) { - status = VL53L0X_start_measurement(); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_measurement_poll_for_completion(); - } - - /* Change PAL State in case of single ranging or single histogram */ - if (status == VL53L0X_ERROR_NONE - && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) { - PalState = VL53L0X_STATE_IDLE; - } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(uint8_t *p_x_talk_compensation_enable) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t temp8; - - temp8 = CurrentParameters.XTalkCompensationEnable; - *p_x_talk_compensation_enable = temp8; - - return status; + if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {Start_Measurement();} + + Poll_Measure_Completion(); + + /* Change Device State in case of single ranging or single histogram */ + if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) + { Set_Current_State( VL53L0X_STATE_IDLE ); } } -VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, - FixPoint1616_t *p_total_xtalk_rate_mcps) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - uint8_t xtalk_comp_enable; - FixPoint1616_t total_xtalk_mega_cps; - FixPoint1616_t xtalk_per_spad_mega_cps; - - *p_total_xtalk_rate_mcps = 0; - - status = VL53L0X_get_x_talk_compensation_enable(&xtalk_comp_enable); - if (status == VL53L0X_ERROR_NONE) { - - if (xtalk_comp_enable) { - - xtalk_per_spad_mega_cps = CurrentParameters.XTalkCompensationRateMegaCps; - - /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */ - total_xtalk_mega_cps = - p_ranging_measurement_data->EffectiveSpadRtnCount * - xtalk_per_spad_mega_cps; - - /* FixPoint0824 >> 8 = FixPoint1616 */ - *p_total_xtalk_rate_mcps = - (total_xtalk_mega_cps + 0x80) >> 8; - } - } - - return status; +TFP1616 VL53L0X::Get_total_xtalk_rate(TRangeResults *p_ranging_results) +{ TFP1616 total_xtalk_MHz; + + // CurrParams.XTalk_Compens_En was Get_xtalk_compensation_enable + if ( (ErrState == VL53L0X_OK) & (CurrParams.XTalk_Compens_En ) ) + { /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */ + total_xtalk_MHz = p_ranging_results->EffectiveSPADRtnCount * + CurrParams.Xtalk_CompRate_MHz; + + /* FixPoint0824 >> 8 = FixPoint1616 */ + return (total_xtalk_MHz + 0x80) >> 8; + } + else { return 0; } } -VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, - FixPoint1616_t *p_total_signal_rate_mcps) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - FixPoint1616_t total_xtalk_mega_cps; +void VL53L0X::Get_total_SIG_rate(TRangeResults *p_ranging_results, + TFP1616 *p_total_SIG_rate_mcps) +{ TFP1616 total_xtalk_MHz; + + *p_total_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz; + total_xtalk_MHz = Get_total_xtalk_rate(p_ranging_results); - *p_total_signal_rate_mcps = - p_ranging_measurement_data->SignalRateRtnMegaCps; - - status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data,&total_xtalk_mega_cps); - - if (status == VL53L0X_ERROR_NONE) { - *p_total_signal_rate_mcps += total_xtalk_mega_cps; - } - - return status; + if (ErrState == VL53L0X_OK) { *p_total_SIG_rate_mcps += total_xtalk_MHz;} } /* To convert ms into register value */ -uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(uint32_t timeout_period_us, +uint32_t VL53L0X::Calc_timeout_mclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) -{ - uint32_t macro_period_ps; - uint32_t macro_period_ns; - uint32_t timeout_period_mclks = 0; - - macro_period_ps = VL53L0X_calc_macro_period_ps(vcsel_period_pclks); - macro_period_ns = (macro_period_ps + 500) / 1000; - - timeout_period_mclks = - (uint32_t)(((timeout_period_us * 1000) - + (macro_period_ns / 2)) / macro_period_ns); - - return timeout_period_mclks; +{ uint32_t macro_period_ns; + + macro_period_ns = (uint32_t)(vcsel_period_pclks) * VL53L0X_MACRO_PERIOD_NS; + + return (uint32_t)(((timeout_period_us * 1000) + + (macro_period_ns / 2)) / macro_period_ns); } -uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num) -{ - /* Implements an integer square root - * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots - */ - +uint32_t VL53L0X::ISQRT(uint32_t num) +{ /* Implements an integer square root + * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */ uint32_t res = 0; uint32_t bit = 1 << 30; - /* The second-to-top bit is set: - * 1 << 14 for 16-bits,1 << 30 for 32 bits */ + /* The second-to-top bit is set: 1 << 14 for 16-bits,1 << 30 for 32 bits */ /* "bit" starts at the highest power of four <= the argument. */ while (bit > num) { bit >>= 2; } @@ -2277,116 +1298,102 @@ if (num >= res + bit) { num -= res + bit; res = (res >> 1) + bit; - } else { - res >>= 1; - } + } else { res >>= 1; } bit >>= 2; } - return res; } -VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(FixPoint1616_t total_signal_rate_mcps, - FixPoint1616_t total_corr_signal_rate_mcps, - FixPoint1616_t pw_mult, +void VL53L0X::Calc_dmax(TFP1616 total_SIG_rate_mcps, + TFP1616 total_corr_SIG_rate_mcps, + TFP1616 pw_mult, uint32_t sigma_estimate_p1, - FixPoint1616_t sigma_estimate_p2, + TFP1616 sigma_estimate_p2, uint32_t peak_vcsel_duration_us, uint32_t *pd_max_mm) -{ - const uint32_t c_sigma_limit = 18; - const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */ - const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */ +{ const uint32_t c_sigma_limit = 18; + const TFP1616 c_SIG_limit = 0x4000; /* 0.25 */ + const TFP1616 c_sigma_est_Ref = 0x00000042; /* 0.001 */ const uint32_t c_amb_eff_width_sigma_est_ns = 6; const uint32_t c_amb_eff_width_d_max_ns = 7; uint32_t dmax_cal_range_mm; - FixPoint1616_t dmax_cal_signal_rate_rtn_mcps; - FixPoint1616_t min_signal_needed; - FixPoint1616_t min_signal_needed_p1; - FixPoint1616_t min_signal_needed_p2; - FixPoint1616_t min_signal_needed_p3; - FixPoint1616_t min_signal_needed_p4; - FixPoint1616_t sigma_limit_tmp; - FixPoint1616_t sigma_est_sq_tmp; - FixPoint1616_t signal_limit_tmp; - FixPoint1616_t signal_at0_mm; - FixPoint1616_t dmax_dark; - FixPoint1616_t dmax_ambient; - FixPoint1616_t dmax_dark_tmp; - FixPoint1616_t sigma_est_p2_tmp; + TFP1616 dmax_cal_SIG_rate_rtn_mcps; + TFP1616 min_SIG_needed; + TFP1616 min_SIG_needed_p1; + TFP1616 min_SIG_needed_p2; + TFP1616 min_SIG_needed_p3; + TFP1616 min_SIG_needed_p4; + TFP1616 sigma_limit_tmp; + TFP1616 sigma_est_sq_tmp; + TFP1616 signal_limit_tmp; + TFP1616 signal_at0_mm; + TFP1616 dmax_dark; + TFP1616 dmax_ambient; + TFP1616 dmax_dark_tmp; + TFP1616 sigma_est_p2_tmp; uint32_t signal_rate_temp_mcps; - VL53L0X_Error status = VL53L0X_ERROR_NONE; - dmax_cal_range_mm = DmaxCalRangeMilliMeter; - dmax_cal_signal_rate_rtn_mcps = DmaxCalSignalRateRtnMegaCps; + dmax_cal_SIG_rate_rtn_mcps = DmaxCalSignalRateRtnMHz; /* uint32 * FixPoint1616 = FixPoint1616 */ - signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps; + signal_at0_mm = dmax_cal_range_mm * dmax_cal_SIG_rate_rtn_mcps; /* FixPoint1616 >> 8 = FixPoint2408 */ signal_at0_mm = (signal_at0_mm + 0x80) >> 8; signal_at0_mm *= dmax_cal_range_mm; - min_signal_needed_p1 = 0; - if (total_corr_signal_rate_mcps > 0) { - - /* Shift by 10 bits to increase resolution prior to the - * division */ - signal_rate_temp_mcps = total_signal_rate_mcps << 10; + min_SIG_needed_p1 = 0; + if (total_corr_SIG_rate_mcps > 0) { + /* Shift by 10 bits to increase resolution prior to the division */ + signal_rate_temp_mcps = total_SIG_rate_mcps << 10; /* Add rounding value prior to division */ - min_signal_needed_p1 = signal_rate_temp_mcps + - (total_corr_signal_rate_mcps / 2); + min_SIG_needed_p1 = signal_rate_temp_mcps + (total_corr_SIG_rate_mcps / 2); /* FixPoint0626/FixPoint1616 = FixPoint2210 */ - min_signal_needed_p1 /= total_corr_signal_rate_mcps; + min_SIG_needed_p1 /= total_corr_SIG_rate_mcps; /* Apply a factored version of the speed of light. Correction to be applied at the end */ - min_signal_needed_p1 *= 3; + min_SIG_needed_p1 *= 3; /* FixPoint2210 * FixPoint2210 = FixPoint1220 */ - min_signal_needed_p1 *= min_signal_needed_p1; + min_SIG_needed_p1 *= min_SIG_needed_p1; /* FixPoint1220 >> 16 = FixPoint2804 */ - min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16; + min_SIG_needed_p1 = (min_SIG_needed_p1 + 0x8000) >> 16; } - min_signal_needed_p2 = pw_mult * sigma_estimate_p1; + min_SIG_needed_p2 = pw_mult * sigma_estimate_p1; /* FixPoint1616 >> 16 = uint32 */ - min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16; + min_SIG_needed_p2 = (min_SIG_needed_p2 + 0x8000) >> 16; /* uint32 * uint32 = uint32 */ - min_signal_needed_p2 *= min_signal_needed_p2; - - /* Check sigmaEstimateP2 - * If this value is too high there is not enough signal rate - * to calculate dmax value so set a suitable value to ensure - * a very small dmax. - */ + min_SIG_needed_p2 *= min_SIG_needed_p2; + + /* Check sigmaEstimateP2; If this value is too high, there is not enough + * signal rate to calculate dmax value so set a suitable value to ensure + * a very small dmax. */ sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16; sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) / c_amb_eff_width_sigma_est_ns; sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns; if (sigma_est_p2_tmp > 0xffff) { - min_signal_needed_p3 = 0xfff00000; + min_SIG_needed_p3 = 0xfff00000; } else { - /* DMAX uses a different ambient width from sigma,so apply - * correction. - * Perform division before multiplication to prevent overflow. - */ + /* DMAX uses a different ambient width from sigma,so apply correction. + * Perform division before multiplication to prevent overflow. */ sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) / c_amb_eff_width_sigma_est_ns; sigma_estimate_p2 *= c_amb_eff_width_d_max_ns; /* FixPoint1616 >> 16 = uint32 */ - min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16; - - min_signal_needed_p3 *= min_signal_needed_p3; + min_SIG_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16; + min_SIG_needed_p3 *= min_SIG_needed_p3; } /* FixPoint1814 / uint32 = FixPoint1814 */ @@ -2396,7 +1403,7 @@ sigma_limit_tmp *= sigma_limit_tmp; /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ - sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref; + sigma_est_sq_tmp = c_sigma_est_Ref * c_sigma_est_Ref; /* FixPoint3232 >> 4 = FixPoint0428 */ sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4; @@ -2405,118 +1412,106 @@ sigma_limit_tmp -= sigma_est_sq_tmp; /* uint32_t * FixPoint0428 = FixPoint0428 */ - min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp; + min_SIG_needed_p4 = 4 * 12 * sigma_limit_tmp; /* FixPoint0428 >> 14 = FixPoint1814 */ - min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14; + min_SIG_needed_p4 = (min_SIG_needed_p4 + 0x2000) >> 14; /* uint32 + uint32 = uint32 */ - min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3); + min_SIG_needed = (min_SIG_needed_p2 + min_SIG_needed_p3); /* uint32 / uint32 = uint32 */ - min_signal_needed += (peak_vcsel_duration_us / 2); - min_signal_needed /= peak_vcsel_duration_us; + min_SIG_needed += (peak_vcsel_duration_us / 2); + min_SIG_needed /= peak_vcsel_duration_us; /* uint32 << 14 = FixPoint1814 */ - min_signal_needed <<= 14; + min_SIG_needed <<= 14; /* FixPoint1814 / FixPoint1814 = uint32 */ - min_signal_needed += (min_signal_needed_p4 / 2); - min_signal_needed /= min_signal_needed_p4; + min_SIG_needed += (min_SIG_needed_p4 / 2); + min_SIG_needed /= min_SIG_needed_p4; /* FixPoint3200 * FixPoint2804 := FixPoint2804*/ - min_signal_needed *= min_signal_needed_p1; + min_SIG_needed *= min_SIG_needed_p1; /* Apply correction by dividing by 1000000. - * This assumes 10E16 on the numerator of the equation - * and 10E-22 on the denominator. + * This assumes 10E16 on the numerator of the equation and 10E-22 on the denominator. * We do this because 32bit fix point calculation can't * handle the larger and smaller elements of this equation, * i.e. speed of light and pulse widths. */ - min_signal_needed = (min_signal_needed + 500) / 1000; - min_signal_needed <<= 4; - - min_signal_needed = (min_signal_needed + 500) / 1000; + min_SIG_needed = (min_SIG_needed + 500) / 1000; + min_SIG_needed <<= 4; + + min_SIG_needed = (min_SIG_needed + 500) / 1000; /* FixPoint1616 >> 8 = FixPoint2408 */ - signal_limit_tmp = (c_signal_limit + 0x80) >> 8; + signal_limit_tmp = (c_SIG_limit + 0x80) >> 8; /* FixPoint2408/FixPoint2408 = uint32 */ if (signal_limit_tmp != 0) { dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2)) / signal_limit_tmp; - } else { - dmax_dark_tmp = 0; - } - - dmax_dark = VL53L0X_isqrt(dmax_dark_tmp); + } else { dmax_dark_tmp = 0; } + + dmax_dark = ISQRT(dmax_dark_tmp); /* FixPoint2408/FixPoint2408 = uint32 */ - if (min_signal_needed != 0) { - dmax_ambient = (signal_at0_mm + min_signal_needed / 2) - / min_signal_needed; - } else { - dmax_ambient = 0; - } - - dmax_ambient = VL53L0X_isqrt(dmax_ambient); + if (min_SIG_needed != 0) + { dmax_ambient = (signal_at0_mm + min_SIG_needed / 2) / min_SIG_needed; } + else { dmax_ambient = 0; } + + dmax_ambient = ISQRT(dmax_ambient); *pd_max_mm = dmax_dark; - if (dmax_dark > dmax_ambient) { - *pd_max_mm = dmax_ambient; - } - - return status; + if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; } } -VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, - FixPoint1616_t *p_sigma_estimate, - uint32_t *p_dmax_mm) -{ - /* Expressed in 100ths of a ns,i.e. centi-ns */ +void VL53L0X::Calc_sigma_estimate(TRangeResults *p_ranging_results, + TFP1616 *p_sigma_estimate, uint32_t *p_dmax_mm) +{ /* Expressed in 100ths of a ns,i.e. centi-ns */ const uint32_t c_pulse_effective_width_centi_ns = 800; /* Expressed in 100ths of a ns,i.e. centi-ns */ const uint32_t c_ambient_effective_width_centi_ns = 600; - const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */ + const TFP1616 c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */ const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */ - const FixPoint1616_t c_sigma_est_max = 0x028F87AE; - const FixPoint1616_t c_sigma_est_rtn_max = 0xF000; - const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 / + const TFP1616 c_sigma_est_max = 0x028F87AE; + const TFP1616 c_sigma_est_rtn_max = 0xF000; + const TFP1616 c_amb_to_SIG_ratio_max = 0xF0000000 / c_ambient_effective_width_centi_ns; /* Time Of Flight per mm (6.6 pico secs) */ - const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A; + const TFP1616 c_tof_per_mm_ps = 0x0006999A; const uint32_t c_16bit_rounding_param = 0x00008000; - const FixPoint1616_t c_max_x_talk_kcps = 0x00320000; + const TFP1616 c_max_xtalk_kcps = 0x00320000; const uint32_t c_pll_period_ps = 1655; uint32_t vcsel_total_events_rtn; uint32_t final_range_timeout_micro_secs; uint32_t pre_range_timeout_micro_secs; uint32_t final_range_integration_time_milli_secs; - FixPoint1616_t sigma_estimate_p1; - FixPoint1616_t sigma_estimate_p2; - FixPoint1616_t sigma_estimate_p3; - FixPoint1616_t delta_t_ps; - FixPoint1616_t pw_mult; - FixPoint1616_t sigma_est_rtn; - FixPoint1616_t sigma_estimate; - FixPoint1616_t x_talk_correction; - FixPoint1616_t ambient_rate_kcps; - FixPoint1616_t peak_signal_rate_kcps; - FixPoint1616_t x_talk_comp_rate_mcps; - uint32_t x_talk_comp_rate_kcps; - VL53L0X_Error status = VL53L0X_ERROR_NONE; - FixPoint1616_t diff1_mcps; - FixPoint1616_t diff2_mcps; - FixPoint1616_t sqr1; - FixPoint1616_t sqr2; - FixPoint1616_t sqr_sum; - FixPoint1616_t sqrt_result_centi_ns; - FixPoint1616_t sqrt_result; - FixPoint1616_t total_signal_rate_mcps; - FixPoint1616_t corrected_signal_rate_mcps; - FixPoint1616_t sigma_est_ref; + TFP1616 sigma_estimate_p1; + TFP1616 sigma_estimate_p2; + TFP1616 sigma_estimate_p3; + TFP1616 delta_t_ps; + TFP1616 pw_mult; + TFP1616 sigma_est_rtn; + TFP1616 sigma_estimate; + TFP1616 xtalk_correction; + TFP1616 ambient_rate_kcps; + TFP1616 peak_SIG_rate_kcps; + TFP1616 xtalk_comp_rate_mcps; + uint32_t xtalk_comp_rate_kcps; + + TFP1616 diff1_mcps; + TFP1616 diff2_mcps; + TFP1616 sqr1; + TFP1616 sqr2; + TFP1616 sqr_sum; + TFP1616 sqrt_result_centi_ns; + TFP1616 sqrt_result; + TFP1616 total_SIG_rate_mcps; + TFP1616 corrected_SIG_rate_mcps; + TFP1616 sigma_est_Ref; uint32_t vcsel_width; uint32_t final_range_macro_pclks; uint32_t pre_range_macro_pclks; @@ -2525,51 +1520,41 @@ uint8_t pre_range_vcsel_pclks; /*! \addtogroup calc_sigma_estimate * @{ - * Estimates the range sigma - */ - - x_talk_comp_rate_mcps = CurrentParameters.XTalkCompensationRateMegaCps; + * Estimates the range sigma */ + + xtalk_comp_rate_mcps = CurrParams.Xtalk_CompRate_MHz; /* We work in kcps rather than mcps as this helps keep within the - * confines of the 32 Fix1616 type. - */ - ambient_rate_kcps = - (p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16; - - corrected_signal_rate_mcps = - p_ranging_measurement_data->SignalRateRtnMegaCps; - - status = VL53L0X_get_total_signal_rate(p_ranging_measurement_data,&total_signal_rate_mcps); - status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data,&x_talk_comp_rate_mcps); + * confines of the 32 Fix1616 type. */ + ambient_rate_kcps = (p_ranging_results->AmbientRateRtnMHz * 1000) >> 16; + + corrected_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz; + + Get_total_SIG_rate(p_ranging_results,&total_SIG_rate_mcps); + xtalk_comp_rate_mcps = Get_total_xtalk_rate(p_ranging_results); /* Signal rate measurement provided by device is the - * peak signal rate,not average. - */ - peak_signal_rate_kcps = (total_signal_rate_mcps * 1000); - peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16; - - x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000; - - if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) { - x_talk_comp_rate_kcps = c_max_x_talk_kcps; - } - - if (status == VL53L0X_ERROR_NONE) { + * peak signal rate,not average. */ + peak_SIG_rate_kcps = (total_SIG_rate_mcps * 1000); + peak_SIG_rate_kcps = (peak_SIG_rate_kcps + 0x8000) >> 16; + + xtalk_comp_rate_kcps = xtalk_comp_rate_mcps * 1000; + + if (xtalk_comp_rate_kcps > c_max_xtalk_kcps) + { xtalk_comp_rate_kcps = c_max_xtalk_kcps; } + + if (ErrState == VL53L0X_OK) { /* Calculate final range macro periods */ final_range_timeout_micro_secs = DevSpecParams.FinalRangeTimeoutMicroSecs; - - final_range_vcsel_pclks = DevSpecParams.FinalRangeVcselPulsePeriod; - - final_range_macro_pclks = VL53L0X_calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks); + final_range_vcsel_pclks = DevSpecParams.FinalRangeVcselPPeriod; + final_range_macro_pclks = Calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks); /* Calculate pre-range macro periods */ pre_range_timeout_micro_secs = DevSpecParams.PreRangeTimeoutMicroSecs; - pre_range_vcsel_pclks = DevSpecParams.PreRangeVcselPulsePeriod; - pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks); + pre_range_vcsel_pclks = DevSpecParams.PreRangeVcselPPeriod; + pre_range_macro_pclks = Calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks); vcsel_width = 3; - if (final_range_vcsel_pclks == 8) { - vcsel_width = 2; - } + if (final_range_vcsel_pclks == 8) { vcsel_width = 2; } peak_vcsel_duration_us = vcsel_width * 2048 * (pre_range_macro_pclks + final_range_macro_pclks); @@ -2578,82 +1563,70 @@ peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; /* Fix1616 >> 8 = Fix2408 */ - total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8; + total_SIG_rate_mcps = (total_SIG_rate_mcps + 0x80) >> 8; /* Fix2408 * uint32 = Fix2408 */ - vcsel_total_events_rtn = total_signal_rate_mcps * - peak_vcsel_duration_us; + vcsel_total_events_rtn = total_SIG_rate_mcps * peak_vcsel_duration_us; /* Fix2408 >> 8 = uint32 */ vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8; /* Fix2408 << 8 = Fix1616 = */ - total_signal_rate_mcps <<= 8; + total_SIG_rate_mcps <<= 8; } - if (status != VL53L0X_ERROR_NONE) { return status; } - - if (peak_signal_rate_kcps == 0) { + if (ErrState != VL53L0X_OK) { return ; } + + if (peak_SIG_rate_kcps == 0) { *p_sigma_estimate = c_sigma_est_max; SigmaEstimate = c_sigma_est_max; *p_dmax_mm = 0; } else { - if (vcsel_total_events_rtn < 1) { - vcsel_total_events_rtn = 1; - } + if (vcsel_total_events_rtn < 1) {vcsel_total_events_rtn = 1; } sigma_estimate_p1 = c_pulse_effective_width_centi_ns; /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */ - sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps; - if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) { - /* Clip to prevent overflow. Will ensure safe - * max result. */ - sigma_estimate_p2 = c_amb_to_signal_ratio_max; - } + sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_SIG_rate_kcps; + if (sigma_estimate_p2 > c_amb_to_SIG_ratio_max) + /* Clip to prevent overflow. Will ensure safe max result. */ + { sigma_estimate_p2 = c_amb_to_SIG_ratio_max; } sigma_estimate_p2 *= c_ambient_effective_width_centi_ns; - sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12); + sigma_estimate_p3 = 2 * ISQRT(vcsel_total_events_rtn * 12); /* uint32 * FixPoint1616 = FixPoint1616 */ - delta_t_ps = p_ranging_measurement_data->RangeMilliMeter * - c_tof_per_mm_ps; - - /* - * vcselRate - xtalkCompRate + delta_t_ps = p_ranging_results->RangeMilliMeter * c_tof_per_mm_ps; + + /* vcselRate - xtalkCompRate * (uint32 << 16) - FixPoint1616 = FixPoint1616. * Divide result by 1000 to convert to mcps. - * 500 is added to ensure rounding when integer division - * truncates. - */ - diff1_mcps = (((peak_signal_rate_kcps << 16) - - 2 * x_talk_comp_rate_kcps) + 500) / 1000; + * 500 is added to ensure rounding when integer division truncates. */ + diff1_mcps = (((peak_SIG_rate_kcps << 16) - 2 * xtalk_comp_rate_kcps) + 500) / 1000; /* vcselRate + xtalkCompRate */ - diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000; - - /* Shift by 8 bits to increase resolution prior to the - * division */ + diff2_mcps = ((peak_SIG_rate_kcps << 16) + 500) / 1000; + + /* Shift by 8 bits to increase resolution prior to the division */ diff1_mcps <<= 8; /* FixPoint0824/FixPoint1616 = FixPoint2408 */ // xTalkCorrection = abs(diff1_mcps/diff2_mcps); -// abs is causing compiler overloading isue in C++,but unsigned types. So,redundant call anyway! - x_talk_correction = diff1_mcps / diff2_mcps; +// abs is causing compiler overloading isue in C++, but unsigned types. So,redundant call anyway! + xtalk_correction = diff1_mcps / diff2_mcps; /* FixPoint2408 << 8 = FixPoint1616 */ - x_talk_correction <<= 8; - - if (p_ranging_measurement_data->RangeStatus != 0) { - pw_mult = 1 << 16; - } else { + xtalk_correction <<= 8; + + if (p_ranging_results->RangeStatus != 0) + { pw_mult = 1 << 16; } + else { /* FixPoint1616/uint32 = FixPoint1616 */ pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */ /* FixPoint1616 * FixPoint1616 = FixPoint3232,however both * values are small enough such that32 bits will not be - * exceeded. - */ - pw_mult *= ((1 << 16) - x_talk_correction); + * exceeded. */ + pw_mult *= ((1 << 16) - xtalk_correction); /* (FixPoint3232 >> 16) = FixPoint1616 */ pw_mult = (pw_mult + c_16bit_rounding_param) >> 16; @@ -2663,8 +1636,7 @@ /* At this point the value will be 1.xx,therefore if we square * the value this will exceed 32 bits. To address this perform - * a single shift to the right before the multiplication. - */ + * a single shift to the right before the multiplication. */ pw_mult >>= 1; /* FixPoint1715 * FixPoint1715 = FixPoint3430 */ pw_mult = pw_mult * pw_mult; @@ -2681,7 +1653,6 @@ /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ sqr1 *= sqr1; - sqr2 = sigma_estimate_p2; /* (FixPoint1616 >> 16) = FixPoint3200 */ @@ -2694,349 +1665,254 @@ sqr_sum = sqr1 + sqr2; /* SQRT(FixPoin6400) = FixPoint3200 */ - sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum); + sqrt_result_centi_ns = ISQRT(sqr_sum); /* (FixPoint3200 << 16) = FixPoint1616 */ sqrt_result_centi_ns <<= 16; - /* - * Note that the Speed Of Light is expressed in um per 1E-10 - * seconds (2997) Therefore to get mm/ns we have to divide by - * 10000 - */ + /* Note that the Speed Of Light is expressed in um per 1E-10 + * seconds (2997) Therefore to get mm/ns we have to divide by 10000 */ sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) / sigma_estimate_p3); sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR; /* Add 5000 before dividing by 10000 to ensure rounding. */ - sigma_est_rtn += 5000; - sigma_est_rtn /= 10000; - - if (sigma_est_rtn > c_sigma_est_rtn_max) { - /* Clip to prevent overflow. Will ensure safe - * max result. */ - sigma_est_rtn = c_sigma_est_rtn_max; - } + sigma_est_rtn = (sigma_est_rtn + 5000) / 10000; + + if (sigma_est_rtn > c_sigma_est_rtn_max) + /* Clip to prevent overflow. Will ensure safe max result. */ + { sigma_est_rtn = c_sigma_est_rtn_max; } + final_range_integration_time_milli_secs = (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000; /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range) - * sqrt(FixPoint1616/int) = FixPoint2408) - */ - sigma_est_ref = - VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs + + * sqrt(FixPoint1616/int) = FixPoint2408) */ + sigma_est_Ref = + ISQRT((c_dflt_final_range_integration_time_milli_secs + final_range_integration_time_milli_secs / 2) / final_range_integration_time_milli_secs); /* FixPoint2408 << 8 = FixPoint1616 */ - sigma_est_ref <<= 8; - sigma_est_ref = (sigma_est_ref + 500) / 1000; + sigma_est_Ref <<= 8; + sigma_est_Ref = (sigma_est_Ref + 500) / 1000; /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ sqr1 = sigma_est_rtn * sigma_est_rtn; /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ - sqr2 = sigma_est_ref * sigma_est_ref; + sqr2 = sigma_est_Ref * sigma_est_Ref; /* sqrt(FixPoint3232) = FixPoint1616 */ - sqrt_result = VL53L0X_isqrt((sqr1 + sqr2)); - /* - * Note that the Shift by 4 bits increases resolution prior to + sqrt_result = ISQRT((sqr1 + sqr2)); + /* Note that the Shift by 4 bits increases resolution prior to * the sqrt,therefore the result must be shifted by 2 bits to - * the right to revert back to the FixPoint1616 format. - */ - + * the right to revert back to the FixPoint1616 format. */ sigma_estimate = 1000 * sqrt_result; - if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) || + if ((peak_SIG_rate_kcps < 1) || (vcsel_total_events_rtn < 1) || (sigma_estimate > c_sigma_est_max)) { - sigma_estimate = c_sigma_est_max; - } + sigma_estimate = c_sigma_est_max; } *p_sigma_estimate = (uint32_t)(sigma_estimate); SigmaEstimate = *p_sigma_estimate; - status = VL53L0X_calc_dmax(total_signal_rate_mcps, - corrected_signal_rate_mcps, + Calc_dmax(total_SIG_rate_mcps, + corrected_SIG_rate_mcps, pw_mult, sigma_estimate_p1, sigma_estimate_p2, peak_vcsel_duration_us, p_dmax_mm); } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(uint8_t device_range_status, - FixPoint1616_t signal_rate, - uint16_t effective_spad_rtn_count, - VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, - uint8_t *p_pal_range_status) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t none_flag; +void VL53L0X::Get_Device_range_status(uint8_t device_range_status, + TFP1616 signal_rate, + uint16_t effective_SPAD_rtn_count, + TRangeResults *p_ranging_results, + uint8_t *p_Device_range_status) +{ uint8_t none_flag; uint8_t sigma_limitflag = 0; - uint8_t signal_ref_clipflag = 0; + uint8_t signal_Ref_clipflag = 0; uint8_t range_ignore_thresholdflag = 0; - uint8_t sigma_limit_check_enable = 0; - uint8_t signal_rate_final_range_limit_check_enable = 0; - uint8_t signal_ref_clip_limit_check_enable = 0; - uint8_t range_ignore_threshold_limit_check_enable = 0; - FixPoint1616_t sigma_estimate; - FixPoint1616_t sigma_limit_value; - FixPoint1616_t signal_ref_clip_value; - FixPoint1616_t range_ignore_threshold_value; - FixPoint1616_t signal_rate_per_spad; + uint8_t sigma_limit_chk_en = 0; + uint8_t signal_rate_final_range_limit_chk_en = 0; + uint8_t signal_Ref_clip_limit_chk_en = 0; + uint8_t range_ignore_threshold_chk_en = 0; + TFP1616 sigma_estimate; + TFP1616 sigma_limit_value; + TFP1616 signal_Ref_clip_value; + TFP1616 range_ignore_threshold; + TFP1616 signal_rate_per_SPAD; uint8_t device_range_status_internal = 0; - uint16_t tmp_word = 0; uint8_t temp8; uint32_t dmax_mm = 0; - FixPoint1616_t last_signal_ref_mcps; - - /* - * VL53L0X has a good ranging when the value of the + + /* VL53L0X has a good ranging when the value of the * DeviceRangeStatus = 11. This function will replace the value 0 with * the value 11 in the DeviceRangeStatus. * In addition,the SigmaEstimator is not included in the VL53L0X - * DeviceRangeStatus,this will be added in the PalRangeStatus. - */ + * DeviceRangeStatus,this will be added in the DeviceRangeStatus. */ device_range_status_internal = ((device_range_status & 0x78) >> 3); - if ( device_range_status_internal == 0 || - device_range_status_internal == 5 || - device_range_status_internal == 7 || - device_range_status_internal == 12 || - device_range_status_internal == 13 || - device_range_status_internal == 14 || - device_range_status_internal == 15 - ) { - none_flag = 1; - } else { none_flag = 0; } - - /* - * Check if Sigma limit is enabled,if yes then do comparison with limit - * value and put the result back into pPalRangeStatus. - */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, - &sigma_limit_check_enable); - } - - if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) { - /* - * compute the Sigma and check with limit - */ - status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data, - &sigma_estimate, - &dmax_mm); - if (status == VL53L0X_ERROR_NONE) { - p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm; - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, - &sigma_limit_value); - - if ((sigma_limit_value > 0) && - (sigma_estimate > sigma_limit_value)) { - /* Limit Fail */ - sigma_limitflag = 1; - } - } - } - - /* - * Check if Signal ref clip limit is enabled,if yes then do comparison - * with limit value and put the result back into pPalRangeStatus. - */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, - &signal_ref_clip_limit_check_enable); - } - - if ((signal_ref_clip_limit_check_enable != 0) && - (status == VL53L0X_ERROR_NONE)) { - - status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, - &signal_ref_clip_value); + if ( device_range_status_internal == 0 || + device_range_status_internal == 5 || + device_range_status_internal == 7 || + device_range_status_internal == 12 || + device_range_status_internal == 13 || + device_range_status_internal == 14 || + device_range_status_internal == 15 ) + { none_flag = 1; } + else { none_flag = 0; } + + /* Check if Sigma limit is enabled,if yes then do comparison with limit + * value and put the result back into pDeviceRangeStatus. */ + if (ErrState == VL53L0X_OK) + { sigma_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); } + + if ((sigma_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) { + /* compute the Sigma and check with limit */ + Calc_sigma_estimate(p_ranging_results, &sigma_estimate, &dmax_mm); + if (ErrState == VL53L0X_OK) + { p_ranging_results->RangeDMaxMilliMeter = dmax_mm; } + + if (ErrState == VL53L0X_OK) + { sigma_limit_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); + + if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value)) + { sigma_limitflag = 1; }/* Limit Fail */ + } + } + + /* Check if Signal ref clip limit is enabled,if yes then do comparison + * with limit value and put the result back into pDeviceRangeStatus. */ + if (ErrState == VL53L0X_OK) + {signal_Ref_clip_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_REF_CLIP);} + + if ((signal_Ref_clip_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) + { signal_Ref_clip_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIG_REF_CLIP); /* Read LastSignalRefMcps from device */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x01); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, - &tmp_word); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x00); - } - - last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word); - LastSignalRefMcps = last_signal_ref_mcps; - - if ((signal_ref_clip_value > 0) && - (last_signal_ref_mcps > signal_ref_clip_value)) { - /* Limit Fail */ - signal_ref_clipflag = 1; - } - } - - /* - * Check if Signal ref clip limit is enabled,if yes then do comparison - * with limit value and put the result back into pPalRangeStatus. - * EffectiveSpadRtnCount has a format 8.8 - * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL - */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, - &range_ignore_threshold_limit_check_enable); - } - - if ((range_ignore_threshold_limit_check_enable != 0) && - (status == VL53L0X_ERROR_NONE)) { - - /* Compute the signal rate per spad */ - if (effective_spad_rtn_count == 0) { - signal_rate_per_spad = 0; - } else { - signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate) - / effective_spad_rtn_count); - } - - status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, - &range_ignore_threshold_value); - - if ((range_ignore_threshold_value > 0) && - (signal_rate_per_spad < range_ignore_threshold_value)) { - /* Limit Fail add 2^6 to range status */ + Write_Byte(0xFF,0x01); + LastSignalRefMcps = FP97_TO_FP1616( Read_Word(REG_RESULT_PEAK_SIG_RATE_REF)); + Write_Byte(0xFF,0x00); + + if ((signal_Ref_clip_value > 0) && (LastSignalRefMcps > signal_Ref_clip_value)) \ + { signal_Ref_clipflag = 1; /* Limit Fail */ } + } + + /* Check if Signal ref clip limit is enabled,if yes then do comparison + * with limit value and put the result back into pDeviceRangeStatus. + * EffectiveSPADRtnCount has a format 8.8 + * If (Return signal rate < (1.5 x Xtalk x number of SPADS)) : FAIL */ + if (ErrState == VL53L0X_OK) + { range_ignore_threshold_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); } + + if ((range_ignore_threshold_chk_en != 0) && (ErrState == VL53L0X_OK)) + {/* Compute the signal rate per SPAD */ + if (effective_SPAD_rtn_count == 0) { signal_rate_per_SPAD = 0; } + else { signal_rate_per_SPAD = + (TFP1616)((256 * signal_rate) / effective_SPAD_rtn_count); } + + range_ignore_threshold=Get_limit_chk_val(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); + + if ((range_ignore_threshold > 0) && (signal_rate_per_SPAD < range_ignore_threshold)) { + /* Limit Fail add 2^6 to range ErrState */ range_ignore_thresholdflag = 1; } } - if (status == VL53L0X_ERROR_NONE) { + if (ErrState == VL53L0X_OK) { if (none_flag == 1) { - *p_pal_range_status = 255; /* NONE */ + *p_Device_range_status = 255; /* NONE */ } else if (device_range_status_internal == 1 || device_range_status_internal == 2 || device_range_status_internal == 3) { - *p_pal_range_status = 5; /* HW fail */ + *p_Device_range_status = 5; /* HW fail */ } else if (device_range_status_internal == 6 || device_range_status_internal == 9) { - *p_pal_range_status = 4; /* Phase fail */ + *p_Device_range_status = 4; /* Phase fail */ } else if (device_range_status_internal == 8 || device_range_status_internal == 10 || - signal_ref_clipflag == 1) { - *p_pal_range_status = 3; /* Min range */ + signal_Ref_clipflag == 1) { + *p_Device_range_status = 3; /* Min range */ } else if (device_range_status_internal == 4 || range_ignore_thresholdflag == 1) { - *p_pal_range_status = 2; /* Signal Fail */ + *p_Device_range_status = 2; /* Signal Fail */ } else if (sigma_limitflag == 1) { - *p_pal_range_status = 1; /* Sigma Fail */ + *p_Device_range_status = 1; /* Sigma Fail */ } else { - *p_pal_range_status = 0; /* Range Valid */ + *p_Device_range_status = 0; /* Range Valid */ } } /* DMAX only relevant during range error */ - if (*p_pal_range_status == 0) { - p_ranging_measurement_data->RangeDMaxMilliMeter = 0; + if (*p_Device_range_status == 0) { p_ranging_results->RangeDMaxMilliMeter = 0; } + + /* fill the Limit Check ErrState */ + signal_rate_final_range_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE); + + if (ErrState == VL53L0X_OK) { + if ((sigma_limit_chk_en == 0) || (sigma_limitflag == 1)) + { temp8 = 1; } else { temp8 = 0; } + CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = temp8; + + if ((device_range_status_internal == 4) || (signal_rate_final_range_limit_chk_en == 0)) + { temp8 = 1; } else { temp8 = 0; } + CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE] = temp8; + + if ((signal_Ref_clip_limit_chk_en == 0) || (signal_Ref_clipflag == 1)) + { temp8 = 1; } else { temp8 = 0; } + CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_REF_CLIP] = temp8; + + if ((range_ignore_threshold_chk_en == 0) || (range_ignore_thresholdflag == 1)) + { temp8 = 1; } else { temp8 = 0;} + CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = temp8; } - - /* fill the Limit Check Status */ - - status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, - &signal_rate_final_range_limit_check_enable); - - if (status == VL53L0X_ERROR_NONE) { - if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1)) { - temp8 = 1; - } else { - temp8 = 0; - } - CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = temp8; - - if ((device_range_status_internal == 4) || - (signal_rate_final_range_limit_check_enable == 0)) { - temp8 = 1; - } else { - temp8 = 0; - } - CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = temp8; - - if ((signal_ref_clip_limit_check_enable == 0) || - (signal_ref_clipflag == 1)) { - temp8 = 1; - } else { - temp8 = 0; - } - - CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = temp8; - - if ((range_ignore_threshold_limit_check_enable == 0) || - (range_ignore_thresholdflag == 1)) { - temp8 = 1; - } else { - temp8 = 0; - } - CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = temp8; - } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t device_range_status; +void VL53L0X::Get_ranging_results(TRangeResults *p_ranging_results) +{ uint8_t device_range_status; uint8_t range_fractional_enable; - uint8_t pal_range_status; - uint8_t x_talk_compensation_enable; + uint8_t Device_range_status; + uint8_t xtalk_compensation_enable; uint16_t ambient_rate; - FixPoint1616_t signal_rate; - uint16_t x_talk_compensation_rate_mega_cps; - uint16_t effective_spad_rtn_count; + TFP1616 signal_rate; + uint16_t Xtalk_CompRate_MHz; + uint16_t effective_SPAD_rtn_count; uint16_t tmpuint16; uint16_t xtalk_range_milli_meter; uint16_t linearity_corrective_gain; uint8_t localBuffer[12]; - VL53L0X_RangingMeasurementData_t last_range_data_buffer; - - /* - * use multi read even if some registers are not useful,result will - * be more efficient - * start reading at 0x14 dec20 - * end reading at 0x21 dec33 total 14 bytes to read - */ - status = VL53L0X_read_multi(0x14,localBuffer,12); - - if (status == VL53L0X_ERROR_NONE) { - - p_ranging_measurement_data->ZoneId = 0; /* Only one zone */ - p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */ + TRangeResults last_range_data_buffer; + + if (ErrState != VL53L0X_OK) { return; } // Do nothing while in error state + + /* use multi read even if some registers are not useful,result will + * be more efficient start reading at REG_RESULT_RANGE_STATUS = 0x14 + * end reading at 0x21 dec33 total 14 bytes to read */ + I2c_Read(REG_RESULT_RANGE_STATUS, localBuffer,12); + + if (ErrState == VL53L0X_OK) { + p_ranging_results->ZoneId = 0; /* Only one zone */ + p_ranging_results->TimeStamp = 0; /* Not Implemented */ tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]); /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional - *(format 11.2) else no fractional - */ - - p_ranging_measurement_data->MeasurementTimeUsec = 0; - - signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6])); - /* peak_signal_count_rate_rtn_mcps */ - p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate; + *(format 11.2) else no fractional */ + + p_ranging_results->MeasurementTimeUsec = 0; + + signal_rate = FP97_TO_FP1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6])); + /* peak_SIG_count_rate_rtn_mcps */ + p_ranging_results->SignalRateRtnMHz = signal_rate; ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]); - p_ranging_measurement_data->AmbientRateRtnMegaCps = - VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate); - - effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], - localBuffer[2]); - /* EffectiveSpadRtnCount is 8.8 format */ - p_ranging_measurement_data->EffectiveSpadRtnCount = - effective_spad_rtn_count; + p_ranging_results->AmbientRateRtnMHz = FP97_TO_FP1616(ambient_rate); + + effective_SPAD_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]); + /* EffectiveSPADRtnCount is 8.8 format */ + p_ranging_results->EffectiveSPADRtnCount = effective_SPAD_rtn_count; device_range_status = localBuffer[0]; @@ -3051,846 +1927,575 @@ * tmpuint16 + 500) / 1000); /* Implement Xtalk */ - x_talk_compensation_rate_mega_cps = CurrentParameters.XTalkCompensationRateMegaCps; - x_talk_compensation_enable = CurrentParameters.XTalkCompensationEnable; - - if (x_talk_compensation_enable) { - - if ((signal_rate - - ((x_talk_compensation_rate_mega_cps - * effective_spad_rtn_count) >> 8)) - <= 0) { - if (range_fractional_enable) { - xtalk_range_milli_meter = 8888; - } else { - xtalk_range_milli_meter = 8888 << 2; - } + Xtalk_CompRate_MHz = CurrParams.Xtalk_CompRate_MHz; + xtalk_compensation_enable = CurrParams.XTalk_Compens_En; + + if (xtalk_compensation_enable) { + if ((signal_rate - ((Xtalk_CompRate_MHz + * effective_SPAD_rtn_count) >> 8)) <= 0) { + if (range_fractional_enable) { xtalk_range_milli_meter = 8888; + } else { xtalk_range_milli_meter = 8888 << 2; } } else { - xtalk_range_milli_meter = - (tmpuint16 * signal_rate) - / (signal_rate - - ((x_talk_compensation_rate_mega_cps - * effective_spad_rtn_count) - >> 8)); + xtalk_range_milli_meter = (tmpuint16 * signal_rate) + / (signal_rate - ((Xtalk_CompRate_MHz * effective_SPAD_rtn_count) >> 8)); } - tmpuint16 = xtalk_range_milli_meter; } } if (range_fractional_enable) { - p_ranging_measurement_data->RangeMilliMeter = - (uint16_t)((tmpuint16) >> 2); - p_ranging_measurement_data->RangeFractionalPart = + p_ranging_results->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2); + p_ranging_results->RangeFractionalPart = (uint8_t)((tmpuint16 & 0x03) << 6); } else { - p_ranging_measurement_data->RangeMilliMeter = tmpuint16; - p_ranging_measurement_data->RangeFractionalPart = 0; + p_ranging_results->RangeMilliMeter = tmpuint16; + p_ranging_results->RangeFractionalPart = 0; } - /* - * For a standard definition of RangeStatus,this should + /* For a standard definition of RangeStatus,this should * return 0 in case of good result after a ranging - * The range status depends on the device so call a device - * specific function to obtain the right Status. - */ - status |= VL53L0X_get_pal_range_status(device_range_status, - signal_rate,effective_spad_rtn_count, - p_ranging_measurement_data,&pal_range_status); - if (status == VL53L0X_ERROR_NONE) { - p_ranging_measurement_data->RangeStatus = pal_range_status; + * The range ErrState depends on the device so call a device + * specific function to obtain the right ErrState. */ + Get_Device_range_status(device_range_status,signal_rate,effective_SPAD_rtn_count, + p_ranging_results,&Device_range_status); + if (ErrState == VL53L0X_OK) { + p_ranging_results->RangeStatus = Device_range_status; } } - if (status == VL53L0X_ERROR_NONE) { - /* Copy last read data into Dev buffer */ + if (ErrState == VL53L0X_OK) { + /* Copy last read data into device+ buffer */ last_range_data_buffer = LastRangeMeasure; last_range_data_buffer.RangeMilliMeter = - p_ranging_measurement_data->RangeMilliMeter; + p_ranging_results->RangeMilliMeter; last_range_data_buffer.RangeFractionalPart = - p_ranging_measurement_data->RangeFractionalPart; + p_ranging_results->RangeFractionalPart; last_range_data_buffer.RangeDMaxMilliMeter = - p_ranging_measurement_data->RangeDMaxMilliMeter; + p_ranging_results->RangeDMaxMilliMeter; last_range_data_buffer.MeasurementTimeUsec = - p_ranging_measurement_data->MeasurementTimeUsec; - last_range_data_buffer.SignalRateRtnMegaCps = - p_ranging_measurement_data->SignalRateRtnMegaCps; - last_range_data_buffer.AmbientRateRtnMegaCps = - p_ranging_measurement_data->AmbientRateRtnMegaCps; - last_range_data_buffer.EffectiveSpadRtnCount = - p_ranging_measurement_data->EffectiveSpadRtnCount; + p_ranging_results->MeasurementTimeUsec; + last_range_data_buffer.SignalRateRtnMHz = + p_ranging_results->SignalRateRtnMHz; + last_range_data_buffer.AmbientRateRtnMHz = + p_ranging_results->AmbientRateRtnMHz; + last_range_data_buffer.EffectiveSPADRtnCount = + p_ranging_results->EffectiveSPADRtnCount; last_range_data_buffer.RangeStatus = - p_ranging_measurement_data->RangeStatus; + p_ranging_results->RangeStatus; LastRangeMeasure = last_range_data_buffer; } - return status; } -VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - /* This function will do a complete single ranging - * Here we fix the mode! */ - status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_perform_single_measurement(); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_clear_interrupt_mask(0); - } - - return status; +void VL53L0X::Perf_single_ranging_measurement( + TRangeResults *p_ranging_results) +{ if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! + + /* This function will do a complete single ranging Here we fix the mode! */ + Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); + + Perf_single_measurement(); + + Get_ranging_results(p_ranging_results); + + Clear_interrupt_mask(0); } -VL53L0X_Error VL53L0X::perform_ref_signal_measurement(uint16_t *p_ref_signal_rate) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - VL53L0X_RangingMeasurementData_t ranging_measurement_data; - - uint8_t sequence_config = 0; +uint16_t VL53L0X::Get_Perf_Ref_SIG_measurement() +{ TRangeResults ranging_results; + uint8_t orig_sequence_config; + uint16_t Ref_SIG_rate ; /* store the value of the sequence config, * this will be reset before the end of the function*/ - sequence_config = SequenceConfig; + orig_sequence_config = SequenceConfig; /* This function performs a reference signal rate measurement.*/ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0xC0); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x01); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, - p_ref_signal_rate); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x00); - } - - if (status == VL53L0X_ERROR_NONE) { - /* restore the previous Sequence Config */ - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, - sequence_config); - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = sequence_config; - } - } - - return status; + Set_SequenceConfig( 0xC0 ); // sets REG_SYSTEM_SEQUENCE_CONFIG + + Perf_single_ranging_measurement(&ranging_results); + + Write_Byte(0xFF,0x01); + Ref_SIG_rate = Read_Word(REG_RESULT_PEAK_SIG_RATE_REF); + Write_Byte(0xFF,0x00); + + /* restore the previous Sequence Config */ + Set_SequenceConfig( orig_sequence_config ); // resets REG_SYSTEM_SEQUENCE_CONFIG + + return Ref_SIG_rate; } -VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count, - uint8_t *is_aperture_spads) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t last_spad_array[6]; +void VL53L0X::Perf_Ref_SPAD_management(uint32_t *ref_SPAD_count, + uint8_t *is_aperture_SPADS) +{ uint8_t last_SPAD_array[6]; uint8_t start_select = 0xB4; - uint32_t minimum_spad_count = 3; - uint32_t max_spad_count = 44; - uint32_t current_spad_index = 0; - uint32_t last_spad_index = 0; - int32_t next_good_spad = 0; - uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */ - uint16_t peak_signal_rate_ref; - uint32_t need_apt_spads = 0; + uint32_t minimum_SPAD_count = 3; + uint32_t max_SPAD_count = 44; + uint32_t current_SPAD_index = 0; + uint32_t last_SPAD_index = 0; + int32_t next_good_SPAD = 0; + uint16_t target_Ref_rate = 0x0A00; /* 20 MHz in 9:7 format */ + uint16_t peak_SIG_rate_Ref; + uint32_t need_apt_SPADS = 0; uint32_t index = 0; - uint32_t spad_array_size = 6; + uint32_t SPAD_array_size = 6; uint32_t signal_rate_diff = 0; - uint32_t last_signal_rate_diff = 0; + uint32_t last_SIG_rate_diff = 0; uint8_t complete = 0; uint8_t vhv_settings = 0; uint8_t phase_cal = 0; - uint32_t ref_spad_count_int = 0; - uint8_t is_aperture_spads_int = 0; + uint32_t ref_SPAD_count_int = 0; + uint8_t is_aperture_SPADS_int = 0; /* * The reference SPAD initialization procedure determines the minimum - * amount of reference spads to be enables to achieve a target reference + * amount of reference SPADS to be enables to achieve a target reference * signal rate and should be performed once during initialization. * - * Either aperture or non-aperture spads are applied but never both. - * Firstly non-aperture spads are set,begining with 5 spads,and - * increased one spad at a time until the closest measurement to the + * Either aperture or non-aperture SPADS are applied but never both. + * Firstly non-aperture SPADS are set,begining with 5 SPADS,and + * increased one SPAD at a time until the closest measurement to the * target rate is achieved. * - * If the target rate is exceeded when 5 non-aperture spads are enabled, - * initialization is performed instead with aperture spads. + * If the target rate is exceeded when 5 non-aperture SPADS are enabled, + * initialization is performed instead with aperture SPADS. * - * When setting spads,a 'Good Spad Map' is applied. + * When setting SPADS,a 'Good SPAD Map' is applied. * * This procedure operates within a SPAD window of interest of a maximum - * 44 spads. + * 44 SPADS. * The start point is currently fixed to 180,which lies towards the end * of the non-aperture quadrant and runs in to the adjacent aperture - * quadrant. - */ - target_ref_rate = targetRefRate; - - /* - * Initialize Spad arrays. - * Currently the good spad map is initialised to 'All good'. - * This is a short term implementation. The good spad map will be + * quadrant. */ + target_Ref_rate = targetRefRate; + + /* Initialize SPAD arrays. + * Currently the good SPAD map is initialised to 'All good'. + * This is a short term implementation. The good SPAD map will be * provided as an input. * Note that there are 6 bytes. Only the first 44 bits will be used to - * represent spads. - */ - for (index = 0; index < spad_array_size; index++) { - SpadData.RefSpadEnables[index] = 0; - } - - status = VL53L0X_write_byte(0xFF,0x01); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x00); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, - start_select); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0); - } + * represent SPADS. */ + for (index = 0; index < SPAD_array_size; index++) { + SPADData.RefSPADEnables[index] = 0; } + + Write_Byte(0xFF,0x01); + Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00); + Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C); + Write_Byte(0xFF,0x00); + Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT,start_select); + Write_Byte(REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0); /* Perform ref calibration */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_perform_ref_calibration(&vhv_settings, &phase_cal, 0); - } - - if (status == VL53L0X_ERROR_NONE) { - /* Enable Minimum NON-APERTURE Spads */ - current_spad_index = 0; - last_spad_index = current_spad_index; - need_apt_spads = 0; - status = enable_ref_spads(need_apt_spads, - SpadData.RefGoodSpadMap, - SpadData.RefSpadEnables, - spad_array_size, + if (ErrState == VL53L0X_OK) + {Perf_Ref_calibration(&vhv_settings, &phase_cal, 0);} + + if (ErrState == VL53L0X_OK) { + /* Enable Minimum NON-APERTURE SPADS */ + current_SPAD_index = 0; + last_SPAD_index = current_SPAD_index; + need_apt_SPADS = 0; + Enable_Ref_SPADS(need_apt_SPADS, + SPADData.RefGoodSPADMap, + SPADData.RefSPADEnables, + SPAD_array_size, start_select, - current_spad_index, - minimum_spad_count, - &last_spad_index); + current_SPAD_index, + minimum_SPAD_count, + &last_SPAD_index); } - if (status == VL53L0X_ERROR_NONE) { - current_spad_index = last_spad_index; - - status = perform_ref_signal_measurement(&peak_signal_rate_ref); - if ((status == VL53L0X_ERROR_NONE) && - (peak_signal_rate_ref > target_ref_rate)) { - /* Signal rate measurement too high, - * switch to APERTURE SPADs */ - - for (index = 0; index < spad_array_size; index++) { - SpadData.RefSpadEnables[index] = 0; - } - - - /* Increment to the first APERTURE spad */ - while ((is_aperture(start_select + current_spad_index) - == 0) && (current_spad_index < max_spad_count)) { - current_spad_index++; - } - - need_apt_spads = 1; - - status = enable_ref_spads(need_apt_spads, - SpadData.RefGoodSpadMap, - SpadData.RefSpadEnables, - spad_array_size, + if (ErrState == VL53L0X_OK) { + current_SPAD_index = last_SPAD_index; + + peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement(); + if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate)) + { /* Signal rate measurement too high, switch to APERTURE SPADS */ + for (index = 0; index < SPAD_array_size; index++) + { SPADData.RefSPADEnables[index] = 0; } + + /* Increment to the first APERTURE SPAD */ + while ((Is_ApertureSPAD(start_select + current_SPAD_index) + == 0) && (current_SPAD_index < max_SPAD_count)) + { current_SPAD_index++; } + + need_apt_SPADS = 1; + + Enable_Ref_SPADS(need_apt_SPADS, + SPADData.RefGoodSPADMap, + SPADData.RefSPADEnables, + SPAD_array_size, start_select, - current_spad_index, - minimum_spad_count, - &last_spad_index); - - if (status == VL53L0X_ERROR_NONE) { - current_spad_index = last_spad_index; - status = perform_ref_signal_measurement(&peak_signal_rate_ref); - - if ((status == VL53L0X_ERROR_NONE) && - (peak_signal_rate_ref > target_ref_rate)) { - /* Signal rate still too high after - * setting the minimum number of - * APERTURE spads. Can do no more - * therefore set the min number of - * aperture spads as the result. - */ - is_aperture_spads_int = 1; - ref_spad_count_int = minimum_spad_count; + current_SPAD_index, + minimum_SPAD_count, + &last_SPAD_index); + + if (ErrState == VL53L0X_OK) { + current_SPAD_index = last_SPAD_index; + peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement(); + + if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate)) + { /* Signal rate still too high after setting the minimum number of + * APERTURE SPADS. Can do no more therefore set the min number of + * aperture SPADS as the result. */ + is_aperture_SPADS_int = 1; + ref_SPAD_count_int = minimum_SPAD_count; } } - } else { need_apt_spads = 0;} + } else { need_apt_SPADS = 0;} } - if ((status == VL53L0X_ERROR_NONE) && - (peak_signal_rate_ref < target_ref_rate)) { - /* At this point,the minimum number of either aperture - * or non-aperture spads have been set. Proceed to add - * spads and perform measurements until the target - * reference is reached. - */ - is_aperture_spads_int = need_apt_spads; - ref_spad_count_int = minimum_spad_count; - - memcpy(last_spad_array,SpadData.RefSpadEnables, - spad_array_size); - last_signal_rate_diff = abs(peak_signal_rate_ref - - target_ref_rate); + if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref < target_Ref_rate)) + { /* At this point,the minimum number of either aperture + * or non-aperture SPADS have been set. Proceed to add + * SPADS and perform measurements until the target reference is reached.*/ + is_aperture_SPADS_int = need_apt_SPADS; + ref_SPAD_count_int = minimum_SPAD_count; + + memcpy(last_SPAD_array,SPADData.RefSPADEnables, SPAD_array_size); + last_SIG_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate); complete = 0; while (!complete) { - get_next_good_spad(SpadData.RefGoodSpadMap, - spad_array_size,current_spad_index, - &next_good_spad); - - if (next_good_spad == -1) { - status = VL53L0X_ERROR_REF_SPAD_INIT; + Get_Next_Good_SPAD(SPADData.RefGoodSPADMap, + SPAD_array_size,current_SPAD_index, &next_good_SPAD); + + if (next_good_SPAD == -1) { + ErrState = VL53L0X_ERROR_REF_SPAD_INIT; break; } - /* Cannot combine Aperture and Non-Aperture spads,so - * ensure the current spad is of the correct type. - */ - if (is_aperture((uint32_t)start_select + next_good_spad) != - need_apt_spads) { - /* At this point we have enabled the maximum - * number of Aperture spads. - */ + /* Cannot combine Aperture and Non-Aperture SPADS,so + * ensure the current SPAD is of the correct type. */ + if (Is_ApertureSPAD((uint32_t)start_select + next_good_SPAD) != + need_apt_SPADS) { + /* At this point we have enabled the maximum number of Aperture SPADS. */ complete = 1; break; } - (ref_spad_count_int)++; - - current_spad_index = next_good_spad; - status = enable_spad_bit(SpadData.RefSpadEnables, - spad_array_size,current_spad_index); - - if (status == VL53L0X_ERROR_NONE) { - current_spad_index++; - /* Proceed to apply the additional spad and - * perform measurement. */ - status = set_ref_spad_map(SpadData.RefSpadEnables); + (ref_SPAD_count_int)++; + + current_SPAD_index = next_good_SPAD; + Enable_SPAD_bit(SPADData.RefSPADEnables, + SPAD_array_size,current_SPAD_index); + + if (ErrState == VL53L0X_OK) { + current_SPAD_index++; + /* Proceed to apply the additional SPAD and perform measurement. */ + I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPADData.RefSPADEnables,6); //Set_Ref_SPAD_map } - if (status != VL53L0X_ERROR_NONE) { break; } - - status = perform_ref_signal_measurement(&peak_signal_rate_ref); - - if (status != VL53L0X_ERROR_NONE) { break; } - - signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate); - - if (peak_signal_rate_ref > target_ref_rate) { - /* Select the spad map that provides the + if (ErrState != VL53L0X_OK) { break; } + + peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement(); + + if (ErrState != VL53L0X_OK) { break; } + + signal_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate); + + if (peak_SIG_rate_Ref > target_Ref_rate) { + /* Select the SPAD map that provides the * measurement closest to the target rate, - * either above or below it. - */ - if (signal_rate_diff > last_signal_rate_diff) { - /* Previous spad map produced a closer - * measurement,so choose this. */ - status = set_ref_spad_map(last_spad_array); - memcpy(SpadData.RefSpadEnables, - last_spad_array,spad_array_size); - - (ref_spad_count_int)--; + * either above or below it. */ + if (signal_rate_diff > last_SIG_rate_diff) { + /* Previous SPAD map produced a closer measurement,so choose this. */ + I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, last_SPAD_array,6); // Set_Ref_SPAD_map(); + memcpy(SPADData.RefSPADEnables,last_SPAD_array,SPAD_array_size); + (ref_SPAD_count_int)--; } complete = 1; } else { - /* Continue to add spads */ - last_signal_rate_diff = signal_rate_diff; - memcpy(last_spad_array, - SpadData.RefSpadEnables, - spad_array_size); + /* Continue to add SPADS */ + last_SIG_rate_diff = signal_rate_diff; + memcpy(last_SPAD_array, SPADData.RefSPADEnables,SPAD_array_size); } - } /* while */ } - if (status == VL53L0X_ERROR_NONE) { - *ref_spad_count = ref_spad_count_int; - *is_aperture_spads = is_aperture_spads_int; - - DevSpecParams.RefSpadsInitialised = 1; - DevSpecParams.ReferenceSpadCount = (uint8_t)(*ref_spad_count); - DevSpecParams.ReferenceSpadType = *is_aperture_spads; + if (ErrState == VL53L0X_OK) { + *ref_SPAD_count = ref_SPAD_count_int; + *is_aperture_SPADS = is_aperture_SPADS_int; + DevSpecParams.RefSPADSInitialised = 1; + DevSpecParams.ReferenceSPADCount = (uint8_t)(*ref_SPAD_count); + DevSpecParams.ReferenceSPADType = *is_aperture_SPADS; } - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(uint32_t count,uint8_t is_aperture_spads) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint32_t current_spad_index = 0; - uint8_t start_select = 0xB4; - uint32_t spad_array_size = 6; - uint32_t max_spad_count = 44; - uint32_t last_spad_index; +void VL53L0X::Set_Reference_SPADS(uint32_t count,uint8_t is_aperture_SPADS) +{ uint32_t current_SPAD_index = 0; + uint8_t start_select = 0xB4; + uint32_t SPAD_array_size = 6; + uint32_t max_SPAD_count = 44; + uint32_t last_SPAD_index; uint32_t index; - /* - * This function applies a requested number of reference spads,either - * aperture or - * non-aperture,as requested. - * The good spad map will be applied. - */ - status = VL53L0X_write_byte(0xFF,0x01); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C); - } - - if (status == VL53L0X_ERROR_NONE) { status = VL53L0X_write_byte(0xFF,0x00); } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, - start_select); - } - - for (index = 0; index < spad_array_size; index++) { - SpadData.RefSpadEnables[index] = 0; - } - - if (is_aperture_spads) { - /* Increment to the first APERTURE spad */ - while ((is_aperture(start_select + current_spad_index) == 0) && - (current_spad_index < max_spad_count)) { - current_spad_index++; + /* This function applies a requested number of reference SPADS,either + * aperture or non-aperture,as requested. The good SPAD map will be applied.*/ + Write_Byte(0xFF,0x01); + Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00); + Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C); + Write_Byte(0xFF,0x00); + Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT, start_select); + + for (index = 0; index < SPAD_array_size; index++) { + SPADData.RefSPADEnables[index] = 0; } + + if (is_aperture_SPADS) { + /* Increment to the first APERTURE SPAD */ + while ((Is_ApertureSPAD(start_select + current_SPAD_index) == 0) && + (current_SPAD_index < max_SPAD_count)) { + current_SPAD_index++; } } - status = enable_ref_spads(is_aperture_spads, - SpadData.RefGoodSpadMap, - SpadData.RefSpadEnables, - spad_array_size, + Enable_Ref_SPADS(is_aperture_SPADS, + SPADData.RefGoodSPADMap, + SPADData.RefSPADEnables, + SPAD_array_size, start_select, - current_spad_index, + current_SPAD_index, count, - &last_spad_index); - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.RefSpadsInitialised = 1; - DevSpecParams.ReferenceSpadCount = (uint8_t)(count); - DevSpecParams.ReferenceSpadType = is_aperture_spads; + &last_SPAD_index); + + if (ErrState == VL53L0X_OK) { + DevSpecParams.RefSPADSInitialised = 1; + DevSpecParams.ReferenceSPADCount = (uint8_t)(count); + DevSpecParams.ReferenceSPADType = is_aperture_SPADS; } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count,uint8_t *is_aperture_spads) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = wrapped_VL53L0X_perform_ref_spad_management(ref_spad_count, - is_aperture_spads); - - return status; -} - -/* Group PAL Init Functions */ -VL53L0X_Error VL53L0X::VL53L0X_set_device_address(uint8_t device_address) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = VL53L0X_write_byte(VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, - device_address / 2); - return status; } -VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config(uint8_t pin, - VL53L0X_DeviceModes device_mode,VL53L0X_GpioFunctionality functionality, - VL53L0X_InterruptPolarity polarity) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t data; - - if (pin != 0) { - status = VL53L0X_ERROR_GPIO_NOT_EXISTING; - } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) { - if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) { - data = 0x10; - } else { - data = 1; - } - - status = VL53L0X_write_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,data); - - } else { - if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) { - - status |= VL53L0X_write_byte(0xff,0x01); - status |= VL53L0X_write_byte(0x00,0x00); - - status |= VL53L0X_write_byte(0xff,0x00); - status |= VL53L0X_write_byte(0x80,0x01); - status |= VL53L0X_write_byte(0x85,0x02); - - status |= VL53L0X_write_byte(0xff,0x04); - status |= VL53L0X_write_byte(0xcd,0x00); - status |= VL53L0X_write_byte(0xcc,0x11); - - status |= VL53L0X_write_byte(0xff,0x07); - status |= VL53L0X_write_byte(0xbe,0x00); - - status |= VL53L0X_write_byte(0xff,0x06); - status |= VL53L0X_write_byte(0xcc,0x09); - - status |= VL53L0X_write_byte(0xff,0x00); - status |= VL53L0X_write_byte(0xff,0x01); - status |= VL53L0X_write_byte(0x00,0x00); - - } else { - - if (status == VL53L0X_ERROR_NONE) { - switch (functionality) { - case VL53L0X_GPIOFUNCTIONALITY_OFF: - data = 0x00; - break; - case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW: - data = 0x01; - break; - case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH: - data = 0x02; - break; - case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT: - data = 0x03; - break; - case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY: - data = 0x04; - break; - default: - status = - VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED; - } - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO,data); - } - - if (status == VL53L0X_ERROR_NONE) { - if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) { - data = 0; - } else { - data = (uint8_t)(1 << 4); - } - status = VL53L0X_update_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,data); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.Pin0GpioFunctionality = functionality; - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_clear_interrupt_mask(0); - } - } - } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable(uint8_t *p_enabled) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_RANGE_CONFIG,p_enabled); - - if (status == VL53L0X_ERROR_NONE) {*p_enabled = (*p_enabled & 1);} - - return status; -} - -uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks) -{ - /*! - * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format - */ - - uint16_t encoded_timeout = 0; - uint32_t ls_byte = 0; +void VL53L0X::Set_GPIO_config(VL53L0X_DeviceModes device_mode, + TGPIO_Func functionality, VL53L0X_InterruptPolarity polarity) +{ uint8_t pol_data; + + if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) + { pol_data = 0x00;} else { pol_data = 0x10;} + + switch ( device_mode ) { + case VL53L0X_DEVICEMODE_GPIO_DRIVE: + Write_Byte(REG_GPIO_HV_MUX_ACTIVE_HIGH,pol_data); + break; + case VL53L0X_DEVICEMODE_GPIO_OSC: + Write_Byte(0xff,0x01); + Write_Byte(0x00,0x00); + Write_Byte(0xff,0x00); + Write_Byte(0x80,0x01); + Write_Byte(0x85,0x02); + Write_Byte(0xff,0x04); + Write_Byte(0xcd,0x00); + Write_Byte(0xcc,0x11); + Write_Byte(0xff,0x07); + Write_Byte(0xbe,0x00); + Write_Byte(0xff,0x06); + Write_Byte(0xcc,0x09); + Write_Byte(0xff,0x00); + Write_Byte(0xff,0x01); + Write_Byte(0x00,0x00); + break; + default: + if (functionality>GPIO_FUNC_NEW_MEASURE_READY) + { ErrState = VL53L0X_ERROR_GPIO_FUNC_NOT_SUPPORTED; } + else { Write_Byte(REG_SYSINT_CONFIG_GPIO,functionality); } + + if (ErrState == VL53L0X_OK) + { Register_BitMask(REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,pol_data); } + + if (ErrState == VL53L0X_OK) + {DevSpecParams.GpioFunctionality = functionality; } + + Clear_interrupt_mask(0); + } // switch +} // Set_GPIO_config + +/* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */ +uint16_t VL53L0X::Encode_timeout(uint32_t timeout_macro_clks) +{ uint16_t encoded_timeout = 0; uint16_t ms_byte = 0; if (timeout_macro_clks > 0) { - ls_byte = timeout_macro_clks - 1; - - while ((ls_byte & 0xFFFFFF00) > 0) { - ls_byte = ls_byte >> 1; + timeout_macro_clks = timeout_macro_clks - 1; + while ((timeout_macro_clks & 0xFFFFFF00) > 0) { + timeout_macro_clks = timeout_macro_clks >> 1; ms_byte++; - } - - encoded_timeout = (ms_byte << 8) + (uint16_t)(ls_byte & 0x000000FF); - } - + } // while + encoded_timeout = (ms_byte << 8) + (uint16_t)(timeout_macro_clks & 0x000000FF); + } return encoded_timeout; - } -VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id, +void VL53L0X::Set_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id, uint32_t timeout_micro_secs) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t current_vcsel_pulse_period_p_clk; +{ uint8_t current_vcsel_PPeriod_p_clk; uint8_t msrc_encoded_time_out; uint16_t pre_range_encoded_time_out; uint16_t pre_range_time_out_m_clks; uint16_t msrc_range_time_out_m_clks; uint32_t final_range_time_out_m_clks; uint16_t final_range_encoded_time_out; - VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; - - if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) || - (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) || - (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) { - - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); - - if (status == VL53L0X_ERROR_NONE) { - msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(timeout_micro_secs, - (uint8_t)current_vcsel_pulse_period_p_clk); - - if (msrc_range_time_out_m_clks > 256) { - msrc_encoded_time_out = 255; - } else { - msrc_encoded_time_out = - (uint8_t)msrc_range_time_out_m_clks - 1; - } + VL53L0X_Sequence_Steps_t sequence_steps; + + switch (sequence_step_id) { + case VL53L0X_SEQUENCESTEP_TCC: + case VL53L0X_SEQUENCESTEP_DSS: + case VL53L0X_SEQUENCESTEP_MSRC: + current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; + + if (ErrState == VL53L0X_OK) { + msrc_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs, + (uint8_t)current_vcsel_PPeriod_p_clk); + + if (msrc_range_time_out_m_clks > 256) { msrc_encoded_time_out = 255;} + else {msrc_encoded_time_out = (uint8_t)msrc_range_time_out_m_clks - 1; } DevSpecParams.LastEncodedTimeout = msrc_encoded_time_out; - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, - msrc_encoded_time_out); - } - } else { - - if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); - pre_range_time_out_m_clks = - VL53L0X_calc_timeout_mclks(timeout_micro_secs, - (uint8_t)current_vcsel_pulse_period_p_clk); - pre_range_encoded_time_out = VL53L0X_encode_timeout(pre_range_time_out_m_clks); - - DevSpecParams.LastEncodedTimeout = pre_range_encoded_time_out; - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, - pre_range_encoded_time_out); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.PreRangeTimeoutMicroSecs=timeout_micro_secs; - } - } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { - - /* For the final range timeout,the pre-range timeout - * must be added. To do this both final and pre-range - * timeouts must be expressed in macro periods MClks - * because they have different vcsel periods. - */ - - VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps); + } + Write_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP,msrc_encoded_time_out); + break; + + case VL53L0X_SEQUENCESTEP_PRE_RANGE: + current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; + + pre_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs, + (uint8_t)current_vcsel_PPeriod_p_clk); + pre_range_encoded_time_out = Encode_timeout(pre_range_time_out_m_clks); + + DevSpecParams.LastEncodedTimeout = pre_range_encoded_time_out; + + Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out); + + if (ErrState == VL53L0X_OK) + {DevSpecParams.PreRangeTimeoutMicroSecs=timeout_micro_secs; } + break; + + case VL53L0X_SEQUENCESTEP_FINAL_RANGE: + /* For the final range timeout,the pre-range timeout must be added. + * To do this both final and pre-range timeouts must be expressed in + * macro periods MClks because they have different vcsel periods.*/ + sequence_steps = Get_sequence_step_enables(); pre_range_time_out_m_clks = 0; - if (scheduler_sequence_steps.PreRangeOn) { - - /* Retrieve PRE-RANGE VCSEL Period */ - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, - ¤t_vcsel_pulse_period_p_clk); - - /* Retrieve PRE-RANGE Timeout in Macro periods - * (MCLKS) */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_word(0x51, - &pre_range_encoded_time_out); - pre_range_time_out_m_clks = - VL53L0X_decode_timeout(pre_range_encoded_time_out); - } - } - - /* Calculate FINAL RANGE Timeout in Macro Periods - * (MCLKS) and add PRE-RANGE value - */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE, - ¤t_vcsel_pulse_period_p_clk); - } - if (status == VL53L0X_ERROR_NONE) { - final_range_time_out_m_clks = - VL53L0X_calc_timeout_mclks(timeout_micro_secs, - (uint8_t) current_vcsel_pulse_period_p_clk); + + if (sequence_steps.PreRangeOn) + { current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ + ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; + + /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ + if (ErrState == VL53L0X_OK) + { pre_range_encoded_time_out = Read_Word(0x51); + pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out); + } + } + + /* Calculate FINAL RANGE Timeout in Macro Periode (MCLKS) and add PRE-RANGE value */ + if (ErrState == VL53L0X_OK) + { current_vcsel_PPeriod_p_clk /* Get and converts the VCSEL period register into actual clock periods */ + = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; } + + if (ErrState == VL53L0X_OK) + { final_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs, + (uint8_t) current_vcsel_PPeriod_p_clk); final_range_time_out_m_clks += pre_range_time_out_m_clks; - final_range_encoded_time_out = - VL53L0X_encode_timeout(final_range_time_out_m_clks); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_word(0x71, - final_range_encoded_time_out); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.FinalRangeTimeoutMicroSecs = timeout_micro_secs; - } - } - } else { - status = VL53L0X_ERROR_INVALID_PARAMS; - } - } - return status; -} - -VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_us - (uint32_t measurement_timing_budget_us) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint32_t final_range_timing_budget_us; - VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; - uint32_t msrc_dcc_tcc_timeout_us = 2000; + final_range_encoded_time_out = Encode_timeout(final_range_time_out_m_clks); + + Write_Word(0x71,final_range_encoded_time_out); + } + + if (ErrState == VL53L0X_OK) + { DevSpecParams.FinalRangeTimeoutMicroSecs = timeout_micro_secs; } + break; + + default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; + } // switch (sequence_step_id) + } + +void VL53L0X::Set_Measure_Time_Budget_us (uint32_t Measure_Time_Budget_us) +{ uint32_t final_range_timing_budget_us; + VL53L0X_Sequence_Steps_t sequence_steps; + uint32_t msrc_dcc_tcc_timeout_us= 2000; uint32_t start_overhead_us = 1910; uint32_t end_overhead_us = 960; uint32_t msrc_overhead_us = 660; uint32_t tcc_overhead_us = 590; uint32_t dss_overhead_us = 690; uint32_t pre_range_overhead_us = 660; - uint32_t final_range_overhead_us = 550; + uint32_t final_range_overhead_us= 550; uint32_t pre_range_timeout_us = 0; uint32_t c_min_timing_budget_us = 20000; uint32_t sub_timeout = 0; - if (measurement_timing_budget_us - < c_min_timing_budget_us) { - status = VL53L0X_ERROR_INVALID_PARAMS; - return status; - } - - final_range_timing_budget_us = - measurement_timing_budget_us - - (start_overhead_us + end_overhead_us); - - status = VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps); - - if (status == VL53L0X_ERROR_NONE && - (scheduler_sequence_steps.TccOn || - scheduler_sequence_steps.MsrcOn || - scheduler_sequence_steps.DssOn)) { + if (Measure_Time_Budget_us < c_min_timing_budget_us) + { ErrState = VL53L0X_ERROR_INVALID_PARAMS; + return ; } + + final_range_timing_budget_us = Measure_Time_Budget_us - + (start_overhead_us + end_overhead_us); + + sequence_steps = Get_sequence_step_enables(); + + if (ErrState == VL53L0X_OK && + (sequence_steps.TccOn || + sequence_steps.MsrcOn || + sequence_steps.DssOn)) { /* TCC,MSRC and DSS all share the same timeout */ - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us); - /* Subtract the TCC,MSRC and DSS timeouts if they are - * enabled. */ - - if (status != VL53L0X_ERROR_NONE) { return status; } + /* Subtract the TCC,MSRC and DSS timeouts if they are enabled. */ + if (ErrState != VL53L0X_OK) {return ; } /* TCC */ - if (scheduler_sequence_steps.TccOn) { - - sub_timeout = msrc_dcc_tcc_timeout_us - + tcc_overhead_us; - - if (sub_timeout < - final_range_timing_budget_us) { - final_range_timing_budget_us -= - sub_timeout; - } else { - /* Requested timeout too big. */ - status = VL53L0X_ERROR_INVALID_PARAMS; + if (sequence_steps.TccOn) { + + sub_timeout = msrc_dcc_tcc_timeout_us + tcc_overhead_us; + + if (sub_timeout < final_range_timing_budget_us) { + final_range_timing_budget_us -= sub_timeout; + } else { /* Requested timeout too big. */ + ErrState = VL53L0X_ERROR_INVALID_PARAMS; } } - if (status != VL53L0X_ERROR_NONE) { return status; } + if (ErrState != VL53L0X_OK) {return; } /* DSS */ - if (scheduler_sequence_steps.DssOn) { - - sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + - dss_overhead_us); - - if (sub_timeout < final_range_timing_budget_us) { - final_range_timing_budget_us - -= sub_timeout; - } else { - /* Requested timeout too big. */ - status = VL53L0X_ERROR_INVALID_PARAMS; - } - } else if (scheduler_sequence_steps.MsrcOn) { - /* MSRC */ - sub_timeout = msrc_dcc_tcc_timeout_us + - msrc_overhead_us; - - if (sub_timeout < final_range_timing_budget_us) { - final_range_timing_budget_us - -= sub_timeout; - } else { - /* Requested timeout too big. */ - status = VL53L0X_ERROR_INVALID_PARAMS; - } - } - + if (sequence_steps.DssOn) + { sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us); + + if (sub_timeout < final_range_timing_budget_us) + { final_range_timing_budget_us -= sub_timeout; } + else { /* Requested timeout too big. */ + ErrState = VL53L0X_ERROR_INVALID_PARAMS; } + } + else if (sequence_steps.MsrcOn) /* MSRC */ + { sub_timeout = msrc_dcc_tcc_timeout_us + msrc_overhead_us; + + if (sub_timeout < final_range_timing_budget_us) + { final_range_timing_budget_us -= sub_timeout; } + else /* Requested timeout too big. */ + { ErrState = VL53L0X_ERROR_INVALID_PARAMS; } + } } - if (status != VL53L0X_ERROR_NONE) { return status; } - - if (scheduler_sequence_steps.PreRangeOn) { - + if (ErrState != VL53L0X_OK) {return; } + + if (sequence_steps.PreRangeOn) { /* Subtract the Pre-range timeout if enabled. */ - - status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, + Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us); - - sub_timeout = pre_range_timeout_us + - pre_range_overhead_us; + sub_timeout = pre_range_timeout_us + pre_range_overhead_us; if (sub_timeout < final_range_timing_budget_us) { final_range_timing_budget_us -= sub_timeout; } else { /* Requested timeout too big. */ - status = VL53L0X_ERROR_INVALID_PARAMS; + ErrState = VL53L0X_ERROR_INVALID_PARAMS; } } - - if (status == VL53L0X_ERROR_NONE && - scheduler_sequence_steps.FinalRangeOn) { - - final_range_timing_budget_us -= - final_range_overhead_us; + if (ErrState == VL53L0X_OK && sequence_steps.FinalRangeOn) + { final_range_timing_budget_us -= final_range_overhead_us; /* Final Range Timeout * Note that the final range timeout is determined by the timing @@ -3899,716 +2504,305 @@ * will be set. Otherwise the remaining time will be applied to * the final range. */ - status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, + Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, final_range_timing_budget_us); - CurrentParameters.MeasurementTimingBudget_us = measurement_timing_budget_us; + CurrParams.Measure_Time_Budget_us = Measure_Time_Budget_us; } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_us(uint32_t measurement_timing_budget_us) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = wrapped_VL53L0X_set_measurement_timing_budget_us(measurement_timing_budget_us); - - return status; } -VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id,uint8_t sequence_step_enabled) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t sequence_config = 0; - uint8_t sequence_config_new = 0; - uint32_t measurement_timing_budget_us; - - status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &sequence_config); - - sequence_config_new = sequence_config; - - if (status == VL53L0X_ERROR_NONE) { - if (sequence_step_enabled == 1) { - - /* Enable requested sequence step - */ - switch (sequence_step_id) { - case VL53L0X_SEQUENCESTEP_TCC: - sequence_config_new |= 0x10; - break; - case VL53L0X_SEQUENCESTEP_DSS: - sequence_config_new |= 0x28; - break; - case VL53L0X_SEQUENCESTEP_MSRC: - sequence_config_new |= 0x04; - break; - case VL53L0X_SEQUENCESTEP_PRE_RANGE: - sequence_config_new |= 0x40; - break; - case VL53L0X_SEQUENCESTEP_FINAL_RANGE: - sequence_config_new |= 0x80; - break; - default: - status = VL53L0X_ERROR_INVALID_PARAMS; - } - } else { - /* Disable requested sequence step - */ - switch (sequence_step_id) { - case VL53L0X_SEQUENCESTEP_TCC: - sequence_config_new &= 0xef; - break; - case VL53L0X_SEQUENCESTEP_DSS: - sequence_config_new &= 0xd7; - break; - case VL53L0X_SEQUENCESTEP_MSRC: - sequence_config_new &= 0xfb; - break; - case VL53L0X_SEQUENCESTEP_PRE_RANGE: - sequence_config_new &= 0xbf; - break; - case VL53L0X_SEQUENCESTEP_FINAL_RANGE: - sequence_config_new &= 0x7f; - break; - default: - status = VL53L0X_ERROR_INVALID_PARAMS; - } - } - } - - if (sequence_config_new != sequence_config) { - /* Apply New Setting */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,sequence_config_new); - } - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = sequence_config_new; - } - - /* Recalculate timing budget */ - if (status == VL53L0X_ERROR_NONE) { - measurement_timing_budget_us = CurrentParameters.MeasurementTimingBudget_us; - VL53L0X_set_measurement_timing_budget_us(measurement_timing_budget_us); - } - } - - return status; +const uint8_t SEQUENCESTEP_MASK[] = +{ 0x10, //VL53L0X_SEQUENCESTEP_TCC = 0 + 0x28, //VL53L0X_SEQUENCESTEP_DSS = 1 + 0x04, //VL53L0X_SEQUENCESTEP_MSRC= 2 + 0x40, //VL53L0X_SEQUENCESTEP_PRE_RANGE= 3 + 0x80}; //VL53L0X_SEQUENCESTEP_FINAL_RANGE = 4 + +void VL53L0X::Set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id, + uint8_t sequence_step_enabled) +{ uint8_t new_config = 0; + + // SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); + // instead of reading from the device, use the SequenceConfig local data field!! + + if (sequence_step_enabled == 1) /* Enable requested sequence step */ + { new_config = SequenceConfig | SEQUENCESTEP_MASK[sequence_step_id]; } + else /* Disable requested sequence step */ + { new_config = SequenceConfig & (0xff - SEQUENCESTEP_MASK[sequence_step_id]); } + + if (new_config != SequenceConfig) { /* Apply New Setting */ + Set_SequenceConfig( new_config ); + if (ErrState == VL53L0X_OK) /* Recalculate timing budget */ + { Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); } + } // if (new_config != sequence_config) } -VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable(uint16_t limit_check_id, - uint8_t limit_check_enable) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - FixPoint1616_t temp_fix1616 = 0; - uint8_t limit_check_enable_int = 0; - uint8_t limit_check_disable = 0; - uint8_t temp8; - - if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { - status = VL53L0X_ERROR_INVALID_PARAMS; - } else { - if (limit_check_enable == 0) { - temp_fix1616 = 0; - limit_check_enable_int = 0; - limit_check_disable = 1; - - } else { - temp_fix1616 = CurrentParameters.LimitChecksValue[limit_check_id]; - limit_check_disable = 0; - /* this to be sure to have either 0 or 1 */ - limit_check_enable_int = 1; - } - - switch (limit_check_id) { - - case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: - /* internal computation: */ - CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_enable_int; - - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: - - status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, - VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616)); - - break; - - case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: - /* internal computation: */ - CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_enable_int; - break; - - case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: - /* internal computation: */ - CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_enable_int; - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: - temp8 = (uint8_t)(limit_check_disable << 1); - status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL, - 0xFE,temp8); - break; - - case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: - temp8 = (uint8_t)(limit_check_disable << 4); - status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL, - 0xEF,temp8); - break; - - default: - status = VL53L0X_ERROR_INVALID_PARAMS; - } - } - - if (status == VL53L0X_ERROR_NONE) { - if (limit_check_enable == 0) { - CurrentParameters.LimitChecksEnable[limit_check_id] = 0; - } else { - CurrentParameters.LimitChecksEnable[limit_check_id] = 1; - } - } - - return status; +void VL53L0X::Set_limit_chk_en(uint16_t limit_check_id, uint8_t limit_chk_en) +{ TFP1616 temp_fix1616 = 0; + if (limit_chk_en!=0) {limit_chk_en=1;} // make sure we only have 0 or 1 as values!!! + + switch (limit_check_id) { + case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* internal computation: */ + case VL53L0X_CHECKEN_SIG_REF_CLIP: /* internal computation: */ + case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: /* internal computation: */ + CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en; + break; + + case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE: + temp_fix1616 = limit_chk_en * CurrParams.Limit_Chk_Val[limit_check_id]; + Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, + FP1616_TO_FP97(temp_fix1616)); + break; + + case VL53L0X_CHECKEN_SIG_RATE_MSRC: + Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xFE, (1-limit_chk_en)<< 1); + break; + + case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE: + Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEF, (1-limit_chk_en)<< 4); + break; + + default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; + } // switch + + if (ErrState == VL53L0X_OK) { CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en; } } -VL53L0X_Error VL53L0X::VL53L0X_static_init() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - VL53L0X_DeviceParameters_t new_curr_parameters = {0}; - uint8_t *p_tuning_setting_buffer; +void VL53L0X::Static_init() +{ VL53L0X_DeviceParams_t new_curr_parameters; + uint8_t *p_tuning_setting_buffer; uint16_t tempword = 0; - uint8_t tempbyte = 0; - uint8_t use_internal_tuning_settings = 0; + uint8_t tempbyte = 0; uint32_t count = 0; - uint8_t is_aperture_spads = 0; - uint32_t ref_spad_count = 0; - uint8_t aperture_spads = 0; - uint8_t vcsel_pulse_period_pclk; + uint8_t is_aperture_SPADS = 0; + uint32_t ref_SPAD_count = 0; + uint8_t aperture_SPADS = 0; + uint8_t vcsel_PPeriod_pclk; uint32_t seq_timeout_micro_secs; - status = VL53L0X_get_info_from_device(1); - - /* set the ref spad from NVM */ - count = (uint32_t)DevSpecParams.ReferenceSpadCount; - aperture_spads = DevSpecParams.ReferenceSpadType; + Get_info_from_device(1); + + /* set the ref SPAD from NVM */ + count = (uint32_t)DevSpecParams.ReferenceSPADCount; + aperture_SPADS = DevSpecParams.ReferenceSPADType; /* NVM value invalid */ - if ((aperture_spads > 1) || - ((aperture_spads == 1) && (count > 32)) || - ((aperture_spads == 0) && (count > 12))) { - status = wrapped_VL53L0X_perform_ref_spad_management(&ref_spad_count, - &is_aperture_spads); - } else { - status = VL53L0X_set_reference_spads(count,aperture_spads); - } - + if ((aperture_SPADS > 1) || ((aperture_SPADS == 1) && (count > 32)) || + ((aperture_SPADS == 0) && (count > 12))) + { Perf_Ref_SPAD_management(&ref_SPAD_count, &is_aperture_SPADS); } + else + { Set_Reference_SPADS(count,aperture_SPADS); } /* Initialize tuning settings buffer to prevent compiler warning. */ p_tuning_setting_buffer = DefaultTuningSettings; - if (status == VL53L0X_ERROR_NONE) { - use_internal_tuning_settings = UseInternalTuningSettings; - - if (use_internal_tuning_settings == 0) { - p_tuning_setting_buffer = pTuningSettingsPointer; - } else { - p_tuning_setting_buffer = DefaultTuningSettings; - } - - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_load_tuning_settings(p_tuning_setting_buffer); - } - - - /* Set interrupt config to new sample ready */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_gpio_config(0,0, - VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, - VL53L0X_INTERRUPTPOLARITY_LOW); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x01); - status |= VL53L0X_read_word(0x84,&tempword); - status |= VL53L0X_write_byte(0xFF,0x00); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.OscFrequencyMHz= - VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword); - } - - /* After static init,some device parameters may be changed, - * so update them */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_device_parameters(&new_curr_parameters); } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_fraction_enable(&tempbyte); - if (status == VL53L0X_ERROR_NONE) { - RangeFractionalEnable = tempbyte; } - } - - if (status == VL53L0X_ERROR_NONE) { CurrentParameters = new_curr_parameters; } - + if (ErrState == VL53L0X_OK) { + if (UseInternalTuningSettings == 0) + { p_tuning_setting_buffer = pTuningSettingsPointer; } + else + { p_tuning_setting_buffer = DefaultTuningSettings; } + } + + if (ErrState == VL53L0X_OK) + { Load_tuning_settings(p_tuning_setting_buffer); } + + /* Set interrupt config to new sample ready */ + if (ErrState == VL53L0X_OK) + { Set_GPIO_config(0,GPIO_FUNC_NEW_MEASURE_READY,VL53L0X_INTERRUPTPOLARITY_LOW); } + + Write_Byte(0xFF,0x01); + tempword = Read_Word(0x84); + Write_Byte(0xFF,0x00); + + if (ErrState == VL53L0X_OK) + { DevSpecParams.OscFrequencyMHz=FP412_TO_FP1616(tempword); } + + /* After static init,some device parameters may be changed, so update them */ + new_curr_parameters = Get_device_parameters(); + + if (ErrState == VL53L0X_OK) { tempbyte = Read_Byte(REG_SYSTEM_RANGE_CONFIG); } + if (ErrState == VL53L0X_OK) { RangeFractionalEnable = (tempbyte & 1); } + if (ErrState == VL53L0X_OK) { CurrParams = new_curr_parameters; } /* read the sequence config and save it */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,&tempbyte); - if (status == VL53L0X_ERROR_NONE) { - SequenceConfig = tempbyte; - } - } + Set_SequenceConfig( Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG) ); // checks for ErrState /* Disable MSRC and TCC by default */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0); - } - - /* Set PAL State to standby */ - if (status == VL53L0X_ERROR_NONE) { PalState = VL53L0X_STATE_IDLE; } + if (ErrState == VL53L0X_OK) + { Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0); } + + if (ErrState == VL53L0X_OK) + { Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0); } + + /* Set State to standby */ + Set_Current_State( VL53L0X_STATE_IDLE) ; /* Store pre-range vcsel period */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_vcsel_pulse_period( - VL53L0X_VCSEL_PERIOD_PRE_RANGE, - &vcsel_pulse_period_pclk); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk; - } + if (ErrState == VL53L0X_OK)/* Gets and converts the VCSEL period register into actual clock periods */ + { vcsel_PPeriod_pclk = (Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; } + + if ( ErrState == VL53L0X_OK) + { DevSpecParams.PreRangeVcselPPeriod = vcsel_PPeriod_pclk; } /* Store final-range vcsel period */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_get_vcsel_pulse_period( - VL53L0X_VCSEL_PERIOD_FINAL_RANGE, - &vcsel_pulse_period_pclk); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk; - } + if (ErrState == VL53L0X_OK) + { vcsel_PPeriod_pclk /* Get and convert the VCSEL period register into actual clock periods */ + = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; } + + if (ErrState == VL53L0X_OK) + { DevSpecParams.FinalRangeVcselPPeriod = vcsel_PPeriod_pclk; } /* Store pre-range timeout */ - if (status == VL53L0X_ERROR_NONE) { - status = get_sequence_step_timeout( - VL53L0X_SEQUENCESTEP_PRE_RANGE, - &seq_timeout_micro_secs); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.PreRangeTimeoutMicroSecs = seq_timeout_micro_secs; - } + if (ErrState == VL53L0X_OK) + { Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_PRE_RANGE, + &seq_timeout_micro_secs); } + + if (ErrState == VL53L0X_OK) + { DevSpecParams.PreRangeTimeoutMicroSecs = seq_timeout_micro_secs; } /* Store final-range timeout */ - if (status == VL53L0X_ERROR_NONE) { - status = get_sequence_step_timeout( - VL53L0X_SEQUENCESTEP_FINAL_RANGE, - &seq_timeout_micro_secs); - } - - if (status == VL53L0X_ERROR_NONE) { - DevSpecParams.FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs; - } - - return status; + if (ErrState == VL53L0X_OK) + { Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_FINAL_RANGE, + &seq_timeout_micro_secs);} + + if (ErrState == VL53L0X_OK) + { DevSpecParams.FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs; } } - -VL53L0X_Error VL53L0X::VL53L0X_stop_measurement() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - - status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START, - VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT); - - status = VL53L0X_write_byte(0xFF,0x01); - status = VL53L0X_write_byte(0x00,0x00); - status = VL53L0X_write_byte(0x91,0x00); - status = VL53L0X_write_byte(0x00,0x01); - status = VL53L0X_write_byte(0xFF,0x00); - - if (status == VL53L0X_ERROR_NONE) { - /* Set PAL State to Idle */ - PalState = VL53L0X_STATE_IDLE; - } +void VL53L0X::Stop_Measurement() +{ Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_SINGLESHOT); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x00); + Write_Byte(0x91,0x00); + Write_Byte(0x00,0x01); + Write_Byte(0xFF,0x00); + + Set_Current_State( VL53L0X_STATE_IDLE ); /* Check if need to apply interrupt settings */ - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_check_and_load_interrupt_settings(0); - } - - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(uint32_t *p_stop_status) -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t byte = 0; - - status = VL53L0X_write_byte(0xFF,0x01); - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_read_byte(0x04,&byte); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_write_byte(0xFF,0x0); - } - - *p_stop_status = byte; - - if (byte == 0) { - status = VL53L0X_write_byte(0x80,0x01); - status = VL53L0X_write_byte(0xFF,0x01); - status = VL53L0X_write_byte(0x00,0x00); - status = VL53L0X_write_byte(0x91,StopVariable); - status = VL53L0X_write_byte(0x00,0x01); - status = VL53L0X_write_byte(0xFF,0x00); - status = VL53L0X_write_byte(0x80,0x00); - } - - return status; -} - -/****************** Write and read functions from I2C *************************/ - -VL53L0X_Error VL53L0X::VL53L0X_write_multi(uint8_t index,uint8_t *p_data,uint32_t count) -{ - int status; - status = VL53L0X_i2c_write(index,p_data,(uint16_t)count); - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_read_multi(uint8_t index,uint8_t *p_data,uint32_t count) -{ - int status; - - if (count >= VL53L0X_MAX_I2C_XFER_SIZE) { - status = VL53L0X_ERROR_INVALID_PARAMS; - } - - status = VL53L0X_i2c_read(index,p_data,(uint16_t)count); - - return status; -} - - -VL53L0X_Error VL53L0X::VL53L0X_write_byte(uint8_t index,uint8_t data) -{ - int status; - status = VL53L0X_i2c_write(index,&data,1); - return status; + Check_and_load_interrupt_settings(0); } -VL53L0X_Error VL53L0X::VL53L0X_write_word(uint8_t index,uint16_t data) -{ - int status; - uint8_t buffer[2]; - - buffer[0] = data >> 8; - buffer[1] = data & 0x00FF; - status = VL53L0X_i2c_write(index,(uint8_t *)buffer,2); - return status; -} - -VL53L0X_Error VL53L0X::VL53L0X_write_dword(uint8_t index,uint32_t data) -{ - int status; - uint8_t buffer[4]; - - buffer[0] = (data >> 24) & 0xFF; - buffer[1] = (data >> 16) & 0xFF; - buffer[2] = (data >> 8) & 0xFF; - buffer[3] = (data >> 0) & 0xFF; - status = VL53L0X_i2c_write(index,(uint8_t *)buffer,4); - return status; -} - - -VL53L0X_Error VL53L0X::VL53L0X_read_byte(uint8_t index,uint8_t *p_data) -{ - int status; - status = VL53L0X_i2c_read(index,p_data,1); - - if (status) { return -1; } - return 0; -} - -VL53L0X_Error VL53L0X::VL53L0X_read_word(uint8_t index,uint16_t *p_data) -{ - int status; - uint8_t buffer[2] = {0,0}; - - status = VL53L0X_i2c_read(index,buffer,2); - if (!status) { *p_data = (buffer[0] << 8) + buffer[1]; } - return status; - -} - -VL53L0X_Error VL53L0X::VL53L0X_read_dword(uint8_t index,uint32_t *p_data) -{ - int status; - uint8_t buffer[4] = {0,0,0,0}; - - status = VL53L0X_i2c_read(index,buffer,4); - if (!status) { - *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; - } - return status; - -} - -VL53L0X_Error VL53L0X::VL53L0X_update_byte(uint8_t index,uint8_t and_data,uint8_t or_data) -{ - int status; - uint8_t buffer = 0; - - /* read data direct onto buffer */ - status = VL53L0X_i2c_read(index,&buffer,1); - if (!status) { - buffer = (buffer & and_data) | or_data; - status = VL53L0X_i2c_write(index,&buffer,(uint8_t)1); - } - return status; +uint8_t VL53L0X::Get_Stop_Completed() +{ uint8_t Abyte = 0; + + Write_Byte(0xFF,0x01); + Abyte = Read_Byte(0x04); + Write_Byte(0xFF,0x0); + + if ((ErrState == VL53L0X_OK) & (Abyte == 0)) + { Write_Byte(0x80,0x01); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x00); + Write_Byte(0x91,StopVariable); + Write_Byte(0x00,0x01); + Write_Byte(0xFF,0x00); + Write_Byte(0x80,0x00); + } + return Abyte; } -VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t RegisterAddr,uint8_t *p_data, - uint16_t NumByteToWrite) -{ - int ret; - ret = _dev_i2c->i2c_write(p_data,I2cDevAddr,RegisterAddr,NumByteToWrite); - - if (ret) { return -1; } - return 0; -} - -VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t RegisterAddr,uint8_t *p_data, - uint16_t NumByteToRead) -{ - int ret; - ret = _dev_i2c->i2c_read(p_data,I2cDevAddr,RegisterAddr,NumByteToRead); - - if (ret) { return -1; } - return 0; -} - -int VL53L0X::read_id(uint8_t *id) -{ - int status = 0; - uint16_t rl_id = 0; - - status = VL53L0X_read_word(VL53L0X_REG_IDENTIFICATION_MODEL_ID,&rl_id); - if (rl_id == 0xEEAA) { return status; } - - return -1; -} - - -VL53L0X_Error VL53L0X::wait_measurement_data_ready() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint8_t new_dat_ready = 0; - uint32_t loop_nb; - - // Wait until it finished - // use timeout to avoid deadlock - if (status == VL53L0X_ERROR_NONE) { - loop_nb = 0; - do { - status = VL53L0X_get_measurement_data_ready(&new_dat_ready); - if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) { - break; - } - loop_nb = loop_nb + 1; - VL53L0X_polling_delay(); - } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); - - if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { - status = VL53L0X_ERROR_TIME_OUT; - } - } - - return status; +void VL53L0X::Wait_Measurement_Ready() +{ uint32_t loop_nb = 0; + + // Wait until it finished, or loopo count reached = avoids deadlock + while ( !Get_Measurement_Ready() & (ErrState == VL53L0X_OK) ) + { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) + { ErrState = VL53L0X_ERROR_TIME_OUT;} + else { Polling_delay(); } + } // while ends } -VL53L0X_Error VL53L0X::wait_stop_completed() -{ - VL53L0X_Error status = VL53L0X_ERROR_NONE; - uint32_t stop_completed = 0; - uint32_t loop_nb; - - // Wait until it finished - // use timeout to avoid deadlock - if (status == VL53L0X_ERROR_NONE) { - loop_nb = 0; - do { - status = VL53L0X_get_stop_completed_status(&stop_completed); - if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) { - break; - } - loop_nb = loop_nb + 1; - VL53L0X_polling_delay(); - } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); - - if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { - status = VL53L0X_ERROR_TIME_OUT; - } - } - - return status; +void VL53L0X::Wait_Stop_Completed() +{ uint32_t loop_nb = 0; + + // Wait until Stop_Completed, or loopo count reached = avoids deadlock + while ( (ErrState == VL53L0X_OK) & !Get_Stop_Completed() ) + { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) + { ErrState = VL53L0X_ERROR_TIME_OUT;} + else { Polling_delay(); } + } // while ends } - -int VL53L0X::init_sensor(uint8_t new_addr) -{ int status; - - VL53L0X_off(); - VL53L0X_on(); - - status = is_present(); - if (!status) { - status = VL53L0X_data_init(); - if (status != VL53L0X_ERROR_NONE) { - printf("Failed to init VL53L0X sensor!\n\r"); - return status; - } - - // deduce silicon version - status = VL53L0X_get_device_info(&_device_info); - status = prepare(); - if (status != VL53L0X_ERROR_NONE) { - printf("Failed to prepare VL53L0X!\n\r"); - return status; - } - - if (new_addr != VL53L0X_DEFAULT_ADDRESS) { - status = set_device_address(new_addr); - if (status) { - printf("Failed to change I2C address!\n\r"); - return status; - } - } else { - printf("Invalid new address!\n\r"); - return VL53L0X_ERROR_INVALID_PARAMS; - } - } - return status; +void VL53L0X::Range_meas_int_continuous_mode(void (*fptr)(void)) +{ Stop_Measurement(); // it is safer to do this while sensor is stopped + + Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, + GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH); + if (ErrState==VL53L0X_OK) { + Attach_interrupt_measure_detection_irq(fptr); + Enable_interrupt_measure_detection_irq(); } + + Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS); + // NB: return value was previously only passed to logging macro,but did not get passed back + + if (ErrState==VL53L0X_OK) { Range_start_continuous_mode(); } } -int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void)) -{ - int status; - - status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped - - status = VL53L0X_set_gpio_config(0,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, - VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, - VL53L0X_INTERRUPTPOLARITY_HIGH); - if (!status) { - attach_interrupt_measure_detection_irq(fptr); - enable_interrupt_measure_detection_irq(); - } - - clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS); - // NB: return value was previously only passed to logging macro,but did not get passed back - - if (!status) { - status = range_start_continuous_mode(); - } - return status; -} - - -int VL53L0X::start_measurement(OperatingMode operating_mode,void (*fptr)(void)) -{ - int Status = VL53L0X_ERROR_NONE; - uint8_t VhvSettings; + +VL53L0X_Error VL53L0X::Start_Measurement(TOperatingMode operating_mode, void (*fptr)(void)) +{ uint8_t VhvSettings; uint8_t PhaseCal; // *** from mass market cube expansion v1.1,ranging with satellites. // default settings,for normal range. - FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536); - FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536); + TFP1616 signalLimit = (TFP1616)(0.25 * 65536); + TFP1616 sigmaLimit = (TFP1616)(18 * 65536); uint32_t timingBudget = 33000; uint8_t preRangeVcselPeriod = 14; uint8_t finalRangeVcselPeriod = 10; - if (operating_mode == range_continuous_interrupt) { - if (_gpio1Int == NULL) { - printf("GPIO1 Error\r\n"); - return 1; - } - - Status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped - - Status = VL53L0X_set_gpio_config(0,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, - VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, - VL53L0X_INTERRUPTPOLARITY_HIGH); - - if (Status == VL53L0X_ERROR_NONE) { - attach_interrupt_measure_detection_irq(fptr); - enable_interrupt_measure_detection_irq(); - } - - clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS); - // NB: return value was previously only passed to logging macro,but did not get passed back - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode - } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_start_measurement(); - } - } - - if (operating_mode == range_single_shot_polling) { - // singelshot,polled ranging - if (Status == VL53L0X_ERROR_NONE) { - // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement - Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode - } + switch (operating_mode) { + case op_INT: + if (_gpio1Int == NULL) { ErrState=1; return ErrState; } + Stop_Measurement(); // it is safer to do this while sensor is stopped + + Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, + GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH); + + if (ErrState == VL53L0X_OK) + { Attach_interrupt_measure_detection_irq(fptr); + Enable_interrupt_measure_detection_irq(); } + + Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS); + // NB: return value was previously only passed to logging macro, but did not get passed back + + // Setup in continuous ranging mode + Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); + Start_Measurement(); + break; + + case op_single_shot_poll: + // singelshot,polled ranging; no need to do this when we use VL53L0X_PerformSingleRangingMeasurement + Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode // Enable/Disable Sigma and Signal check - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_limit_check_enable( - VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,1); - } - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_limit_check_enable( - VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,1); - } - -// *** from mass market cube expansion v1.1,ranging with satellites. - /* Ranging configuration */ -//* -// switch(rangingConfig) { -// case LONG_RANGE: - signalLimit = (FixPoint1616_t)(0.1 * 65536); - sigmaLimit = (FixPoint1616_t)(60 * 65536); + if (ErrState == VL53L0X_OK) + { Set_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,1); } + + if (ErrState == VL53L0X_OK) + { Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); } + + + /* Ranging configuration */ + // *** from mass market cube expansion v1.1,ranging with satellites. + // switch(rangingConfig) { + // case LONG_RANGE: + signalLimit = (TFP1616)(0.1 * 65536); + sigmaLimit = (TFP1616)(60 * 65536); timingBudget = 33000; preRangeVcselPeriod = 18; finalRangeVcselPeriod = 14; /* break; case HIGH_ACCURACY: - signalLimit = (FixPoint1616_t)(0.25*65536); - sigmaLimit = (FixPoint1616_t)(18*65536); + signalLimit = (TFP1616)(0.25*65536); + sigmaLimit = (TFP1616)(18*65536); timingBudget = 200000; preRangeVcselPeriod = 14; finalRangeVcselPeriod = 10; break; case HIGH_SPEED: - signalLimit = (FixPoint1616_t)(0.25*65536); - sigmaLimit = (FixPoint1616_t)(32*65536); + signalLimit = (TFP1616)(0.25*65536); + sigmaLimit = (TFP1616)(32*65536); timingBudget = 20000; preRangeVcselPeriod = 14; finalRangeVcselPeriod = 10; @@ -4618,116 +2812,366 @@ } */ - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_limit_check_value( - VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,signalLimit); - } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_limit_check_value( - VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,sigmaLimit); - } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_measurement_timing_budget_us(timingBudget); - } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_vcsel_pulse_period( - VL53L0X_VCSEL_PERIOD_PRE_RANGE,preRangeVcselPeriod); - } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_set_vcsel_pulse_period( - VL53L0X_VCSEL_PERIOD_FINAL_RANGE,finalRangeVcselPeriod); - } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_perform_ref_calibration(&VhvSettings,&PhaseCal,1); - } - } - - if (operating_mode == range_continuous_polling) { - if (Status == VL53L0X_ERROR_NONE) { - //printf("Call of VL53L0X_SetDeviceMode\n"); - Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode - } - - if (Status == VL53L0X_ERROR_NONE) { - //printf("Call of VL53L0X_StartMeasurement\n"); - Status = VL53L0X_start_measurement(); - } - } - - return Status; + if (ErrState == VL53L0X_OK) + { Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,signalLimit);} + + if (ErrState == VL53L0X_OK) + { Set_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,sigmaLimit);} + + if (ErrState == VL53L0X_OK) + { Set_Measure_Time_Budget_us(timingBudget);} + + if (ErrState == VL53L0X_OK) + { Set_vcsel_PPeriod(VL53L0X_VCSEL_PRE_RANGE,preRangeVcselPeriod); } + + if (ErrState == VL53L0X_OK) + { Set_vcsel_PPeriod(VL53L0X_VCSEL_FINAL_RANGE,finalRangeVcselPeriod);} + + if (ErrState == VL53L0X_OK) + { Perf_Ref_calibration(&VhvSettings,&PhaseCal,1); } + break; + case op_poll: // Setup in continuous ranging mode + Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); + Start_Measurement(); + } // switch + return ErrState; +} + +VL53L0X_Error VL53L0X::Stop_Measurement(TOperatingMode operating_mode) +{ if ((ErrState == VL53L0X_OK) & + (operating_mode == op_INT || operating_mode == op_poll) ) + { // only stop if in one of the continuous modes !!!! + Stop_Measurement(); + Wait_Stop_Completed(); + Clear_interrupt_mask( REG_SYSINT_GPIO_NEW_SAMPLE_READY); + } + return ErrState; +} + +TRangeResults VL53L0X::Handle_irq(TOperatingMode operating_mode) +{ TRangeResults RangeResults; + RangeResults = Get_Measurement(operating_mode); + Enable_interrupt_measure_detection_irq(); + return RangeResults; +} + +/****************** Private device functions *************************/ + +void VL53L0X::Wait_read_strobe() +{ uint32_t loop_nb = 0; + + Write_Byte(0x83,0x00); // set strobe register to 0 + + /* polling while no error, no strobe, and not reached max number of loop to avoid deadlock*/ + while ((ErrState == VL53L0X_OK) && (Read_Byte(0x83) == 0x00) ) + { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) + { ErrState = VL53L0X_ERROR_TIME_OUT; } } + + Write_Byte(0x83,0x01); // set strobe register back to 1 'manually' } - -int VL53L0X::get_measurement(OperatingMode operating_mode,VL53L0X_RangingMeasurementData_t *p_data) -{ - int Status = VL53L0X_ERROR_NONE; - - if (operating_mode == range_single_shot_polling) { - Status = VL53L0X_perform_single_ranging_measurement(p_data); } - - if (operating_mode == range_continuous_polling) { - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_measurement_poll_for_completion(); } - - if (Status == VL53L0X_ERROR_NONE) { - Status = VL53L0X_get_ranging_measurement_data(p_data); - // Clear the interrupt - VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); - VL53L0X_polling_delay(); +void VL53L0X::Get_info_from_device(uint8_t option) +{ uint8_t byte; + uint32_t tmp_dword; + uint8_t module_id; + uint8_t revision; + uint8_t reference_SPAD_count = 0; + uint8_t reference_SPAD_type = 0; + uint32_t part_uid_upper = 0; + uint32_t part_uid_lower = 0; + uint32_t offset_fixed1104_mm = 0; + int16_t offset_um = 0; + uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4; + uint32_t dist_meas_fixed1104_400_mm = 0; + uint32_t signal_rate_meas_fixed1104_400_mm = 0; + char product_id[19]; + uint8_t read_data_from_device_done; + TFP1616 signal_rate_meas_fixed400_mm_fix = 0; + uint8_t nvm_Ref_good_SPAD_map[REF_SPAD_BUFFER_SIZE]; + int i; + + read_data_from_device_done = DevSpecParams.ReadDataFromDeviceDone; + + /* This access is done only once after that a GetDeviceInfo or datainit is done*/ + if (read_data_from_device_done != 7) { + Write_Byte(0x80,0x01); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x00); + Write_Byte(0xFF,0x06); + byte = Read_Byte(0x83); + Write_Byte(0x83,byte | 4); + Write_Byte(0xFF,0x07); + Write_Byte(0x81,0x01); + Polling_delay(); // warning, does nothing!! + Write_Byte(0x80,0x01); + + if (((option & 1) == 1) && + ((read_data_from_device_done & 1) == 0)) { + Write_Byte(0x94,0x6b); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + reference_SPAD_count = (uint8_t)((tmp_dword >> 8) & 0x7f); + reference_SPAD_type = (uint8_t)((tmp_dword >> 15) & 0x01); + + Write_Byte(0x94,0x24); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + nvm_Ref_good_SPAD_map[0] = (uint8_t)((tmp_dword >> 24)& 0xff); + nvm_Ref_good_SPAD_map[1] = (uint8_t)((tmp_dword >> 16)& 0xff); + nvm_Ref_good_SPAD_map[2] = (uint8_t)((tmp_dword >> 8)& 0xff); + nvm_Ref_good_SPAD_map[3] = (uint8_t)(tmp_dword & 0xff); + + Write_Byte(0x94,0x25); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + nvm_Ref_good_SPAD_map[4] = (uint8_t)((tmp_dword >> 24)& 0xff); + nvm_Ref_good_SPAD_map[5] = (uint8_t)((tmp_dword >> 16)& 0xff); } - } - - if (operating_mode == range_continuous_interrupt) { - Status = VL53L0X_get_ranging_measurement_data(p_data); - VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | - VL53L0X_REG_RESULT_INTERRUPT_STATUS); + + if (((option & 2) == 2) && ((read_data_from_device_done & 2) == 0)) { + Write_Byte(0x94,0x02); + Wait_read_strobe(); + module_id = Read_Byte(0x90); + + Write_Byte(0x94,0x7B); + Wait_read_strobe(); + revision = Read_Byte(0x90); + + Write_Byte(0x94,0x77); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + product_id[0] = (char)((tmp_dword >> 25) & 0x07f); + product_id[1] = (char)((tmp_dword >> 18) & 0x07f); + product_id[2] = (char)((tmp_dword >> 11) & 0x07f); + product_id[3] = (char)((tmp_dword >> 4) & 0x07f); + + byte = (uint8_t)((tmp_dword & 0x00f) << 3); + + Write_Byte(0x94,0x78); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + product_id[4] = (char)(byte +((tmp_dword >> 29) & 0x07f)); + product_id[5] = (char)((tmp_dword >> 22) & 0x07f); + product_id[6] = (char)((tmp_dword >> 15) & 0x07f); + product_id[7] = (char)((tmp_dword >> 8) & 0x07f); + product_id[8] = (char)((tmp_dword >> 1) & 0x07f); + byte = (uint8_t)((tmp_dword & 0x001) << 6); + + Write_Byte(0x94,0x79); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + product_id[9] = (char)(byte +((tmp_dword >> 26) & 0x07f)); + product_id[10] = (char)((tmp_dword >> 19) & 0x07f); + product_id[11] = (char)((tmp_dword >> 12) & 0x07f); + product_id[12] = (char)((tmp_dword >> 5) & 0x07f); + + byte = (uint8_t)((tmp_dword & 0x01f) << 2); + + Write_Byte(0x94,0x7A); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + product_id[13] = (char)(byte +((tmp_dword >> 30) & 0x07f)); + product_id[14] = (char)((tmp_dword >> 23) & 0x07f); + product_id[15] = (char)((tmp_dword >> 16) & 0x07f); + product_id[16] = (char)((tmp_dword >> 9) & 0x07f); + product_id[17] = (char)((tmp_dword >> 2) & 0x07f); + product_id[18] = '\0'; + } + + if (((option & 4) == 4) && ((read_data_from_device_done & 4) == 0)) + { Write_Byte(0x94,0x7B); + Wait_read_strobe(); + part_uid_upper = Read_DWord(0x90); + + Write_Byte(0x94,0x7C); + Wait_read_strobe(); + part_uid_lower = Read_DWord(0x90); + + Write_Byte(0x94,0x73); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + signal_rate_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff) << 8; + + Write_Byte(0x94,0x74); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + signal_rate_meas_fixed1104_400_mm |= ((tmp_dword & + 0xff000000) >> 24); + + Write_Byte(0x94,0x75); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)<< 8; + + Write_Byte(0x94,0x76); + Wait_read_strobe(); + tmp_dword = Read_DWord(0x90); + dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000) >> 24); + } + + Write_Byte(0x81,0x00); + Write_Byte(0xFF,0x06); + Write_Byte(0x83,Read_Byte(0x83) & 0xfb); + Write_Byte(0xFF,0x01); + Write_Byte(0x00,0x01); + Write_Byte(0xFF,0x00); + Write_Byte(0x80,0x00); } - return Status; -} - - -int VL53L0X::stop_measurement(OperatingMode operating_mode) -{ - int status = VL53L0X_ERROR_NONE; - - // don't need to stop for a singleshot range! - if (operating_mode == range_single_shot_polling) { } - - if (operating_mode == range_continuous_interrupt || - operating_mode == range_continuous_polling) { - // continuous mode - if (status == VL53L0X_ERROR_NONE) { - //printf("Call of VL53L0X_StopMeasurement\n"); - status = VL53L0X_stop_measurement(); + if ((ErrState == VL53L0X_OK) && (read_data_from_device_done != 7)) { + /* Assign to variable if ErrState is ok */ + if (((option & 1) == 1) && ((read_data_from_device_done & 1) == 0)) + { DevSpecParams.ReferenceSPADCount=reference_SPAD_count; + DevSpecParams.ReferenceSPADType =reference_SPAD_type; + for (i = 0; i < REF_SPAD_BUFFER_SIZE; i++) + { SPADData.RefGoodSPADMap[i] = nvm_Ref_good_SPAD_map[i]; } + } + + if (((option & 2) == 2) &&((read_data_from_device_done & 2) == 0)) + { DevSpecParams.ModuleId = module_id; + DevSpecParams.Revision = revision; + strcpy(DevSpecParams.ProductId, product_id); + } + + if (((option & 4) == 4) && ((read_data_from_device_done & 4) == 0)) { + DevSpecParams.PartUIDUpper = part_uid_upper; + DevSpecParams.PartUIDLower = part_uid_lower; + signal_rate_meas_fixed400_mm_fix = + FP97_TO_FP1616(signal_rate_meas_fixed1104_400_mm); + DevSpecParams.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix; + DevSpecParams.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix; + + offset_um = 0; + if (dist_meas_fixed1104_400_mm != 0) { + offset_fixed1104_mm = dist_meas_fixed1104_400_mm - + dist_meas_tgt_fixed1104_mm; + offset_um = (offset_fixed1104_mm * 1000) >> 4; + offset_um *= -1; + } + NVM_Offset_Cal_um = offset_um; } - - if (status == VL53L0X_ERROR_NONE) { - //printf("Wait Stop to be competed\n"); - status = wait_stop_completed(); - } - - if (status == VL53L0X_ERROR_NONE) { - status = VL53L0X_clear_interrupt_mask( - VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); } + byte = (uint8_t)(read_data_from_device_done | option); + DevSpecParams.ReadDataFromDeviceDone = byte; } - - return status; -} - - -int VL53L0X::handle_irq(OperatingMode operating_mode, - VL53L0X_RangingMeasurementData_t *data) -{ - int status; - status = get_measurement(operating_mode,data); - enable_interrupt_measure_detection_irq(); - return status; } /******************************************************************************/ +/****************** Small Service and wrapper functions **********************/ +/******************************************************************************/ +uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10,REF_ARRAY_SPAD_5, + REF_ARRAY_SPAD_0,REF_ARRAY_SPAD_5 }; + + +uint32_t VL53L0X::Decode_timeout(uint16_t encoded_timeout) +{ /*Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 */ + return ((uint32_t) (encoded_timeout & 0x00FF) + << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1; +} + +uint8_t VL53L0X::Is_ApertureSPAD(uint32_t SPAD_index) +{ /* This function reports if a given SPAD index is an aperture SPAD by + * deriving the quadrant = SPAD_index >> 6. */ + if (refArrayQuadrants[SPAD_index >> 6] == REF_ARRAY_SPAD_0) + { return 0; } else { return 1; } +} + +/******************************************************************************/ +/****************** Write and read functions from I2C *************************/ +/******************************************************************************/ + +void VL53L0X::Write_Byte(uint8_t index, uint8_t data) +{ I2c_Write(index,&data,1); } + +void VL53L0X::Write_Word(uint8_t index,uint16_t data) +{ uint8_t buffer[2]; + buffer[0] = data >> 8; + buffer[1] = data & 0x00FF; + I2c_Write(index,(uint8_t *)buffer,2); +} + +void VL53L0X::Write_DWord(uint8_t index, uint32_t data) +{ uint8_t buffer[4]; + buffer[0] = (data >> 24) & 0xFF; + buffer[1] = (data >> 16) & 0xFF; + buffer[2] = (data >> 8) & 0xFF; + buffer[3] = (data >> 0) & 0xFF; + I2c_Write(index,(uint8_t *)buffer,4); +} + +uint8_t VL53L0X::Read_Byte(uint8_t index) +{ uint8_t result; + I2c_Read(index,&result,1); + return result; +} + +uint16_t VL53L0X::Read_Word(uint8_t index) +{ uint8_t buffer[2] = {0,0}; + I2c_Read(index, &buffer[0], 2); + return (buffer[0] << 8) + buffer[1]; +} + +uint32_t VL53L0X::Read_DWord(uint8_t index) +{ uint8_t buffer[4] = {0,0,0,0}; + I2c_Read(index,buffer,4); + return (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; +} + +void VL53L0X::Register_BitMask(uint8_t index,uint8_t and_mask,uint8_t or_mask) +{ uint8_t buffer = 0; + /* read data direct onto buffer */ + I2c_Read(index,&buffer,1); + if (ErrState==VL53L0X_OK) { + buffer = (buffer & and_mask) | or_mask; + I2c_Write(index,&buffer,(uint8_t)1); + } +} + +/** + * @brief Writes a buffer towards the I2C peripheral device. + * @param RegisterAddr specifies the internal address register + * @param p_data pointer to the byte-array data to send + * where to start writing to (must be correctly masked). + * @param NumByteToWrite number of bytes to be written. + * @note On some devices if NumByteToWrite is greater + * than one, the RegisterAddr must be masked correctly! */ +void VL53L0X::I2c_Write( uint8_t RegisterAddr, uint8_t *p_data, + uint16_t NumByteToWrite ) +{ int ret; + uint8_t tmp[TEMP_BUF_SIZE]; + + if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State!!!! + + if (NumByteToWrite >= TEMP_BUF_SIZE) + {ErrState = VL53L0X_ERROR_I2C_BUF_OVERFLOW; return; }; + + /* First, send device address. Then, send data and terminate with STOP condition */ + tmp[0] = RegisterAddr; + memcpy(tmp+1, p_data, NumByteToWrite); + ret = _dev_i2c->write(I2cDevAddr, (const char*)tmp, NumByteToWrite+1, false); + + if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; } +} + + /**@brief Reads a buffer from the I2C peripheral device. + * @param pBuffer pointer to the byte-array to read data in to + * @param I2cDevAddr specifies the peripheral device slave address. + * @param RegisterAddr specifies the internal address register + * where to start reading from (must be correctly masked). + * @param NumByteToRead number of bytes to be read, maximum VL53L0X_MAX_I2C_XFER_SIZE + * @note On some devices if NumByteToWrite is greater + * than one, the RegisterAddr must be masked correctly! + */ +void VL53L0X::I2c_Read(uint8_t RegisterAddr, uint8_t *p_data, + uint16_t NumByteToRead) +{ int ret; + if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State, return value undefined!!!! + + /* Send device address, without STOP condition */ + ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true); + if(!ret) /* Read data, with STOP condition */ + { ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); } + + if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; } +} + +/******************************************************************************/