Condensed Version of Public VL53L0X
VL53L0X.cpp@12:aa177f0e4c10, 2019-04-10 (annotated)
- Committer:
- sepp_nepp
- Date:
- Wed Apr 10 19:03:16 2019 +0000
- Revision:
- 12:aa177f0e4c10
- Parent:
- 11:c6f95a42d4d7
Troy a gain
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nikapov | 0:a1a69d32f310 | 1 | /** |
nikapov | 0:a1a69d32f310 | 2 | ****************************************************************************** |
sepp_nepp | 11:c6f95a42d4d7 | 3 | * @file Class.cpp |
nikapov | 0:a1a69d32f310 | 4 | * @author IMG |
nikapov | 0:a1a69d32f310 | 5 | * @version V0.0.1 |
nikapov | 0:a1a69d32f310 | 6 | * @date 28-June-2016 |
nikapov | 0:a1a69d32f310 | 7 | * @brief Implementation file for the VL53L0X driver class |
nikapov | 0:a1a69d32f310 | 8 | ****************************************************************************** |
nikapov | 0:a1a69d32f310 | 9 | * @attention |
nikapov | 0:a1a69d32f310 | 10 | * |
nikapov | 0:a1a69d32f310 | 11 | * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> |
nikapov | 0:a1a69d32f310 | 12 | * |
sepp_nepp | 7:41cbc431e1f4 | 13 | * Redistribution and use in source and binary forms,with or without modification, |
nikapov | 0:a1a69d32f310 | 14 | * are permitted provided that the following conditions are met: |
nikapov | 0:a1a69d32f310 | 15 | * 1. Redistributions of source code must retain the above copyright notice, |
nikapov | 0:a1a69d32f310 | 16 | * this list of conditions and the following disclaimer. |
nikapov | 0:a1a69d32f310 | 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
nikapov | 0:a1a69d32f310 | 18 | * this list of conditions and the following disclaimer in the documentation |
nikapov | 0:a1a69d32f310 | 19 | * and/or other materials provided with the distribution. |
nikapov | 0:a1a69d32f310 | 20 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
nikapov | 0:a1a69d32f310 | 21 | * may be used to endorse or promote products derived from this software |
nikapov | 0:a1a69d32f310 | 22 | * without specific prior written permission. |
nikapov | 0:a1a69d32f310 | 23 | * |
nikapov | 0:a1a69d32f310 | 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
sepp_nepp | 7:41cbc431e1f4 | 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE |
nikapov | 0:a1a69d32f310 | 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
nikapov | 0:a1a69d32f310 | 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
sepp_nepp | 7:41cbc431e1f4 | 28 | * FOR ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL |
sepp_nepp | 7:41cbc431e1f4 | 29 | * DAMAGES (INCLUDING,BUT NOT LIMITED TO,PROCUREMENT OF SUBSTITUTE GOODS OR |
sepp_nepp | 7:41cbc431e1f4 | 30 | * SERVICES; LOSS OF USE,DATA,OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
sepp_nepp | 7:41cbc431e1f4 | 31 | * CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY, |
nikapov | 0:a1a69d32f310 | 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
sepp_nepp | 7:41cbc431e1f4 | 33 | * OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
nikapov | 0:a1a69d32f310 | 34 | * |
nikapov | 0:a1a69d32f310 | 35 | ****************************************************************************** |
nikapov | 0:a1a69d32f310 | 36 | */ |
nikapov | 0:a1a69d32f310 | 37 | |
sepp_nepp | 7:41cbc431e1f4 | 38 | // Some example regex that were used to replace useless macros |
sepp_nepp | 7:41cbc431e1f4 | 39 | // \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E |
sepp_nepp | 11:c6f95a42d4d7 | 40 | // _device->CurrParams.\1[\3] = \5; |
sepp_nepp | 11:c6f95a42d4d7 | 41 | // to replace this "#define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value)" by "_device->CurrParams.field[index] = value" |
sepp_nepp | 11:c6f95a42d4d7 | 42 | |
sepp_nepp | 11:c6f95a42d4d7 | 43 | // to replace "Read_Byte(0x90,&module_id);" by "module_id = Read_Byte(0x90);" search and replace |
sepp_nepp | 11:c6f95a42d4d7 | 44 | // \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E |
sepp_nepp | 11:c6f95a42d4d7 | 45 | // \3 = Read_Byte\(\1\); |
sepp_nepp | 7:41cbc431e1f4 | 46 | |
nikapov | 0:a1a69d32f310 | 47 | /* Includes */ |
nikapov | 0:a1a69d32f310 | 48 | #include <stdlib.h> |
nikapov | 0:a1a69d32f310 | 49 | #include "VL53L0X.h" |
sepp_nepp | 10:cd251e0fc2fd | 50 | #include "VL53L0X_tuning.h" |
sepp_nepp | 7:41cbc431e1f4 | 51 | |
sepp_nepp | 12:aa177f0e4c10 | 52 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 53 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 54 | /****************** All initialization functions *************************/ |
sepp_nepp | 12:aa177f0e4c10 | 55 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 56 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 57 | |
sepp_nepp | 11:c6f95a42d4d7 | 58 | // Function Data_init and Init_Sensor is united into Start_Sensor |
sepp_nepp | 11:c6f95a42d4d7 | 59 | VL53L0X_Error VL53L0X::Start_Sensor(uint8_t new_addr) |
sepp_nepp | 11:c6f95a42d4d7 | 60 | { ErrState = VL53L0X_OK; |
sepp_nepp | 11:c6f95a42d4d7 | 61 | |
sepp_nepp | 11:c6f95a42d4d7 | 62 | if (_gpio0) { // Can the shutdown pin be controlled? |
sepp_nepp | 11:c6f95a42d4d7 | 63 | *_gpio0 = 0; wait_ms(1); // quick shutdown |
sepp_nepp | 11:c6f95a42d4d7 | 64 | *_gpio0 = 1; wait_ms(10); // and back ON again |
sepp_nepp | 11:c6f95a42d4d7 | 65 | } |
sepp_nepp | 11:c6f95a42d4d7 | 66 | |
sepp_nepp | 11:c6f95a42d4d7 | 67 | /* Setup the I2C bus. By default the I2C is running at 1V8 if you |
sepp_nepp | 11:c6f95a42d4d7 | 68 | * want to change it you need to include this define at compilation level. */ |
sepp_nepp | 11:c6f95a42d4d7 | 69 | #ifdef USE_I2C_2V8 |
sepp_nepp | 11:c6f95a42d4d7 | 70 | VL53L0X_UpdateByte(REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 71 | #endif |
sepp_nepp | 11:c6f95a42d4d7 | 72 | /* Set I2C standard mode */ |
sepp_nepp | 11:c6f95a42d4d7 | 73 | Write_Byte(0x88,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 74 | |
sepp_nepp | 11:c6f95a42d4d7 | 75 | // read and check the device ID from the ID register |
sepp_nepp | 11:c6f95a42d4d7 | 76 | Device_Info.ProductType = Read_Byte(REG_IDENTIFICATION_MODEL_ID); |
sepp_nepp | 11:c6f95a42d4d7 | 77 | if ( (ErrState == VL53L0X_OK) && (Device_Info.ProductType != 0xEEAA) ) |
sepp_nepp | 11:c6f95a42d4d7 | 78 | {return VL53L0X_ERROR_I2C_WRONG_DEV_ID; } |
sepp_nepp | 11:c6f95a42d4d7 | 79 | |
sepp_nepp | 11:c6f95a42d4d7 | 80 | // reconfigure the address with a new address if requested |
sepp_nepp | 11:c6f95a42d4d7 | 81 | if ( (ErrState == VL53L0X_OK) && (new_addr != VL53L0X_DEFAULT_ADDRESS) ) |
sepp_nepp | 11:c6f95a42d4d7 | 82 | { Write_Byte(REG_I2C_SLAVE_DEVICE_ADDRESS, new_addr / 2); |
sepp_nepp | 11:c6f95a42d4d7 | 83 | I2cDevAddr = new_addr; |
sepp_nepp | 11:c6f95a42d4d7 | 84 | } |
sepp_nepp | 11:c6f95a42d4d7 | 85 | // quite if an error was raised |
sepp_nepp | 11:c6f95a42d4d7 | 86 | if (ErrState != VL53L0X_OK) {return ErrState; } |
sepp_nepp | 11:c6f95a42d4d7 | 87 | |
sepp_nepp | 11:c6f95a42d4d7 | 88 | /* Set Default static parameters |
sepp_nepp | 11:c6f95a42d4d7 | 89 | *set first temporary values 9.44MHz * 65536 = 618660 */ |
sepp_nepp | 12:aa177f0e4c10 | 90 | OscFrequencyMHz = 618660; |
sepp_nepp | 12:aa177f0e4c10 | 91 | RefSPADSInitialised = 0; |
sepp_nepp | 12:aa177f0e4c10 | 92 | |
sepp_nepp | 12:aa177f0e4c10 | 93 | // Read All NVM from device |
sepp_nepp | 12:aa177f0e4c10 | 94 | ReadNVMDataFromDeviceDone = 0; |
sepp_nepp | 12:aa177f0e4c10 | 95 | Get_all_NVM_info_from_device(); |
sepp_nepp | 11:c6f95a42d4d7 | 96 | |
sepp_nepp | 11:c6f95a42d4d7 | 97 | #ifdef USE_IQC_STATION |
sepp_nepp | 12:aa177f0e4c10 | 98 | // must make sure that first your read all NVM info used by the API */ |
sepp_nepp | 11:c6f95a42d4d7 | 99 | VL53L0X_Apply_Offset_Cal(); |
sepp_nepp | 11:c6f95a42d4d7 | 100 | #endif |
sepp_nepp | 11:c6f95a42d4d7 | 101 | |
sepp_nepp | 11:c6f95a42d4d7 | 102 | /* Default value is 1000 for Linearity Corrective Gain */ |
sepp_nepp | 11:c6f95a42d4d7 | 103 | LinearityCorrectiveGain = 1000; |
sepp_nepp | 11:c6f95a42d4d7 | 104 | |
sepp_nepp | 11:c6f95a42d4d7 | 105 | /* Dmax default Parameter */ |
sepp_nepp | 11:c6f95a42d4d7 | 106 | DmaxCalRangeMilliMeter = 400; |
sepp_nepp | 11:c6f95a42d4d7 | 107 | DmaxCalSignalRateRtnMHz = (TFP1616)((0x00016B85)); /* 1.42 No Cover Glass*/ |
sepp_nepp | 11:c6f95a42d4d7 | 108 | |
sepp_nepp | 11:c6f95a42d4d7 | 109 | /* Get default parameters */ |
sepp_nepp | 11:c6f95a42d4d7 | 110 | CurrParams = Get_device_parameters(); |
sepp_nepp | 11:c6f95a42d4d7 | 111 | |
sepp_nepp | 11:c6f95a42d4d7 | 112 | /* Set Default Xtalk_CompRate_MHz to 0 */ |
sepp_nepp | 11:c6f95a42d4d7 | 113 | CurrParams.Xtalk_CompRate_MHz = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 114 | |
sepp_nepp | 11:c6f95a42d4d7 | 115 | /* initialize CurrParams values */ |
sepp_nepp | 11:c6f95a42d4d7 | 116 | CurrParams.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; |
sepp_nepp | 11:c6f95a42d4d7 | 117 | CurrParams.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED; |
sepp_nepp | 11:c6f95a42d4d7 | 118 | |
sepp_nepp | 11:c6f95a42d4d7 | 119 | /* Sigma estimator variable */ |
sepp_nepp | 11:c6f95a42d4d7 | 120 | SigmaEstRefArray = 100; |
sepp_nepp | 11:c6f95a42d4d7 | 121 | SigmaEstEffPulseWidth = 900; |
sepp_nepp | 11:c6f95a42d4d7 | 122 | SigmaEstEffAmbWidth = 500; |
sepp_nepp | 11:c6f95a42d4d7 | 123 | targetRefRate = 0x0A00; /* 20 MHz in 9:7 format */ |
sepp_nepp | 11:c6f95a42d4d7 | 124 | |
sepp_nepp | 11:c6f95a42d4d7 | 125 | /* Use internal default settings */ |
sepp_nepp | 11:c6f95a42d4d7 | 126 | UseInternalTuningSettings = 1; |
sepp_nepp | 11:c6f95a42d4d7 | 127 | Write_Byte(0x80,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 128 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 129 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 130 | StopVariable = Read_Byte(0x91); |
sepp_nepp | 11:c6f95a42d4d7 | 131 | Write_Byte(0x00,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 132 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 133 | Write_Byte(0x80,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 134 | |
sepp_nepp | 11:c6f95a42d4d7 | 135 | // quite if an error was raised |
sepp_nepp | 11:c6f95a42d4d7 | 136 | if (ErrState != VL53L0X_OK) {return ErrState; } |
sepp_nepp | 11:c6f95a42d4d7 | 137 | |
sepp_nepp | 11:c6f95a42d4d7 | 138 | /* Disable the following SW-internal checks plaus set some values */ |
sepp_nepp | 11:c6f95a42d4d7 | 139 | CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_REF_CLIP] = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 140 | CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIG_REF_CLIP] = (35 * 65536); |
sepp_nepp | 11:c6f95a42d4d7 | 141 | CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 142 | CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 143 | |
sepp_nepp | 11:c6f95a42d4d7 | 144 | /* Disable the following Device-Internal Checks: */ |
sepp_nepp | 11:c6f95a42d4d7 | 145 | CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_MSRC] = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 146 | CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE] = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 147 | Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEE, 0); |
sepp_nepp | 11:c6f95a42d4d7 | 148 | |
sepp_nepp | 11:c6f95a42d4d7 | 149 | /* Only enable this internal Check : */ |
sepp_nepp | 11:c6f95a42d4d7 | 150 | CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = 1; |
sepp_nepp | 11:c6f95a42d4d7 | 151 | CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = (18 * 65536); |
sepp_nepp | 11:c6f95a42d4d7 | 152 | |
sepp_nepp | 11:c6f95a42d4d7 | 153 | /* Plus Enable VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE check */ |
sepp_nepp | 11:c6f95a42d4d7 | 154 | Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); |
sepp_nepp | 11:c6f95a42d4d7 | 155 | |
sepp_nepp | 11:c6f95a42d4d7 | 156 | if (ErrState == VL53L0X_OK) { /* 0.25 in FP1616 notation 65536 */ |
sepp_nepp | 11:c6f95a42d4d7 | 157 | Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE, |
sepp_nepp | 11:c6f95a42d4d7 | 158 | (TFP1616)(25 * 65536 / 100)); } |
sepp_nepp | 11:c6f95a42d4d7 | 159 | |
sepp_nepp | 11:c6f95a42d4d7 | 160 | // quit if an error was raised |
sepp_nepp | 11:c6f95a42d4d7 | 161 | if (ErrState != VL53L0X_OK) {return ErrState; } |
sepp_nepp | 11:c6f95a42d4d7 | 162 | |
sepp_nepp | 11:c6f95a42d4d7 | 163 | // Preset the Config States |
sepp_nepp | 11:c6f95a42d4d7 | 164 | SequenceConfig = 0xFF ; |
sepp_nepp | 11:c6f95a42d4d7 | 165 | |
sepp_nepp | 11:c6f95a42d4d7 | 166 | /* Set Device state to tell that we are waiting for call to VL53L0X_StaticInit */ |
sepp_nepp | 11:c6f95a42d4d7 | 167 | Current_State = VL53L0X_STATE_WAIT_STATICINIT ; |
sepp_nepp | 11:c6f95a42d4d7 | 168 | |
sepp_nepp | 11:c6f95a42d4d7 | 169 | Fill_device_info(); // Retrieve Silicon version, stored in Device_Info |
sepp_nepp | 11:c6f95a42d4d7 | 170 | |
sepp_nepp | 11:c6f95a42d4d7 | 171 | uint32_t ref_SPAD_count; |
sepp_nepp | 11:c6f95a42d4d7 | 172 | uint8_t is_aperture_SPADS; |
sepp_nepp | 11:c6f95a42d4d7 | 173 | uint8_t vhv_settings; |
sepp_nepp | 11:c6f95a42d4d7 | 174 | uint8_t phase_cal; |
sepp_nepp | 11:c6f95a42d4d7 | 175 | |
sepp_nepp | 12:aa177f0e4c10 | 176 | // make sure the Get_all_NVM_info_from_device was called before calling static init |
sepp_nepp | 11:c6f95a42d4d7 | 177 | if (ErrState == VL53L0X_OK) { Static_init(); } // Device Initialization |
sepp_nepp | 11:c6f95a42d4d7 | 178 | |
sepp_nepp | 11:c6f95a42d4d7 | 179 | if (ErrState == VL53L0X_OK) { // Device Calibration |
sepp_nepp | 11:c6f95a42d4d7 | 180 | Perf_Ref_calibration( &vhv_settings, &phase_cal, 1); } |
sepp_nepp | 11:c6f95a42d4d7 | 181 | |
sepp_nepp | 11:c6f95a42d4d7 | 182 | if (ErrState == VL53L0X_OK) { // SPAD Configuration |
sepp_nepp | 11:c6f95a42d4d7 | 183 | Perf_Ref_SPAD_management( &ref_SPAD_count, &is_aperture_SPADS); } |
sepp_nepp | 11:c6f95a42d4d7 | 184 | |
sepp_nepp | 11:c6f95a42d4d7 | 185 | return ErrState; |
sepp_nepp | 11:c6f95a42d4d7 | 186 | } |
sepp_nepp | 11:c6f95a42d4d7 | 187 | |
sepp_nepp | 11:c6f95a42d4d7 | 188 | void VL53L0X::Fill_device_info() |
sepp_nepp | 11:c6f95a42d4d7 | 189 | { uint8_t revision; |
sepp_nepp | 11:c6f95a42d4d7 | 190 | |
sepp_nepp | 11:c6f95a42d4d7 | 191 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 192 | { if (ModuleId == 0) |
sepp_nepp | 11:c6f95a42d4d7 | 193 | { revision = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 194 | strcpy(Device_Info.ProductId,""); } |
sepp_nepp | 11:c6f95a42d4d7 | 195 | else |
sepp_nepp | 12:aa177f0e4c10 | 196 | { revision = Revision; |
sepp_nepp | 12:aa177f0e4c10 | 197 | strcpy(Device_Info.ProductId,ProductId); |
sepp_nepp | 11:c6f95a42d4d7 | 198 | } |
sepp_nepp | 11:c6f95a42d4d7 | 199 | if (revision == 0) |
sepp_nepp | 11:c6f95a42d4d7 | 200 | { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS0); } |
sepp_nepp | 11:c6f95a42d4d7 | 201 | else if ((revision <= 34) && (revision != 32)) |
sepp_nepp | 11:c6f95a42d4d7 | 202 | { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS1); } |
sepp_nepp | 11:c6f95a42d4d7 | 203 | else if (revision < 39) |
sepp_nepp | 11:c6f95a42d4d7 | 204 | { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS2); } |
sepp_nepp | 11:c6f95a42d4d7 | 205 | else { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_ES1); } |
sepp_nepp | 11:c6f95a42d4d7 | 206 | strcpy(Device_Info.Type,VL53L0X_STRING_DEVICE_INFO_TYPE); |
sepp_nepp | 11:c6f95a42d4d7 | 207 | } |
sepp_nepp | 11:c6f95a42d4d7 | 208 | |
sepp_nepp | 11:c6f95a42d4d7 | 209 | Device_Info.ProductRevisionMajor = 1; |
sepp_nepp | 11:c6f95a42d4d7 | 210 | Device_Info.ProductRevisionMinor = |
sepp_nepp | 11:c6f95a42d4d7 | 211 | (Read_Byte(REG_IDENTIFICATION_REVISION_ID) & 0xF0) >> 4; |
nikapov | 0:a1a69d32f310 | 212 | } |
sepp_nepp | 11:c6f95a42d4d7 | 213 | |
sepp_nepp | 12:aa177f0e4c10 | 214 | /* That was 'Get_info_from_device( unit8_t option)' where option was composed of |
sepp_nepp | 12:aa177f0e4c10 | 215 | 1, 2, 4 depending what info was requested. |
sepp_nepp | 12:aa177f0e4c10 | 216 | It was decided to combine all that into a single Get ALL infos from the device |
sepp_nepp | 12:aa177f0e4c10 | 217 | that is called once at Sensor Init so all the job is done. |
sepp_nepp | 12:aa177f0e4c10 | 218 | In addition, originally values would first be read into local variables and then |
sepp_nepp | 12:aa177f0e4c10 | 219 | only copied to class fields if no errors. |
sepp_nepp | 12:aa177f0e4c10 | 220 | However, if at device initialization an error is raised the whole class instance |
sepp_nepp | 12:aa177f0e4c10 | 221 | cannot be used anyway, so decided that we directly read into class fields. */ |
sepp_nepp | 12:aa177f0e4c10 | 222 | |
sepp_nepp | 12:aa177f0e4c10 | 223 | void VL53L0X::Get_all_NVM_info_from_device() |
sepp_nepp | 12:aa177f0e4c10 | 224 | { uint32_t tmp_dword; |
sepp_nepp | 12:aa177f0e4c10 | 225 | uint32_t offset_fixed1104_mm = 0; |
sepp_nepp | 12:aa177f0e4c10 | 226 | int16_t offset_um = 0; |
sepp_nepp | 12:aa177f0e4c10 | 227 | uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4; |
sepp_nepp | 12:aa177f0e4c10 | 228 | uint32_t dist_meas_fixed1104_400_mm = 0; |
sepp_nepp | 12:aa177f0e4c10 | 229 | uint32_t signal_rate_meas_fixed1104_400_mm = 0; |
sepp_nepp | 12:aa177f0e4c10 | 230 | int i; |
sepp_nepp | 12:aa177f0e4c10 | 231 | |
sepp_nepp | 12:aa177f0e4c10 | 232 | /* This access is done only once after that a GetDeviceInfo or datainit is done*/ |
sepp_nepp | 12:aa177f0e4c10 | 233 | if (ReadNVMDataFromDeviceDone == 7) { return ; } |
sepp_nepp | 12:aa177f0e4c10 | 234 | |
sepp_nepp | 12:aa177f0e4c10 | 235 | Write_Byte(0x80,0x01); |
sepp_nepp | 12:aa177f0e4c10 | 236 | Write_Byte(0xFF,0x01); |
sepp_nepp | 12:aa177f0e4c10 | 237 | Write_Byte(0x00,0x00); |
sepp_nepp | 12:aa177f0e4c10 | 238 | Write_Byte(0xFF,0x06); |
sepp_nepp | 12:aa177f0e4c10 | 239 | Register_BitMask(0x83,0xFF,0x04) |
sepp_nepp | 12:aa177f0e4c10 | 240 | Write_Byte(0xFF,0x07); |
sepp_nepp | 12:aa177f0e4c10 | 241 | Write_Byte(0x81,0x01); |
sepp_nepp | 12:aa177f0e4c10 | 242 | Polling_delay(); // warning, does nothing!! |
sepp_nepp | 12:aa177f0e4c10 | 243 | Write_Byte(0x80,0x01); |
sepp_nepp | 12:aa177f0e4c10 | 244 | |
sepp_nepp | 12:aa177f0e4c10 | 245 | /* ************* First Block of NVM data is read: ************/ |
sepp_nepp | 12:aa177f0e4c10 | 246 | tmp_dword = Get_NVM_DWord(0x6b); |
sepp_nepp | 12:aa177f0e4c10 | 247 | ReferenceSPADCount = (uint8_t)((tmp_dword >> 8) & 0x7f); |
sepp_nepp | 12:aa177f0e4c10 | 248 | ReferenceSPADType = (uint8_t)((tmp_dword >> 15) & 0x01); |
sepp_nepp | 12:aa177f0e4c10 | 249 | |
sepp_nepp | 12:aa177f0e4c10 | 250 | tmp_dword = Get_NVM_DWord(0x24); |
sepp_nepp | 12:aa177f0e4c10 | 251 | SPADData.RefGoodSPADMap[0] = (uint8_t)((tmp_dword >> 24)& 0xff); |
sepp_nepp | 12:aa177f0e4c10 | 252 | SPADData.RefGoodSPADMap[1] = (uint8_t)((tmp_dword >> 16)& 0xff); |
sepp_nepp | 12:aa177f0e4c10 | 253 | SPADData.RefGoodSPADMap[2] = (uint8_t)((tmp_dword >> 8)& 0xff); |
sepp_nepp | 12:aa177f0e4c10 | 254 | SPADData.RefGoodSPADMap[3] = (uint8_t)(tmp_dword & 0xff); |
sepp_nepp | 12:aa177f0e4c10 | 255 | |
sepp_nepp | 12:aa177f0e4c10 | 256 | tmp_dword = Get_NVM_DWord(0x25); |
sepp_nepp | 12:aa177f0e4c10 | 257 | SPADData.RefGoodSPADMap[4] = (uint8_t)((tmp_dword >> 24)& 0xff); |
sepp_nepp | 12:aa177f0e4c10 | 258 | SPADData.RefGoodSPADMap[5] = (uint8_t)((tmp_dword >> 16)& 0xff); |
sepp_nepp | 12:aa177f0e4c10 | 259 | |
sepp_nepp | 12:aa177f0e4c10 | 260 | /* ************* Second Block of NVM data is read: ************/ |
sepp_nepp | 12:aa177f0e4c10 | 261 | ModuleId = Get_NVM_Byte(0x02); |
sepp_nepp | 12:aa177f0e4c10 | 262 | Revision = Get_NVM_Byte(0x02); |
sepp_nepp | 12:aa177f0e4c10 | 263 | |
sepp_nepp | 12:aa177f0e4c10 | 264 | tmp_dword = Get_NVM_DWord(0x77); |
sepp_nepp | 12:aa177f0e4c10 | 265 | ProductId[0] = (char)((tmp_dword >> 25) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 266 | ProductId[1] = (char)((tmp_dword >> 18) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 267 | ProductId[2] = (char)((tmp_dword >> 11) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 268 | ProductId[3] = (char)((tmp_dword >> 4) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 269 | ProductId[4] = (char)((tmp_dword << 3) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 270 | |
sepp_nepp | 12:aa177f0e4c10 | 271 | tmp_dword = Get_NVM_DWord(0x78); |
sepp_nepp | 12:aa177f0e4c10 | 272 | ProductId[4] = ProductId[4] | |
sepp_nepp | 12:aa177f0e4c10 | 273 | (char)((tmp_dword >> 29) & 0x07f)); |
sepp_nepp | 12:aa177f0e4c10 | 274 | ProductId[5] = (char)((tmp_dword >> 22) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 275 | ProductId[6] = (char)((tmp_dword >> 15) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 276 | ProductId[7] = (char)((tmp_dword >> 8) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 277 | ProductId[8] = (char)((tmp_dword >> 1) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 278 | ProductId[9] = (char)((tmp_dword << 6) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 279 | |
sepp_nepp | 12:aa177f0e4c10 | 280 | tmp_dword = Get_NVM_DWord(0x79); |
sepp_nepp | 12:aa177f0e4c10 | 281 | ProductId[9] = ProductId[9] | |
sepp_nepp | 12:aa177f0e4c10 | 282 | (char)((tmp_dword >> 26) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 283 | ProductId[10] = (char)((tmp_dword >> 19) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 284 | ProductId[11] = (char)((tmp_dword >> 12) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 285 | ProductId[12] = (char)((tmp_dword >> 5) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 286 | ProductId[13] = (char)((tmp_dword << 2) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 287 | |
sepp_nepp | 12:aa177f0e4c10 | 288 | tmp_dword = Get_NVM_DWord(0x7A); |
sepp_nepp | 12:aa177f0e4c10 | 289 | ProductId[13] = ProductId[13] | |
sepp_nepp | 12:aa177f0e4c10 | 290 | (char)((tmp_dword >> 30) & 0x07f)); |
sepp_nepp | 12:aa177f0e4c10 | 291 | ProductId[14] = (char)((tmp_dword >> 23) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 292 | ProductId[15] = (char)((tmp_dword >> 16) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 293 | ProductId[16] = (char)((tmp_dword >> 9) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 294 | ProductId[17] = (char)((tmp_dword >> 2) & 0x07f); |
sepp_nepp | 12:aa177f0e4c10 | 295 | ProductId[18] = '\0'; |
sepp_nepp | 12:aa177f0e4c10 | 296 | |
sepp_nepp | 12:aa177f0e4c10 | 297 | /* ************* Third Block of NVM data is read: ************/ |
sepp_nepp | 12:aa177f0e4c10 | 298 | PartUIDUpper = Get_NVM_DWord(0x7B); |
sepp_nepp | 12:aa177f0e4c10 | 299 | PartUIDLower = Get_NVM_DWord(0x7C); |
sepp_nepp | 12:aa177f0e4c10 | 300 | SignalRateMeasFixed400mm = // convert from FP97_TO_FP1616 |
sepp_nepp | 12:aa177f0e4c10 | 301 | ( (( Get_NVM_DWord(0x73) << 17) & 0x1fE0000) | |
sepp_nepp | 12:aa177f0e4c10 | 302 | (( Get_NVM_DWord(0x74) >> 15) & 0x001fE00) ) ; |
sepp_nepp | 12:aa177f0e4c10 | 303 | |
sepp_nepp | 12:aa177f0e4c10 | 304 | dist_meas_fixed1104_400_mm = |
sepp_nepp | 12:aa177f0e4c10 | 305 | (( Get_NVM_DWord(0x75) << 8) & 0xff00) | |
sepp_nepp | 12:aa177f0e4c10 | 306 | (( Get_NVM_DWord(0x76) >> 24) & 0x00ff); |
sepp_nepp | 12:aa177f0e4c10 | 307 | |
sepp_nepp | 12:aa177f0e4c10 | 308 | if (dist_meas_fixed1104_400_mm != 0) { |
sepp_nepp | 12:aa177f0e4c10 | 309 | offset_fixed1104_mm = dist_meas_fixed1104_400_mm - |
sepp_nepp | 12:aa177f0e4c10 | 310 | dist_meas_tgt_fixed1104_mm; |
sepp_nepp | 12:aa177f0e4c10 | 311 | NVM_Offset_Cal_um = (offset_fixed1104_mm * 1000) >> 4; |
sepp_nepp | 12:aa177f0e4c10 | 312 | NVM_Offset_Cal_um *= -1; } |
sepp_nepp | 12:aa177f0e4c10 | 313 | else { NVM_Offset_Cal_um = 0; } |
sepp_nepp | 12:aa177f0e4c10 | 314 | |
sepp_nepp | 12:aa177f0e4c10 | 315 | Write_Byte(0x81,0x00); |
sepp_nepp | 12:aa177f0e4c10 | 316 | Write_Byte(0xFF,0x06); |
sepp_nepp | 12:aa177f0e4c10 | 317 | Register_BitMask(0x83,0xfb,0x00) |
sepp_nepp | 12:aa177f0e4c10 | 318 | Write_Byte(0xFF,0x01); |
sepp_nepp | 12:aa177f0e4c10 | 319 | Write_Byte(0x00,0x01); |
sepp_nepp | 12:aa177f0e4c10 | 320 | Write_Byte(0xFF,0x00); |
sepp_nepp | 12:aa177f0e4c10 | 321 | Write_Byte(0x80,0x00); |
sepp_nepp | 12:aa177f0e4c10 | 322 | ReadNVMDataFromDeviceDone = 7; |
sepp_nepp | 12:aa177f0e4c10 | 323 | } |
sepp_nepp | 11:c6f95a42d4d7 | 324 | |
sepp_nepp | 11:c6f95a42d4d7 | 325 | uint32_t VL53L0X::Get_distance() |
sepp_nepp | 11:c6f95a42d4d7 | 326 | { ErrState = VL53L0X_OK; |
sepp_nepp | 11:c6f95a42d4d7 | 327 | TRangeResults p_ranging_results; |
sepp_nepp | 11:c6f95a42d4d7 | 328 | |
sepp_nepp | 11:c6f95a42d4d7 | 329 | Start_Measurement(op_single_shot_poll, NULL); |
sepp_nepp | 11:c6f95a42d4d7 | 330 | if (ErrState==VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 331 | { p_ranging_results = Get_Measurement(op_single_shot_poll); } |
sepp_nepp | 11:c6f95a42d4d7 | 332 | |
sepp_nepp | 11:c6f95a42d4d7 | 333 | Stop_Measurement(op_single_shot_poll); |
sepp_nepp | 11:c6f95a42d4d7 | 334 | |
sepp_nepp | 11:c6f95a42d4d7 | 335 | if (p_ranging_results.RangeStatus == 0) // we have a valid range ? |
sepp_nepp | 11:c6f95a42d4d7 | 336 | { return p_ranging_results.RangeMilliMeter; } |
sepp_nepp | 11:c6f95a42d4d7 | 337 | else |
sepp_nepp | 11:c6f95a42d4d7 | 338 | { ErrState = VL53L0X_ERROR_RANGE_ERROR; return 0;} |
nikapov | 0:a1a69d32f310 | 339 | } |
nikapov | 0:a1a69d32f310 | 340 | |
sepp_nepp | 12:aa177f0e4c10 | 341 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 342 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 343 | /****************** Actual Measurement functions *************************/ |
sepp_nepp | 12:aa177f0e4c10 | 344 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 345 | /******************************************************************************/ |
sepp_nepp | 12:aa177f0e4c10 | 346 | |
sepp_nepp | 11:c6f95a42d4d7 | 347 | TRangeResults VL53L0X::Get_Measurement(TOperatingMode operating_mode) |
sepp_nepp | 11:c6f95a42d4d7 | 348 | { TRangeResults p_data; |
sepp_nepp | 11:c6f95a42d4d7 | 349 | |
sepp_nepp | 11:c6f95a42d4d7 | 350 | switch (operating_mode) { |
sepp_nepp | 11:c6f95a42d4d7 | 351 | case op_single_shot_poll: |
sepp_nepp | 11:c6f95a42d4d7 | 352 | Perf_single_ranging_measurement(&p_data); |
sepp_nepp | 11:c6f95a42d4d7 | 353 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 354 | case op_poll: |
sepp_nepp | 11:c6f95a42d4d7 | 355 | Poll_Measure_Completion(); |
sepp_nepp | 11:c6f95a42d4d7 | 356 | Get_ranging_results(&p_data); |
sepp_nepp | 11:c6f95a42d4d7 | 357 | if (ErrState == VL53L0X_OK) { // Clear the interrupt |
sepp_nepp | 11:c6f95a42d4d7 | 358 | Clear_interrupt_mask(REG_SYSINT_GPIO_NEW_SAMPLE_READY); |
sepp_nepp | 11:c6f95a42d4d7 | 359 | Polling_delay(); |
sepp_nepp | 11:c6f95a42d4d7 | 360 | } |
sepp_nepp | 11:c6f95a42d4d7 | 361 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 362 | case op_INT: |
sepp_nepp | 11:c6f95a42d4d7 | 363 | Get_ranging_results(&p_data); |
sepp_nepp | 11:c6f95a42d4d7 | 364 | Clear_interrupt_mask(REG_SYSINT_CLEAR | REG_RESULT_INTERRUPT_STATUS); |
sepp_nepp | 11:c6f95a42d4d7 | 365 | } // switch |
sepp_nepp | 11:c6f95a42d4d7 | 366 | return p_data; |
sepp_nepp | 11:c6f95a42d4d7 | 367 | } |
sepp_nepp | 11:c6f95a42d4d7 | 368 | |
sepp_nepp | 11:c6f95a42d4d7 | 369 | /** Get part to part calibration offset; Should only be used after a |
sepp_nepp | 11:c6f95a42d4d7 | 370 | successful call to @a VL53L0X_DataInit to backup device NVM value **/ |
sepp_nepp | 11:c6f95a42d4d7 | 371 | int32_t VL53L0X::Get_Offset_Cal_um() |
sepp_nepp | 11:c6f95a42d4d7 | 372 | { uint16_t range_offset_register; |
nikapov | 0:a1a69d32f310 | 373 | int16_t c_max_offset = 2047; |
nikapov | 0:a1a69d32f310 | 374 | int16_t c_offset_range = 4096; |
nikapov | 0:a1a69d32f310 | 375 | |
nikapov | 0:a1a69d32f310 | 376 | /* Note that offset has 10.2 format */ |
sepp_nepp | 11:c6f95a42d4d7 | 377 | range_offset_register = Read_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM); |
sepp_nepp | 11:c6f95a42d4d7 | 378 | |
sepp_nepp | 11:c6f95a42d4d7 | 379 | if (ErrState == VL53L0X_OK) { |
nikapov | 0:a1a69d32f310 | 380 | range_offset_register = (range_offset_register & 0x0fff); |
nikapov | 0:a1a69d32f310 | 381 | |
sepp_nepp | 11:c6f95a42d4d7 | 382 | /* Apply 12 bit 2's complement conversion */ |
sepp_nepp | 11:c6f95a42d4d7 | 383 | if (range_offset_register > c_max_offset) |
sepp_nepp | 11:c6f95a42d4d7 | 384 | { return (int16_t)(range_offset_register - c_offset_range) * 250; } |
sepp_nepp | 11:c6f95a42d4d7 | 385 | else |
sepp_nepp | 11:c6f95a42d4d7 | 386 | { return (int16_t)range_offset_register * 250;} |
nikapov | 0:a1a69d32f310 | 387 | } |
sepp_nepp | 11:c6f95a42d4d7 | 388 | else return 0; |
nikapov | 0:a1a69d32f310 | 389 | } |
nikapov | 0:a1a69d32f310 | 390 | |
sepp_nepp | 11:c6f95a42d4d7 | 391 | void VL53L0X::Set_Offset_Cal_um(int32_t Offset_Cal_um) |
sepp_nepp | 11:c6f95a42d4d7 | 392 | { int32_t c_max_offset_um = 511000; |
sepp_nepp | 10:cd251e0fc2fd | 393 | int32_t c_min_offset_um = -512000; |
sepp_nepp | 11:c6f95a42d4d7 | 394 | int16_t c_offset_range = 4096; |
nikapov | 0:a1a69d32f310 | 395 | uint32_t encoded_offset_val; |
nikapov | 0:a1a69d32f310 | 396 | |
sepp_nepp | 11:c6f95a42d4d7 | 397 | if (Offset_Cal_um > c_max_offset_um) { Offset_Cal_um = c_max_offset_um; } |
sepp_nepp | 11:c6f95a42d4d7 | 398 | else |
sepp_nepp | 11:c6f95a42d4d7 | 399 | if (Offset_Cal_um < c_min_offset_um) { Offset_Cal_um = c_min_offset_um; } |
nikapov | 0:a1a69d32f310 | 400 | |
nikapov | 0:a1a69d32f310 | 401 | /* The offset register is 10.2 format and units are mm |
sepp_nepp | 10:cd251e0fc2fd | 402 | * therefore conversion is applied by a division of 250. */ |
sepp_nepp | 11:c6f95a42d4d7 | 403 | if (Offset_Cal_um >= 0) { encoded_offset_val = Offset_Cal_um / 250; } |
sepp_nepp | 11:c6f95a42d4d7 | 404 | else { encoded_offset_val = c_offset_range + Offset_Cal_um / 250; } |
sepp_nepp | 11:c6f95a42d4d7 | 405 | |
sepp_nepp | 11:c6f95a42d4d7 | 406 | Write_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, encoded_offset_val); |
nikapov | 0:a1a69d32f310 | 407 | } |
nikapov | 0:a1a69d32f310 | 408 | |
sepp_nepp | 11:c6f95a42d4d7 | 409 | void VL53L0X::VL53L0X_Apply_Offset_Cal() |
sepp_nepp | 11:c6f95a42d4d7 | 410 | { int32_t Summed_Offset_Cal_um; |
sepp_nepp | 11:c6f95a42d4d7 | 411 | |
sepp_nepp | 11:c6f95a42d4d7 | 412 | /* Read back current device offset, and remember in case later someone wants to use it */ |
sepp_nepp | 11:c6f95a42d4d7 | 413 | if (ErrState == VL53L0X_OK) { Last_Offset_Cal_um = Get_Offset_Cal_um(); } |
nikapov | 0:a1a69d32f310 | 414 | |
nikapov | 0:a1a69d32f310 | 415 | /* Apply Offset Adjustment derived from 400mm measurements */ |
sepp_nepp | 11:c6f95a42d4d7 | 416 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 417 | { Summed_Offset_Cal_um = Last_Offset_Cal_um + (int32_t) NVM_Offset_Cal_um; |
sepp_nepp | 11:c6f95a42d4d7 | 418 | Set_Offset_Cal_um(Summed_Offset_Cal_um); |
sepp_nepp | 11:c6f95a42d4d7 | 419 | /* remember current,adjusted offset */ |
sepp_nepp | 11:c6f95a42d4d7 | 420 | if (ErrState == VL53L0X_OK) { CurrParams.Offset_Cal_um = Summed_Offset_Cal_um; } |
sepp_nepp | 11:c6f95a42d4d7 | 421 | } |
sepp_nepp | 11:c6f95a42d4d7 | 422 | } |
sepp_nepp | 11:c6f95a42d4d7 | 423 | |
sepp_nepp | 11:c6f95a42d4d7 | 424 | void VL53L0X::Get_measure_period_ms(uint32_t *p_measure_period_ms) |
sepp_nepp | 11:c6f95a42d4d7 | 425 | { uint16_t osc_calibrate_val; |
sepp_nepp | 11:c6f95a42d4d7 | 426 | uint32_t im_period_ms; |
sepp_nepp | 11:c6f95a42d4d7 | 427 | |
sepp_nepp | 11:c6f95a42d4d7 | 428 | osc_calibrate_val = Read_Word(REG_OSC_CALIBRATE_VAL); |
sepp_nepp | 11:c6f95a42d4d7 | 429 | |
sepp_nepp | 11:c6f95a42d4d7 | 430 | if (ErrState == VL53L0X_OK) { im_period_ms = Read_DWord(REG_SYSTEM_MEASURE_PERIOD); } |
sepp_nepp | 11:c6f95a42d4d7 | 431 | |
sepp_nepp | 11:c6f95a42d4d7 | 432 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 433 | if (osc_calibrate_val != 0) |
sepp_nepp | 11:c6f95a42d4d7 | 434 | {*p_measure_period_ms = im_period_ms / osc_calibrate_val; } |
sepp_nepp | 11:c6f95a42d4d7 | 435 | CurrParams.Measure_Period_ms = *p_measure_period_ms; |
sepp_nepp | 11:c6f95a42d4d7 | 436 | } |
sepp_nepp | 11:c6f95a42d4d7 | 437 | } |
sepp_nepp | 11:c6f95a42d4d7 | 438 | |
sepp_nepp | 11:c6f95a42d4d7 | 439 | void VL53L0X::Get_Xtalk_CompRate_MHz( TFP1616 *p_Xtalk_CompRate_MHz) |
sepp_nepp | 11:c6f95a42d4d7 | 440 | { uint16_t value; |
sepp_nepp | 11:c6f95a42d4d7 | 441 | TFP1616 temp_fix1616; |
sepp_nepp | 11:c6f95a42d4d7 | 442 | |
sepp_nepp | 11:c6f95a42d4d7 | 443 | value = Read_Word(REG_XTALK_COMPENS_RATE_MHz); |
sepp_nepp | 11:c6f95a42d4d7 | 444 | |
sepp_nepp | 11:c6f95a42d4d7 | 445 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 446 | if (value == 0) { |
sepp_nepp | 11:c6f95a42d4d7 | 447 | /* the Xtalk is disabled return value from memory */ |
sepp_nepp | 11:c6f95a42d4d7 | 448 | temp_fix1616 = CurrParams.Xtalk_CompRate_MHz; |
sepp_nepp | 11:c6f95a42d4d7 | 449 | *p_Xtalk_CompRate_MHz = temp_fix1616; |
sepp_nepp | 11:c6f95a42d4d7 | 450 | CurrParams.XTalk_Compens_En = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 451 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 452 | temp_fix1616 = FP313_TO_FP1616(value); |
sepp_nepp | 11:c6f95a42d4d7 | 453 | *p_Xtalk_CompRate_MHz = temp_fix1616; |
sepp_nepp | 11:c6f95a42d4d7 | 454 | CurrParams.Xtalk_CompRate_MHz = temp_fix1616; |
sepp_nepp | 11:c6f95a42d4d7 | 455 | CurrParams.XTalk_Compens_En = 1; |
nikapov | 0:a1a69d32f310 | 456 | } |
nikapov | 0:a1a69d32f310 | 457 | } |
nikapov | 0:a1a69d32f310 | 458 | } |
nikapov | 0:a1a69d32f310 | 459 | |
sepp_nepp | 11:c6f95a42d4d7 | 460 | TFP1616 VL53L0X::Get_limit_chk_val( uint16_t limit_check_id ) |
sepp_nepp | 11:c6f95a42d4d7 | 461 | { uint16_t temp16; |
sepp_nepp | 11:c6f95a42d4d7 | 462 | TFP1616 temp_fix1616; |
nikapov | 0:a1a69d32f310 | 463 | |
nikapov | 0:a1a69d32f310 | 464 | switch (limit_check_id) { |
sepp_nepp | 11:c6f95a42d4d7 | 465 | case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* only internal computations: */ |
sepp_nepp | 11:c6f95a42d4d7 | 466 | case VL53L0X_CHECKEN_SIG_REF_CLIP: |
sepp_nepp | 11:c6f95a42d4d7 | 467 | case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: |
sepp_nepp | 11:c6f95a42d4d7 | 468 | return CurrParams.Limit_Chk_Val[limit_check_id];// need no more 'break'; |
sepp_nepp | 11:c6f95a42d4d7 | 469 | |
sepp_nepp | 11:c6f95a42d4d7 | 470 | case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 471 | temp16 = Read_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT); |
sepp_nepp | 11:c6f95a42d4d7 | 472 | temp_fix1616 = FP97_TO_FP1616(temp16); |
sepp_nepp | 11:c6f95a42d4d7 | 473 | if (temp_fix1616 == 0) /* disabled: return value from memory instead*/ |
sepp_nepp | 11:c6f95a42d4d7 | 474 | { temp_fix1616 = CurrParams.Limit_Chk_Val[limit_check_id]; |
sepp_nepp | 11:c6f95a42d4d7 | 475 | CurrParams.Limit_Chk_En[limit_check_id] = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 476 | else |
sepp_nepp | 11:c6f95a42d4d7 | 477 | { CurrParams.Limit_Chk_Val[limit_check_id] = temp_fix1616; |
sepp_nepp | 11:c6f95a42d4d7 | 478 | CurrParams.Limit_Chk_En[limit_check_id] = 1; } |
sepp_nepp | 11:c6f95a42d4d7 | 479 | return temp_fix1616; // need no more 'break'; |
sepp_nepp | 11:c6f95a42d4d7 | 480 | |
sepp_nepp | 11:c6f95a42d4d7 | 481 | case VL53L0X_CHECKEN_SIG_RATE_MSRC: |
sepp_nepp | 11:c6f95a42d4d7 | 482 | case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 483 | temp16 = Read_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT); |
sepp_nepp | 11:c6f95a42d4d7 | 484 | return FP97_TO_FP1616(temp16); // need no more break; |
nikapov | 0:a1a69d32f310 | 485 | |
nikapov | 0:a1a69d32f310 | 486 | default: |
sepp_nepp | 11:c6f95a42d4d7 | 487 | ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
sepp_nepp | 11:c6f95a42d4d7 | 488 | return 0; |
nikapov | 0:a1a69d32f310 | 489 | } |
nikapov | 0:a1a69d32f310 | 490 | } |
nikapov | 0:a1a69d32f310 | 491 | |
sepp_nepp | 11:c6f95a42d4d7 | 492 | uint8_t VL53L0X::Get_limit_chk_en(uint16_t limit_check_id ) |
sepp_nepp | 11:c6f95a42d4d7 | 493 | { if (limit_check_id >= VL53L0X_CHECKEN_NUMBER_OF_CHECKS) |
sepp_nepp | 11:c6f95a42d4d7 | 494 | { ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
sepp_nepp | 11:c6f95a42d4d7 | 495 | return 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 496 | else { return CurrParams.Limit_Chk_En[limit_check_id]; } |
nikapov | 0:a1a69d32f310 | 497 | } |
nikapov | 0:a1a69d32f310 | 498 | |
sepp_nepp | 11:c6f95a42d4d7 | 499 | uint8_t VL53L0X::Get_Wrap_Around_Chk_En() |
sepp_nepp | 11:c6f95a42d4d7 | 500 | { /* Now using the private state field SequenceConfig instead of reading from device: |
sepp_nepp | 11:c6f95a42d4d7 | 501 | uint8_t SequenceConfig; |
sepp_nepp | 11:c6f95a42d4d7 | 502 | SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); |
sepp_nepp | 11:c6f95a42d4d7 | 503 | Set_SequenceConfig( SequenceConfig ); // checks for ErrState |
sepp_nepp | 5:b95f6951f7d5 | 504 | |
sepp_nepp | 11:c6f95a42d4d7 | 505 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 506 | */ |
sepp_nepp | 11:c6f95a42d4d7 | 507 | CurrParams.Wrap_Around_Chk_En = (SequenceConfig >> 7) & 0x01; |
sepp_nepp | 11:c6f95a42d4d7 | 508 | return CurrParams.Wrap_Around_Chk_En; |
sepp_nepp | 11:c6f95a42d4d7 | 509 | // } else return 0; |
nikapov | 0:a1a69d32f310 | 510 | } |
nikapov | 0:a1a69d32f310 | 511 | |
sepp_nepp | 11:c6f95a42d4d7 | 512 | VL53L0X_Sequence_Steps_t VL53L0X::Get_sequence_step_enables() |
sepp_nepp | 11:c6f95a42d4d7 | 513 | { VL53L0X_Sequence_Steps_t p_sequence_steps; |
sepp_nepp | 11:c6f95a42d4d7 | 514 | /* Now using the private state field SequenceConfig instead of reading from device: |
sepp_nepp | 11:c6f95a42d4d7 | 515 | uint8_t SequenceConfig; |
sepp_nepp | 11:c6f95a42d4d7 | 516 | |
sepp_nepp | 11:c6f95a42d4d7 | 517 | SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); |
sepp_nepp | 11:c6f95a42d4d7 | 518 | |
sepp_nepp | 11:c6f95a42d4d7 | 519 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 520 | */ |
sepp_nepp | 11:c6f95a42d4d7 | 521 | p_sequence_steps.TccOn = (SequenceConfig & 0x10) >> 4; |
sepp_nepp | 11:c6f95a42d4d7 | 522 | p_sequence_steps.DssOn = (SequenceConfig & 0x08) >> 3; |
sepp_nepp | 11:c6f95a42d4d7 | 523 | p_sequence_steps.MsrcOn = (SequenceConfig & 0x04) >> 2; |
sepp_nepp | 11:c6f95a42d4d7 | 524 | p_sequence_steps.PreRangeOn = (SequenceConfig & 0x40) >> 6; |
sepp_nepp | 11:c6f95a42d4d7 | 525 | p_sequence_steps.FinalRangeOn = (SequenceConfig & 0x80) >> 7; |
sepp_nepp | 11:c6f95a42d4d7 | 526 | // } |
sepp_nepp | 11:c6f95a42d4d7 | 527 | return p_sequence_steps; |
nikapov | 0:a1a69d32f310 | 528 | } |
nikapov | 0:a1a69d32f310 | 529 | |
sepp_nepp | 11:c6f95a42d4d7 | 530 | void VL53L0X::Set_vcsel_PPeriod(VL53L0X_Range_Phase Vcsel_Range_Phase, uint8_t vcsel_PPeriod_pclk) |
sepp_nepp | 11:c6f95a42d4d7 | 531 | { uint8_t vcsel_period_reg; |
nikapov | 0:a1a69d32f310 | 532 | uint8_t min_pre_vcsel_period_pclk = 12; |
nikapov | 0:a1a69d32f310 | 533 | uint8_t max_pre_vcsel_period_pclk = 18; |
nikapov | 0:a1a69d32f310 | 534 | uint8_t min_final_vcsel_period_pclk = 8; |
nikapov | 0:a1a69d32f310 | 535 | uint8_t max_final_vcsel_period_pclk = 14; |
sepp_nepp | 10:cd251e0fc2fd | 536 | uint32_t final_range_timeout_us; |
sepp_nepp | 10:cd251e0fc2fd | 537 | uint32_t pre_range_timeout_us; |
sepp_nepp | 10:cd251e0fc2fd | 538 | uint32_t msrc_timeout_us; |
nikapov | 0:a1a69d32f310 | 539 | uint8_t phase_cal_int = 0; |
nikapov | 0:a1a69d32f310 | 540 | |
nikapov | 0:a1a69d32f310 | 541 | /* Check if valid clock period requested */ |
sepp_nepp | 11:c6f95a42d4d7 | 542 | if ( ((vcsel_PPeriod_pclk % 2) != 0 ) /* Value must be an even number */ |
sepp_nepp | 11:c6f95a42d4d7 | 543 | || |
sepp_nepp | 11:c6f95a42d4d7 | 544 | ( Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE && |
sepp_nepp | 11:c6f95a42d4d7 | 545 | ((vcsel_PPeriod_pclk < min_pre_vcsel_period_pclk)|| |
sepp_nepp | 11:c6f95a42d4d7 | 546 | (vcsel_PPeriod_pclk > max_pre_vcsel_period_pclk) ) ) |
sepp_nepp | 11:c6f95a42d4d7 | 547 | || |
sepp_nepp | 11:c6f95a42d4d7 | 548 | ( Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE && |
sepp_nepp | 11:c6f95a42d4d7 | 549 | (vcsel_PPeriod_pclk < min_final_vcsel_period_pclk || |
sepp_nepp | 11:c6f95a42d4d7 | 550 | vcsel_PPeriod_pclk > max_final_vcsel_period_pclk) ) ) |
sepp_nepp | 11:c6f95a42d4d7 | 551 | { ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
sepp_nepp | 11:c6f95a42d4d7 | 552 | return;} |
nikapov | 0:a1a69d32f310 | 553 | |
nikapov | 0:a1a69d32f310 | 554 | /* Apply specific settings for the requested clock period */ |
sepp_nepp | 11:c6f95a42d4d7 | 555 | if (Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE) { |
sepp_nepp | 11:c6f95a42d4d7 | 556 | /* Set phase check limits for pre-ranging*/ |
sepp_nepp | 11:c6f95a42d4d7 | 557 | if (vcsel_PPeriod_pclk == 12) { |
sepp_nepp | 11:c6f95a42d4d7 | 558 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x18); |
sepp_nepp | 11:c6f95a42d4d7 | 559 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 560 | } else if (vcsel_PPeriod_pclk == 14) { |
sepp_nepp | 11:c6f95a42d4d7 | 561 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x30); |
sepp_nepp | 11:c6f95a42d4d7 | 562 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 563 | } else if (vcsel_PPeriod_pclk == 16) { |
sepp_nepp | 11:c6f95a42d4d7 | 564 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x40); |
sepp_nepp | 11:c6f95a42d4d7 | 565 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 566 | } else if (vcsel_PPeriod_pclk == 18) { |
sepp_nepp | 11:c6f95a42d4d7 | 567 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x50); |
sepp_nepp | 11:c6f95a42d4d7 | 568 | Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); |
nikapov | 0:a1a69d32f310 | 569 | } |
sepp_nepp | 11:c6f95a42d4d7 | 570 | } else if (Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE) { |
sepp_nepp | 11:c6f95a42d4d7 | 571 | if (vcsel_PPeriod_pclk == 8) { |
sepp_nepp | 11:c6f95a42d4d7 | 572 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10); |
sepp_nepp | 11:c6f95a42d4d7 | 573 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW ,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 574 | Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH ,0x02); |
sepp_nepp | 11:c6f95a42d4d7 | 575 | Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C); |
sepp_nepp | 11:c6f95a42d4d7 | 576 | Write_Byte(0xff,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 577 | Write_Byte(REG_ALGO_PHASECAL_LIM,0x30); |
sepp_nepp | 11:c6f95a42d4d7 | 578 | Write_Byte(0xff,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 579 | } else if (vcsel_PPeriod_pclk == 10) { |
sepp_nepp | 11:c6f95a42d4d7 | 580 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28); |
sepp_nepp | 11:c6f95a42d4d7 | 581 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 582 | Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); |
sepp_nepp | 11:c6f95a42d4d7 | 583 | Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09); |
sepp_nepp | 11:c6f95a42d4d7 | 584 | Write_Byte(0xff,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 585 | Write_Byte(REG_ALGO_PHASECAL_LIM,0x20); |
sepp_nepp | 11:c6f95a42d4d7 | 586 | Write_Byte(0xff,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 587 | } else if (vcsel_PPeriod_pclk == 12) { |
sepp_nepp | 11:c6f95a42d4d7 | 588 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x38); |
sepp_nepp | 11:c6f95a42d4d7 | 589 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 590 | Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); |
sepp_nepp | 11:c6f95a42d4d7 | 591 | Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 592 | Write_Byte(0xff,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 593 | Write_Byte(REG_ALGO_PHASECAL_LIM,0x20); |
sepp_nepp | 11:c6f95a42d4d7 | 594 | Write_Byte(0xff,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 595 | } else if (vcsel_PPeriod_pclk == 14) { |
sepp_nepp | 11:c6f95a42d4d7 | 596 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048); |
sepp_nepp | 11:c6f95a42d4d7 | 597 | Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); |
sepp_nepp | 11:c6f95a42d4d7 | 598 | Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03); |
sepp_nepp | 11:c6f95a42d4d7 | 599 | Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07); |
sepp_nepp | 11:c6f95a42d4d7 | 600 | Write_Byte(0xff,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 601 | Write_Byte(REG_ALGO_PHASECAL_LIM, 0x20); |
sepp_nepp | 11:c6f95a42d4d7 | 602 | Write_Byte(0xff,0x00); |
nikapov | 0:a1a69d32f310 | 603 | } |
nikapov | 0:a1a69d32f310 | 604 | } |
nikapov | 0:a1a69d32f310 | 605 | |
sepp_nepp | 7:41cbc431e1f4 | 606 | /* Re-calculate and apply timeouts,in macro periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 607 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 608 | /* Converts the encoded VCSEL period register value into the real period in PLL clocks */ |
sepp_nepp | 11:c6f95a42d4d7 | 609 | /* Flattened from procedure called Encode_vcsel_period */ |
sepp_nepp | 11:c6f95a42d4d7 | 610 | vcsel_period_reg = (vcsel_PPeriod_pclk >> 1) - 1; |
sepp_nepp | 11:c6f95a42d4d7 | 611 | |
nikapov | 0:a1a69d32f310 | 612 | /* When the VCSEL period for the pre or final range is changed, |
nikapov | 0:a1a69d32f310 | 613 | * the corresponding timeout must be read from the device using |
sepp_nepp | 7:41cbc431e1f4 | 614 | * the current VCSEL period,then the new VCSEL period can be |
nikapov | 0:a1a69d32f310 | 615 | * applied. The timeout then must be written back to the device |
nikapov | 0:a1a69d32f310 | 616 | * using the new VCSEL period. |
sepp_nepp | 7:41cbc431e1f4 | 617 | * For the MSRC timeout,the same applies - this timeout being |
nikapov | 0:a1a69d32f310 | 618 | * dependant on the pre-range vcsel period. |
nikapov | 0:a1a69d32f310 | 619 | */ |
sepp_nepp | 11:c6f95a42d4d7 | 620 | switch (Vcsel_Range_Phase) { |
sepp_nepp | 11:c6f95a42d4d7 | 621 | case VL53L0X_VCSEL_PRE_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 622 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&pre_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 623 | |
sepp_nepp | 11:c6f95a42d4d7 | 624 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 625 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,&msrc_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 626 | |
sepp_nepp | 11:c6f95a42d4d7 | 627 | Write_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg); |
sepp_nepp | 11:c6f95a42d4d7 | 628 | |
sepp_nepp | 11:c6f95a42d4d7 | 629 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 630 | Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,pre_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 631 | |
sepp_nepp | 11:c6f95a42d4d7 | 632 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 633 | Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,msrc_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 634 | |
sepp_nepp | 12:aa177f0e4c10 | 635 | PreRangeVcselPPeriod = vcsel_PPeriod_pclk; |
sepp_nepp | 11:c6f95a42d4d7 | 636 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 637 | |
sepp_nepp | 11:c6f95a42d4d7 | 638 | case VL53L0X_VCSEL_FINAL_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 639 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&final_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 640 | |
sepp_nepp | 11:c6f95a42d4d7 | 641 | Write_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg); |
sepp_nepp | 11:c6f95a42d4d7 | 642 | |
sepp_nepp | 11:c6f95a42d4d7 | 643 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 644 | Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,final_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 645 | |
sepp_nepp | 12:aa177f0e4c10 | 646 | FinalRangeVcselPPeriod = vcsel_PPeriod_pclk; |
sepp_nepp | 11:c6f95a42d4d7 | 647 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 648 | default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
nikapov | 0:a1a69d32f310 | 649 | } |
nikapov | 0:a1a69d32f310 | 650 | } |
nikapov | 0:a1a69d32f310 | 651 | |
sepp_nepp | 11:c6f95a42d4d7 | 652 | /* Finally,the timing budget is re-applied */ |
sepp_nepp | 11:c6f95a42d4d7 | 653 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 654 | { Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); } |
sepp_nepp | 11:c6f95a42d4d7 | 655 | |
sepp_nepp | 11:c6f95a42d4d7 | 656 | /* Perform the phase calibration. This is needed after changing on vcsel period. |
sepp_nepp | 7:41cbc431e1f4 | 657 | * get_data_enable = 0,restore_config = 1 */ |
sepp_nepp | 11:c6f95a42d4d7 | 658 | Perf_phase_calibration(&phase_cal_int,0,1); |
nikapov | 0:a1a69d32f310 | 659 | } |
nikapov | 0:a1a69d32f310 | 660 | |
sepp_nepp | 11:c6f95a42d4d7 | 661 | #define VL53L0X_MACRO_PERIOD_NS 3813; // = ( VL53L0X_PLL_PERIOD_PS * VL53L0X_MACRO_PERIOD_VCLKS / 1000 ) |
nikapov | 0:a1a69d32f310 | 662 | |
nikapov | 0:a1a69d32f310 | 663 | /* To convert register value into us */ |
sepp_nepp | 11:c6f95a42d4d7 | 664 | uint32_t VL53L0X::Calc_timeout_us(uint16_t timeout_period_mclks, |
nikapov | 0:a1a69d32f310 | 665 | uint8_t vcsel_period_pclks) |
nikapov | 0:a1a69d32f310 | 666 | { |
nikapov | 0:a1a69d32f310 | 667 | uint32_t macro_period_ns; |
nikapov | 0:a1a69d32f310 | 668 | uint32_t actual_timeout_period_us = 0; |
nikapov | 0:a1a69d32f310 | 669 | |
sepp_nepp | 11:c6f95a42d4d7 | 670 | macro_period_ns = (uint32_t) (vcsel_period_pclks ) * VL53L0X_MACRO_PERIOD_NS; |
sepp_nepp | 11:c6f95a42d4d7 | 671 | |
sepp_nepp | 11:c6f95a42d4d7 | 672 | actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 673 | |
nikapov | 0:a1a69d32f310 | 674 | return actual_timeout_period_us; |
nikapov | 0:a1a69d32f310 | 675 | } |
nikapov | 0:a1a69d32f310 | 676 | |
sepp_nepp | 11:c6f95a42d4d7 | 677 | void VL53L0X::Get_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id, |
nikapov | 0:a1a69d32f310 | 678 | uint32_t *p_time_out_micro_secs) |
sepp_nepp | 11:c6f95a42d4d7 | 679 | { uint8_t current_vcsel_PPeriod_p_clk; |
nikapov | 0:a1a69d32f310 | 680 | uint8_t encoded_time_out_byte = 0; |
sepp_nepp | 10:cd251e0fc2fd | 681 | uint32_t timeout_us = 0; |
nikapov | 0:a1a69d32f310 | 682 | uint16_t pre_range_encoded_time_out = 0; |
nikapov | 0:a1a69d32f310 | 683 | uint16_t msrc_time_out_m_clks; |
nikapov | 0:a1a69d32f310 | 684 | uint16_t pre_range_time_out_m_clks; |
nikapov | 0:a1a69d32f310 | 685 | uint16_t final_range_time_out_m_clks = 0; |
nikapov | 0:a1a69d32f310 | 686 | uint16_t final_range_encoded_time_out; |
sepp_nepp | 11:c6f95a42d4d7 | 687 | VL53L0X_Sequence_Steps_t sequence_steps; |
sepp_nepp | 11:c6f95a42d4d7 | 688 | |
sepp_nepp | 11:c6f95a42d4d7 | 689 | if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) || |
sepp_nepp | 11:c6f95a42d4d7 | 690 | (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) || |
sepp_nepp | 11:c6f95a42d4d7 | 691 | (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC) ) { |
sepp_nepp | 11:c6f95a42d4d7 | 692 | |
sepp_nepp | 11:c6f95a42d4d7 | 693 | current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 694 | ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
sepp_nepp | 11:c6f95a42d4d7 | 695 | |
sepp_nepp | 11:c6f95a42d4d7 | 696 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 697 | encoded_time_out_byte = Read_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP); |
nikapov | 0:a1a69d32f310 | 698 | } |
sepp_nepp | 11:c6f95a42d4d7 | 699 | msrc_time_out_m_clks = Decode_timeout(encoded_time_out_byte); |
sepp_nepp | 11:c6f95a42d4d7 | 700 | |
sepp_nepp | 11:c6f95a42d4d7 | 701 | timeout_us = Calc_timeout_us(msrc_time_out_m_clks, |
sepp_nepp | 11:c6f95a42d4d7 | 702 | current_vcsel_PPeriod_p_clk); |
nikapov | 0:a1a69d32f310 | 703 | } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { |
sepp_nepp | 11:c6f95a42d4d7 | 704 | |
sepp_nepp | 11:c6f95a42d4d7 | 705 | current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 706 | ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
nikapov | 0:a1a69d32f310 | 707 | |
nikapov | 0:a1a69d32f310 | 708 | /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ |
sepp_nepp | 11:c6f95a42d4d7 | 709 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 710 | |
sepp_nepp | 11:c6f95a42d4d7 | 711 | pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI); |
sepp_nepp | 11:c6f95a42d4d7 | 712 | |
sepp_nepp | 11:c6f95a42d4d7 | 713 | pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out); |
sepp_nepp | 11:c6f95a42d4d7 | 714 | |
sepp_nepp | 11:c6f95a42d4d7 | 715 | timeout_us = Calc_timeout_us(pre_range_time_out_m_clks, |
sepp_nepp | 11:c6f95a42d4d7 | 716 | current_vcsel_PPeriod_p_clk); |
nikapov | 0:a1a69d32f310 | 717 | } |
nikapov | 0:a1a69d32f310 | 718 | } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { |
nikapov | 0:a1a69d32f310 | 719 | |
sepp_nepp | 11:c6f95a42d4d7 | 720 | sequence_steps = Get_sequence_step_enables(); |
nikapov | 0:a1a69d32f310 | 721 | pre_range_time_out_m_clks = 0; |
nikapov | 0:a1a69d32f310 | 722 | |
sepp_nepp | 11:c6f95a42d4d7 | 723 | if (sequence_steps.PreRangeOn) { |
sepp_nepp | 11:c6f95a42d4d7 | 724 | current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 725 | ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
sepp_nepp | 11:c6f95a42d4d7 | 726 | |
sepp_nepp | 11:c6f95a42d4d7 | 727 | /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ |
sepp_nepp | 11:c6f95a42d4d7 | 728 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 729 | pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI); |
sepp_nepp | 11:c6f95a42d4d7 | 730 | pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out); |
nikapov | 0:a1a69d32f310 | 731 | } |
nikapov | 0:a1a69d32f310 | 732 | } |
nikapov | 0:a1a69d32f310 | 733 | |
sepp_nepp | 11:c6f95a42d4d7 | 734 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 735 | current_vcsel_PPeriod_p_clk = /* Get and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 736 | ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
sepp_nepp | 11:c6f95a42d4d7 | 737 | |
nikapov | 0:a1a69d32f310 | 738 | } |
nikapov | 0:a1a69d32f310 | 739 | |
nikapov | 0:a1a69d32f310 | 740 | /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */ |
sepp_nepp | 11:c6f95a42d4d7 | 741 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 742 | final_range_encoded_time_out = Read_Word(REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI); |
sepp_nepp | 11:c6f95a42d4d7 | 743 | final_range_time_out_m_clks = Decode_timeout(final_range_encoded_time_out); |
nikapov | 0:a1a69d32f310 | 744 | } |
nikapov | 0:a1a69d32f310 | 745 | |
nikapov | 0:a1a69d32f310 | 746 | final_range_time_out_m_clks -= pre_range_time_out_m_clks; |
sepp_nepp | 11:c6f95a42d4d7 | 747 | timeout_us = Calc_timeout_us(final_range_time_out_m_clks,current_vcsel_PPeriod_p_clk); |
nikapov | 0:a1a69d32f310 | 748 | } |
nikapov | 0:a1a69d32f310 | 749 | |
sepp_nepp | 10:cd251e0fc2fd | 750 | *p_time_out_micro_secs = timeout_us; |
nikapov | 0:a1a69d32f310 | 751 | } |
nikapov | 0:a1a69d32f310 | 752 | |
sepp_nepp | 11:c6f95a42d4d7 | 753 | uint32_t VL53L0X::Get_Measure_Time_Budget_us() |
sepp_nepp | 11:c6f95a42d4d7 | 754 | { VL53L0X_Sequence_Steps_t sequence_steps; |
sepp_nepp | 11:c6f95a42d4d7 | 755 | uint32_t p_Measure_Time_Budget_us; |
sepp_nepp | 10:cd251e0fc2fd | 756 | uint32_t final_range_timeout_us; |
sepp_nepp | 11:c6f95a42d4d7 | 757 | uint32_t msrc_dcc_tcc_timeout_us= 2000; |
sepp_nepp | 10:cd251e0fc2fd | 758 | uint32_t start_overhead_us = 1910; |
sepp_nepp | 11:c6f95a42d4d7 | 759 | uint32_t end_overhead_us = 960; |
sepp_nepp | 10:cd251e0fc2fd | 760 | uint32_t msrc_overhead_us = 660; |
sepp_nepp | 11:c6f95a42d4d7 | 761 | uint32_t tcc_overhead_us = 590; |
sepp_nepp | 11:c6f95a42d4d7 | 762 | uint32_t dss_overhead_us = 690; |
sepp_nepp | 10:cd251e0fc2fd | 763 | uint32_t pre_range_overhead_us = 660; |
sepp_nepp | 11:c6f95a42d4d7 | 764 | uint32_t final_range_overhead_us= 550; |
sepp_nepp | 10:cd251e0fc2fd | 765 | uint32_t pre_range_timeout_us = 0; |
nikapov | 0:a1a69d32f310 | 766 | |
sepp_nepp | 11:c6f95a42d4d7 | 767 | if (ErrState != VL53L0X_OK) {return 0; } // do nothing while in Error State!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 768 | |
nikapov | 0:a1a69d32f310 | 769 | /* Start and end overhead times always present */ |
sepp_nepp | 11:c6f95a42d4d7 | 770 | p_Measure_Time_Budget_us = start_overhead_us + end_overhead_us; |
sepp_nepp | 11:c6f95a42d4d7 | 771 | |
sepp_nepp | 11:c6f95a42d4d7 | 772 | sequence_steps = Get_sequence_step_enables(); |
sepp_nepp | 11:c6f95a42d4d7 | 773 | |
sepp_nepp | 11:c6f95a42d4d7 | 774 | if (sequence_steps.TccOn || sequence_steps.MsrcOn || sequence_steps.DssOn) |
sepp_nepp | 11:c6f95a42d4d7 | 775 | { Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 776 | |
sepp_nepp | 11:c6f95a42d4d7 | 777 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 778 | if (sequence_steps.TccOn) |
sepp_nepp | 11:c6f95a42d4d7 | 779 | { p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + tcc_overhead_us; } |
sepp_nepp | 11:c6f95a42d4d7 | 780 | |
sepp_nepp | 11:c6f95a42d4d7 | 781 | if (sequence_steps.DssOn) { |
sepp_nepp | 11:c6f95a42d4d7 | 782 | p_Measure_Time_Budget_us += 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us); |
sepp_nepp | 11:c6f95a42d4d7 | 783 | } else if (sequence_steps.MsrcOn) { |
sepp_nepp | 11:c6f95a42d4d7 | 784 | p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + msrc_overhead_us; |
nikapov | 0:a1a69d32f310 | 785 | } |
nikapov | 0:a1a69d32f310 | 786 | } |
nikapov | 0:a1a69d32f310 | 787 | } |
nikapov | 0:a1a69d32f310 | 788 | |
sepp_nepp | 11:c6f95a42d4d7 | 789 | if ( (ErrState == VL53L0X_OK) && sequence_steps.PreRangeOn) { |
sepp_nepp | 11:c6f95a42d4d7 | 790 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 791 | p_Measure_Time_Budget_us += pre_range_timeout_us + pre_range_overhead_us; |
sepp_nepp | 11:c6f95a42d4d7 | 792 | } |
sepp_nepp | 11:c6f95a42d4d7 | 793 | |
sepp_nepp | 11:c6f95a42d4d7 | 794 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 795 | if (sequence_steps.FinalRangeOn) { |
sepp_nepp | 11:c6f95a42d4d7 | 796 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, &final_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 797 | p_Measure_Time_Budget_us += (final_range_timeout_us + final_range_overhead_us); |
nikapov | 0:a1a69d32f310 | 798 | } |
nikapov | 0:a1a69d32f310 | 799 | } |
nikapov | 0:a1a69d32f310 | 800 | |
sepp_nepp | 11:c6f95a42d4d7 | 801 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 802 | { CurrParams.Measure_Time_Budget_us = p_Measure_Time_Budget_us; } |
sepp_nepp | 11:c6f95a42d4d7 | 803 | |
sepp_nepp | 11:c6f95a42d4d7 | 804 | return p_Measure_Time_Budget_us; |
nikapov | 0:a1a69d32f310 | 805 | } |
nikapov | 0:a1a69d32f310 | 806 | |
sepp_nepp | 11:c6f95a42d4d7 | 807 | VL53L0X_DeviceParams_t VL53L0X::Get_device_parameters() |
sepp_nepp | 11:c6f95a42d4d7 | 808 | { VL53L0X_DeviceParams_t device_params = {0}; |
sepp_nepp | 11:c6f95a42d4d7 | 809 | int i; |
sepp_nepp | 11:c6f95a42d4d7 | 810 | |
sepp_nepp | 11:c6f95a42d4d7 | 811 | if (ErrState != VL53L0X_OK) {return device_params; } // do nothing while in Error State!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 812 | |
sepp_nepp | 11:c6f95a42d4d7 | 813 | device_params.DeviceMode = CurrParams.DeviceMode; |
sepp_nepp | 11:c6f95a42d4d7 | 814 | device_params.XTalk_Compens_En = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 815 | device_params.Offset_Cal_um = Get_Offset_Cal_um(); |
sepp_nepp | 11:c6f95a42d4d7 | 816 | |
sepp_nepp | 11:c6f95a42d4d7 | 817 | Get_measure_period_ms(&(device_params.Measure_Period_ms)); |
sepp_nepp | 11:c6f95a42d4d7 | 818 | |
sepp_nepp | 11:c6f95a42d4d7 | 819 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 820 | Get_Xtalk_CompRate_MHz(&(device_params.Xtalk_CompRate_MHz)); |
sepp_nepp | 11:c6f95a42d4d7 | 821 | |
sepp_nepp | 11:c6f95a42d4d7 | 822 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 823 | for (i = 0; i < VL53L0X_CHECKEN_NUMBER_OF_CHECKS; i++) |
sepp_nepp | 11:c6f95a42d4d7 | 824 | {/* get first the values,then the enables. GetLimitCheckValue will |
sepp_nepp | 11:c6f95a42d4d7 | 825 | modify the enable flags */ |
sepp_nepp | 11:c6f95a42d4d7 | 826 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 827 | { device_params.Limit_Chk_Val[i] = Get_limit_chk_val(i); } |
sepp_nepp | 11:c6f95a42d4d7 | 828 | else { break; } |
sepp_nepp | 11:c6f95a42d4d7 | 829 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 830 | { device_params.Limit_Chk_En[i]= Get_limit_chk_en(i);} |
sepp_nepp | 10:cd251e0fc2fd | 831 | else { break; } |
nikapov | 0:a1a69d32f310 | 832 | } |
nikapov | 0:a1a69d32f310 | 833 | } |
nikapov | 0:a1a69d32f310 | 834 | |
sepp_nepp | 11:c6f95a42d4d7 | 835 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 836 | device_params.Wrap_Around_Chk_En = Get_Wrap_Around_Chk_En();} |
sepp_nepp | 11:c6f95a42d4d7 | 837 | |
sepp_nepp | 11:c6f95a42d4d7 | 838 | /* Need to be done at the end as it uses VCSELPPeriod */ |
sepp_nepp | 11:c6f95a42d4d7 | 839 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 840 | device_params.Measure_Time_Budget_us = Get_Measure_Time_Budget_us(); } |
sepp_nepp | 11:c6f95a42d4d7 | 841 | |
sepp_nepp | 11:c6f95a42d4d7 | 842 | return device_params; |
nikapov | 0:a1a69d32f310 | 843 | } |
nikapov | 0:a1a69d32f310 | 844 | |
sepp_nepp | 11:c6f95a42d4d7 | 845 | void VL53L0X::Set_limit_chk_val(uint16_t limit_check_id, TFP1616 limit_chk_val) |
sepp_nepp | 11:c6f95a42d4d7 | 846 | { /* first verify that the ID is within bounds .. */ |
sepp_nepp | 11:c6f95a42d4d7 | 847 | if (limit_check_id>=VL53L0X_CHECKEN_NUMBER_OF_CHECKS) |
sepp_nepp | 11:c6f95a42d4d7 | 848 | { ErrState = VL53L0X_ERROR_INVALID_PARAMS; return; } |
sepp_nepp | 11:c6f95a42d4d7 | 849 | |
sepp_nepp | 11:c6f95a42d4d7 | 850 | /* Under all other circumstances store value in local array: */ |
sepp_nepp | 11:c6f95a42d4d7 | 851 | CurrParams.Limit_Chk_Val[limit_check_id] = limit_chk_val; |
sepp_nepp | 11:c6f95a42d4d7 | 852 | |
sepp_nepp | 11:c6f95a42d4d7 | 853 | /* in addition, if enabled, then write the external ones also to the Registers */ |
sepp_nepp | 11:c6f95a42d4d7 | 854 | if (CurrParams.Limit_Chk_En[ limit_check_id ]) |
nikapov | 0:a1a69d32f310 | 855 | switch (limit_check_id) { |
sepp_nepp | 11:c6f95a42d4d7 | 856 | case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 857 | Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, |
sepp_nepp | 11:c6f95a42d4d7 | 858 | FP1616_TO_FP97(limit_chk_val)); |
nikapov | 0:a1a69d32f310 | 859 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 860 | case VL53L0X_CHECKEN_SIG_RATE_MSRC: |
sepp_nepp | 11:c6f95a42d4d7 | 861 | case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 862 | Write_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, |
sepp_nepp | 11:c6f95a42d4d7 | 863 | FP1616_TO_FP97(limit_chk_val)); |
sepp_nepp | 11:c6f95a42d4d7 | 864 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 865 | } // switch |
nikapov | 0:a1a69d32f310 | 866 | } |
nikapov | 0:a1a69d32f310 | 867 | |
sepp_nepp | 11:c6f95a42d4d7 | 868 | |
sepp_nepp | 11:c6f95a42d4d7 | 869 | |
sepp_nepp | 11:c6f95a42d4d7 | 870 | |
sepp_nepp | 11:c6f95a42d4d7 | 871 | void VL53L0X::Get_interrupt_mask_status(uint32_t *p_interrupt_mask_status) |
sepp_nepp | 11:c6f95a42d4d7 | 872 | { uint8_t intStat; |
sepp_nepp | 11:c6f95a42d4d7 | 873 | |
sepp_nepp | 11:c6f95a42d4d7 | 874 | intStat = Read_Byte(REG_RESULT_INTERRUPT_STATUS); |
sepp_nepp | 11:c6f95a42d4d7 | 875 | *p_interrupt_mask_status = intStat & 0x07; |
sepp_nepp | 11:c6f95a42d4d7 | 876 | if (intStat & 0x18) { ErrState = VL53L0X_ERROR_RANGE_ERROR; } |
sepp_nepp | 11:c6f95a42d4d7 | 877 | } |
sepp_nepp | 11:c6f95a42d4d7 | 878 | |
sepp_nepp | 11:c6f95a42d4d7 | 879 | uint8_t VL53L0X::Get_Measurement_Ready() |
sepp_nepp | 11:c6f95a42d4d7 | 880 | { uint8_t sys_range_status_register; |
sepp_nepp | 11:c6f95a42d4d7 | 881 | uint32_t interrupt_mask; |
sepp_nepp | 11:c6f95a42d4d7 | 882 | |
sepp_nepp | 12:aa177f0e4c10 | 883 | if (GpioFunctionality == REG_SYSINT_GPIO_NEW_SAMPLE_READY) |
sepp_nepp | 11:c6f95a42d4d7 | 884 | { Get_interrupt_mask_status(&interrupt_mask); |
sepp_nepp | 11:c6f95a42d4d7 | 885 | if (interrupt_mask == REG_SYSINT_GPIO_NEW_SAMPLE_READY) |
sepp_nepp | 11:c6f95a42d4d7 | 886 | { return 1; } else { return 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 887 | } |
sepp_nepp | 11:c6f95a42d4d7 | 888 | else |
sepp_nepp | 11:c6f95a42d4d7 | 889 | { sys_range_status_register = Read_Byte(REG_RESULT_RANGE_STATUS); |
sepp_nepp | 11:c6f95a42d4d7 | 890 | if ( ( ErrState == VL53L0X_OK ) & (sys_range_status_register & 0x01) ) |
sepp_nepp | 11:c6f95a42d4d7 | 891 | { return 1; } else { return 0; } |
sepp_nepp | 10:cd251e0fc2fd | 892 | } |
nikapov | 0:a1a69d32f310 | 893 | } |
nikapov | 0:a1a69d32f310 | 894 | |
sepp_nepp | 11:c6f95a42d4d7 | 895 | void VL53L0X::Polling_delay() |
nikapov | 0:a1a69d32f310 | 896 | { |
sepp_nepp | 11:c6f95a42d4d7 | 897 | // do nothing VL53L0X_OsDelay(); |
nikapov | 0:a1a69d32f310 | 898 | } |
nikapov | 0:a1a69d32f310 | 899 | |
sepp_nepp | 11:c6f95a42d4d7 | 900 | void VL53L0X::Poll_Measure_Completion() |
sepp_nepp | 11:c6f95a42d4d7 | 901 | { uint8_t new_data_ready; |
sepp_nepp | 11:c6f95a42d4d7 | 902 | uint32_t loop_nb = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 903 | |
sepp_nepp | 11:c6f95a42d4d7 | 904 | if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error |
sepp_nepp | 11:c6f95a42d4d7 | 905 | |
sepp_nepp | 11:c6f95a42d4d7 | 906 | new_data_ready = Get_Measurement_Ready(); |
sepp_nepp | 11:c6f95a42d4d7 | 907 | |
sepp_nepp | 11:c6f95a42d4d7 | 908 | while ((ErrState==0) && (new_data_ready != 1) ) |
sepp_nepp | 11:c6f95a42d4d7 | 909 | { Polling_delay(); |
sepp_nepp | 11:c6f95a42d4d7 | 910 | new_data_ready = Get_Measurement_Ready(); |
sepp_nepp | 11:c6f95a42d4d7 | 911 | if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) ErrState=VL53L0X_ERROR_TIME_OUT; |
sepp_nepp | 11:c6f95a42d4d7 | 912 | } // while ; |
nikapov | 0:a1a69d32f310 | 913 | } |
nikapov | 0:a1a69d32f310 | 914 | |
sepp_nepp | 11:c6f95a42d4d7 | 915 | /* Group Device Interrupt Functions */ |
sepp_nepp | 11:c6f95a42d4d7 | 916 | void VL53L0X::Clear_interrupt_mask(uint32_t interrupt_mask) |
sepp_nepp | 11:c6f95a42d4d7 | 917 | { uint8_t loop_count = 0; |
nikapov | 0:a1a69d32f310 | 918 | uint8_t byte; |
nikapov | 0:a1a69d32f310 | 919 | |
sepp_nepp | 11:c6f95a42d4d7 | 920 | if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error |
sepp_nepp | 11:c6f95a42d4d7 | 921 | |
sepp_nepp | 7:41cbc431e1f4 | 922 | /* clear bit 0 range interrupt,bit 1 error interrupt */ |
nikapov | 0:a1a69d32f310 | 923 | do { |
sepp_nepp | 11:c6f95a42d4d7 | 924 | Write_Byte(REG_SYSINT_CLEAR,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 925 | Write_Byte(REG_SYSINT_CLEAR,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 926 | byte = Read_Byte(REG_RESULT_INTERRUPT_STATUS); |
sepp_nepp | 11:c6f95a42d4d7 | 927 | if (loop_count++ > 3) {ErrState =VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;} |
sepp_nepp | 11:c6f95a42d4d7 | 928 | } while (((byte & 0x07) != 0x00) && (ErrState == VL53L0X_OK)); |
sepp_nepp | 11:c6f95a42d4d7 | 929 | |
nikapov | 0:a1a69d32f310 | 930 | } |
nikapov | 0:a1a69d32f310 | 931 | |
sepp_nepp | 11:c6f95a42d4d7 | 932 | void VL53L0X::Perf_single_Ref_calibration(uint8_t vhv_init_byte) |
sepp_nepp | 11:c6f95a42d4d7 | 933 | { if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 934 | Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_START_STOP | vhv_init_byte); |
sepp_nepp | 11:c6f95a42d4d7 | 935 | Poll_Measure_Completion(); |
sepp_nepp | 11:c6f95a42d4d7 | 936 | Clear_interrupt_mask(0); |
sepp_nepp | 11:c6f95a42d4d7 | 937 | Write_Byte(REG_SYSRANGE_START,0x00); |
nikapov | 0:a1a69d32f310 | 938 | } |
nikapov | 0:a1a69d32f310 | 939 | |
sepp_nepp | 11:c6f95a42d4d7 | 940 | void VL53L0X::Ref_calibration_io(uint8_t read_not_write, |
sepp_nepp | 7:41cbc431e1f4 | 941 | uint8_t vhv_settings,uint8_t phase_cal, |
sepp_nepp | 7:41cbc431e1f4 | 942 | uint8_t *p_vhv_settings,uint8_t *p_phase_cal, |
sepp_nepp | 7:41cbc431e1f4 | 943 | const uint8_t vhv_enable,const uint8_t phase_enable) |
sepp_nepp | 11:c6f95a42d4d7 | 944 | { uint8_t phase_calint = 0; |
nikapov | 0:a1a69d32f310 | 945 | |
nikapov | 0:a1a69d32f310 | 946 | /* Read VHV from device */ |
sepp_nepp | 11:c6f95a42d4d7 | 947 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 948 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 949 | Write_Byte(0xFF,0x00); |
nikapov | 0:a1a69d32f310 | 950 | |
nikapov | 0:a1a69d32f310 | 951 | if (read_not_write) { |
sepp_nepp | 11:c6f95a42d4d7 | 952 | if (vhv_enable ) { *p_vhv_settings = Read_Byte(0xCB); } |
sepp_nepp | 11:c6f95a42d4d7 | 953 | if (phase_enable) { phase_calint = Read_Byte(0xEE); } |
sepp_nepp | 11:c6f95a42d4d7 | 954 | } |
sepp_nepp | 11:c6f95a42d4d7 | 955 | else { |
sepp_nepp | 11:c6f95a42d4d7 | 956 | if (vhv_enable ) { Write_Byte(0xCB,vhv_settings); } |
sepp_nepp | 11:c6f95a42d4d7 | 957 | if (phase_enable) { Register_BitMask(0xEE,0x80,phase_cal); } |
sepp_nepp | 11:c6f95a42d4d7 | 958 | } |
sepp_nepp | 11:c6f95a42d4d7 | 959 | |
sepp_nepp | 11:c6f95a42d4d7 | 960 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 961 | Write_Byte(0x00,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 962 | Write_Byte(0xFF,0x00); |
nikapov | 0:a1a69d32f310 | 963 | |
nikapov | 0:a1a69d32f310 | 964 | *p_phase_cal = (uint8_t)(phase_calint & 0xEF); |
nikapov | 0:a1a69d32f310 | 965 | } |
nikapov | 0:a1a69d32f310 | 966 | |
sepp_nepp | 11:c6f95a42d4d7 | 967 | void VL53L0X::Perf_vhv_calibration(uint8_t *p_vhv_settings, |
sepp_nepp | 11:c6f95a42d4d7 | 968 | const uint8_t get_data_enable, const uint8_t restore_config) |
sepp_nepp | 11:c6f95a42d4d7 | 969 | { uint8_t orig_sequence_config = 0; |
nikapov | 0:a1a69d32f310 | 970 | uint8_t vhv_settings = 0; |
nikapov | 0:a1a69d32f310 | 971 | uint8_t phase_cal = 0; |
nikapov | 0:a1a69d32f310 | 972 | uint8_t phase_cal_int = 0; |
nikapov | 0:a1a69d32f310 | 973 | |
nikapov | 0:a1a69d32f310 | 974 | /* store the value of the sequence config, |
sepp_nepp | 11:c6f95a42d4d7 | 975 | * this will be reset before the end of the function */ |
sepp_nepp | 11:c6f95a42d4d7 | 976 | orig_sequence_config = SequenceConfig; |
nikapov | 0:a1a69d32f310 | 977 | |
nikapov | 0:a1a69d32f310 | 978 | /* Run VHV */ |
sepp_nepp | 11:c6f95a42d4d7 | 979 | Set_SequenceConfig( 0x01 ); |
sepp_nepp | 11:c6f95a42d4d7 | 980 | Perf_single_Ref_calibration(0x40); |
nikapov | 0:a1a69d32f310 | 981 | |
nikapov | 0:a1a69d32f310 | 982 | /* Read VHV from device */ |
sepp_nepp | 11:c6f95a42d4d7 | 983 | if ((ErrState == VL53L0X_OK) && (get_data_enable == 1)) |
sepp_nepp | 11:c6f95a42d4d7 | 984 | { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */ |
sepp_nepp | 11:c6f95a42d4d7 | 985 | p_vhv_settings,&phase_cal_int, 1,0); } |
sepp_nepp | 11:c6f95a42d4d7 | 986 | else { *p_vhv_settings = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 987 | |
sepp_nepp | 11:c6f95a42d4d7 | 988 | if (restore_config) { /* restore the previous Sequence Config */ |
sepp_nepp | 11:c6f95a42d4d7 | 989 | Set_SequenceConfig( orig_sequence_config ); } // checks for ErrState |
nikapov | 0:a1a69d32f310 | 990 | } |
nikapov | 0:a1a69d32f310 | 991 | |
sepp_nepp | 11:c6f95a42d4d7 | 992 | void VL53L0X::Perf_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable, |
nikapov | 0:a1a69d32f310 | 993 | const uint8_t restore_config) |
sepp_nepp | 11:c6f95a42d4d7 | 994 | { uint8_t orig_sequence_config; |
nikapov | 0:a1a69d32f310 | 995 | uint8_t vhv_settings = 0; |
nikapov | 0:a1a69d32f310 | 996 | uint8_t phase_cal = 0; |
nikapov | 0:a1a69d32f310 | 997 | uint8_t vhv_settingsint; |
nikapov | 0:a1a69d32f310 | 998 | |
sepp_nepp | 11:c6f95a42d4d7 | 999 | if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error |
sepp_nepp | 11:c6f95a42d4d7 | 1000 | |
sepp_nepp | 11:c6f95a42d4d7 | 1001 | /* store the value of the sequence config, this will be reset before the end of the function */ |
sepp_nepp | 11:c6f95a42d4d7 | 1002 | orig_sequence_config = SequenceConfig; |
sepp_nepp | 11:c6f95a42d4d7 | 1003 | |
sepp_nepp | 11:c6f95a42d4d7 | 1004 | /* Run PhaseCal: */ |
sepp_nepp | 11:c6f95a42d4d7 | 1005 | Set_SequenceConfig( 0x02 ); // sets REG_SYSTEM_SEQUENCE_CONFIG |
sepp_nepp | 11:c6f95a42d4d7 | 1006 | Perf_single_Ref_calibration(0x0); |
nikapov | 0:a1a69d32f310 | 1007 | |
nikapov | 0:a1a69d32f310 | 1008 | /* Read PhaseCal from device */ |
sepp_nepp | 11:c6f95a42d4d7 | 1009 | if ((ErrState == VL53L0X_OK) && (get_data_enable == 1)) |
sepp_nepp | 11:c6f95a42d4d7 | 1010 | { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */ |
sepp_nepp | 11:c6f95a42d4d7 | 1011 | &vhv_settingsint,p_phase_cal, 0,1); } |
sepp_nepp | 11:c6f95a42d4d7 | 1012 | else { *p_phase_cal = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1013 | |
sepp_nepp | 11:c6f95a42d4d7 | 1014 | if (restore_config) { /* restore the previous Sequence Config */ |
sepp_nepp | 11:c6f95a42d4d7 | 1015 | Set_SequenceConfig( orig_sequence_config ); } |
nikapov | 0:a1a69d32f310 | 1016 | } |
nikapov | 0:a1a69d32f310 | 1017 | |
sepp_nepp | 11:c6f95a42d4d7 | 1018 | void VL53L0X::Perf_Ref_calibration(uint8_t *p_vhv_settings, |
sepp_nepp | 10:cd251e0fc2fd | 1019 | uint8_t *p_phase_cal, uint8_t get_data_enable) |
sepp_nepp | 11:c6f95a42d4d7 | 1020 | { uint8_t orig_sequence_config; |
nikapov | 0:a1a69d32f310 | 1021 | |
nikapov | 0:a1a69d32f310 | 1022 | /* store the value of the sequence config, |
sepp_nepp | 10:cd251e0fc2fd | 1023 | * this will be reset before the end of the function */ |
sepp_nepp | 11:c6f95a42d4d7 | 1024 | orig_sequence_config = SequenceConfig; |
nikapov | 0:a1a69d32f310 | 1025 | |
nikapov | 0:a1a69d32f310 | 1026 | /* In the following function we don't save the config to optimize |
nikapov | 0:a1a69d32f310 | 1027 | * writes on device. Config is saved and restored only once. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1028 | Perf_vhv_calibration(p_vhv_settings,get_data_enable,0); |
sepp_nepp | 11:c6f95a42d4d7 | 1029 | Perf_phase_calibration(p_phase_cal,get_data_enable,0); |
sepp_nepp | 11:c6f95a42d4d7 | 1030 | |
sepp_nepp | 11:c6f95a42d4d7 | 1031 | /* restore the previous Sequence Config */ |
sepp_nepp | 11:c6f95a42d4d7 | 1032 | Set_SequenceConfig( orig_sequence_config ); // sets REG_SYSTEM_SEQUENCE_CONFIG |
nikapov | 0:a1a69d32f310 | 1033 | } |
nikapov | 0:a1a69d32f310 | 1034 | |
sepp_nepp | 11:c6f95a42d4d7 | 1035 | void VL53L0X::Get_Next_Good_SPAD(uint8_t good_SPAD_array[],uint32_t size, |
sepp_nepp | 7:41cbc431e1f4 | 1036 | uint32_t curr,int32_t *p_next) |
sepp_nepp | 10:cd251e0fc2fd | 1037 | { uint32_t start_index; |
nikapov | 0:a1a69d32f310 | 1038 | uint32_t fine_offset; |
sepp_nepp | 11:c6f95a42d4d7 | 1039 | uint32_t c_SPADS_per_byte = 8; |
nikapov | 0:a1a69d32f310 | 1040 | uint32_t coarse_index; |
nikapov | 0:a1a69d32f310 | 1041 | uint32_t fine_index; |
nikapov | 0:a1a69d32f310 | 1042 | uint8_t data_byte; |
nikapov | 0:a1a69d32f310 | 1043 | uint8_t success = 0; |
nikapov | 0:a1a69d32f310 | 1044 | |
sepp_nepp | 11:c6f95a42d4d7 | 1045 | /* Starting with the current good SPAD,loop through the array to find |
nikapov | 0:a1a69d32f310 | 1046 | * the next. i.e. the next bit set in the sequence. |
nikapov | 0:a1a69d32f310 | 1047 | * The coarse index is the byte index of the array and the fine index is |
sepp_nepp | 11:c6f95a42d4d7 | 1048 | * the index of the bit within each byte. */ |
nikapov | 0:a1a69d32f310 | 1049 | *p_next = -1; |
nikapov | 0:a1a69d32f310 | 1050 | |
sepp_nepp | 11:c6f95a42d4d7 | 1051 | start_index = curr / c_SPADS_per_byte; |
sepp_nepp | 11:c6f95a42d4d7 | 1052 | fine_offset = curr % c_SPADS_per_byte; |
nikapov | 0:a1a69d32f310 | 1053 | |
nikapov | 0:a1a69d32f310 | 1054 | for (coarse_index = start_index; ((coarse_index < size) && !success); |
nikapov | 0:a1a69d32f310 | 1055 | coarse_index++) { |
nikapov | 0:a1a69d32f310 | 1056 | fine_index = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1057 | data_byte = good_SPAD_array[coarse_index]; |
nikapov | 0:a1a69d32f310 | 1058 | |
nikapov | 0:a1a69d32f310 | 1059 | if (coarse_index == start_index) { |
nikapov | 0:a1a69d32f310 | 1060 | /* locate the bit position of the provided current |
sepp_nepp | 11:c6f95a42d4d7 | 1061 | * SPAD bit before iterating */ |
nikapov | 0:a1a69d32f310 | 1062 | data_byte >>= fine_offset; |
nikapov | 0:a1a69d32f310 | 1063 | fine_index = fine_offset; |
nikapov | 0:a1a69d32f310 | 1064 | } |
nikapov | 0:a1a69d32f310 | 1065 | |
sepp_nepp | 11:c6f95a42d4d7 | 1066 | while (fine_index < c_SPADS_per_byte) { |
nikapov | 0:a1a69d32f310 | 1067 | if ((data_byte & 0x1) == 1) { |
nikapov | 0:a1a69d32f310 | 1068 | success = 1; |
sepp_nepp | 11:c6f95a42d4d7 | 1069 | *p_next = coarse_index * c_SPADS_per_byte + fine_index; |
nikapov | 0:a1a69d32f310 | 1070 | break; |
nikapov | 0:a1a69d32f310 | 1071 | } |
nikapov | 0:a1a69d32f310 | 1072 | data_byte >>= 1; |
nikapov | 0:a1a69d32f310 | 1073 | fine_index++; |
nikapov | 0:a1a69d32f310 | 1074 | } |
nikapov | 0:a1a69d32f310 | 1075 | } |
nikapov | 0:a1a69d32f310 | 1076 | } |
nikapov | 0:a1a69d32f310 | 1077 | |
sepp_nepp | 11:c6f95a42d4d7 | 1078 | void VL53L0X::Enable_SPAD_bit(uint8_t SPAD_array[],uint32_t size,uint32_t SPAD_index) |
sepp_nepp | 11:c6f95a42d4d7 | 1079 | { uint32_t c_SPADS_per_byte = 8; |
nikapov | 0:a1a69d32f310 | 1080 | uint32_t coarse_index; |
nikapov | 0:a1a69d32f310 | 1081 | uint32_t fine_index; |
nikapov | 0:a1a69d32f310 | 1082 | |
sepp_nepp | 11:c6f95a42d4d7 | 1083 | coarse_index = SPAD_index / c_SPADS_per_byte; |
sepp_nepp | 11:c6f95a42d4d7 | 1084 | fine_index = SPAD_index % c_SPADS_per_byte; |
sepp_nepp | 11:c6f95a42d4d7 | 1085 | if (coarse_index >= size) { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; } |
sepp_nepp | 11:c6f95a42d4d7 | 1086 | else { SPAD_array[coarse_index] |= (1 << fine_index); } |
nikapov | 0:a1a69d32f310 | 1087 | } |
nikapov | 0:a1a69d32f310 | 1088 | |
sepp_nepp | 11:c6f95a42d4d7 | 1089 | void VL53L0X::Enable_Ref_SPADS( uint8_t aperture_SPADS, uint8_t good_SPAD_array[], |
sepp_nepp | 11:c6f95a42d4d7 | 1090 | uint8_t SPAD_array[], uint32_t size, uint32_t start, uint32_t offset, |
sepp_nepp | 11:c6f95a42d4d7 | 1091 | uint32_t SPAD_count, uint32_t *p_last_SPAD ) |
sepp_nepp | 11:c6f95a42d4d7 | 1092 | { uint32_t index; |
nikapov | 0:a1a69d32f310 | 1093 | uint32_t i; |
sepp_nepp | 11:c6f95a42d4d7 | 1094 | int32_t next_good_SPAD = offset; |
sepp_nepp | 11:c6f95a42d4d7 | 1095 | uint32_t current_SPAD; |
sepp_nepp | 11:c6f95a42d4d7 | 1096 | uint8_t check_SPAD_array[6]; |
sepp_nepp | 11:c6f95a42d4d7 | 1097 | |
sepp_nepp | 11:c6f95a42d4d7 | 1098 | /* This function takes in a SPAD array which may or may not have SPADS |
nikapov | 0:a1a69d32f310 | 1099 | * already enabled and appends from a given offset a requested number |
sepp_nepp | 11:c6f95a42d4d7 | 1100 | * of new SPAD enables. The 'good SPAD map' is applied to |
sepp_nepp | 11:c6f95a42d4d7 | 1101 | * determine the next SPADS to enable. |
nikapov | 0:a1a69d32f310 | 1102 | * |
sepp_nepp | 11:c6f95a42d4d7 | 1103 | * This function applies to only aperture or only non-aperture SPADS. |
nikapov | 0:a1a69d32f310 | 1104 | * Checks are performed to ensure this. |
nikapov | 0:a1a69d32f310 | 1105 | */ |
nikapov | 0:a1a69d32f310 | 1106 | |
sepp_nepp | 11:c6f95a42d4d7 | 1107 | current_SPAD = offset; |
sepp_nepp | 11:c6f95a42d4d7 | 1108 | for (index = 0; index < SPAD_count; index++) { |
sepp_nepp | 11:c6f95a42d4d7 | 1109 | Get_Next_Good_SPAD(good_SPAD_array,size,current_SPAD, &next_good_SPAD); |
sepp_nepp | 11:c6f95a42d4d7 | 1110 | |
sepp_nepp | 11:c6f95a42d4d7 | 1111 | if (next_good_SPAD == -1) |
sepp_nepp | 11:c6f95a42d4d7 | 1112 | { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; |
sepp_nepp | 11:c6f95a42d4d7 | 1113 | break; } |
sepp_nepp | 11:c6f95a42d4d7 | 1114 | |
nikapov | 0:a1a69d32f310 | 1115 | |
nikapov | 0:a1a69d32f310 | 1116 | /* Confirm that the next good SPAD is non-aperture */ |
sepp_nepp | 11:c6f95a42d4d7 | 1117 | if (Is_ApertureSPAD(start + next_good_SPAD) != aperture_SPADS) { |
nikapov | 0:a1a69d32f310 | 1118 | /* if we can't get the required number of good aperture |
sepp_nepp | 11:c6f95a42d4d7 | 1119 | * SPADS from the current quadrant then this is an error */ |
sepp_nepp | 11:c6f95a42d4d7 | 1120 | ErrState = VL53L0X_ERROR_REF_SPAD_INIT; |
sepp_nepp | 11:c6f95a42d4d7 | 1121 | break;} |
sepp_nepp | 11:c6f95a42d4d7 | 1122 | |
sepp_nepp | 11:c6f95a42d4d7 | 1123 | current_SPAD = (uint32_t)next_good_SPAD; |
sepp_nepp | 11:c6f95a42d4d7 | 1124 | Enable_SPAD_bit(SPAD_array,size,current_SPAD); |
sepp_nepp | 11:c6f95a42d4d7 | 1125 | current_SPAD++; |
nikapov | 0:a1a69d32f310 | 1126 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1127 | *p_last_SPAD = current_SPAD; |
sepp_nepp | 11:c6f95a42d4d7 | 1128 | |
sepp_nepp | 11:c6f95a42d4d7 | 1129 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 1130 | { I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPAD_array,6); } // set_Ref_SPAD_map() |
sepp_nepp | 11:c6f95a42d4d7 | 1131 | |
sepp_nepp | 11:c6f95a42d4d7 | 1132 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 1133 | // Get the ref_SPAD_map from the device |
sepp_nepp | 11:c6f95a42d4d7 | 1134 | I2c_Read(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,check_SPAD_array,6); |
sepp_nepp | 11:c6f95a42d4d7 | 1135 | |
sepp_nepp | 11:c6f95a42d4d7 | 1136 | /* Compare SPAD maps. If not equal report error. */ |
nikapov | 0:a1a69d32f310 | 1137 | i = 0; |
nikapov | 0:a1a69d32f310 | 1138 | while (i < size) { |
sepp_nepp | 11:c6f95a42d4d7 | 1139 | if (SPAD_array[i] != check_SPAD_array[i]) { |
sepp_nepp | 11:c6f95a42d4d7 | 1140 | ErrState = VL53L0X_ERROR_REF_SPAD_INIT; |
nikapov | 0:a1a69d32f310 | 1141 | break; |
nikapov | 0:a1a69d32f310 | 1142 | } |
nikapov | 0:a1a69d32f310 | 1143 | i++; |
nikapov | 0:a1a69d32f310 | 1144 | } |
nikapov | 0:a1a69d32f310 | 1145 | } |
nikapov | 0:a1a69d32f310 | 1146 | } |
nikapov | 0:a1a69d32f310 | 1147 | |
sepp_nepp | 11:c6f95a42d4d7 | 1148 | void VL53L0X::Set_device_mode(VL53L0X_DeviceModes device_mode) |
sepp_nepp | 11:c6f95a42d4d7 | 1149 | { if (ErrState != VL53L0X_OK) {return; } // no reaction while in Error State!!!! |
nikapov | 0:a1a69d32f310 | 1150 | |
nikapov | 0:a1a69d32f310 | 1151 | switch (device_mode) { |
nikapov | 0:a1a69d32f310 | 1152 | case VL53L0X_DEVICEMODE_SINGLE_RANGING: |
nikapov | 0:a1a69d32f310 | 1153 | case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: |
nikapov | 0:a1a69d32f310 | 1154 | case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: |
nikapov | 0:a1a69d32f310 | 1155 | case VL53L0X_DEVICEMODE_GPIO_DRIVE: |
sepp_nepp | 11:c6f95a42d4d7 | 1156 | case VL53L0X_DEVICEMODE_GPIO_OSC: /* Supported modes */ |
sepp_nepp | 11:c6f95a42d4d7 | 1157 | CurrParams.DeviceMode = device_mode; |
nikapov | 0:a1a69d32f310 | 1158 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 1159 | default: /* Unsupported mode */ |
sepp_nepp | 11:c6f95a42d4d7 | 1160 | ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED; |
nikapov | 0:a1a69d32f310 | 1161 | } |
nikapov | 0:a1a69d32f310 | 1162 | } |
nikapov | 0:a1a69d32f310 | 1163 | |
sepp_nepp | 11:c6f95a42d4d7 | 1164 | void VL53L0X::Set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 threshold_low, |
sepp_nepp | 11:c6f95a42d4d7 | 1165 | TFP1616 threshold_high) |
sepp_nepp | 11:c6f95a42d4d7 | 1166 | { uint16_t threshold16; |
sepp_nepp | 5:b95f6951f7d5 | 1167 | |
sepp_nepp | 5:b95f6951f7d5 | 1168 | /* no dependency on DeviceMode for FlightSense */ |
nikapov | 0:a1a69d32f310 | 1169 | /* Need to divide by 2 because the FW will apply a x2 */ |
nikapov | 0:a1a69d32f310 | 1170 | threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff); |
sepp_nepp | 11:c6f95a42d4d7 | 1171 | Write_Word(REG_SYSTEM_THRESH_LOW,threshold16); |
sepp_nepp | 11:c6f95a42d4d7 | 1172 | |
sepp_nepp | 11:c6f95a42d4d7 | 1173 | /* Need to divide by 2 because the FW will apply a x2 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1174 | threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff); |
sepp_nepp | 11:c6f95a42d4d7 | 1175 | Write_Word(REG_SYSTEM_THRESH_HIGH,threshold16); |
nikapov | 0:a1a69d32f310 | 1176 | } |
nikapov | 0:a1a69d32f310 | 1177 | |
sepp_nepp | 11:c6f95a42d4d7 | 1178 | void VL53L0X::Get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 *p_threshold_low, |
sepp_nepp | 11:c6f95a42d4d7 | 1179 | TFP1616 *p_threshold_high) |
sepp_nepp | 11:c6f95a42d4d7 | 1180 | { uint16_t threshold16; |
sepp_nepp | 5:b95f6951f7d5 | 1181 | |
sepp_nepp | 5:b95f6951f7d5 | 1182 | /* no dependency on DeviceMode for FlightSense */ |
sepp_nepp | 11:c6f95a42d4d7 | 1183 | threshold16 = Read_Word(REG_SYSTEM_THRESH_LOW); |
nikapov | 0:a1a69d32f310 | 1184 | /* Need to multiply by 2 because the FW will apply a x2 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1185 | *p_threshold_low = (TFP1616)((0x00fff & threshold16) << 17); |
sepp_nepp | 11:c6f95a42d4d7 | 1186 | |
sepp_nepp | 11:c6f95a42d4d7 | 1187 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 1188 | threshold16 = Read_Word(REG_SYSTEM_THRESH_HIGH); |
nikapov | 0:a1a69d32f310 | 1189 | /* Need to multiply by 2 because the FW will apply a x2 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1190 | *p_threshold_high = (TFP1616)((0x00fff & threshold16) << 17); |
nikapov | 0:a1a69d32f310 | 1191 | } |
nikapov | 0:a1a69d32f310 | 1192 | } |
nikapov | 0:a1a69d32f310 | 1193 | |
sepp_nepp | 11:c6f95a42d4d7 | 1194 | void VL53L0X::Load_tuning_settings(uint8_t *p_tuning_setting_buffer) |
sepp_nepp | 11:c6f95a42d4d7 | 1195 | { int i; |
nikapov | 0:a1a69d32f310 | 1196 | int index; |
nikapov | 0:a1a69d32f310 | 1197 | uint8_t msb; |
nikapov | 0:a1a69d32f310 | 1198 | uint8_t lsb; |
nikapov | 0:a1a69d32f310 | 1199 | uint8_t select_param; |
nikapov | 0:a1a69d32f310 | 1200 | uint8_t number_of_writes; |
nikapov | 0:a1a69d32f310 | 1201 | uint8_t address; |
nikapov | 0:a1a69d32f310 | 1202 | uint8_t local_buffer[4]; /* max */ |
nikapov | 0:a1a69d32f310 | 1203 | uint16_t temp16; |
nikapov | 0:a1a69d32f310 | 1204 | |
nikapov | 0:a1a69d32f310 | 1205 | index = 0; |
nikapov | 0:a1a69d32f310 | 1206 | |
nikapov | 0:a1a69d32f310 | 1207 | while ((*(p_tuning_setting_buffer + index) != 0) && |
sepp_nepp | 11:c6f95a42d4d7 | 1208 | (ErrState == VL53L0X_OK)) { |
nikapov | 0:a1a69d32f310 | 1209 | number_of_writes = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1210 | index++; |
nikapov | 0:a1a69d32f310 | 1211 | if (number_of_writes == 0xFF) { |
nikapov | 0:a1a69d32f310 | 1212 | /* internal parameters */ |
nikapov | 0:a1a69d32f310 | 1213 | select_param = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1214 | index++; |
nikapov | 0:a1a69d32f310 | 1215 | switch (select_param) { |
nikapov | 0:a1a69d32f310 | 1216 | case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */ |
nikapov | 0:a1a69d32f310 | 1217 | msb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1218 | index++; |
nikapov | 0:a1a69d32f310 | 1219 | lsb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1220 | index++; |
sepp_nepp | 7:41cbc431e1f4 | 1221 | temp16 = VL53L0X_MAKEUINT16(lsb,msb); |
sepp_nepp | 10:cd251e0fc2fd | 1222 | SigmaEstRefArray = temp16; |
nikapov | 0:a1a69d32f310 | 1223 | break; |
nikapov | 0:a1a69d32f310 | 1224 | case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */ |
nikapov | 0:a1a69d32f310 | 1225 | msb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1226 | index++; |
nikapov | 0:a1a69d32f310 | 1227 | lsb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1228 | index++; |
sepp_nepp | 7:41cbc431e1f4 | 1229 | temp16 = VL53L0X_MAKEUINT16(lsb,msb); |
sepp_nepp | 10:cd251e0fc2fd | 1230 | SigmaEstEffPulseWidth = temp16; |
nikapov | 0:a1a69d32f310 | 1231 | break; |
nikapov | 0:a1a69d32f310 | 1232 | case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */ |
nikapov | 0:a1a69d32f310 | 1233 | msb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1234 | index++; |
nikapov | 0:a1a69d32f310 | 1235 | lsb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1236 | index++; |
sepp_nepp | 7:41cbc431e1f4 | 1237 | temp16 = VL53L0X_MAKEUINT16(lsb,msb); |
sepp_nepp | 10:cd251e0fc2fd | 1238 | SigmaEstEffAmbWidth = temp16; |
nikapov | 0:a1a69d32f310 | 1239 | break; |
nikapov | 0:a1a69d32f310 | 1240 | case 3: /* uint16_t targetRefRate -> 2 bytes */ |
nikapov | 0:a1a69d32f310 | 1241 | msb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1242 | index++; |
nikapov | 0:a1a69d32f310 | 1243 | lsb = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1244 | index++; |
sepp_nepp | 7:41cbc431e1f4 | 1245 | temp16 = VL53L0X_MAKEUINT16(lsb,msb); |
sepp_nepp | 10:cd251e0fc2fd | 1246 | targetRefRate = temp16; |
nikapov | 0:a1a69d32f310 | 1247 | break; |
nikapov | 0:a1a69d32f310 | 1248 | default: /* invalid parameter */ |
sepp_nepp | 11:c6f95a42d4d7 | 1249 | ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
nikapov | 0:a1a69d32f310 | 1250 | } |
nikapov | 0:a1a69d32f310 | 1251 | } else if (number_of_writes <= 4) { |
nikapov | 0:a1a69d32f310 | 1252 | address = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1253 | index++; |
nikapov | 0:a1a69d32f310 | 1254 | for (i = 0; i < number_of_writes; i++) { |
sepp_nepp | 11:c6f95a42d4d7 | 1255 | local_buffer[i] = *(p_tuning_setting_buffer + index); |
nikapov | 0:a1a69d32f310 | 1256 | index++; |
nikapov | 0:a1a69d32f310 | 1257 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1258 | I2c_Write(address,local_buffer,number_of_writes); |
nikapov | 0:a1a69d32f310 | 1259 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 1260 | ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
nikapov | 0:a1a69d32f310 | 1261 | } |
nikapov | 0:a1a69d32f310 | 1262 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1263 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1264 | |
sepp_nepp | 11:c6f95a42d4d7 | 1265 | void VL53L0X::Check_and_load_interrupt_settings(uint8_t start_not_stopflag) |
sepp_nepp | 11:c6f95a42d4d7 | 1266 | { uint8_t interrupt_config; |
sepp_nepp | 11:c6f95a42d4d7 | 1267 | TFP1616 threshold_low; |
sepp_nepp | 11:c6f95a42d4d7 | 1268 | TFP1616 threshold_high; |
sepp_nepp | 5:b95f6951f7d5 | 1269 | |
sepp_nepp | 11:c6f95a42d4d7 | 1270 | if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error |
sepp_nepp | 11:c6f95a42d4d7 | 1271 | |
sepp_nepp | 12:aa177f0e4c10 | 1272 | interrupt_config = GpioFunctionality; |
sepp_nepp | 11:c6f95a42d4d7 | 1273 | |
sepp_nepp | 11:c6f95a42d4d7 | 1274 | if ((interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_LOW ) || |
sepp_nepp | 11:c6f95a42d4d7 | 1275 | (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_HIGH) || |
sepp_nepp | 11:c6f95a42d4d7 | 1276 | (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_OUT )) { |
sepp_nepp | 11:c6f95a42d4d7 | 1277 | Get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, |
sepp_nepp | 7:41cbc431e1f4 | 1278 | &threshold_low,&threshold_high); |
nikapov | 0:a1a69d32f310 | 1279 | |
sepp_nepp | 11:c6f95a42d4d7 | 1280 | if (((threshold_low > 255 * 65536) || (threshold_high > 255 * 65536)) && |
sepp_nepp | 11:c6f95a42d4d7 | 1281 | (ErrState == VL53L0X_OK)) |
sepp_nepp | 11:c6f95a42d4d7 | 1282 | { if (start_not_stopflag != 0) |
sepp_nepp | 11:c6f95a42d4d7 | 1283 | {Load_tuning_settings(InterruptThresholdSettings); } |
sepp_nepp | 11:c6f95a42d4d7 | 1284 | else |
sepp_nepp | 11:c6f95a42d4d7 | 1285 | {Write_Byte(0xFF,0x04); |
sepp_nepp | 11:c6f95a42d4d7 | 1286 | Write_Byte(0x70,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 1287 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 1288 | Write_Byte(0x80,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 1289 | } |
nikapov | 0:a1a69d32f310 | 1290 | } |
nikapov | 0:a1a69d32f310 | 1291 | } |
nikapov | 0:a1a69d32f310 | 1292 | } |
nikapov | 0:a1a69d32f310 | 1293 | |
sepp_nepp | 11:c6f95a42d4d7 | 1294 | void VL53L0X::Start_Measurement() |
sepp_nepp | 11:c6f95a42d4d7 | 1295 | { VL53L0X_DeviceModes device_mode; |
nikapov | 0:a1a69d32f310 | 1296 | uint8_t byte; |
sepp_nepp | 11:c6f95a42d4d7 | 1297 | uint8_t start_stop_byte = REG_SYSRANGE_MODE_START_STOP; |
nikapov | 0:a1a69d32f310 | 1298 | uint32_t loop_nb; |
nikapov | 0:a1a69d32f310 | 1299 | |
sepp_nepp | 11:c6f95a42d4d7 | 1300 | if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 1301 | |
nikapov | 0:a1a69d32f310 | 1302 | /* Get Current DeviceMode */ |
sepp_nepp | 11:c6f95a42d4d7 | 1303 | device_mode = CurrParams.DeviceMode; |
sepp_nepp | 11:c6f95a42d4d7 | 1304 | |
sepp_nepp | 11:c6f95a42d4d7 | 1305 | Write_Byte(0x80,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 1306 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 1307 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 1308 | Write_Byte(0x91,StopVariable); |
sepp_nepp | 11:c6f95a42d4d7 | 1309 | Write_Byte(0x00,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 1310 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 1311 | Write_Byte(0x80,0x00); |
nikapov | 0:a1a69d32f310 | 1312 | |
nikapov | 0:a1a69d32f310 | 1313 | switch (device_mode) { |
nikapov | 0:a1a69d32f310 | 1314 | case VL53L0X_DEVICEMODE_SINGLE_RANGING: |
sepp_nepp | 11:c6f95a42d4d7 | 1315 | Write_Byte(REG_SYSRANGE_START,0x01); |
nikapov | 0:a1a69d32f310 | 1316 | |
nikapov | 0:a1a69d32f310 | 1317 | byte = start_stop_byte; |
sepp_nepp | 11:c6f95a42d4d7 | 1318 | if (ErrState == VL53L0X_OK) { |
nikapov | 0:a1a69d32f310 | 1319 | /* Wait until start bit has been cleared */ |
nikapov | 0:a1a69d32f310 | 1320 | loop_nb = 0; |
nikapov | 0:a1a69d32f310 | 1321 | do { |
nikapov | 0:a1a69d32f310 | 1322 | if (loop_nb > 0) |
sepp_nepp | 11:c6f95a42d4d7 | 1323 | byte = Read_Byte(REG_SYSRANGE_START); |
nikapov | 0:a1a69d32f310 | 1324 | loop_nb = loop_nb + 1; |
nikapov | 0:a1a69d32f310 | 1325 | } while (((byte & start_stop_byte) == start_stop_byte) |
sepp_nepp | 11:c6f95a42d4d7 | 1326 | && (ErrState == VL53L0X_OK) |
nikapov | 0:a1a69d32f310 | 1327 | && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP)); |
nikapov | 0:a1a69d32f310 | 1328 | |
Davidroid | 3:e9269ff624ed | 1329 | if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { |
sepp_nepp | 11:c6f95a42d4d7 | 1330 | ErrState = VL53L0X_ERROR_TIME_OUT; |
Davidroid | 3:e9269ff624ed | 1331 | } |
nikapov | 0:a1a69d32f310 | 1332 | } |
nikapov | 0:a1a69d32f310 | 1333 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 1334 | |
sepp_nepp | 11:c6f95a42d4d7 | 1335 | case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: |
sepp_nepp | 11:c6f95a42d4d7 | 1336 | /* Back-to-back mode, Check if need to apply interrupt settings */ |
sepp_nepp | 11:c6f95a42d4d7 | 1337 | Check_and_load_interrupt_settings(1); |
sepp_nepp | 11:c6f95a42d4d7 | 1338 | Write_Byte(REG_SYSRANGE_START,REG_SYSRANGE_MODE_BACKTOBACK); |
sepp_nepp | 11:c6f95a42d4d7 | 1339 | Set_Current_State( VL53L0X_STATE_RUNNING ); |
sepp_nepp | 11:c6f95a42d4d7 | 1340 | break; |
nikapov | 0:a1a69d32f310 | 1341 | case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: |
sepp_nepp | 11:c6f95a42d4d7 | 1342 | /* Continuous mode; Check if need to apply interrupt settings */ |
sepp_nepp | 11:c6f95a42d4d7 | 1343 | Check_and_load_interrupt_settings(1); |
sepp_nepp | 11:c6f95a42d4d7 | 1344 | Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_TIMED); |
sepp_nepp | 11:c6f95a42d4d7 | 1345 | Set_Current_State( VL53L0X_STATE_RUNNING ); |
nikapov | 0:a1a69d32f310 | 1346 | break; |
nikapov | 0:a1a69d32f310 | 1347 | default: |
nikapov | 0:a1a69d32f310 | 1348 | /* Selected mode not supported */ |
sepp_nepp | 11:c6f95a42d4d7 | 1349 | ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED; |
nikapov | 0:a1a69d32f310 | 1350 | } |
nikapov | 0:a1a69d32f310 | 1351 | } |
nikapov | 0:a1a69d32f310 | 1352 | |
sepp_nepp | 11:c6f95a42d4d7 | 1353 | /* Group Device Measurement Functions */ |
sepp_nepp | 11:c6f95a42d4d7 | 1354 | void VL53L0X::Perf_single_measurement() |
sepp_nepp | 11:c6f95a42d4d7 | 1355 | { VL53L0X_DeviceModes device_mode; |
sepp_nepp | 11:c6f95a42d4d7 | 1356 | |
sepp_nepp | 11:c6f95a42d4d7 | 1357 | if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! |
nikapov | 0:a1a69d32f310 | 1358 | |
nikapov | 0:a1a69d32f310 | 1359 | /* Get Current DeviceMode */ |
sepp_nepp | 11:c6f95a42d4d7 | 1360 | device_mode = CurrParams.DeviceMode; |
nikapov | 0:a1a69d32f310 | 1361 | |
nikapov | 0:a1a69d32f310 | 1362 | /* Start immediately to run a single ranging measurement in case of |
nikapov | 0:a1a69d32f310 | 1363 | * single ranging or single histogram */ |
sepp_nepp | 11:c6f95a42d4d7 | 1364 | if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {Start_Measurement();} |
sepp_nepp | 11:c6f95a42d4d7 | 1365 | |
sepp_nepp | 11:c6f95a42d4d7 | 1366 | Poll_Measure_Completion(); |
sepp_nepp | 11:c6f95a42d4d7 | 1367 | |
sepp_nepp | 11:c6f95a42d4d7 | 1368 | /* Change Device State in case of single ranging or single histogram */ |
sepp_nepp | 11:c6f95a42d4d7 | 1369 | if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) |
sepp_nepp | 11:c6f95a42d4d7 | 1370 | { Set_Current_State( VL53L0X_STATE_IDLE ); } |
nikapov | 0:a1a69d32f310 | 1371 | } |
nikapov | 0:a1a69d32f310 | 1372 | |
sepp_nepp | 11:c6f95a42d4d7 | 1373 | TFP1616 VL53L0X::Get_total_xtalk_rate(TRangeResults *p_ranging_results) |
sepp_nepp | 11:c6f95a42d4d7 | 1374 | { TFP1616 total_xtalk_MHz; |
sepp_nepp | 11:c6f95a42d4d7 | 1375 | |
sepp_nepp | 11:c6f95a42d4d7 | 1376 | // CurrParams.XTalk_Compens_En was Get_xtalk_compensation_enable |
sepp_nepp | 11:c6f95a42d4d7 | 1377 | if ( (ErrState == VL53L0X_OK) & (CurrParams.XTalk_Compens_En ) ) |
sepp_nepp | 11:c6f95a42d4d7 | 1378 | { /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1379 | total_xtalk_MHz = p_ranging_results->EffectiveSPADRtnCount * |
sepp_nepp | 11:c6f95a42d4d7 | 1380 | CurrParams.Xtalk_CompRate_MHz; |
sepp_nepp | 11:c6f95a42d4d7 | 1381 | |
sepp_nepp | 11:c6f95a42d4d7 | 1382 | /* FixPoint0824 >> 8 = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1383 | return (total_xtalk_MHz + 0x80) >> 8; |
sepp_nepp | 11:c6f95a42d4d7 | 1384 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1385 | else { return 0; } |
nikapov | 0:a1a69d32f310 | 1386 | } |
nikapov | 0:a1a69d32f310 | 1387 | |
sepp_nepp | 11:c6f95a42d4d7 | 1388 | void VL53L0X::Get_total_SIG_rate(TRangeResults *p_ranging_results, |
sepp_nepp | 11:c6f95a42d4d7 | 1389 | TFP1616 *p_total_SIG_rate_mcps) |
sepp_nepp | 11:c6f95a42d4d7 | 1390 | { TFP1616 total_xtalk_MHz; |
sepp_nepp | 11:c6f95a42d4d7 | 1391 | |
sepp_nepp | 11:c6f95a42d4d7 | 1392 | *p_total_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz; |
sepp_nepp | 11:c6f95a42d4d7 | 1393 | total_xtalk_MHz = Get_total_xtalk_rate(p_ranging_results); |
sepp_nepp | 5:b95f6951f7d5 | 1394 | |
sepp_nepp | 11:c6f95a42d4d7 | 1395 | if (ErrState == VL53L0X_OK) { *p_total_SIG_rate_mcps += total_xtalk_MHz;} |
nikapov | 0:a1a69d32f310 | 1396 | } |
nikapov | 0:a1a69d32f310 | 1397 | |
nikapov | 0:a1a69d32f310 | 1398 | /* To convert ms into register value */ |
sepp_nepp | 11:c6f95a42d4d7 | 1399 | uint32_t VL53L0X::Calc_timeout_mclks(uint32_t timeout_period_us, |
nikapov | 0:a1a69d32f310 | 1400 | uint8_t vcsel_period_pclks) |
sepp_nepp | 11:c6f95a42d4d7 | 1401 | { uint32_t macro_period_ns; |
sepp_nepp | 11:c6f95a42d4d7 | 1402 | |
sepp_nepp | 11:c6f95a42d4d7 | 1403 | macro_period_ns = (uint32_t)(vcsel_period_pclks) * VL53L0X_MACRO_PERIOD_NS; |
sepp_nepp | 11:c6f95a42d4d7 | 1404 | |
sepp_nepp | 11:c6f95a42d4d7 | 1405 | return (uint32_t)(((timeout_period_us * 1000) |
sepp_nepp | 11:c6f95a42d4d7 | 1406 | + (macro_period_ns / 2)) / macro_period_ns); |
nikapov | 0:a1a69d32f310 | 1407 | } |
nikapov | 0:a1a69d32f310 | 1408 | |
sepp_nepp | 11:c6f95a42d4d7 | 1409 | uint32_t VL53L0X::ISQRT(uint32_t num) |
sepp_nepp | 11:c6f95a42d4d7 | 1410 | { /* Implements an integer square root |
sepp_nepp | 11:c6f95a42d4d7 | 1411 | * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */ |
nikapov | 0:a1a69d32f310 | 1412 | uint32_t res = 0; |
nikapov | 0:a1a69d32f310 | 1413 | uint32_t bit = 1 << 30; |
sepp_nepp | 11:c6f95a42d4d7 | 1414 | /* The second-to-top bit is set: 1 << 14 for 16-bits,1 << 30 for 32 bits */ |
nikapov | 0:a1a69d32f310 | 1415 | |
nikapov | 0:a1a69d32f310 | 1416 | /* "bit" starts at the highest power of four <= the argument. */ |
sepp_nepp | 10:cd251e0fc2fd | 1417 | while (bit > num) { bit >>= 2; } |
nikapov | 0:a1a69d32f310 | 1418 | |
nikapov | 0:a1a69d32f310 | 1419 | while (bit != 0) { |
nikapov | 0:a1a69d32f310 | 1420 | if (num >= res + bit) { |
nikapov | 0:a1a69d32f310 | 1421 | num -= res + bit; |
nikapov | 0:a1a69d32f310 | 1422 | res = (res >> 1) + bit; |
sepp_nepp | 11:c6f95a42d4d7 | 1423 | } else { res >>= 1; } |
nikapov | 0:a1a69d32f310 | 1424 | bit >>= 2; |
nikapov | 0:a1a69d32f310 | 1425 | } |
nikapov | 0:a1a69d32f310 | 1426 | return res; |
nikapov | 0:a1a69d32f310 | 1427 | } |
nikapov | 0:a1a69d32f310 | 1428 | |
sepp_nepp | 11:c6f95a42d4d7 | 1429 | void VL53L0X::Calc_dmax(TFP1616 total_SIG_rate_mcps, |
sepp_nepp | 11:c6f95a42d4d7 | 1430 | TFP1616 total_corr_SIG_rate_mcps, |
sepp_nepp | 11:c6f95a42d4d7 | 1431 | TFP1616 pw_mult, |
nikapov | 0:a1a69d32f310 | 1432 | uint32_t sigma_estimate_p1, |
sepp_nepp | 11:c6f95a42d4d7 | 1433 | TFP1616 sigma_estimate_p2, |
nikapov | 0:a1a69d32f310 | 1434 | uint32_t peak_vcsel_duration_us, |
nikapov | 0:a1a69d32f310 | 1435 | uint32_t *pd_max_mm) |
sepp_nepp | 11:c6f95a42d4d7 | 1436 | { const uint32_t c_sigma_limit = 18; |
sepp_nepp | 11:c6f95a42d4d7 | 1437 | const TFP1616 c_SIG_limit = 0x4000; /* 0.25 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1438 | const TFP1616 c_sigma_est_Ref = 0x00000042; /* 0.001 */ |
nikapov | 0:a1a69d32f310 | 1439 | const uint32_t c_amb_eff_width_sigma_est_ns = 6; |
nikapov | 0:a1a69d32f310 | 1440 | const uint32_t c_amb_eff_width_d_max_ns = 7; |
nikapov | 0:a1a69d32f310 | 1441 | uint32_t dmax_cal_range_mm; |
sepp_nepp | 11:c6f95a42d4d7 | 1442 | TFP1616 dmax_cal_SIG_rate_rtn_mcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1443 | TFP1616 min_SIG_needed; |
sepp_nepp | 11:c6f95a42d4d7 | 1444 | TFP1616 min_SIG_needed_p1; |
sepp_nepp | 11:c6f95a42d4d7 | 1445 | TFP1616 min_SIG_needed_p2; |
sepp_nepp | 11:c6f95a42d4d7 | 1446 | TFP1616 min_SIG_needed_p3; |
sepp_nepp | 11:c6f95a42d4d7 | 1447 | TFP1616 min_SIG_needed_p4; |
sepp_nepp | 11:c6f95a42d4d7 | 1448 | TFP1616 sigma_limit_tmp; |
sepp_nepp | 11:c6f95a42d4d7 | 1449 | TFP1616 sigma_est_sq_tmp; |
sepp_nepp | 11:c6f95a42d4d7 | 1450 | TFP1616 signal_limit_tmp; |
sepp_nepp | 11:c6f95a42d4d7 | 1451 | TFP1616 signal_at0_mm; |
sepp_nepp | 11:c6f95a42d4d7 | 1452 | TFP1616 dmax_dark; |
sepp_nepp | 11:c6f95a42d4d7 | 1453 | TFP1616 dmax_ambient; |
sepp_nepp | 11:c6f95a42d4d7 | 1454 | TFP1616 dmax_dark_tmp; |
sepp_nepp | 11:c6f95a42d4d7 | 1455 | TFP1616 sigma_est_p2_tmp; |
nikapov | 0:a1a69d32f310 | 1456 | uint32_t signal_rate_temp_mcps; |
nikapov | 0:a1a69d32f310 | 1457 | |
sepp_nepp | 10:cd251e0fc2fd | 1458 | dmax_cal_range_mm = DmaxCalRangeMilliMeter; |
sepp_nepp | 10:cd251e0fc2fd | 1459 | |
sepp_nepp | 11:c6f95a42d4d7 | 1460 | dmax_cal_SIG_rate_rtn_mcps = DmaxCalSignalRateRtnMHz; |
nikapov | 0:a1a69d32f310 | 1461 | |
nikapov | 0:a1a69d32f310 | 1462 | /* uint32 * FixPoint1616 = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1463 | signal_at0_mm = dmax_cal_range_mm * dmax_cal_SIG_rate_rtn_mcps; |
nikapov | 0:a1a69d32f310 | 1464 | |
nikapov | 0:a1a69d32f310 | 1465 | /* FixPoint1616 >> 8 = FixPoint2408 */ |
nikapov | 0:a1a69d32f310 | 1466 | signal_at0_mm = (signal_at0_mm + 0x80) >> 8; |
nikapov | 0:a1a69d32f310 | 1467 | signal_at0_mm *= dmax_cal_range_mm; |
nikapov | 0:a1a69d32f310 | 1468 | |
sepp_nepp | 11:c6f95a42d4d7 | 1469 | min_SIG_needed_p1 = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1470 | if (total_corr_SIG_rate_mcps > 0) { |
sepp_nepp | 11:c6f95a42d4d7 | 1471 | /* Shift by 10 bits to increase resolution prior to the division */ |
sepp_nepp | 11:c6f95a42d4d7 | 1472 | signal_rate_temp_mcps = total_SIG_rate_mcps << 10; |
nikapov | 0:a1a69d32f310 | 1473 | |
nikapov | 0:a1a69d32f310 | 1474 | /* Add rounding value prior to division */ |
sepp_nepp | 11:c6f95a42d4d7 | 1475 | min_SIG_needed_p1 = signal_rate_temp_mcps + (total_corr_SIG_rate_mcps / 2); |
nikapov | 0:a1a69d32f310 | 1476 | |
nikapov | 0:a1a69d32f310 | 1477 | /* FixPoint0626/FixPoint1616 = FixPoint2210 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1478 | min_SIG_needed_p1 /= total_corr_SIG_rate_mcps; |
nikapov | 0:a1a69d32f310 | 1479 | |
nikapov | 0:a1a69d32f310 | 1480 | /* Apply a factored version of the speed of light. |
nikapov | 0:a1a69d32f310 | 1481 | Correction to be applied at the end */ |
sepp_nepp | 11:c6f95a42d4d7 | 1482 | min_SIG_needed_p1 *= 3; |
nikapov | 0:a1a69d32f310 | 1483 | |
nikapov | 0:a1a69d32f310 | 1484 | /* FixPoint2210 * FixPoint2210 = FixPoint1220 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1485 | min_SIG_needed_p1 *= min_SIG_needed_p1; |
nikapov | 0:a1a69d32f310 | 1486 | |
nikapov | 0:a1a69d32f310 | 1487 | /* FixPoint1220 >> 16 = FixPoint2804 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1488 | min_SIG_needed_p1 = (min_SIG_needed_p1 + 0x8000) >> 16; |
nikapov | 0:a1a69d32f310 | 1489 | } |
nikapov | 0:a1a69d32f310 | 1490 | |
sepp_nepp | 11:c6f95a42d4d7 | 1491 | min_SIG_needed_p2 = pw_mult * sigma_estimate_p1; |
nikapov | 0:a1a69d32f310 | 1492 | |
nikapov | 0:a1a69d32f310 | 1493 | /* FixPoint1616 >> 16 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1494 | min_SIG_needed_p2 = (min_SIG_needed_p2 + 0x8000) >> 16; |
nikapov | 0:a1a69d32f310 | 1495 | |
nikapov | 0:a1a69d32f310 | 1496 | /* uint32 * uint32 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1497 | min_SIG_needed_p2 *= min_SIG_needed_p2; |
sepp_nepp | 11:c6f95a42d4d7 | 1498 | |
sepp_nepp | 11:c6f95a42d4d7 | 1499 | /* Check sigmaEstimateP2; If this value is too high, there is not enough |
sepp_nepp | 11:c6f95a42d4d7 | 1500 | * signal rate to calculate dmax value so set a suitable value to ensure |
sepp_nepp | 11:c6f95a42d4d7 | 1501 | * a very small dmax. */ |
nikapov | 0:a1a69d32f310 | 1502 | sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16; |
nikapov | 0:a1a69d32f310 | 1503 | sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) / |
nikapov | 0:a1a69d32f310 | 1504 | c_amb_eff_width_sigma_est_ns; |
nikapov | 0:a1a69d32f310 | 1505 | sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns; |
nikapov | 0:a1a69d32f310 | 1506 | |
nikapov | 0:a1a69d32f310 | 1507 | if (sigma_est_p2_tmp > 0xffff) { |
sepp_nepp | 11:c6f95a42d4d7 | 1508 | min_SIG_needed_p3 = 0xfff00000; |
nikapov | 0:a1a69d32f310 | 1509 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 1510 | /* DMAX uses a different ambient width from sigma,so apply correction. |
sepp_nepp | 11:c6f95a42d4d7 | 1511 | * Perform division before multiplication to prevent overflow. */ |
nikapov | 0:a1a69d32f310 | 1512 | sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) / |
nikapov | 0:a1a69d32f310 | 1513 | c_amb_eff_width_sigma_est_ns; |
nikapov | 0:a1a69d32f310 | 1514 | sigma_estimate_p2 *= c_amb_eff_width_d_max_ns; |
nikapov | 0:a1a69d32f310 | 1515 | |
nikapov | 0:a1a69d32f310 | 1516 | /* FixPoint1616 >> 16 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1517 | min_SIG_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16; |
sepp_nepp | 11:c6f95a42d4d7 | 1518 | min_SIG_needed_p3 *= min_SIG_needed_p3; |
nikapov | 0:a1a69d32f310 | 1519 | } |
nikapov | 0:a1a69d32f310 | 1520 | |
nikapov | 0:a1a69d32f310 | 1521 | /* FixPoint1814 / uint32 = FixPoint1814 */ |
nikapov | 0:a1a69d32f310 | 1522 | sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1523 | |
nikapov | 0:a1a69d32f310 | 1524 | /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */ |
nikapov | 0:a1a69d32f310 | 1525 | sigma_limit_tmp *= sigma_limit_tmp; |
nikapov | 0:a1a69d32f310 | 1526 | |
nikapov | 0:a1a69d32f310 | 1527 | /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1528 | sigma_est_sq_tmp = c_sigma_est_Ref * c_sigma_est_Ref; |
nikapov | 0:a1a69d32f310 | 1529 | |
nikapov | 0:a1a69d32f310 | 1530 | /* FixPoint3232 >> 4 = FixPoint0428 */ |
nikapov | 0:a1a69d32f310 | 1531 | sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4; |
nikapov | 0:a1a69d32f310 | 1532 | |
nikapov | 0:a1a69d32f310 | 1533 | /* FixPoint0428 - FixPoint0428 = FixPoint0428 */ |
nikapov | 0:a1a69d32f310 | 1534 | sigma_limit_tmp -= sigma_est_sq_tmp; |
nikapov | 0:a1a69d32f310 | 1535 | |
nikapov | 0:a1a69d32f310 | 1536 | /* uint32_t * FixPoint0428 = FixPoint0428 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1537 | min_SIG_needed_p4 = 4 * 12 * sigma_limit_tmp; |
nikapov | 0:a1a69d32f310 | 1538 | |
nikapov | 0:a1a69d32f310 | 1539 | /* FixPoint0428 >> 14 = FixPoint1814 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1540 | min_SIG_needed_p4 = (min_SIG_needed_p4 + 0x2000) >> 14; |
nikapov | 0:a1a69d32f310 | 1541 | |
nikapov | 0:a1a69d32f310 | 1542 | /* uint32 + uint32 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1543 | min_SIG_needed = (min_SIG_needed_p2 + min_SIG_needed_p3); |
nikapov | 0:a1a69d32f310 | 1544 | |
nikapov | 0:a1a69d32f310 | 1545 | /* uint32 / uint32 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1546 | min_SIG_needed += (peak_vcsel_duration_us / 2); |
sepp_nepp | 11:c6f95a42d4d7 | 1547 | min_SIG_needed /= peak_vcsel_duration_us; |
nikapov | 0:a1a69d32f310 | 1548 | |
nikapov | 0:a1a69d32f310 | 1549 | /* uint32 << 14 = FixPoint1814 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1550 | min_SIG_needed <<= 14; |
nikapov | 0:a1a69d32f310 | 1551 | |
nikapov | 0:a1a69d32f310 | 1552 | /* FixPoint1814 / FixPoint1814 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1553 | min_SIG_needed += (min_SIG_needed_p4 / 2); |
sepp_nepp | 11:c6f95a42d4d7 | 1554 | min_SIG_needed /= min_SIG_needed_p4; |
nikapov | 0:a1a69d32f310 | 1555 | |
nikapov | 0:a1a69d32f310 | 1556 | /* FixPoint3200 * FixPoint2804 := FixPoint2804*/ |
sepp_nepp | 11:c6f95a42d4d7 | 1557 | min_SIG_needed *= min_SIG_needed_p1; |
nikapov | 0:a1a69d32f310 | 1558 | |
nikapov | 0:a1a69d32f310 | 1559 | /* Apply correction by dividing by 1000000. |
sepp_nepp | 11:c6f95a42d4d7 | 1560 | * This assumes 10E16 on the numerator of the equation and 10E-22 on the denominator. |
nikapov | 0:a1a69d32f310 | 1561 | * We do this because 32bit fix point calculation can't |
nikapov | 0:a1a69d32f310 | 1562 | * handle the larger and smaller elements of this equation, |
nikapov | 0:a1a69d32f310 | 1563 | * i.e. speed of light and pulse widths. |
nikapov | 0:a1a69d32f310 | 1564 | */ |
sepp_nepp | 11:c6f95a42d4d7 | 1565 | min_SIG_needed = (min_SIG_needed + 500) / 1000; |
sepp_nepp | 11:c6f95a42d4d7 | 1566 | min_SIG_needed <<= 4; |
sepp_nepp | 11:c6f95a42d4d7 | 1567 | |
sepp_nepp | 11:c6f95a42d4d7 | 1568 | min_SIG_needed = (min_SIG_needed + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1569 | |
nikapov | 0:a1a69d32f310 | 1570 | /* FixPoint1616 >> 8 = FixPoint2408 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1571 | signal_limit_tmp = (c_SIG_limit + 0x80) >> 8; |
nikapov | 0:a1a69d32f310 | 1572 | |
nikapov | 0:a1a69d32f310 | 1573 | /* FixPoint2408/FixPoint2408 = uint32 */ |
nikapov | 0:a1a69d32f310 | 1574 | if (signal_limit_tmp != 0) { |
nikapov | 0:a1a69d32f310 | 1575 | dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2)) |
nikapov | 0:a1a69d32f310 | 1576 | / signal_limit_tmp; |
sepp_nepp | 11:c6f95a42d4d7 | 1577 | } else { dmax_dark_tmp = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1578 | |
sepp_nepp | 11:c6f95a42d4d7 | 1579 | dmax_dark = ISQRT(dmax_dark_tmp); |
nikapov | 0:a1a69d32f310 | 1580 | |
nikapov | 0:a1a69d32f310 | 1581 | /* FixPoint2408/FixPoint2408 = uint32 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1582 | if (min_SIG_needed != 0) |
sepp_nepp | 11:c6f95a42d4d7 | 1583 | { dmax_ambient = (signal_at0_mm + min_SIG_needed / 2) / min_SIG_needed; } |
sepp_nepp | 11:c6f95a42d4d7 | 1584 | else { dmax_ambient = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1585 | |
sepp_nepp | 11:c6f95a42d4d7 | 1586 | dmax_ambient = ISQRT(dmax_ambient); |
nikapov | 0:a1a69d32f310 | 1587 | |
nikapov | 0:a1a69d32f310 | 1588 | *pd_max_mm = dmax_dark; |
sepp_nepp | 11:c6f95a42d4d7 | 1589 | if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; } |
nikapov | 0:a1a69d32f310 | 1590 | } |
nikapov | 0:a1a69d32f310 | 1591 | |
sepp_nepp | 11:c6f95a42d4d7 | 1592 | void VL53L0X::Calc_sigma_estimate(TRangeResults *p_ranging_results, |
sepp_nepp | 11:c6f95a42d4d7 | 1593 | TFP1616 *p_sigma_estimate, uint32_t *p_dmax_mm) |
sepp_nepp | 11:c6f95a42d4d7 | 1594 | { /* Expressed in 100ths of a ns,i.e. centi-ns */ |
nikapov | 0:a1a69d32f310 | 1595 | const uint32_t c_pulse_effective_width_centi_ns = 800; |
sepp_nepp | 7:41cbc431e1f4 | 1596 | /* Expressed in 100ths of a ns,i.e. centi-ns */ |
nikapov | 0:a1a69d32f310 | 1597 | const uint32_t c_ambient_effective_width_centi_ns = 600; |
sepp_nepp | 11:c6f95a42d4d7 | 1598 | const TFP1616 c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */ |
nikapov | 0:a1a69d32f310 | 1599 | const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */ |
sepp_nepp | 11:c6f95a42d4d7 | 1600 | const TFP1616 c_sigma_est_max = 0x028F87AE; |
sepp_nepp | 11:c6f95a42d4d7 | 1601 | const TFP1616 c_sigma_est_rtn_max = 0xF000; |
sepp_nepp | 11:c6f95a42d4d7 | 1602 | const TFP1616 c_amb_to_SIG_ratio_max = 0xF0000000 / |
nikapov | 0:a1a69d32f310 | 1603 | c_ambient_effective_width_centi_ns; |
nikapov | 0:a1a69d32f310 | 1604 | /* Time Of Flight per mm (6.6 pico secs) */ |
sepp_nepp | 11:c6f95a42d4d7 | 1605 | const TFP1616 c_tof_per_mm_ps = 0x0006999A; |
nikapov | 0:a1a69d32f310 | 1606 | const uint32_t c_16bit_rounding_param = 0x00008000; |
sepp_nepp | 11:c6f95a42d4d7 | 1607 | const TFP1616 c_max_xtalk_kcps = 0x00320000; |
nikapov | 0:a1a69d32f310 | 1608 | const uint32_t c_pll_period_ps = 1655; |
nikapov | 0:a1a69d32f310 | 1609 | |
nikapov | 0:a1a69d32f310 | 1610 | uint32_t vcsel_total_events_rtn; |
nikapov | 0:a1a69d32f310 | 1611 | uint32_t final_range_timeout_micro_secs; |
nikapov | 0:a1a69d32f310 | 1612 | uint32_t pre_range_timeout_micro_secs; |
nikapov | 0:a1a69d32f310 | 1613 | uint32_t final_range_integration_time_milli_secs; |
sepp_nepp | 11:c6f95a42d4d7 | 1614 | TFP1616 sigma_estimate_p1; |
sepp_nepp | 11:c6f95a42d4d7 | 1615 | TFP1616 sigma_estimate_p2; |
sepp_nepp | 11:c6f95a42d4d7 | 1616 | TFP1616 sigma_estimate_p3; |
sepp_nepp | 11:c6f95a42d4d7 | 1617 | TFP1616 delta_t_ps; |
sepp_nepp | 11:c6f95a42d4d7 | 1618 | TFP1616 pw_mult; |
sepp_nepp | 11:c6f95a42d4d7 | 1619 | TFP1616 sigma_est_rtn; |
sepp_nepp | 11:c6f95a42d4d7 | 1620 | TFP1616 sigma_estimate; |
sepp_nepp | 11:c6f95a42d4d7 | 1621 | TFP1616 xtalk_correction; |
sepp_nepp | 11:c6f95a42d4d7 | 1622 | TFP1616 ambient_rate_kcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1623 | TFP1616 peak_SIG_rate_kcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1624 | TFP1616 xtalk_comp_rate_mcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1625 | uint32_t xtalk_comp_rate_kcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1626 | |
sepp_nepp | 11:c6f95a42d4d7 | 1627 | TFP1616 diff1_mcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1628 | TFP1616 diff2_mcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1629 | TFP1616 sqr1; |
sepp_nepp | 11:c6f95a42d4d7 | 1630 | TFP1616 sqr2; |
sepp_nepp | 11:c6f95a42d4d7 | 1631 | TFP1616 sqr_sum; |
sepp_nepp | 11:c6f95a42d4d7 | 1632 | TFP1616 sqrt_result_centi_ns; |
sepp_nepp | 11:c6f95a42d4d7 | 1633 | TFP1616 sqrt_result; |
sepp_nepp | 11:c6f95a42d4d7 | 1634 | TFP1616 total_SIG_rate_mcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1635 | TFP1616 corrected_SIG_rate_mcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1636 | TFP1616 sigma_est_Ref; |
nikapov | 0:a1a69d32f310 | 1637 | uint32_t vcsel_width; |
nikapov | 0:a1a69d32f310 | 1638 | uint32_t final_range_macro_pclks; |
nikapov | 0:a1a69d32f310 | 1639 | uint32_t pre_range_macro_pclks; |
nikapov | 0:a1a69d32f310 | 1640 | uint32_t peak_vcsel_duration_us; |
nikapov | 0:a1a69d32f310 | 1641 | uint8_t final_range_vcsel_pclks; |
nikapov | 0:a1a69d32f310 | 1642 | uint8_t pre_range_vcsel_pclks; |
nikapov | 0:a1a69d32f310 | 1643 | /*! \addtogroup calc_sigma_estimate |
nikapov | 0:a1a69d32f310 | 1644 | * @{ |
sepp_nepp | 11:c6f95a42d4d7 | 1645 | * Estimates the range sigma */ |
sepp_nepp | 11:c6f95a42d4d7 | 1646 | |
sepp_nepp | 11:c6f95a42d4d7 | 1647 | xtalk_comp_rate_mcps = CurrParams.Xtalk_CompRate_MHz; |
sepp_nepp | 10:cd251e0fc2fd | 1648 | |
sepp_nepp | 10:cd251e0fc2fd | 1649 | /* We work in kcps rather than mcps as this helps keep within the |
sepp_nepp | 11:c6f95a42d4d7 | 1650 | * confines of the 32 Fix1616 type. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1651 | ambient_rate_kcps = (p_ranging_results->AmbientRateRtnMHz * 1000) >> 16; |
sepp_nepp | 11:c6f95a42d4d7 | 1652 | |
sepp_nepp | 11:c6f95a42d4d7 | 1653 | corrected_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz; |
sepp_nepp | 11:c6f95a42d4d7 | 1654 | |
sepp_nepp | 11:c6f95a42d4d7 | 1655 | Get_total_SIG_rate(p_ranging_results,&total_SIG_rate_mcps); |
sepp_nepp | 11:c6f95a42d4d7 | 1656 | xtalk_comp_rate_mcps = Get_total_xtalk_rate(p_ranging_results); |
nikapov | 0:a1a69d32f310 | 1657 | |
nikapov | 0:a1a69d32f310 | 1658 | /* Signal rate measurement provided by device is the |
sepp_nepp | 11:c6f95a42d4d7 | 1659 | * peak signal rate,not average. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1660 | peak_SIG_rate_kcps = (total_SIG_rate_mcps * 1000); |
sepp_nepp | 11:c6f95a42d4d7 | 1661 | peak_SIG_rate_kcps = (peak_SIG_rate_kcps + 0x8000) >> 16; |
sepp_nepp | 11:c6f95a42d4d7 | 1662 | |
sepp_nepp | 11:c6f95a42d4d7 | 1663 | xtalk_comp_rate_kcps = xtalk_comp_rate_mcps * 1000; |
sepp_nepp | 11:c6f95a42d4d7 | 1664 | |
sepp_nepp | 11:c6f95a42d4d7 | 1665 | if (xtalk_comp_rate_kcps > c_max_xtalk_kcps) |
sepp_nepp | 11:c6f95a42d4d7 | 1666 | { xtalk_comp_rate_kcps = c_max_xtalk_kcps; } |
sepp_nepp | 11:c6f95a42d4d7 | 1667 | |
sepp_nepp | 11:c6f95a42d4d7 | 1668 | if (ErrState == VL53L0X_OK) { |
nikapov | 0:a1a69d32f310 | 1669 | /* Calculate final range macro periods */ |
sepp_nepp | 12:aa177f0e4c10 | 1670 | final_range_timeout_micro_secs = FinalRangeTimeoutMicroSecs; |
sepp_nepp | 12:aa177f0e4c10 | 1671 | final_range_vcsel_pclks = FinalRangeVcselPPeriod; |
sepp_nepp | 11:c6f95a42d4d7 | 1672 | final_range_macro_pclks = Calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks); |
nikapov | 0:a1a69d32f310 | 1673 | |
nikapov | 0:a1a69d32f310 | 1674 | /* Calculate pre-range macro periods */ |
sepp_nepp | 12:aa177f0e4c10 | 1675 | pre_range_timeout_micro_secs = PreRangeTimeoutMicroSecs; |
sepp_nepp | 12:aa177f0e4c10 | 1676 | pre_range_vcsel_pclks = PreRangeVcselPPeriod; |
sepp_nepp | 11:c6f95a42d4d7 | 1677 | pre_range_macro_pclks = Calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks); |
nikapov | 0:a1a69d32f310 | 1678 | vcsel_width = 3; |
sepp_nepp | 11:c6f95a42d4d7 | 1679 | if (final_range_vcsel_pclks == 8) { vcsel_width = 2; } |
nikapov | 0:a1a69d32f310 | 1680 | |
nikapov | 0:a1a69d32f310 | 1681 | peak_vcsel_duration_us = vcsel_width * 2048 * |
nikapov | 0:a1a69d32f310 | 1682 | (pre_range_macro_pclks + final_range_macro_pclks); |
nikapov | 0:a1a69d32f310 | 1683 | peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1684 | peak_vcsel_duration_us *= c_pll_period_ps; |
nikapov | 0:a1a69d32f310 | 1685 | peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1686 | |
nikapov | 0:a1a69d32f310 | 1687 | /* Fix1616 >> 8 = Fix2408 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1688 | total_SIG_rate_mcps = (total_SIG_rate_mcps + 0x80) >> 8; |
nikapov | 0:a1a69d32f310 | 1689 | |
nikapov | 0:a1a69d32f310 | 1690 | /* Fix2408 * uint32 = Fix2408 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1691 | vcsel_total_events_rtn = total_SIG_rate_mcps * peak_vcsel_duration_us; |
nikapov | 0:a1a69d32f310 | 1692 | |
nikapov | 0:a1a69d32f310 | 1693 | /* Fix2408 >> 8 = uint32 */ |
nikapov | 0:a1a69d32f310 | 1694 | vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8; |
nikapov | 0:a1a69d32f310 | 1695 | |
nikapov | 0:a1a69d32f310 | 1696 | /* Fix2408 << 8 = Fix1616 = */ |
sepp_nepp | 11:c6f95a42d4d7 | 1697 | total_SIG_rate_mcps <<= 8; |
nikapov | 0:a1a69d32f310 | 1698 | } |
nikapov | 0:a1a69d32f310 | 1699 | |
sepp_nepp | 11:c6f95a42d4d7 | 1700 | if (ErrState != VL53L0X_OK) { return ; } |
sepp_nepp | 11:c6f95a42d4d7 | 1701 | |
sepp_nepp | 11:c6f95a42d4d7 | 1702 | if (peak_SIG_rate_kcps == 0) { |
nikapov | 0:a1a69d32f310 | 1703 | *p_sigma_estimate = c_sigma_est_max; |
sepp_nepp | 10:cd251e0fc2fd | 1704 | SigmaEstimate = c_sigma_est_max; |
nikapov | 0:a1a69d32f310 | 1705 | *p_dmax_mm = 0; |
nikapov | 0:a1a69d32f310 | 1706 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 1707 | if (vcsel_total_events_rtn < 1) {vcsel_total_events_rtn = 1; } |
nikapov | 0:a1a69d32f310 | 1708 | |
nikapov | 0:a1a69d32f310 | 1709 | sigma_estimate_p1 = c_pulse_effective_width_centi_ns; |
nikapov | 0:a1a69d32f310 | 1710 | |
nikapov | 0:a1a69d32f310 | 1711 | /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1712 | sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_SIG_rate_kcps; |
sepp_nepp | 11:c6f95a42d4d7 | 1713 | if (sigma_estimate_p2 > c_amb_to_SIG_ratio_max) |
sepp_nepp | 11:c6f95a42d4d7 | 1714 | /* Clip to prevent overflow. Will ensure safe max result. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1715 | { sigma_estimate_p2 = c_amb_to_SIG_ratio_max; } |
nikapov | 0:a1a69d32f310 | 1716 | sigma_estimate_p2 *= c_ambient_effective_width_centi_ns; |
sepp_nepp | 11:c6f95a42d4d7 | 1717 | sigma_estimate_p3 = 2 * ISQRT(vcsel_total_events_rtn * 12); |
nikapov | 0:a1a69d32f310 | 1718 | |
nikapov | 0:a1a69d32f310 | 1719 | /* uint32 * FixPoint1616 = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1720 | delta_t_ps = p_ranging_results->RangeMilliMeter * c_tof_per_mm_ps; |
sepp_nepp | 11:c6f95a42d4d7 | 1721 | |
sepp_nepp | 11:c6f95a42d4d7 | 1722 | /* vcselRate - xtalkCompRate |
nikapov | 0:a1a69d32f310 | 1723 | * (uint32 << 16) - FixPoint1616 = FixPoint1616. |
nikapov | 0:a1a69d32f310 | 1724 | * Divide result by 1000 to convert to mcps. |
sepp_nepp | 11:c6f95a42d4d7 | 1725 | * 500 is added to ensure rounding when integer division truncates. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1726 | diff1_mcps = (((peak_SIG_rate_kcps << 16) - 2 * xtalk_comp_rate_kcps) + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1727 | |
nikapov | 0:a1a69d32f310 | 1728 | /* vcselRate + xtalkCompRate */ |
sepp_nepp | 11:c6f95a42d4d7 | 1729 | diff2_mcps = ((peak_SIG_rate_kcps << 16) + 500) / 1000; |
sepp_nepp | 11:c6f95a42d4d7 | 1730 | |
sepp_nepp | 11:c6f95a42d4d7 | 1731 | /* Shift by 8 bits to increase resolution prior to the division */ |
nikapov | 0:a1a69d32f310 | 1732 | diff1_mcps <<= 8; |
nikapov | 0:a1a69d32f310 | 1733 | |
nikapov | 0:a1a69d32f310 | 1734 | /* FixPoint0824/FixPoint1616 = FixPoint2408 */ |
nikapov | 0:a1a69d32f310 | 1735 | // xTalkCorrection = abs(diff1_mcps/diff2_mcps); |
sepp_nepp | 11:c6f95a42d4d7 | 1736 | // abs is causing compiler overloading isue in C++, but unsigned types. So,redundant call anyway! |
sepp_nepp | 11:c6f95a42d4d7 | 1737 | xtalk_correction = diff1_mcps / diff2_mcps; |
nikapov | 0:a1a69d32f310 | 1738 | |
nikapov | 0:a1a69d32f310 | 1739 | /* FixPoint2408 << 8 = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1740 | xtalk_correction <<= 8; |
sepp_nepp | 11:c6f95a42d4d7 | 1741 | |
sepp_nepp | 11:c6f95a42d4d7 | 1742 | if (p_ranging_results->RangeStatus != 0) |
sepp_nepp | 11:c6f95a42d4d7 | 1743 | { pw_mult = 1 << 16; } |
sepp_nepp | 11:c6f95a42d4d7 | 1744 | else { |
nikapov | 0:a1a69d32f310 | 1745 | /* FixPoint1616/uint32 = FixPoint1616 */ |
nikapov | 0:a1a69d32f310 | 1746 | pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */ |
nikapov | 0:a1a69d32f310 | 1747 | |
sepp_nepp | 10:cd251e0fc2fd | 1748 | /* FixPoint1616 * FixPoint1616 = FixPoint3232,however both |
nikapov | 0:a1a69d32f310 | 1749 | * values are small enough such that32 bits will not be |
sepp_nepp | 11:c6f95a42d4d7 | 1750 | * exceeded. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1751 | pw_mult *= ((1 << 16) - xtalk_correction); |
nikapov | 0:a1a69d32f310 | 1752 | |
nikapov | 0:a1a69d32f310 | 1753 | /* (FixPoint3232 >> 16) = FixPoint1616 */ |
nikapov | 0:a1a69d32f310 | 1754 | pw_mult = (pw_mult + c_16bit_rounding_param) >> 16; |
nikapov | 0:a1a69d32f310 | 1755 | |
nikapov | 0:a1a69d32f310 | 1756 | /* FixPoint1616 + FixPoint1616 = FixPoint1616 */ |
nikapov | 0:a1a69d32f310 | 1757 | pw_mult += (1 << 16); |
nikapov | 0:a1a69d32f310 | 1758 | |
sepp_nepp | 10:cd251e0fc2fd | 1759 | /* At this point the value will be 1.xx,therefore if we square |
nikapov | 0:a1a69d32f310 | 1760 | * the value this will exceed 32 bits. To address this perform |
sepp_nepp | 11:c6f95a42d4d7 | 1761 | * a single shift to the right before the multiplication. */ |
nikapov | 0:a1a69d32f310 | 1762 | pw_mult >>= 1; |
nikapov | 0:a1a69d32f310 | 1763 | /* FixPoint1715 * FixPoint1715 = FixPoint3430 */ |
nikapov | 0:a1a69d32f310 | 1764 | pw_mult = pw_mult * pw_mult; |
nikapov | 0:a1a69d32f310 | 1765 | |
nikapov | 0:a1a69d32f310 | 1766 | /* (FixPoint3430 >> 14) = Fix1616 */ |
nikapov | 0:a1a69d32f310 | 1767 | pw_mult >>= 14; |
nikapov | 0:a1a69d32f310 | 1768 | } |
nikapov | 0:a1a69d32f310 | 1769 | |
nikapov | 0:a1a69d32f310 | 1770 | /* FixPoint1616 * uint32 = FixPoint1616 */ |
nikapov | 0:a1a69d32f310 | 1771 | sqr1 = pw_mult * sigma_estimate_p1; |
nikapov | 0:a1a69d32f310 | 1772 | |
nikapov | 0:a1a69d32f310 | 1773 | /* (FixPoint1616 >> 16) = FixPoint3200 */ |
nikapov | 0:a1a69d32f310 | 1774 | sqr1 = (sqr1 + 0x8000) >> 16; |
nikapov | 0:a1a69d32f310 | 1775 | |
nikapov | 0:a1a69d32f310 | 1776 | /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ |
nikapov | 0:a1a69d32f310 | 1777 | sqr1 *= sqr1; |
nikapov | 0:a1a69d32f310 | 1778 | sqr2 = sigma_estimate_p2; |
nikapov | 0:a1a69d32f310 | 1779 | |
nikapov | 0:a1a69d32f310 | 1780 | /* (FixPoint1616 >> 16) = FixPoint3200 */ |
nikapov | 0:a1a69d32f310 | 1781 | sqr2 = (sqr2 + 0x8000) >> 16; |
nikapov | 0:a1a69d32f310 | 1782 | |
nikapov | 0:a1a69d32f310 | 1783 | /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ |
nikapov | 0:a1a69d32f310 | 1784 | sqr2 *= sqr2; |
nikapov | 0:a1a69d32f310 | 1785 | |
nikapov | 0:a1a69d32f310 | 1786 | /* FixPoint64000 + FixPoint6400 = FixPoint6400 */ |
nikapov | 0:a1a69d32f310 | 1787 | sqr_sum = sqr1 + sqr2; |
nikapov | 0:a1a69d32f310 | 1788 | |
nikapov | 0:a1a69d32f310 | 1789 | /* SQRT(FixPoin6400) = FixPoint3200 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1790 | sqrt_result_centi_ns = ISQRT(sqr_sum); |
nikapov | 0:a1a69d32f310 | 1791 | |
nikapov | 0:a1a69d32f310 | 1792 | /* (FixPoint3200 << 16) = FixPoint1616 */ |
nikapov | 0:a1a69d32f310 | 1793 | sqrt_result_centi_ns <<= 16; |
nikapov | 0:a1a69d32f310 | 1794 | |
sepp_nepp | 11:c6f95a42d4d7 | 1795 | /* Note that the Speed Of Light is expressed in um per 1E-10 |
sepp_nepp | 11:c6f95a42d4d7 | 1796 | * seconds (2997) Therefore to get mm/ns we have to divide by 10000 */ |
nikapov | 0:a1a69d32f310 | 1797 | sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) / |
nikapov | 0:a1a69d32f310 | 1798 | sigma_estimate_p3); |
nikapov | 0:a1a69d32f310 | 1799 | sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR; |
nikapov | 0:a1a69d32f310 | 1800 | |
nikapov | 0:a1a69d32f310 | 1801 | /* Add 5000 before dividing by 10000 to ensure rounding. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1802 | sigma_est_rtn = (sigma_est_rtn + 5000) / 10000; |
sepp_nepp | 11:c6f95a42d4d7 | 1803 | |
sepp_nepp | 11:c6f95a42d4d7 | 1804 | if (sigma_est_rtn > c_sigma_est_rtn_max) |
sepp_nepp | 11:c6f95a42d4d7 | 1805 | /* Clip to prevent overflow. Will ensure safe max result. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1806 | { sigma_est_rtn = c_sigma_est_rtn_max; } |
sepp_nepp | 11:c6f95a42d4d7 | 1807 | |
nikapov | 0:a1a69d32f310 | 1808 | final_range_integration_time_milli_secs = |
nikapov | 0:a1a69d32f310 | 1809 | (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1810 | |
nikapov | 0:a1a69d32f310 | 1811 | /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range) |
sepp_nepp | 11:c6f95a42d4d7 | 1812 | * sqrt(FixPoint1616/int) = FixPoint2408) */ |
sepp_nepp | 11:c6f95a42d4d7 | 1813 | sigma_est_Ref = |
sepp_nepp | 11:c6f95a42d4d7 | 1814 | ISQRT((c_dflt_final_range_integration_time_milli_secs + |
nikapov | 0:a1a69d32f310 | 1815 | final_range_integration_time_milli_secs / 2) / |
nikapov | 0:a1a69d32f310 | 1816 | final_range_integration_time_milli_secs); |
nikapov | 0:a1a69d32f310 | 1817 | |
nikapov | 0:a1a69d32f310 | 1818 | /* FixPoint2408 << 8 = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1819 | sigma_est_Ref <<= 8; |
sepp_nepp | 11:c6f95a42d4d7 | 1820 | sigma_est_Ref = (sigma_est_Ref + 500) / 1000; |
nikapov | 0:a1a69d32f310 | 1821 | |
nikapov | 0:a1a69d32f310 | 1822 | /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ |
nikapov | 0:a1a69d32f310 | 1823 | sqr1 = sigma_est_rtn * sigma_est_rtn; |
nikapov | 0:a1a69d32f310 | 1824 | /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1825 | sqr2 = sigma_est_Ref * sigma_est_Ref; |
nikapov | 0:a1a69d32f310 | 1826 | |
nikapov | 0:a1a69d32f310 | 1827 | /* sqrt(FixPoint3232) = FixPoint1616 */ |
sepp_nepp | 11:c6f95a42d4d7 | 1828 | sqrt_result = ISQRT((sqr1 + sqr2)); |
sepp_nepp | 11:c6f95a42d4d7 | 1829 | /* Note that the Shift by 4 bits increases resolution prior to |
sepp_nepp | 7:41cbc431e1f4 | 1830 | * the sqrt,therefore the result must be shifted by 2 bits to |
sepp_nepp | 11:c6f95a42d4d7 | 1831 | * the right to revert back to the FixPoint1616 format. */ |
nikapov | 0:a1a69d32f310 | 1832 | sigma_estimate = 1000 * sqrt_result; |
nikapov | 0:a1a69d32f310 | 1833 | |
sepp_nepp | 11:c6f95a42d4d7 | 1834 | if ((peak_SIG_rate_kcps < 1) || (vcsel_total_events_rtn < 1) || |
nikapov | 0:a1a69d32f310 | 1835 | (sigma_estimate > c_sigma_est_max)) { |
sepp_nepp | 11:c6f95a42d4d7 | 1836 | sigma_estimate = c_sigma_est_max; } |
nikapov | 0:a1a69d32f310 | 1837 | |
nikapov | 0:a1a69d32f310 | 1838 | *p_sigma_estimate = (uint32_t)(sigma_estimate); |
sepp_nepp | 10:cd251e0fc2fd | 1839 | SigmaEstimate = *p_sigma_estimate; |
sepp_nepp | 11:c6f95a42d4d7 | 1840 | Calc_dmax(total_SIG_rate_mcps, |
sepp_nepp | 11:c6f95a42d4d7 | 1841 | corrected_SIG_rate_mcps, |
nikapov | 0:a1a69d32f310 | 1842 | pw_mult, |
nikapov | 0:a1a69d32f310 | 1843 | sigma_estimate_p1, |
nikapov | 0:a1a69d32f310 | 1844 | sigma_estimate_p2, |
nikapov | 0:a1a69d32f310 | 1845 | peak_vcsel_duration_us, |
nikapov | 0:a1a69d32f310 | 1846 | p_dmax_mm); |
nikapov | 0:a1a69d32f310 | 1847 | } |
nikapov | 0:a1a69d32f310 | 1848 | } |
nikapov | 0:a1a69d32f310 | 1849 | |
sepp_nepp | 11:c6f95a42d4d7 | 1850 | void VL53L0X::Get_Device_range_status(uint8_t device_range_status, |
sepp_nepp | 11:c6f95a42d4d7 | 1851 | TFP1616 signal_rate, |
sepp_nepp | 11:c6f95a42d4d7 | 1852 | uint16_t effective_SPAD_rtn_count, |
sepp_nepp | 11:c6f95a42d4d7 | 1853 | TRangeResults *p_ranging_results, |
sepp_nepp | 11:c6f95a42d4d7 | 1854 | uint8_t *p_Device_range_status) |
sepp_nepp | 11:c6f95a42d4d7 | 1855 | { uint8_t none_flag; |
nikapov | 0:a1a69d32f310 | 1856 | uint8_t sigma_limitflag = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1857 | uint8_t signal_Ref_clipflag = 0; |
nikapov | 0:a1a69d32f310 | 1858 | uint8_t range_ignore_thresholdflag = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1859 | uint8_t sigma_limit_chk_en = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1860 | uint8_t signal_rate_final_range_limit_chk_en = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1861 | uint8_t signal_Ref_clip_limit_chk_en = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1862 | uint8_t range_ignore_threshold_chk_en = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1863 | TFP1616 sigma_estimate; |
sepp_nepp | 11:c6f95a42d4d7 | 1864 | TFP1616 sigma_limit_value; |
sepp_nepp | 11:c6f95a42d4d7 | 1865 | TFP1616 signal_Ref_clip_value; |
sepp_nepp | 11:c6f95a42d4d7 | 1866 | TFP1616 range_ignore_threshold; |
sepp_nepp | 11:c6f95a42d4d7 | 1867 | TFP1616 signal_rate_per_SPAD; |
nikapov | 0:a1a69d32f310 | 1868 | uint8_t device_range_status_internal = 0; |
nikapov | 0:a1a69d32f310 | 1869 | uint8_t temp8; |
nikapov | 0:a1a69d32f310 | 1870 | uint32_t dmax_mm = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 1871 | |
sepp_nepp | 11:c6f95a42d4d7 | 1872 | /* VL53L0X has a good ranging when the value of the |
nikapov | 0:a1a69d32f310 | 1873 | * DeviceRangeStatus = 11. This function will replace the value 0 with |
nikapov | 0:a1a69d32f310 | 1874 | * the value 11 in the DeviceRangeStatus. |
sepp_nepp | 7:41cbc431e1f4 | 1875 | * In addition,the SigmaEstimator is not included in the VL53L0X |
sepp_nepp | 11:c6f95a42d4d7 | 1876 | * DeviceRangeStatus,this will be added in the DeviceRangeStatus. */ |
nikapov | 0:a1a69d32f310 | 1877 | |
nikapov | 0:a1a69d32f310 | 1878 | device_range_status_internal = ((device_range_status & 0x78) >> 3); |
nikapov | 0:a1a69d32f310 | 1879 | |
sepp_nepp | 11:c6f95a42d4d7 | 1880 | if ( device_range_status_internal == 0 || |
sepp_nepp | 11:c6f95a42d4d7 | 1881 | device_range_status_internal == 5 || |
sepp_nepp | 11:c6f95a42d4d7 | 1882 | device_range_status_internal == 7 || |
sepp_nepp | 11:c6f95a42d4d7 | 1883 | device_range_status_internal == 12 || |
sepp_nepp | 11:c6f95a42d4d7 | 1884 | device_range_status_internal == 13 || |
sepp_nepp | 11:c6f95a42d4d7 | 1885 | device_range_status_internal == 14 || |
sepp_nepp | 11:c6f95a42d4d7 | 1886 | device_range_status_internal == 15 ) |
sepp_nepp | 11:c6f95a42d4d7 | 1887 | { none_flag = 1; } |
sepp_nepp | 11:c6f95a42d4d7 | 1888 | else { none_flag = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1889 | |
sepp_nepp | 11:c6f95a42d4d7 | 1890 | /* Check if Sigma limit is enabled,if yes then do comparison with limit |
sepp_nepp | 11:c6f95a42d4d7 | 1891 | * value and put the result back into pDeviceRangeStatus. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1892 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 1893 | { sigma_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); } |
sepp_nepp | 11:c6f95a42d4d7 | 1894 | |
sepp_nepp | 11:c6f95a42d4d7 | 1895 | if ((sigma_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) { |
sepp_nepp | 11:c6f95a42d4d7 | 1896 | /* compute the Sigma and check with limit */ |
sepp_nepp | 11:c6f95a42d4d7 | 1897 | Calc_sigma_estimate(p_ranging_results, &sigma_estimate, &dmax_mm); |
sepp_nepp | 11:c6f95a42d4d7 | 1898 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 1899 | { p_ranging_results->RangeDMaxMilliMeter = dmax_mm; } |
sepp_nepp | 11:c6f95a42d4d7 | 1900 | |
sepp_nepp | 11:c6f95a42d4d7 | 1901 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 1902 | { sigma_limit_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); |
sepp_nepp | 11:c6f95a42d4d7 | 1903 | |
sepp_nepp | 11:c6f95a42d4d7 | 1904 | if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value)) |
sepp_nepp | 11:c6f95a42d4d7 | 1905 | { sigma_limitflag = 1; }/* Limit Fail */ |
sepp_nepp | 11:c6f95a42d4d7 | 1906 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1907 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1908 | |
sepp_nepp | 11:c6f95a42d4d7 | 1909 | /* Check if Signal ref clip limit is enabled,if yes then do comparison |
sepp_nepp | 11:c6f95a42d4d7 | 1910 | * with limit value and put the result back into pDeviceRangeStatus. */ |
sepp_nepp | 11:c6f95a42d4d7 | 1911 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 1912 | {signal_Ref_clip_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_REF_CLIP);} |
sepp_nepp | 11:c6f95a42d4d7 | 1913 | |
sepp_nepp | 11:c6f95a42d4d7 | 1914 | if ((signal_Ref_clip_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) |
sepp_nepp | 11:c6f95a42d4d7 | 1915 | { signal_Ref_clip_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIG_REF_CLIP); |
nikapov | 0:a1a69d32f310 | 1916 | |
nikapov | 0:a1a69d32f310 | 1917 | /* Read LastSignalRefMcps from device */ |
sepp_nepp | 11:c6f95a42d4d7 | 1918 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 1919 | LastSignalRefMcps = FP97_TO_FP1616( Read_Word(REG_RESULT_PEAK_SIG_RATE_REF)); |
sepp_nepp | 11:c6f95a42d4d7 | 1920 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 1921 | |
sepp_nepp | 11:c6f95a42d4d7 | 1922 | if ((signal_Ref_clip_value > 0) && (LastSignalRefMcps > signal_Ref_clip_value)) \ |
sepp_nepp | 11:c6f95a42d4d7 | 1923 | { signal_Ref_clipflag = 1; /* Limit Fail */ } |
sepp_nepp | 11:c6f95a42d4d7 | 1924 | } |
sepp_nepp | 11:c6f95a42d4d7 | 1925 | |
sepp_nepp | 11:c6f95a42d4d7 | 1926 | /* Check if Signal ref clip limit is enabled,if yes then do comparison |
sepp_nepp | 11:c6f95a42d4d7 | 1927 | * with limit value and put the result back into pDeviceRangeStatus. |
sepp_nepp | 11:c6f95a42d4d7 | 1928 | * EffectiveSPADRtnCount has a format 8.8 |
sepp_nepp | 11:c6f95a42d4d7 | 1929 | * If (Return signal rate < (1.5 x Xtalk x number of SPADS)) : FAIL */ |
sepp_nepp | 11:c6f95a42d4d7 | 1930 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 1931 | { range_ignore_threshold_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); } |
sepp_nepp | 11:c6f95a42d4d7 | 1932 | |
sepp_nepp | 11:c6f95a42d4d7 | 1933 | if ((range_ignore_threshold_chk_en != 0) && (ErrState == VL53L0X_OK)) |
sepp_nepp | 11:c6f95a42d4d7 | 1934 | {/* Compute the signal rate per SPAD */ |
sepp_nepp | 11:c6f95a42d4d7 | 1935 | if (effective_SPAD_rtn_count == 0) { signal_rate_per_SPAD = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1936 | else { signal_rate_per_SPAD = |
sepp_nepp | 11:c6f95a42d4d7 | 1937 | (TFP1616)((256 * signal_rate) / effective_SPAD_rtn_count); } |
sepp_nepp | 11:c6f95a42d4d7 | 1938 | |
sepp_nepp | 11:c6f95a42d4d7 | 1939 | range_ignore_threshold=Get_limit_chk_val(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); |
sepp_nepp | 11:c6f95a42d4d7 | 1940 | |
sepp_nepp | 11:c6f95a42d4d7 | 1941 | if ((range_ignore_threshold > 0) && (signal_rate_per_SPAD < range_ignore_threshold)) { |
sepp_nepp | 11:c6f95a42d4d7 | 1942 | /* Limit Fail add 2^6 to range ErrState */ |
nikapov | 0:a1a69d32f310 | 1943 | range_ignore_thresholdflag = 1; |
nikapov | 0:a1a69d32f310 | 1944 | } |
nikapov | 0:a1a69d32f310 | 1945 | } |
nikapov | 0:a1a69d32f310 | 1946 | |
sepp_nepp | 11:c6f95a42d4d7 | 1947 | if (ErrState == VL53L0X_OK) { |
nikapov | 0:a1a69d32f310 | 1948 | if (none_flag == 1) { |
sepp_nepp | 11:c6f95a42d4d7 | 1949 | *p_Device_range_status = 255; /* NONE */ |
nikapov | 0:a1a69d32f310 | 1950 | } else if (device_range_status_internal == 1 || |
nikapov | 0:a1a69d32f310 | 1951 | device_range_status_internal == 2 || |
nikapov | 0:a1a69d32f310 | 1952 | device_range_status_internal == 3) { |
sepp_nepp | 11:c6f95a42d4d7 | 1953 | *p_Device_range_status = 5; /* HW fail */ |
nikapov | 0:a1a69d32f310 | 1954 | } else if (device_range_status_internal == 6 || |
nikapov | 0:a1a69d32f310 | 1955 | device_range_status_internal == 9) { |
sepp_nepp | 11:c6f95a42d4d7 | 1956 | *p_Device_range_status = 4; /* Phase fail */ |
nikapov | 0:a1a69d32f310 | 1957 | } else if (device_range_status_internal == 8 || |
nikapov | 0:a1a69d32f310 | 1958 | device_range_status_internal == 10 || |
sepp_nepp | 11:c6f95a42d4d7 | 1959 | signal_Ref_clipflag == 1) { |
sepp_nepp | 11:c6f95a42d4d7 | 1960 | *p_Device_range_status = 3; /* Min range */ |
nikapov | 0:a1a69d32f310 | 1961 | } else if (device_range_status_internal == 4 || |
nikapov | 0:a1a69d32f310 | 1962 | range_ignore_thresholdflag == 1) { |
sepp_nepp | 11:c6f95a42d4d7 | 1963 | *p_Device_range_status = 2; /* Signal Fail */ |
nikapov | 0:a1a69d32f310 | 1964 | } else if (sigma_limitflag == 1) { |
sepp_nepp | 11:c6f95a42d4d7 | 1965 | *p_Device_range_status = 1; /* Sigma Fail */ |
nikapov | 0:a1a69d32f310 | 1966 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 1967 | *p_Device_range_status = 0; /* Range Valid */ |
nikapov | 0:a1a69d32f310 | 1968 | } |
nikapov | 0:a1a69d32f310 | 1969 | } |
nikapov | 0:a1a69d32f310 | 1970 | |
nikapov | 0:a1a69d32f310 | 1971 | /* DMAX only relevant during range error */ |
sepp_nepp | 11:c6f95a42d4d7 | 1972 | if (*p_Device_range_status == 0) { p_ranging_results->RangeDMaxMilliMeter = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1973 | |
sepp_nepp | 11:c6f95a42d4d7 | 1974 | /* fill the Limit Check ErrState */ |
sepp_nepp | 11:c6f95a42d4d7 | 1975 | signal_rate_final_range_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE); |
sepp_nepp | 11:c6f95a42d4d7 | 1976 | |
sepp_nepp | 11:c6f95a42d4d7 | 1977 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 1978 | if ((sigma_limit_chk_en == 0) || (sigma_limitflag == 1)) |
sepp_nepp | 11:c6f95a42d4d7 | 1979 | { temp8 = 1; } else { temp8 = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1980 | CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = temp8; |
sepp_nepp | 11:c6f95a42d4d7 | 1981 | |
sepp_nepp | 11:c6f95a42d4d7 | 1982 | if ((device_range_status_internal == 4) || (signal_rate_final_range_limit_chk_en == 0)) |
sepp_nepp | 11:c6f95a42d4d7 | 1983 | { temp8 = 1; } else { temp8 = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1984 | CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE] = temp8; |
sepp_nepp | 11:c6f95a42d4d7 | 1985 | |
sepp_nepp | 11:c6f95a42d4d7 | 1986 | if ((signal_Ref_clip_limit_chk_en == 0) || (signal_Ref_clipflag == 1)) |
sepp_nepp | 11:c6f95a42d4d7 | 1987 | { temp8 = 1; } else { temp8 = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 1988 | CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_REF_CLIP] = temp8; |
sepp_nepp | 11:c6f95a42d4d7 | 1989 | |
sepp_nepp | 11:c6f95a42d4d7 | 1990 | if ((range_ignore_threshold_chk_en == 0) || (range_ignore_thresholdflag == 1)) |
sepp_nepp | 11:c6f95a42d4d7 | 1991 | { temp8 = 1; } else { temp8 = 0;} |
sepp_nepp | 11:c6f95a42d4d7 | 1992 | CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = temp8; |
Davidroid | 3:e9269ff624ed | 1993 | } |
nikapov | 0:a1a69d32f310 | 1994 | } |
nikapov | 0:a1a69d32f310 | 1995 | |
sepp_nepp | 11:c6f95a42d4d7 | 1996 | void VL53L0X::Get_ranging_results(TRangeResults *p_ranging_results) |
sepp_nepp | 11:c6f95a42d4d7 | 1997 | { uint8_t device_range_status; |
nikapov | 0:a1a69d32f310 | 1998 | uint8_t range_fractional_enable; |
sepp_nepp | 11:c6f95a42d4d7 | 1999 | uint8_t Device_range_status; |
sepp_nepp | 11:c6f95a42d4d7 | 2000 | uint8_t xtalk_compensation_enable; |
nikapov | 0:a1a69d32f310 | 2001 | uint16_t ambient_rate; |
sepp_nepp | 11:c6f95a42d4d7 | 2002 | TFP1616 signal_rate; |
sepp_nepp | 11:c6f95a42d4d7 | 2003 | uint16_t Xtalk_CompRate_MHz; |
sepp_nepp | 11:c6f95a42d4d7 | 2004 | uint16_t effective_SPAD_rtn_count; |
nikapov | 0:a1a69d32f310 | 2005 | uint16_t tmpuint16; |
nikapov | 0:a1a69d32f310 | 2006 | uint16_t xtalk_range_milli_meter; |
nikapov | 0:a1a69d32f310 | 2007 | uint16_t linearity_corrective_gain; |
nikapov | 0:a1a69d32f310 | 2008 | uint8_t localBuffer[12]; |
sepp_nepp | 11:c6f95a42d4d7 | 2009 | TRangeResults last_range_data_buffer; |
sepp_nepp | 11:c6f95a42d4d7 | 2010 | |
sepp_nepp | 11:c6f95a42d4d7 | 2011 | if (ErrState != VL53L0X_OK) { return; } // Do nothing while in error state |
sepp_nepp | 11:c6f95a42d4d7 | 2012 | |
sepp_nepp | 11:c6f95a42d4d7 | 2013 | /* use multi read even if some registers are not useful,result will |
sepp_nepp | 11:c6f95a42d4d7 | 2014 | * be more efficient start reading at REG_RESULT_RANGE_STATUS = 0x14 |
sepp_nepp | 11:c6f95a42d4d7 | 2015 | * end reading at 0x21 dec33 total 14 bytes to read */ |
sepp_nepp | 11:c6f95a42d4d7 | 2016 | I2c_Read(REG_RESULT_RANGE_STATUS, localBuffer,12); |
sepp_nepp | 11:c6f95a42d4d7 | 2017 | |
sepp_nepp | 11:c6f95a42d4d7 | 2018 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2019 | p_ranging_results->ZoneId = 0; /* Only one zone */ |
sepp_nepp | 11:c6f95a42d4d7 | 2020 | p_ranging_results->TimeStamp = 0; /* Not Implemented */ |
nikapov | 0:a1a69d32f310 | 2021 | |
sepp_nepp | 7:41cbc431e1f4 | 2022 | tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]); |
nikapov | 0:a1a69d32f310 | 2023 | /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional |
sepp_nepp | 11:c6f95a42d4d7 | 2024 | *(format 11.2) else no fractional */ |
sepp_nepp | 11:c6f95a42d4d7 | 2025 | |
sepp_nepp | 11:c6f95a42d4d7 | 2026 | p_ranging_results->MeasurementTimeUsec = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2027 | |
sepp_nepp | 11:c6f95a42d4d7 | 2028 | signal_rate = FP97_TO_FP1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6])); |
sepp_nepp | 11:c6f95a42d4d7 | 2029 | /* peak_SIG_count_rate_rtn_mcps */ |
sepp_nepp | 11:c6f95a42d4d7 | 2030 | p_ranging_results->SignalRateRtnMHz = signal_rate; |
nikapov | 0:a1a69d32f310 | 2031 | |
sepp_nepp | 7:41cbc431e1f4 | 2032 | ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]); |
sepp_nepp | 11:c6f95a42d4d7 | 2033 | p_ranging_results->AmbientRateRtnMHz = FP97_TO_FP1616(ambient_rate); |
sepp_nepp | 11:c6f95a42d4d7 | 2034 | |
sepp_nepp | 11:c6f95a42d4d7 | 2035 | effective_SPAD_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]); |
sepp_nepp | 11:c6f95a42d4d7 | 2036 | /* EffectiveSPADRtnCount is 8.8 format */ |
sepp_nepp | 11:c6f95a42d4d7 | 2037 | p_ranging_results->EffectiveSPADRtnCount = effective_SPAD_rtn_count; |
nikapov | 0:a1a69d32f310 | 2038 | |
nikapov | 0:a1a69d32f310 | 2039 | device_range_status = localBuffer[0]; |
nikapov | 0:a1a69d32f310 | 2040 | |
nikapov | 0:a1a69d32f310 | 2041 | /* Get Linearity Corrective Gain */ |
sepp_nepp | 10:cd251e0fc2fd | 2042 | linearity_corrective_gain = LinearityCorrectiveGain; |
nikapov | 0:a1a69d32f310 | 2043 | |
nikapov | 0:a1a69d32f310 | 2044 | /* Get ranging configuration */ |
sepp_nepp | 10:cd251e0fc2fd | 2045 | range_fractional_enable = RangeFractionalEnable; |
nikapov | 0:a1a69d32f310 | 2046 | |
nikapov | 0:a1a69d32f310 | 2047 | if (linearity_corrective_gain != 1000) { |
nikapov | 0:a1a69d32f310 | 2048 | tmpuint16 = (uint16_t)((linearity_corrective_gain |
nikapov | 0:a1a69d32f310 | 2049 | * tmpuint16 + 500) / 1000); |
nikapov | 0:a1a69d32f310 | 2050 | |
nikapov | 0:a1a69d32f310 | 2051 | /* Implement Xtalk */ |
sepp_nepp | 11:c6f95a42d4d7 | 2052 | Xtalk_CompRate_MHz = CurrParams.Xtalk_CompRate_MHz; |
sepp_nepp | 11:c6f95a42d4d7 | 2053 | xtalk_compensation_enable = CurrParams.XTalk_Compens_En; |
sepp_nepp | 11:c6f95a42d4d7 | 2054 | |
sepp_nepp | 11:c6f95a42d4d7 | 2055 | if (xtalk_compensation_enable) { |
sepp_nepp | 11:c6f95a42d4d7 | 2056 | if ((signal_rate - ((Xtalk_CompRate_MHz |
sepp_nepp | 11:c6f95a42d4d7 | 2057 | * effective_SPAD_rtn_count) >> 8)) <= 0) { |
sepp_nepp | 11:c6f95a42d4d7 | 2058 | if (range_fractional_enable) { xtalk_range_milli_meter = 8888; |
sepp_nepp | 11:c6f95a42d4d7 | 2059 | } else { xtalk_range_milli_meter = 8888 << 2; } |
nikapov | 0:a1a69d32f310 | 2060 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 2061 | xtalk_range_milli_meter = (tmpuint16 * signal_rate) |
sepp_nepp | 11:c6f95a42d4d7 | 2062 | / (signal_rate - ((Xtalk_CompRate_MHz * effective_SPAD_rtn_count) >> 8)); |
nikapov | 0:a1a69d32f310 | 2063 | } |
nikapov | 0:a1a69d32f310 | 2064 | tmpuint16 = xtalk_range_milli_meter; |
nikapov | 0:a1a69d32f310 | 2065 | } |
nikapov | 0:a1a69d32f310 | 2066 | } |
nikapov | 0:a1a69d32f310 | 2067 | |
nikapov | 0:a1a69d32f310 | 2068 | if (range_fractional_enable) { |
sepp_nepp | 11:c6f95a42d4d7 | 2069 | p_ranging_results->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2); |
sepp_nepp | 11:c6f95a42d4d7 | 2070 | p_ranging_results->RangeFractionalPart = |
nikapov | 0:a1a69d32f310 | 2071 | (uint8_t)((tmpuint16 & 0x03) << 6); |
nikapov | 0:a1a69d32f310 | 2072 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 2073 | p_ranging_results->RangeMilliMeter = tmpuint16; |
sepp_nepp | 11:c6f95a42d4d7 | 2074 | p_ranging_results->RangeFractionalPart = 0; |
nikapov | 0:a1a69d32f310 | 2075 | } |
nikapov | 0:a1a69d32f310 | 2076 | |
sepp_nepp | 11:c6f95a42d4d7 | 2077 | /* For a standard definition of RangeStatus,this should |
nikapov | 0:a1a69d32f310 | 2078 | * return 0 in case of good result after a ranging |
sepp_nepp | 11:c6f95a42d4d7 | 2079 | * The range ErrState depends on the device so call a device |
sepp_nepp | 11:c6f95a42d4d7 | 2080 | * specific function to obtain the right ErrState. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2081 | Get_Device_range_status(device_range_status,signal_rate,effective_SPAD_rtn_count, |
sepp_nepp | 11:c6f95a42d4d7 | 2082 | p_ranging_results,&Device_range_status); |
sepp_nepp | 11:c6f95a42d4d7 | 2083 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2084 | p_ranging_results->RangeStatus = Device_range_status; |
nikapov | 0:a1a69d32f310 | 2085 | } |
nikapov | 0:a1a69d32f310 | 2086 | } |
nikapov | 0:a1a69d32f310 | 2087 | |
sepp_nepp | 11:c6f95a42d4d7 | 2088 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2089 | /* Copy last read data into device+ buffer */ |
sepp_nepp | 10:cd251e0fc2fd | 2090 | last_range_data_buffer = LastRangeMeasure; |
nikapov | 0:a1a69d32f310 | 2091 | last_range_data_buffer.RangeMilliMeter = |
sepp_nepp | 11:c6f95a42d4d7 | 2092 | p_ranging_results->RangeMilliMeter; |
nikapov | 0:a1a69d32f310 | 2093 | last_range_data_buffer.RangeFractionalPart = |
sepp_nepp | 11:c6f95a42d4d7 | 2094 | p_ranging_results->RangeFractionalPart; |
nikapov | 0:a1a69d32f310 | 2095 | last_range_data_buffer.RangeDMaxMilliMeter = |
sepp_nepp | 11:c6f95a42d4d7 | 2096 | p_ranging_results->RangeDMaxMilliMeter; |
nikapov | 0:a1a69d32f310 | 2097 | last_range_data_buffer.MeasurementTimeUsec = |
sepp_nepp | 11:c6f95a42d4d7 | 2098 | p_ranging_results->MeasurementTimeUsec; |
sepp_nepp | 11:c6f95a42d4d7 | 2099 | last_range_data_buffer.SignalRateRtnMHz = |
sepp_nepp | 11:c6f95a42d4d7 | 2100 | p_ranging_results->SignalRateRtnMHz; |
sepp_nepp | 11:c6f95a42d4d7 | 2101 | last_range_data_buffer.AmbientRateRtnMHz = |
sepp_nepp | 11:c6f95a42d4d7 | 2102 | p_ranging_results->AmbientRateRtnMHz; |
sepp_nepp | 11:c6f95a42d4d7 | 2103 | last_range_data_buffer.EffectiveSPADRtnCount = |
sepp_nepp | 11:c6f95a42d4d7 | 2104 | p_ranging_results->EffectiveSPADRtnCount; |
nikapov | 0:a1a69d32f310 | 2105 | last_range_data_buffer.RangeStatus = |
sepp_nepp | 11:c6f95a42d4d7 | 2106 | p_ranging_results->RangeStatus; |
sepp_nepp | 10:cd251e0fc2fd | 2107 | LastRangeMeasure = last_range_data_buffer; |
nikapov | 0:a1a69d32f310 | 2108 | } |
nikapov | 0:a1a69d32f310 | 2109 | } |
nikapov | 0:a1a69d32f310 | 2110 | |
sepp_nepp | 11:c6f95a42d4d7 | 2111 | void VL53L0X::Perf_single_ranging_measurement( |
sepp_nepp | 11:c6f95a42d4d7 | 2112 | TRangeResults *p_ranging_results) |
sepp_nepp | 11:c6f95a42d4d7 | 2113 | { if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 2114 | |
sepp_nepp | 11:c6f95a42d4d7 | 2115 | /* This function will do a complete single ranging Here we fix the mode! */ |
sepp_nepp | 11:c6f95a42d4d7 | 2116 | Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); |
sepp_nepp | 11:c6f95a42d4d7 | 2117 | |
sepp_nepp | 11:c6f95a42d4d7 | 2118 | Perf_single_measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2119 | |
sepp_nepp | 11:c6f95a42d4d7 | 2120 | Get_ranging_results(p_ranging_results); |
sepp_nepp | 11:c6f95a42d4d7 | 2121 | |
sepp_nepp | 11:c6f95a42d4d7 | 2122 | Clear_interrupt_mask(0); |
nikapov | 0:a1a69d32f310 | 2123 | } |
nikapov | 0:a1a69d32f310 | 2124 | |
sepp_nepp | 11:c6f95a42d4d7 | 2125 | uint16_t VL53L0X::Get_Perf_Ref_SIG_measurement() |
sepp_nepp | 11:c6f95a42d4d7 | 2126 | { TRangeResults ranging_results; |
sepp_nepp | 11:c6f95a42d4d7 | 2127 | uint8_t orig_sequence_config; |
sepp_nepp | 11:c6f95a42d4d7 | 2128 | uint16_t Ref_SIG_rate ; |
nikapov | 0:a1a69d32f310 | 2129 | |
nikapov | 0:a1a69d32f310 | 2130 | /* store the value of the sequence config, |
sepp_nepp | 10:cd251e0fc2fd | 2131 | * this will be reset before the end of the function*/ |
sepp_nepp | 11:c6f95a42d4d7 | 2132 | orig_sequence_config = SequenceConfig; |
sepp_nepp | 10:cd251e0fc2fd | 2133 | |
sepp_nepp | 10:cd251e0fc2fd | 2134 | /* This function performs a reference signal rate measurement.*/ |
sepp_nepp | 11:c6f95a42d4d7 | 2135 | Set_SequenceConfig( 0xC0 ); // sets REG_SYSTEM_SEQUENCE_CONFIG |
sepp_nepp | 11:c6f95a42d4d7 | 2136 | |
sepp_nepp | 11:c6f95a42d4d7 | 2137 | Perf_single_ranging_measurement(&ranging_results); |
sepp_nepp | 11:c6f95a42d4d7 | 2138 | |
sepp_nepp | 11:c6f95a42d4d7 | 2139 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2140 | Ref_SIG_rate = Read_Word(REG_RESULT_PEAK_SIG_RATE_REF); |
sepp_nepp | 11:c6f95a42d4d7 | 2141 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2142 | |
sepp_nepp | 11:c6f95a42d4d7 | 2143 | /* restore the previous Sequence Config */ |
sepp_nepp | 11:c6f95a42d4d7 | 2144 | Set_SequenceConfig( orig_sequence_config ); // resets REG_SYSTEM_SEQUENCE_CONFIG |
sepp_nepp | 11:c6f95a42d4d7 | 2145 | |
sepp_nepp | 11:c6f95a42d4d7 | 2146 | return Ref_SIG_rate; |
nikapov | 0:a1a69d32f310 | 2147 | } |
nikapov | 0:a1a69d32f310 | 2148 | |
sepp_nepp | 11:c6f95a42d4d7 | 2149 | void VL53L0X::Perf_Ref_SPAD_management(uint32_t *ref_SPAD_count, |
sepp_nepp | 11:c6f95a42d4d7 | 2150 | uint8_t *is_aperture_SPADS) |
sepp_nepp | 11:c6f95a42d4d7 | 2151 | { uint8_t last_SPAD_array[6]; |
nikapov | 0:a1a69d32f310 | 2152 | uint8_t start_select = 0xB4; |
sepp_nepp | 11:c6f95a42d4d7 | 2153 | uint32_t minimum_SPAD_count = 3; |
sepp_nepp | 11:c6f95a42d4d7 | 2154 | uint32_t max_SPAD_count = 44; |
sepp_nepp | 11:c6f95a42d4d7 | 2155 | uint32_t current_SPAD_index = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2156 | uint32_t last_SPAD_index = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2157 | int32_t next_good_SPAD = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2158 | uint16_t target_Ref_rate = 0x0A00; /* 20 MHz in 9:7 format */ |
sepp_nepp | 11:c6f95a42d4d7 | 2159 | uint16_t peak_SIG_rate_Ref; |
sepp_nepp | 11:c6f95a42d4d7 | 2160 | uint32_t need_apt_SPADS = 0; |
nikapov | 0:a1a69d32f310 | 2161 | uint32_t index = 0; |
sepp_nepp | 12:aa177f0e4c10 | 2162 | uint32_t REF_SPAD_ARRAY_SIZE = 6; |
nikapov | 0:a1a69d32f310 | 2163 | uint32_t signal_rate_diff = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2164 | uint32_t last_SIG_rate_diff = 0; |
nikapov | 0:a1a69d32f310 | 2165 | uint8_t complete = 0; |
nikapov | 0:a1a69d32f310 | 2166 | uint8_t vhv_settings = 0; |
nikapov | 0:a1a69d32f310 | 2167 | uint8_t phase_cal = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2168 | uint32_t ref_SPAD_count_int = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2169 | uint8_t is_aperture_SPADS_int = 0; |
nikapov | 0:a1a69d32f310 | 2170 | |
nikapov | 0:a1a69d32f310 | 2171 | /* |
nikapov | 0:a1a69d32f310 | 2172 | * The reference SPAD initialization procedure determines the minimum |
sepp_nepp | 11:c6f95a42d4d7 | 2173 | * amount of reference SPADS to be enables to achieve a target reference |
nikapov | 0:a1a69d32f310 | 2174 | * signal rate and should be performed once during initialization. |
nikapov | 0:a1a69d32f310 | 2175 | * |
sepp_nepp | 11:c6f95a42d4d7 | 2176 | * Either aperture or non-aperture SPADS are applied but never both. |
sepp_nepp | 11:c6f95a42d4d7 | 2177 | * Firstly non-aperture SPADS are set,begining with 5 SPADS,and |
sepp_nepp | 11:c6f95a42d4d7 | 2178 | * increased one SPAD at a time until the closest measurement to the |
nikapov | 0:a1a69d32f310 | 2179 | * target rate is achieved. |
nikapov | 0:a1a69d32f310 | 2180 | * |
sepp_nepp | 11:c6f95a42d4d7 | 2181 | * If the target rate is exceeded when 5 non-aperture SPADS are enabled, |
sepp_nepp | 11:c6f95a42d4d7 | 2182 | * initialization is performed instead with aperture SPADS. |
nikapov | 0:a1a69d32f310 | 2183 | * |
sepp_nepp | 11:c6f95a42d4d7 | 2184 | * When setting SPADS,a 'Good SPAD Map' is applied. |
nikapov | 0:a1a69d32f310 | 2185 | * |
nikapov | 0:a1a69d32f310 | 2186 | * This procedure operates within a SPAD window of interest of a maximum |
sepp_nepp | 11:c6f95a42d4d7 | 2187 | * 44 SPADS. |
sepp_nepp | 7:41cbc431e1f4 | 2188 | * The start point is currently fixed to 180,which lies towards the end |
nikapov | 0:a1a69d32f310 | 2189 | * of the non-aperture quadrant and runs in to the adjacent aperture |
sepp_nepp | 11:c6f95a42d4d7 | 2190 | * quadrant. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2191 | target_Ref_rate = targetRefRate; |
sepp_nepp | 11:c6f95a42d4d7 | 2192 | |
sepp_nepp | 11:c6f95a42d4d7 | 2193 | /* Initialize SPAD arrays. |
sepp_nepp | 11:c6f95a42d4d7 | 2194 | * Currently the good SPAD map is initialised to 'All good'. |
sepp_nepp | 11:c6f95a42d4d7 | 2195 | * This is a short term implementation. The good SPAD map will be |
nikapov | 0:a1a69d32f310 | 2196 | * provided as an input. |
nikapov | 0:a1a69d32f310 | 2197 | * Note that there are 6 bytes. Only the first 44 bits will be used to |
sepp_nepp | 11:c6f95a42d4d7 | 2198 | * represent SPADS. */ |
sepp_nepp | 12:aa177f0e4c10 | 2199 | for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) { |
sepp_nepp | 11:c6f95a42d4d7 | 2200 | SPADData.RefSPADEnables[index] = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 2201 | |
sepp_nepp | 11:c6f95a42d4d7 | 2202 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2203 | Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2204 | Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C); |
sepp_nepp | 11:c6f95a42d4d7 | 2205 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2206 | Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT,start_select); |
sepp_nepp | 11:c6f95a42d4d7 | 2207 | Write_Byte(REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0); |
nikapov | 0:a1a69d32f310 | 2208 | |
nikapov | 0:a1a69d32f310 | 2209 | /* Perform ref calibration */ |
sepp_nepp | 11:c6f95a42d4d7 | 2210 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2211 | {Perf_Ref_calibration(&vhv_settings, &phase_cal, 0);} |
sepp_nepp | 11:c6f95a42d4d7 | 2212 | |
sepp_nepp | 11:c6f95a42d4d7 | 2213 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2214 | /* Enable Minimum NON-APERTURE SPADS */ |
sepp_nepp | 11:c6f95a42d4d7 | 2215 | current_SPAD_index = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2216 | last_SPAD_index = current_SPAD_index; |
sepp_nepp | 11:c6f95a42d4d7 | 2217 | need_apt_SPADS = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2218 | Enable_Ref_SPADS(need_apt_SPADS, |
sepp_nepp | 11:c6f95a42d4d7 | 2219 | SPADData.RefGoodSPADMap, |
sepp_nepp | 11:c6f95a42d4d7 | 2220 | SPADData.RefSPADEnables, |
sepp_nepp | 12:aa177f0e4c10 | 2221 | REF_SPAD_ARRAY_SIZE, |
nikapov | 0:a1a69d32f310 | 2222 | start_select, |
sepp_nepp | 11:c6f95a42d4d7 | 2223 | current_SPAD_index, |
sepp_nepp | 11:c6f95a42d4d7 | 2224 | minimum_SPAD_count, |
sepp_nepp | 11:c6f95a42d4d7 | 2225 | &last_SPAD_index); |
nikapov | 0:a1a69d32f310 | 2226 | } |
nikapov | 0:a1a69d32f310 | 2227 | |
sepp_nepp | 11:c6f95a42d4d7 | 2228 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2229 | current_SPAD_index = last_SPAD_index; |
sepp_nepp | 11:c6f95a42d4d7 | 2230 | |
sepp_nepp | 11:c6f95a42d4d7 | 2231 | peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2232 | if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate)) |
sepp_nepp | 11:c6f95a42d4d7 | 2233 | { /* Signal rate measurement too high, switch to APERTURE SPADS */ |
sepp_nepp | 12:aa177f0e4c10 | 2234 | for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) |
sepp_nepp | 11:c6f95a42d4d7 | 2235 | { SPADData.RefSPADEnables[index] = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 2236 | |
sepp_nepp | 11:c6f95a42d4d7 | 2237 | /* Increment to the first APERTURE SPAD */ |
sepp_nepp | 11:c6f95a42d4d7 | 2238 | while ((Is_ApertureSPAD(start_select + current_SPAD_index) |
sepp_nepp | 11:c6f95a42d4d7 | 2239 | == 0) && (current_SPAD_index < max_SPAD_count)) |
sepp_nepp | 11:c6f95a42d4d7 | 2240 | { current_SPAD_index++; } |
sepp_nepp | 11:c6f95a42d4d7 | 2241 | |
sepp_nepp | 11:c6f95a42d4d7 | 2242 | need_apt_SPADS = 1; |
sepp_nepp | 11:c6f95a42d4d7 | 2243 | |
sepp_nepp | 11:c6f95a42d4d7 | 2244 | Enable_Ref_SPADS(need_apt_SPADS, |
sepp_nepp | 11:c6f95a42d4d7 | 2245 | SPADData.RefGoodSPADMap, |
sepp_nepp | 11:c6f95a42d4d7 | 2246 | SPADData.RefSPADEnables, |
sepp_nepp | 12:aa177f0e4c10 | 2247 | REF_SPAD_ARRAY_SIZE, |
nikapov | 0:a1a69d32f310 | 2248 | start_select, |
sepp_nepp | 11:c6f95a42d4d7 | 2249 | current_SPAD_index, |
sepp_nepp | 11:c6f95a42d4d7 | 2250 | minimum_SPAD_count, |
sepp_nepp | 11:c6f95a42d4d7 | 2251 | &last_SPAD_index); |
sepp_nepp | 11:c6f95a42d4d7 | 2252 | |
sepp_nepp | 11:c6f95a42d4d7 | 2253 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2254 | current_SPAD_index = last_SPAD_index; |
sepp_nepp | 11:c6f95a42d4d7 | 2255 | peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2256 | |
sepp_nepp | 11:c6f95a42d4d7 | 2257 | if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate)) |
sepp_nepp | 11:c6f95a42d4d7 | 2258 | { /* Signal rate still too high after setting the minimum number of |
sepp_nepp | 11:c6f95a42d4d7 | 2259 | * APERTURE SPADS. Can do no more therefore set the min number of |
sepp_nepp | 11:c6f95a42d4d7 | 2260 | * aperture SPADS as the result. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2261 | is_aperture_SPADS_int = 1; |
sepp_nepp | 11:c6f95a42d4d7 | 2262 | ref_SPAD_count_int = minimum_SPAD_count; |
nikapov | 0:a1a69d32f310 | 2263 | } |
nikapov | 0:a1a69d32f310 | 2264 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2265 | } else { need_apt_SPADS = 0;} |
nikapov | 0:a1a69d32f310 | 2266 | } |
nikapov | 0:a1a69d32f310 | 2267 | |
sepp_nepp | 11:c6f95a42d4d7 | 2268 | if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref < target_Ref_rate)) |
sepp_nepp | 11:c6f95a42d4d7 | 2269 | { /* At this point,the minimum number of either aperture |
sepp_nepp | 11:c6f95a42d4d7 | 2270 | * or non-aperture SPADS have been set. Proceed to add |
sepp_nepp | 11:c6f95a42d4d7 | 2271 | * SPADS and perform measurements until the target reference is reached.*/ |
sepp_nepp | 11:c6f95a42d4d7 | 2272 | is_aperture_SPADS_int = need_apt_SPADS; |
sepp_nepp | 11:c6f95a42d4d7 | 2273 | ref_SPAD_count_int = minimum_SPAD_count; |
sepp_nepp | 11:c6f95a42d4d7 | 2274 | |
sepp_nepp | 12:aa177f0e4c10 | 2275 | memcpy(last_SPAD_array,SPADData.RefSPADEnables, REF_SPAD_ARRAY_SIZE); |
sepp_nepp | 11:c6f95a42d4d7 | 2276 | last_SIG_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate); |
nikapov | 0:a1a69d32f310 | 2277 | complete = 0; |
nikapov | 0:a1a69d32f310 | 2278 | |
nikapov | 0:a1a69d32f310 | 2279 | while (!complete) { |
sepp_nepp | 11:c6f95a42d4d7 | 2280 | Get_Next_Good_SPAD(SPADData.RefGoodSPADMap, |
sepp_nepp | 12:aa177f0e4c10 | 2281 | REF_SPAD_ARRAY_SIZE,current_SPAD_index, &next_good_SPAD); |
sepp_nepp | 11:c6f95a42d4d7 | 2282 | |
sepp_nepp | 11:c6f95a42d4d7 | 2283 | if (next_good_SPAD == -1) { |
sepp_nepp | 11:c6f95a42d4d7 | 2284 | ErrState = VL53L0X_ERROR_REF_SPAD_INIT; |
nikapov | 0:a1a69d32f310 | 2285 | break; |
nikapov | 0:a1a69d32f310 | 2286 | } |
nikapov | 0:a1a69d32f310 | 2287 | |
sepp_nepp | 11:c6f95a42d4d7 | 2288 | /* Cannot combine Aperture and Non-Aperture SPADS,so |
sepp_nepp | 11:c6f95a42d4d7 | 2289 | * ensure the current SPAD is of the correct type. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2290 | if (Is_ApertureSPAD((uint32_t)start_select + next_good_SPAD) != |
sepp_nepp | 11:c6f95a42d4d7 | 2291 | need_apt_SPADS) { |
sepp_nepp | 11:c6f95a42d4d7 | 2292 | /* At this point we have enabled the maximum number of Aperture SPADS. */ |
nikapov | 0:a1a69d32f310 | 2293 | complete = 1; |
nikapov | 0:a1a69d32f310 | 2294 | break; |
nikapov | 0:a1a69d32f310 | 2295 | } |
nikapov | 0:a1a69d32f310 | 2296 | |
sepp_nepp | 11:c6f95a42d4d7 | 2297 | (ref_SPAD_count_int)++; |
sepp_nepp | 11:c6f95a42d4d7 | 2298 | |
sepp_nepp | 11:c6f95a42d4d7 | 2299 | current_SPAD_index = next_good_SPAD; |
sepp_nepp | 11:c6f95a42d4d7 | 2300 | Enable_SPAD_bit(SPADData.RefSPADEnables, |
sepp_nepp | 12:aa177f0e4c10 | 2301 | REF_SPAD_ARRAY_SIZE,current_SPAD_index); |
sepp_nepp | 11:c6f95a42d4d7 | 2302 | |
sepp_nepp | 11:c6f95a42d4d7 | 2303 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2304 | current_SPAD_index++; |
sepp_nepp | 11:c6f95a42d4d7 | 2305 | /* Proceed to apply the additional SPAD and perform measurement. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2306 | I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPADData.RefSPADEnables,6); //Set_Ref_SPAD_map |
nikapov | 0:a1a69d32f310 | 2307 | } |
nikapov | 0:a1a69d32f310 | 2308 | |
sepp_nepp | 11:c6f95a42d4d7 | 2309 | if (ErrState != VL53L0X_OK) { break; } |
sepp_nepp | 11:c6f95a42d4d7 | 2310 | |
sepp_nepp | 11:c6f95a42d4d7 | 2311 | peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2312 | |
sepp_nepp | 11:c6f95a42d4d7 | 2313 | if (ErrState != VL53L0X_OK) { break; } |
sepp_nepp | 11:c6f95a42d4d7 | 2314 | |
sepp_nepp | 11:c6f95a42d4d7 | 2315 | signal_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate); |
sepp_nepp | 11:c6f95a42d4d7 | 2316 | |
sepp_nepp | 11:c6f95a42d4d7 | 2317 | if (peak_SIG_rate_Ref > target_Ref_rate) { |
sepp_nepp | 11:c6f95a42d4d7 | 2318 | /* Select the SPAD map that provides the |
nikapov | 0:a1a69d32f310 | 2319 | * measurement closest to the target rate, |
sepp_nepp | 11:c6f95a42d4d7 | 2320 | * either above or below it. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2321 | if (signal_rate_diff > last_SIG_rate_diff) { |
sepp_nepp | 11:c6f95a42d4d7 | 2322 | /* Previous SPAD map produced a closer measurement,so choose this. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2323 | I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, last_SPAD_array,6); // Set_Ref_SPAD_map(); |
sepp_nepp | 12:aa177f0e4c10 | 2324 | memcpy(SPADData.RefSPADEnables,last_SPAD_array,REF_SPAD_ARRAY_SIZE); |
sepp_nepp | 11:c6f95a42d4d7 | 2325 | (ref_SPAD_count_int)--; |
nikapov | 0:a1a69d32f310 | 2326 | } |
nikapov | 0:a1a69d32f310 | 2327 | complete = 1; |
nikapov | 0:a1a69d32f310 | 2328 | } else { |
sepp_nepp | 11:c6f95a42d4d7 | 2329 | /* Continue to add SPADS */ |
sepp_nepp | 11:c6f95a42d4d7 | 2330 | last_SIG_rate_diff = signal_rate_diff; |
sepp_nepp | 12:aa177f0e4c10 | 2331 | memcpy(last_SPAD_array, SPADData.RefSPADEnables,REF_SPAD_ARRAY_SIZE); |
nikapov | 0:a1a69d32f310 | 2332 | } |
nikapov | 0:a1a69d32f310 | 2333 | } /* while */ |
nikapov | 0:a1a69d32f310 | 2334 | } |
nikapov | 0:a1a69d32f310 | 2335 | |
sepp_nepp | 11:c6f95a42d4d7 | 2336 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2337 | *ref_SPAD_count = ref_SPAD_count_int; |
sepp_nepp | 11:c6f95a42d4d7 | 2338 | *is_aperture_SPADS = is_aperture_SPADS_int; |
sepp_nepp | 12:aa177f0e4c10 | 2339 | RefSPADSInitialised = 1; |
sepp_nepp | 12:aa177f0e4c10 | 2340 | ReferenceSPADCount = (uint8_t)(*ref_SPAD_count); |
sepp_nepp | 12:aa177f0e4c10 | 2341 | ReferenceSPADType = *is_aperture_SPADS; |
nikapov | 0:a1a69d32f310 | 2342 | } |
nikapov | 0:a1a69d32f310 | 2343 | } |
nikapov | 0:a1a69d32f310 | 2344 | |
sepp_nepp | 11:c6f95a42d4d7 | 2345 | void VL53L0X::Set_Reference_SPADS(uint32_t count,uint8_t is_aperture_SPADS) |
sepp_nepp | 11:c6f95a42d4d7 | 2346 | { uint32_t current_SPAD_index = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2347 | uint8_t start_select = 0xB4; |
sepp_nepp | 11:c6f95a42d4d7 | 2348 | uint32_t max_SPAD_count = 44; |
sepp_nepp | 11:c6f95a42d4d7 | 2349 | uint32_t last_SPAD_index; |
nikapov | 0:a1a69d32f310 | 2350 | uint32_t index; |
nikapov | 0:a1a69d32f310 | 2351 | |
sepp_nepp | 11:c6f95a42d4d7 | 2352 | /* This function applies a requested number of reference SPADS,either |
sepp_nepp | 11:c6f95a42d4d7 | 2353 | * aperture or non-aperture,as requested. The good SPAD map will be applied.*/ |
sepp_nepp | 11:c6f95a42d4d7 | 2354 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2355 | Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2356 | Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C); |
sepp_nepp | 11:c6f95a42d4d7 | 2357 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2358 | Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT, start_select); |
sepp_nepp | 11:c6f95a42d4d7 | 2359 | |
sepp_nepp | 12:aa177f0e4c10 | 2360 | for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) { |
sepp_nepp | 11:c6f95a42d4d7 | 2361 | SPADData.RefSPADEnables[index] = 0; } |
sepp_nepp | 11:c6f95a42d4d7 | 2362 | |
sepp_nepp | 11:c6f95a42d4d7 | 2363 | if (is_aperture_SPADS) { |
sepp_nepp | 11:c6f95a42d4d7 | 2364 | /* Increment to the first APERTURE SPAD */ |
sepp_nepp | 11:c6f95a42d4d7 | 2365 | while ((Is_ApertureSPAD(start_select + current_SPAD_index) == 0) && |
sepp_nepp | 11:c6f95a42d4d7 | 2366 | (current_SPAD_index < max_SPAD_count)) { |
sepp_nepp | 11:c6f95a42d4d7 | 2367 | current_SPAD_index++; |
nikapov | 0:a1a69d32f310 | 2368 | } |
nikapov | 0:a1a69d32f310 | 2369 | } |
sepp_nepp | 12:aa177f0e4c10 | 2370 | Enable_Ref_SPADS(is_aperture_SPADS, SPADData.RefGoodSPADMap, |
sepp_nepp | 11:c6f95a42d4d7 | 2371 | SPADData.RefSPADEnables, |
sepp_nepp | 12:aa177f0e4c10 | 2372 | REF_SPAD_ARRAY_SIZE, |
nikapov | 0:a1a69d32f310 | 2373 | start_select, |
sepp_nepp | 11:c6f95a42d4d7 | 2374 | current_SPAD_index, |
nikapov | 0:a1a69d32f310 | 2375 | count, |
sepp_nepp | 11:c6f95a42d4d7 | 2376 | &last_SPAD_index); |
sepp_nepp | 11:c6f95a42d4d7 | 2377 | |
sepp_nepp | 11:c6f95a42d4d7 | 2378 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 12:aa177f0e4c10 | 2379 | RefSPADSInitialised = 1; |
sepp_nepp | 12:aa177f0e4c10 | 2380 | ReferenceSPADCount = (uint8_t)(count); |
sepp_nepp | 12:aa177f0e4c10 | 2381 | ReferenceSPADType = is_aperture_SPADS; |
nikapov | 0:a1a69d32f310 | 2382 | } |
nikapov | 0:a1a69d32f310 | 2383 | } |
nikapov | 0:a1a69d32f310 | 2384 | |
sepp_nepp | 11:c6f95a42d4d7 | 2385 | void VL53L0X::Set_GPIO_config(VL53L0X_DeviceModes device_mode, |
sepp_nepp | 11:c6f95a42d4d7 | 2386 | TGPIO_Func functionality, VL53L0X_InterruptPolarity polarity) |
sepp_nepp | 11:c6f95a42d4d7 | 2387 | { uint8_t pol_data; |
sepp_nepp | 11:c6f95a42d4d7 | 2388 | |
sepp_nepp | 11:c6f95a42d4d7 | 2389 | if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) |
sepp_nepp | 11:c6f95a42d4d7 | 2390 | { pol_data = 0x00;} else { pol_data = 0x10;} |
sepp_nepp | 11:c6f95a42d4d7 | 2391 | |
sepp_nepp | 11:c6f95a42d4d7 | 2392 | switch ( device_mode ) { |
sepp_nepp | 11:c6f95a42d4d7 | 2393 | case VL53L0X_DEVICEMODE_GPIO_DRIVE: |
sepp_nepp | 11:c6f95a42d4d7 | 2394 | Write_Byte(REG_GPIO_HV_MUX_ACTIVE_HIGH,pol_data); |
sepp_nepp | 11:c6f95a42d4d7 | 2395 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2396 | case VL53L0X_DEVICEMODE_GPIO_OSC: |
sepp_nepp | 11:c6f95a42d4d7 | 2397 | Write_Byte(0xff,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2398 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2399 | Write_Byte(0xff,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2400 | Write_Byte(0x80,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2401 | Write_Byte(0x85,0x02); |
sepp_nepp | 11:c6f95a42d4d7 | 2402 | Write_Byte(0xff,0x04); |
sepp_nepp | 11:c6f95a42d4d7 | 2403 | Write_Byte(0xcd,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2404 | Write_Byte(0xcc,0x11); |
sepp_nepp | 11:c6f95a42d4d7 | 2405 | Write_Byte(0xff,0x07); |
sepp_nepp | 11:c6f95a42d4d7 | 2406 | Write_Byte(0xbe,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2407 | Write_Byte(0xff,0x06); |
sepp_nepp | 11:c6f95a42d4d7 | 2408 | Write_Byte(0xcc,0x09); |
sepp_nepp | 11:c6f95a42d4d7 | 2409 | Write_Byte(0xff,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2410 | Write_Byte(0xff,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2411 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2412 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2413 | default: |
sepp_nepp | 11:c6f95a42d4d7 | 2414 | if (functionality>GPIO_FUNC_NEW_MEASURE_READY) |
sepp_nepp | 11:c6f95a42d4d7 | 2415 | { ErrState = VL53L0X_ERROR_GPIO_FUNC_NOT_SUPPORTED; } |
sepp_nepp | 11:c6f95a42d4d7 | 2416 | else { Write_Byte(REG_SYSINT_CONFIG_GPIO,functionality); } |
sepp_nepp | 11:c6f95a42d4d7 | 2417 | |
sepp_nepp | 11:c6f95a42d4d7 | 2418 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2419 | { Register_BitMask(REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,pol_data); } |
sepp_nepp | 11:c6f95a42d4d7 | 2420 | |
sepp_nepp | 11:c6f95a42d4d7 | 2421 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2422 | {GpioFunctionality = functionality; } |
sepp_nepp | 11:c6f95a42d4d7 | 2423 | |
sepp_nepp | 11:c6f95a42d4d7 | 2424 | Clear_interrupt_mask(0); |
sepp_nepp | 11:c6f95a42d4d7 | 2425 | } // switch |
sepp_nepp | 11:c6f95a42d4d7 | 2426 | } // Set_GPIO_config |
sepp_nepp | 11:c6f95a42d4d7 | 2427 | |
sepp_nepp | 11:c6f95a42d4d7 | 2428 | /* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */ |
sepp_nepp | 11:c6f95a42d4d7 | 2429 | uint16_t VL53L0X::Encode_timeout(uint32_t timeout_macro_clks) |
sepp_nepp | 11:c6f95a42d4d7 | 2430 | { uint16_t encoded_timeout = 0; |
nikapov | 0:a1a69d32f310 | 2431 | uint16_t ms_byte = 0; |
nikapov | 0:a1a69d32f310 | 2432 | |
nikapov | 0:a1a69d32f310 | 2433 | if (timeout_macro_clks > 0) { |
sepp_nepp | 11:c6f95a42d4d7 | 2434 | timeout_macro_clks = timeout_macro_clks - 1; |
sepp_nepp | 11:c6f95a42d4d7 | 2435 | while ((timeout_macro_clks & 0xFFFFFF00) > 0) { |
sepp_nepp | 11:c6f95a42d4d7 | 2436 | timeout_macro_clks = timeout_macro_clks >> 1; |
nikapov | 0:a1a69d32f310 | 2437 | ms_byte++; |
sepp_nepp | 11:c6f95a42d4d7 | 2438 | } // while |
sepp_nepp | 11:c6f95a42d4d7 | 2439 | encoded_timeout = (ms_byte << 8) + (uint16_t)(timeout_macro_clks & 0x000000FF); |
sepp_nepp | 11:c6f95a42d4d7 | 2440 | } |
nikapov | 0:a1a69d32f310 | 2441 | return encoded_timeout; |
nikapov | 0:a1a69d32f310 | 2442 | } |
nikapov | 0:a1a69d32f310 | 2443 | |
sepp_nepp | 11:c6f95a42d4d7 | 2444 | void VL53L0X::Set_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id, |
nikapov | 0:a1a69d32f310 | 2445 | uint32_t timeout_micro_secs) |
sepp_nepp | 11:c6f95a42d4d7 | 2446 | { uint8_t current_vcsel_PPeriod_p_clk; |
nikapov | 0:a1a69d32f310 | 2447 | uint8_t msrc_encoded_time_out; |
nikapov | 0:a1a69d32f310 | 2448 | uint16_t pre_range_encoded_time_out; |
nikapov | 0:a1a69d32f310 | 2449 | uint16_t pre_range_time_out_m_clks; |
nikapov | 0:a1a69d32f310 | 2450 | uint16_t msrc_range_time_out_m_clks; |
nikapov | 0:a1a69d32f310 | 2451 | uint32_t final_range_time_out_m_clks; |
nikapov | 0:a1a69d32f310 | 2452 | uint16_t final_range_encoded_time_out; |
sepp_nepp | 11:c6f95a42d4d7 | 2453 | VL53L0X_Sequence_Steps_t sequence_steps; |
sepp_nepp | 11:c6f95a42d4d7 | 2454 | |
sepp_nepp | 11:c6f95a42d4d7 | 2455 | switch (sequence_step_id) { |
sepp_nepp | 11:c6f95a42d4d7 | 2456 | case VL53L0X_SEQUENCESTEP_TCC: |
sepp_nepp | 11:c6f95a42d4d7 | 2457 | case VL53L0X_SEQUENCESTEP_DSS: |
sepp_nepp | 11:c6f95a42d4d7 | 2458 | case VL53L0X_SEQUENCESTEP_MSRC: |
sepp_nepp | 11:c6f95a42d4d7 | 2459 | current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 2460 | ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
sepp_nepp | 11:c6f95a42d4d7 | 2461 | |
sepp_nepp | 11:c6f95a42d4d7 | 2462 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2463 | msrc_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs, |
sepp_nepp | 11:c6f95a42d4d7 | 2464 | (uint8_t)current_vcsel_PPeriod_p_clk); |
sepp_nepp | 11:c6f95a42d4d7 | 2465 | |
sepp_nepp | 11:c6f95a42d4d7 | 2466 | if (msrc_range_time_out_m_clks > 256) { msrc_encoded_time_out = 255;} |
sepp_nepp | 11:c6f95a42d4d7 | 2467 | else {msrc_encoded_time_out = (uint8_t)msrc_range_time_out_m_clks - 1; } |
nikapov | 0:a1a69d32f310 | 2468 | |
sepp_nepp | 12:aa177f0e4c10 | 2469 | LastEncodedTimeout = msrc_encoded_time_out; |
sepp_nepp | 11:c6f95a42d4d7 | 2470 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2471 | Write_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP,msrc_encoded_time_out); |
sepp_nepp | 11:c6f95a42d4d7 | 2472 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2473 | |
sepp_nepp | 11:c6f95a42d4d7 | 2474 | case VL53L0X_SEQUENCESTEP_PRE_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 2475 | current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 2476 | ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
sepp_nepp | 11:c6f95a42d4d7 | 2477 | |
sepp_nepp | 11:c6f95a42d4d7 | 2478 | pre_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs, |
sepp_nepp | 11:c6f95a42d4d7 | 2479 | (uint8_t)current_vcsel_PPeriod_p_clk); |
sepp_nepp | 11:c6f95a42d4d7 | 2480 | pre_range_encoded_time_out = Encode_timeout(pre_range_time_out_m_clks); |
sepp_nepp | 11:c6f95a42d4d7 | 2481 | |
sepp_nepp | 12:aa177f0e4c10 | 2482 | LastEncodedTimeout = pre_range_encoded_time_out; |
sepp_nepp | 11:c6f95a42d4d7 | 2483 | |
sepp_nepp | 11:c6f95a42d4d7 | 2484 | Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out); |
sepp_nepp | 11:c6f95a42d4d7 | 2485 | |
sepp_nepp | 11:c6f95a42d4d7 | 2486 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2487 | {PreRangeTimeoutMicroSecs=timeout_micro_secs; } |
sepp_nepp | 11:c6f95a42d4d7 | 2488 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2489 | |
sepp_nepp | 11:c6f95a42d4d7 | 2490 | case VL53L0X_SEQUENCESTEP_FINAL_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 2491 | /* For the final range timeout,the pre-range timeout must be added. |
sepp_nepp | 11:c6f95a42d4d7 | 2492 | * To do this both final and pre-range timeouts must be expressed in |
sepp_nepp | 11:c6f95a42d4d7 | 2493 | * macro periods MClks because they have different vcsel periods.*/ |
sepp_nepp | 11:c6f95a42d4d7 | 2494 | sequence_steps = Get_sequence_step_enables(); |
nikapov | 0:a1a69d32f310 | 2495 | pre_range_time_out_m_clks = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2496 | |
sepp_nepp | 11:c6f95a42d4d7 | 2497 | if (sequence_steps.PreRangeOn) |
sepp_nepp | 11:c6f95a42d4d7 | 2498 | { current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 2499 | ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; |
sepp_nepp | 11:c6f95a42d4d7 | 2500 | |
sepp_nepp | 11:c6f95a42d4d7 | 2501 | /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ |
sepp_nepp | 11:c6f95a42d4d7 | 2502 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2503 | { pre_range_encoded_time_out = Read_Word(0x51); |
sepp_nepp | 11:c6f95a42d4d7 | 2504 | pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out); |
sepp_nepp | 11:c6f95a42d4d7 | 2505 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2506 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2507 | |
sepp_nepp | 11:c6f95a42d4d7 | 2508 | /* Calculate FINAL RANGE Timeout in Macro Periode (MCLKS) and add PRE-RANGE value */ |
sepp_nepp | 11:c6f95a42d4d7 | 2509 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2510 | { current_vcsel_PPeriod_p_clk /* Get and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 2511 | = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; } |
sepp_nepp | 11:c6f95a42d4d7 | 2512 | |
sepp_nepp | 11:c6f95a42d4d7 | 2513 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2514 | { final_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs, |
sepp_nepp | 11:c6f95a42d4d7 | 2515 | (uint8_t) current_vcsel_PPeriod_p_clk); |
nikapov | 0:a1a69d32f310 | 2516 | |
nikapov | 0:a1a69d32f310 | 2517 | final_range_time_out_m_clks += pre_range_time_out_m_clks; |
nikapov | 0:a1a69d32f310 | 2518 | |
sepp_nepp | 11:c6f95a42d4d7 | 2519 | final_range_encoded_time_out = Encode_timeout(final_range_time_out_m_clks); |
sepp_nepp | 11:c6f95a42d4d7 | 2520 | |
sepp_nepp | 11:c6f95a42d4d7 | 2521 | Write_Word(0x71,final_range_encoded_time_out); |
sepp_nepp | 11:c6f95a42d4d7 | 2522 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2523 | |
sepp_nepp | 11:c6f95a42d4d7 | 2524 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2525 | { FinalRangeTimeoutMicroSecs = timeout_micro_secs; } |
sepp_nepp | 11:c6f95a42d4d7 | 2526 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2527 | |
sepp_nepp | 11:c6f95a42d4d7 | 2528 | default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
sepp_nepp | 11:c6f95a42d4d7 | 2529 | } // switch (sequence_step_id) |
sepp_nepp | 11:c6f95a42d4d7 | 2530 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2531 | |
sepp_nepp | 11:c6f95a42d4d7 | 2532 | void VL53L0X::Set_Measure_Time_Budget_us (uint32_t Measure_Time_Budget_us) |
sepp_nepp | 11:c6f95a42d4d7 | 2533 | { uint32_t final_range_timing_budget_us; |
sepp_nepp | 11:c6f95a42d4d7 | 2534 | VL53L0X_Sequence_Steps_t sequence_steps; |
sepp_nepp | 11:c6f95a42d4d7 | 2535 | uint32_t msrc_dcc_tcc_timeout_us= 2000; |
sepp_nepp | 10:cd251e0fc2fd | 2536 | uint32_t start_overhead_us = 1910; |
sepp_nepp | 10:cd251e0fc2fd | 2537 | uint32_t end_overhead_us = 960; |
sepp_nepp | 10:cd251e0fc2fd | 2538 | uint32_t msrc_overhead_us = 660; |
sepp_nepp | 10:cd251e0fc2fd | 2539 | uint32_t tcc_overhead_us = 590; |
sepp_nepp | 10:cd251e0fc2fd | 2540 | uint32_t dss_overhead_us = 690; |
sepp_nepp | 10:cd251e0fc2fd | 2541 | uint32_t pre_range_overhead_us = 660; |
sepp_nepp | 11:c6f95a42d4d7 | 2542 | uint32_t final_range_overhead_us= 550; |
sepp_nepp | 10:cd251e0fc2fd | 2543 | uint32_t pre_range_timeout_us = 0; |
sepp_nepp | 10:cd251e0fc2fd | 2544 | uint32_t c_min_timing_budget_us = 20000; |
nikapov | 0:a1a69d32f310 | 2545 | uint32_t sub_timeout = 0; |
nikapov | 0:a1a69d32f310 | 2546 | |
sepp_nepp | 11:c6f95a42d4d7 | 2547 | if (Measure_Time_Budget_us < c_min_timing_budget_us) |
sepp_nepp | 11:c6f95a42d4d7 | 2548 | { ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
sepp_nepp | 11:c6f95a42d4d7 | 2549 | return ; } |
sepp_nepp | 11:c6f95a42d4d7 | 2550 | |
sepp_nepp | 11:c6f95a42d4d7 | 2551 | final_range_timing_budget_us = Measure_Time_Budget_us - |
sepp_nepp | 11:c6f95a42d4d7 | 2552 | (start_overhead_us + end_overhead_us); |
sepp_nepp | 11:c6f95a42d4d7 | 2553 | |
sepp_nepp | 11:c6f95a42d4d7 | 2554 | sequence_steps = Get_sequence_step_enables(); |
sepp_nepp | 11:c6f95a42d4d7 | 2555 | |
sepp_nepp | 11:c6f95a42d4d7 | 2556 | if (ErrState == VL53L0X_OK && |
sepp_nepp | 11:c6f95a42d4d7 | 2557 | (sequence_steps.TccOn || |
sepp_nepp | 11:c6f95a42d4d7 | 2558 | sequence_steps.MsrcOn || |
sepp_nepp | 11:c6f95a42d4d7 | 2559 | sequence_steps.DssOn)) { |
nikapov | 0:a1a69d32f310 | 2560 | |
sepp_nepp | 7:41cbc431e1f4 | 2561 | /* TCC,MSRC and DSS all share the same timeout */ |
sepp_nepp | 11:c6f95a42d4d7 | 2562 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, |
sepp_nepp | 10:cd251e0fc2fd | 2563 | &msrc_dcc_tcc_timeout_us); |
nikapov | 0:a1a69d32f310 | 2564 | |
sepp_nepp | 11:c6f95a42d4d7 | 2565 | /* Subtract the TCC,MSRC and DSS timeouts if they are enabled. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2566 | if (ErrState != VL53L0X_OK) {return ; } |
nikapov | 0:a1a69d32f310 | 2567 | |
nikapov | 0:a1a69d32f310 | 2568 | /* TCC */ |
sepp_nepp | 11:c6f95a42d4d7 | 2569 | if (sequence_steps.TccOn) { |
sepp_nepp | 11:c6f95a42d4d7 | 2570 | |
sepp_nepp | 11:c6f95a42d4d7 | 2571 | sub_timeout = msrc_dcc_tcc_timeout_us + tcc_overhead_us; |
sepp_nepp | 11:c6f95a42d4d7 | 2572 | |
sepp_nepp | 11:c6f95a42d4d7 | 2573 | if (sub_timeout < final_range_timing_budget_us) { |
sepp_nepp | 11:c6f95a42d4d7 | 2574 | final_range_timing_budget_us -= sub_timeout; |
sepp_nepp | 11:c6f95a42d4d7 | 2575 | } else { /* Requested timeout too big. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2576 | ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
nikapov | 0:a1a69d32f310 | 2577 | } |
nikapov | 0:a1a69d32f310 | 2578 | } |
nikapov | 0:a1a69d32f310 | 2579 | |
sepp_nepp | 11:c6f95a42d4d7 | 2580 | if (ErrState != VL53L0X_OK) {return; } |
nikapov | 0:a1a69d32f310 | 2581 | |
nikapov | 0:a1a69d32f310 | 2582 | /* DSS */ |
sepp_nepp | 11:c6f95a42d4d7 | 2583 | if (sequence_steps.DssOn) |
sepp_nepp | 11:c6f95a42d4d7 | 2584 | { sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us); |
sepp_nepp | 11:c6f95a42d4d7 | 2585 | |
sepp_nepp | 11:c6f95a42d4d7 | 2586 | if (sub_timeout < final_range_timing_budget_us) |
sepp_nepp | 11:c6f95a42d4d7 | 2587 | { final_range_timing_budget_us -= sub_timeout; } |
sepp_nepp | 11:c6f95a42d4d7 | 2588 | else { /* Requested timeout too big. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2589 | ErrState = VL53L0X_ERROR_INVALID_PARAMS; } |
sepp_nepp | 11:c6f95a42d4d7 | 2590 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2591 | else if (sequence_steps.MsrcOn) /* MSRC */ |
sepp_nepp | 11:c6f95a42d4d7 | 2592 | { sub_timeout = msrc_dcc_tcc_timeout_us + msrc_overhead_us; |
sepp_nepp | 11:c6f95a42d4d7 | 2593 | |
sepp_nepp | 11:c6f95a42d4d7 | 2594 | if (sub_timeout < final_range_timing_budget_us) |
sepp_nepp | 11:c6f95a42d4d7 | 2595 | { final_range_timing_budget_us -= sub_timeout; } |
sepp_nepp | 11:c6f95a42d4d7 | 2596 | else /* Requested timeout too big. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2597 | { ErrState = VL53L0X_ERROR_INVALID_PARAMS; } |
sepp_nepp | 11:c6f95a42d4d7 | 2598 | } |
nikapov | 0:a1a69d32f310 | 2599 | } |
nikapov | 0:a1a69d32f310 | 2600 | |
sepp_nepp | 11:c6f95a42d4d7 | 2601 | if (ErrState != VL53L0X_OK) {return; } |
sepp_nepp | 11:c6f95a42d4d7 | 2602 | |
sepp_nepp | 11:c6f95a42d4d7 | 2603 | if (sequence_steps.PreRangeOn) { |
nikapov | 0:a1a69d32f310 | 2604 | /* Subtract the Pre-range timeout if enabled. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2605 | Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, |
sepp_nepp | 10:cd251e0fc2fd | 2606 | &pre_range_timeout_us); |
sepp_nepp | 11:c6f95a42d4d7 | 2607 | sub_timeout = pre_range_timeout_us + pre_range_overhead_us; |
sepp_nepp | 10:cd251e0fc2fd | 2608 | |
sepp_nepp | 10:cd251e0fc2fd | 2609 | if (sub_timeout < final_range_timing_budget_us) { |
sepp_nepp | 10:cd251e0fc2fd | 2610 | final_range_timing_budget_us -= sub_timeout; |
nikapov | 0:a1a69d32f310 | 2611 | } else { |
nikapov | 0:a1a69d32f310 | 2612 | /* Requested timeout too big. */ |
sepp_nepp | 11:c6f95a42d4d7 | 2613 | ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
nikapov | 0:a1a69d32f310 | 2614 | } |
nikapov | 0:a1a69d32f310 | 2615 | } |
nikapov | 0:a1a69d32f310 | 2616 | |
sepp_nepp | 11:c6f95a42d4d7 | 2617 | if (ErrState == VL53L0X_OK && sequence_steps.FinalRangeOn) |
sepp_nepp | 11:c6f95a42d4d7 | 2618 | { final_range_timing_budget_us -= final_range_overhead_us; |
nikapov | 0:a1a69d32f310 | 2619 | |
nikapov | 0:a1a69d32f310 | 2620 | /* Final Range Timeout |
nikapov | 0:a1a69d32f310 | 2621 | * Note that the final range timeout is determined by the timing |
nikapov | 0:a1a69d32f310 | 2622 | * budget and the sum of all other timeouts within the sequence. |
sepp_nepp | 7:41cbc431e1f4 | 2623 | * If there is no room for the final range timeout,then an error |
nikapov | 0:a1a69d32f310 | 2624 | * will be set. Otherwise the remaining time will be applied to |
nikapov | 0:a1a69d32f310 | 2625 | * the final range. |
nikapov | 0:a1a69d32f310 | 2626 | */ |
sepp_nepp | 11:c6f95a42d4d7 | 2627 | Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, |
sepp_nepp | 10:cd251e0fc2fd | 2628 | final_range_timing_budget_us); |
sepp_nepp | 10:cd251e0fc2fd | 2629 | |
sepp_nepp | 11:c6f95a42d4d7 | 2630 | CurrParams.Measure_Time_Budget_us = Measure_Time_Budget_us; |
nikapov | 0:a1a69d32f310 | 2631 | } |
nikapov | 0:a1a69d32f310 | 2632 | } |
nikapov | 0:a1a69d32f310 | 2633 | |
sepp_nepp | 11:c6f95a42d4d7 | 2634 | const uint8_t SEQUENCESTEP_MASK[] = |
sepp_nepp | 11:c6f95a42d4d7 | 2635 | { 0x10, //VL53L0X_SEQUENCESTEP_TCC = 0 |
sepp_nepp | 11:c6f95a42d4d7 | 2636 | 0x28, //VL53L0X_SEQUENCESTEP_DSS = 1 |
sepp_nepp | 11:c6f95a42d4d7 | 2637 | 0x04, //VL53L0X_SEQUENCESTEP_MSRC= 2 |
sepp_nepp | 11:c6f95a42d4d7 | 2638 | 0x40, //VL53L0X_SEQUENCESTEP_PRE_RANGE= 3 |
sepp_nepp | 11:c6f95a42d4d7 | 2639 | 0x80}; //VL53L0X_SEQUENCESTEP_FINAL_RANGE = 4 |
sepp_nepp | 11:c6f95a42d4d7 | 2640 | |
sepp_nepp | 11:c6f95a42d4d7 | 2641 | void VL53L0X::Set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id, |
sepp_nepp | 11:c6f95a42d4d7 | 2642 | uint8_t sequence_step_enabled) |
sepp_nepp | 11:c6f95a42d4d7 | 2643 | { uint8_t new_config = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2644 | |
sepp_nepp | 11:c6f95a42d4d7 | 2645 | // SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); |
sepp_nepp | 11:c6f95a42d4d7 | 2646 | // instead of reading from the device, use the SequenceConfig local data field!! |
sepp_nepp | 11:c6f95a42d4d7 | 2647 | |
sepp_nepp | 11:c6f95a42d4d7 | 2648 | if (sequence_step_enabled == 1) /* Enable requested sequence step */ |
sepp_nepp | 11:c6f95a42d4d7 | 2649 | { new_config = SequenceConfig | SEQUENCESTEP_MASK[sequence_step_id]; } |
sepp_nepp | 11:c6f95a42d4d7 | 2650 | else /* Disable requested sequence step */ |
sepp_nepp | 11:c6f95a42d4d7 | 2651 | { new_config = SequenceConfig & (0xff - SEQUENCESTEP_MASK[sequence_step_id]); } |
sepp_nepp | 11:c6f95a42d4d7 | 2652 | |
sepp_nepp | 11:c6f95a42d4d7 | 2653 | if (new_config != SequenceConfig) { /* Apply New Setting */ |
sepp_nepp | 11:c6f95a42d4d7 | 2654 | Set_SequenceConfig( new_config ); |
sepp_nepp | 11:c6f95a42d4d7 | 2655 | if (ErrState == VL53L0X_OK) /* Recalculate timing budget */ |
sepp_nepp | 11:c6f95a42d4d7 | 2656 | { Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); } |
sepp_nepp | 11:c6f95a42d4d7 | 2657 | } // if (new_config != sequence_config) |
nikapov | 0:a1a69d32f310 | 2658 | } |
nikapov | 0:a1a69d32f310 | 2659 | |
sepp_nepp | 11:c6f95a42d4d7 | 2660 | void VL53L0X::Set_limit_chk_en(uint16_t limit_check_id, uint8_t limit_chk_en) |
sepp_nepp | 11:c6f95a42d4d7 | 2661 | { TFP1616 temp_fix1616 = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2662 | if (limit_chk_en!=0) {limit_chk_en=1;} // make sure we only have 0 or 1 as values!!! |
sepp_nepp | 11:c6f95a42d4d7 | 2663 | |
sepp_nepp | 11:c6f95a42d4d7 | 2664 | switch (limit_check_id) { |
sepp_nepp | 11:c6f95a42d4d7 | 2665 | case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* internal computation: */ |
sepp_nepp | 11:c6f95a42d4d7 | 2666 | case VL53L0X_CHECKEN_SIG_REF_CLIP: /* internal computation: */ |
sepp_nepp | 11:c6f95a42d4d7 | 2667 | case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: /* internal computation: */ |
sepp_nepp | 11:c6f95a42d4d7 | 2668 | CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en; |
sepp_nepp | 11:c6f95a42d4d7 | 2669 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2670 | |
sepp_nepp | 11:c6f95a42d4d7 | 2671 | case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 2672 | temp_fix1616 = limit_chk_en * CurrParams.Limit_Chk_Val[limit_check_id]; |
sepp_nepp | 11:c6f95a42d4d7 | 2673 | Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, |
sepp_nepp | 11:c6f95a42d4d7 | 2674 | FP1616_TO_FP97(temp_fix1616)); |
sepp_nepp | 11:c6f95a42d4d7 | 2675 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2676 | |
sepp_nepp | 11:c6f95a42d4d7 | 2677 | case VL53L0X_CHECKEN_SIG_RATE_MSRC: |
sepp_nepp | 11:c6f95a42d4d7 | 2678 | Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xFE, (1-limit_chk_en)<< 1); |
sepp_nepp | 11:c6f95a42d4d7 | 2679 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2680 | |
sepp_nepp | 11:c6f95a42d4d7 | 2681 | case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 2682 | Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEF, (1-limit_chk_en)<< 4); |
sepp_nepp | 11:c6f95a42d4d7 | 2683 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2684 | |
sepp_nepp | 11:c6f95a42d4d7 | 2685 | default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; |
sepp_nepp | 11:c6f95a42d4d7 | 2686 | } // switch |
sepp_nepp | 11:c6f95a42d4d7 | 2687 | |
sepp_nepp | 11:c6f95a42d4d7 | 2688 | if (ErrState == VL53L0X_OK) { CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en; } |
nikapov | 0:a1a69d32f310 | 2689 | } |
nikapov | 0:a1a69d32f310 | 2690 | |
sepp_nepp | 11:c6f95a42d4d7 | 2691 | void VL53L0X::Static_init() |
sepp_nepp | 11:c6f95a42d4d7 | 2692 | { VL53L0X_DeviceParams_t new_curr_parameters; |
sepp_nepp | 11:c6f95a42d4d7 | 2693 | uint8_t *p_tuning_setting_buffer; |
nikapov | 0:a1a69d32f310 | 2694 | uint16_t tempword = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2695 | uint8_t tempbyte = 0; |
nikapov | 0:a1a69d32f310 | 2696 | uint32_t count = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2697 | uint8_t is_aperture_SPADS = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2698 | uint32_t ref_SPAD_count = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2699 | uint8_t aperture_SPADS = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2700 | uint8_t vcsel_PPeriod_pclk; |
nikapov | 0:a1a69d32f310 | 2701 | uint32_t seq_timeout_micro_secs; |
nikapov | 0:a1a69d32f310 | 2702 | |
sepp_nepp | 11:c6f95a42d4d7 | 2703 | /* set the ref SPAD from NVM */ |
sepp_nepp | 12:aa177f0e4c10 | 2704 | count = (uint32_t)ReferenceSPADCount; |
sepp_nepp | 12:aa177f0e4c10 | 2705 | aperture_SPADS = ReferenceSPADType; |
nikapov | 0:a1a69d32f310 | 2706 | |
nikapov | 0:a1a69d32f310 | 2707 | /* NVM value invalid */ |
sepp_nepp | 11:c6f95a42d4d7 | 2708 | if ((aperture_SPADS > 1) || ((aperture_SPADS == 1) && (count > 32)) || |
sepp_nepp | 11:c6f95a42d4d7 | 2709 | ((aperture_SPADS == 0) && (count > 12))) |
sepp_nepp | 11:c6f95a42d4d7 | 2710 | { Perf_Ref_SPAD_management(&ref_SPAD_count, &is_aperture_SPADS); } |
sepp_nepp | 11:c6f95a42d4d7 | 2711 | else |
sepp_nepp | 11:c6f95a42d4d7 | 2712 | { Set_Reference_SPADS(count,aperture_SPADS); } |
nikapov | 0:a1a69d32f310 | 2713 | |
nikapov | 0:a1a69d32f310 | 2714 | /* Initialize tuning settings buffer to prevent compiler warning. */ |
nikapov | 0:a1a69d32f310 | 2715 | p_tuning_setting_buffer = DefaultTuningSettings; |
nikapov | 0:a1a69d32f310 | 2716 | |
sepp_nepp | 11:c6f95a42d4d7 | 2717 | if (ErrState == VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2718 | if (UseInternalTuningSettings == 0) |
sepp_nepp | 11:c6f95a42d4d7 | 2719 | { p_tuning_setting_buffer = pTuningSettingsPointer; } |
sepp_nepp | 11:c6f95a42d4d7 | 2720 | else |
sepp_nepp | 11:c6f95a42d4d7 | 2721 | { p_tuning_setting_buffer = DefaultTuningSettings; } |
sepp_nepp | 11:c6f95a42d4d7 | 2722 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2723 | |
sepp_nepp | 11:c6f95a42d4d7 | 2724 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2725 | { Load_tuning_settings(p_tuning_setting_buffer); } |
sepp_nepp | 11:c6f95a42d4d7 | 2726 | |
sepp_nepp | 11:c6f95a42d4d7 | 2727 | /* Set interrupt config to new sample ready */ |
sepp_nepp | 11:c6f95a42d4d7 | 2728 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2729 | { Set_GPIO_config(0,GPIO_FUNC_NEW_MEASURE_READY,VL53L0X_INTERRUPTPOLARITY_LOW); } |
sepp_nepp | 11:c6f95a42d4d7 | 2730 | |
sepp_nepp | 11:c6f95a42d4d7 | 2731 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2732 | tempword = Read_Word(0x84); |
sepp_nepp | 11:c6f95a42d4d7 | 2733 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2734 | |
sepp_nepp | 11:c6f95a42d4d7 | 2735 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2736 | { OscFrequencyMHz=FP412_TO_FP1616(tempword); } |
sepp_nepp | 11:c6f95a42d4d7 | 2737 | |
sepp_nepp | 11:c6f95a42d4d7 | 2738 | /* After static init,some device parameters may be changed, so update them */ |
sepp_nepp | 11:c6f95a42d4d7 | 2739 | new_curr_parameters = Get_device_parameters(); |
sepp_nepp | 11:c6f95a42d4d7 | 2740 | |
sepp_nepp | 11:c6f95a42d4d7 | 2741 | if (ErrState == VL53L0X_OK) { tempbyte = Read_Byte(REG_SYSTEM_RANGE_CONFIG); } |
sepp_nepp | 11:c6f95a42d4d7 | 2742 | if (ErrState == VL53L0X_OK) { RangeFractionalEnable = (tempbyte & 1); } |
sepp_nepp | 11:c6f95a42d4d7 | 2743 | if (ErrState == VL53L0X_OK) { CurrParams = new_curr_parameters; } |
nikapov | 0:a1a69d32f310 | 2744 | |
nikapov | 0:a1a69d32f310 | 2745 | /* read the sequence config and save it */ |
sepp_nepp | 11:c6f95a42d4d7 | 2746 | Set_SequenceConfig( Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG) ); // checks for ErrState |
nikapov | 0:a1a69d32f310 | 2747 | |
nikapov | 0:a1a69d32f310 | 2748 | /* Disable MSRC and TCC by default */ |
sepp_nepp | 11:c6f95a42d4d7 | 2749 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2750 | { Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0); } |
sepp_nepp | 11:c6f95a42d4d7 | 2751 | |
sepp_nepp | 11:c6f95a42d4d7 | 2752 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2753 | { Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0); } |
sepp_nepp | 11:c6f95a42d4d7 | 2754 | |
sepp_nepp | 11:c6f95a42d4d7 | 2755 | /* Set State to standby */ |
sepp_nepp | 11:c6f95a42d4d7 | 2756 | Set_Current_State( VL53L0X_STATE_IDLE) ; |
nikapov | 0:a1a69d32f310 | 2757 | |
nikapov | 0:a1a69d32f310 | 2758 | /* Store pre-range vcsel period */ |
sepp_nepp | 11:c6f95a42d4d7 | 2759 | if (ErrState == VL53L0X_OK)/* Gets and converts the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 2760 | { vcsel_PPeriod_pclk = (Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; } |
sepp_nepp | 11:c6f95a42d4d7 | 2761 | |
sepp_nepp | 11:c6f95a42d4d7 | 2762 | if ( ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2763 | { PreRangeVcselPPeriod = vcsel_PPeriod_pclk; } |
nikapov | 0:a1a69d32f310 | 2764 | |
nikapov | 0:a1a69d32f310 | 2765 | /* Store final-range vcsel period */ |
sepp_nepp | 11:c6f95a42d4d7 | 2766 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2767 | { vcsel_PPeriod_pclk /* Get and convert the VCSEL period register into actual clock periods */ |
sepp_nepp | 11:c6f95a42d4d7 | 2768 | = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; } |
sepp_nepp | 11:c6f95a42d4d7 | 2769 | |
sepp_nepp | 11:c6f95a42d4d7 | 2770 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2771 | { FinalRangeVcselPPeriod = vcsel_PPeriod_pclk; } |
nikapov | 0:a1a69d32f310 | 2772 | |
nikapov | 0:a1a69d32f310 | 2773 | /* Store pre-range timeout */ |
sepp_nepp | 11:c6f95a42d4d7 | 2774 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2775 | { Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_PRE_RANGE, |
sepp_nepp | 11:c6f95a42d4d7 | 2776 | &seq_timeout_micro_secs); } |
sepp_nepp | 11:c6f95a42d4d7 | 2777 | |
sepp_nepp | 11:c6f95a42d4d7 | 2778 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2779 | { PreRangeTimeoutMicroSecs = seq_timeout_micro_secs; } |
nikapov | 0:a1a69d32f310 | 2780 | |
nikapov | 0:a1a69d32f310 | 2781 | /* Store final-range timeout */ |
sepp_nepp | 11:c6f95a42d4d7 | 2782 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2783 | { Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_FINAL_RANGE, |
sepp_nepp | 11:c6f95a42d4d7 | 2784 | &seq_timeout_micro_secs);} |
sepp_nepp | 11:c6f95a42d4d7 | 2785 | |
sepp_nepp | 11:c6f95a42d4d7 | 2786 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 12:aa177f0e4c10 | 2787 | { FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs; } |
nikapov | 0:a1a69d32f310 | 2788 | } |
nikapov | 0:a1a69d32f310 | 2789 | |
sepp_nepp | 11:c6f95a42d4d7 | 2790 | void VL53L0X::Stop_Measurement() |
sepp_nepp | 11:c6f95a42d4d7 | 2791 | { Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_SINGLESHOT); |
sepp_nepp | 11:c6f95a42d4d7 | 2792 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2793 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2794 | Write_Byte(0x91,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2795 | Write_Byte(0x00,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2796 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2797 | |
sepp_nepp | 11:c6f95a42d4d7 | 2798 | Set_Current_State( VL53L0X_STATE_IDLE ); |
nikapov | 0:a1a69d32f310 | 2799 | |
nikapov | 0:a1a69d32f310 | 2800 | /* Check if need to apply interrupt settings */ |
sepp_nepp | 11:c6f95a42d4d7 | 2801 | Check_and_load_interrupt_settings(0); |
nikapov | 0:a1a69d32f310 | 2802 | } |
nikapov | 0:a1a69d32f310 | 2803 | |
sepp_nepp | 11:c6f95a42d4d7 | 2804 | uint8_t VL53L0X::Get_Stop_Completed() |
sepp_nepp | 11:c6f95a42d4d7 | 2805 | { uint8_t Abyte = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2806 | |
sepp_nepp | 11:c6f95a42d4d7 | 2807 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2808 | Abyte = Read_Byte(0x04); |
sepp_nepp | 11:c6f95a42d4d7 | 2809 | Write_Byte(0xFF,0x0); |
sepp_nepp | 11:c6f95a42d4d7 | 2810 | |
sepp_nepp | 11:c6f95a42d4d7 | 2811 | if ((ErrState == VL53L0X_OK) & (Abyte == 0)) |
sepp_nepp | 11:c6f95a42d4d7 | 2812 | { Write_Byte(0x80,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2813 | Write_Byte(0xFF,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2814 | Write_Byte(0x00,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2815 | Write_Byte(0x91,StopVariable); |
sepp_nepp | 11:c6f95a42d4d7 | 2816 | Write_Byte(0x00,0x01); |
sepp_nepp | 11:c6f95a42d4d7 | 2817 | Write_Byte(0xFF,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2818 | Write_Byte(0x80,0x00); |
sepp_nepp | 11:c6f95a42d4d7 | 2819 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2820 | return Abyte; |
nikapov | 0:a1a69d32f310 | 2821 | } |
nikapov | 0:a1a69d32f310 | 2822 | |
sepp_nepp | 11:c6f95a42d4d7 | 2823 | void VL53L0X::Wait_Measurement_Ready() |
sepp_nepp | 11:c6f95a42d4d7 | 2824 | { uint32_t loop_nb = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2825 | |
sepp_nepp | 11:c6f95a42d4d7 | 2826 | // Wait until it finished, or loopo count reached = avoids deadlock |
sepp_nepp | 11:c6f95a42d4d7 | 2827 | while ( !Get_Measurement_Ready() & (ErrState == VL53L0X_OK) ) |
sepp_nepp | 11:c6f95a42d4d7 | 2828 | { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) |
sepp_nepp | 11:c6f95a42d4d7 | 2829 | { ErrState = VL53L0X_ERROR_TIME_OUT;} |
sepp_nepp | 11:c6f95a42d4d7 | 2830 | else { Polling_delay(); } |
sepp_nepp | 11:c6f95a42d4d7 | 2831 | } // while ends |
nikapov | 0:a1a69d32f310 | 2832 | } |
nikapov | 0:a1a69d32f310 | 2833 | |
sepp_nepp | 11:c6f95a42d4d7 | 2834 | void VL53L0X::Wait_Stop_Completed() |
sepp_nepp | 11:c6f95a42d4d7 | 2835 | { uint32_t loop_nb = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2836 | |
sepp_nepp | 11:c6f95a42d4d7 | 2837 | // Wait until Stop_Completed, or loopo count reached = avoids deadlock |
sepp_nepp | 11:c6f95a42d4d7 | 2838 | while ( (ErrState == VL53L0X_OK) & !Get_Stop_Completed() ) |
sepp_nepp | 11:c6f95a42d4d7 | 2839 | { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) |
sepp_nepp | 11:c6f95a42d4d7 | 2840 | { ErrState = VL53L0X_ERROR_TIME_OUT;} |
sepp_nepp | 11:c6f95a42d4d7 | 2841 | else { Polling_delay(); } |
sepp_nepp | 11:c6f95a42d4d7 | 2842 | } // while ends |
nikapov | 0:a1a69d32f310 | 2843 | } |
nikapov | 0:a1a69d32f310 | 2844 | |
sepp_nepp | 11:c6f95a42d4d7 | 2845 | void VL53L0X::Range_meas_int_continuous_mode(void (*fptr)(void)) |
sepp_nepp | 11:c6f95a42d4d7 | 2846 | { Stop_Measurement(); // it is safer to do this while sensor is stopped |
sepp_nepp | 11:c6f95a42d4d7 | 2847 | |
sepp_nepp | 11:c6f95a42d4d7 | 2848 | Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, |
sepp_nepp | 11:c6f95a42d4d7 | 2849 | GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH); |
sepp_nepp | 11:c6f95a42d4d7 | 2850 | if (ErrState==VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 2851 | Attach_interrupt_measure_detection_irq(fptr); |
sepp_nepp | 11:c6f95a42d4d7 | 2852 | Enable_interrupt_measure_detection_irq(); } |
sepp_nepp | 11:c6f95a42d4d7 | 2853 | |
sepp_nepp | 11:c6f95a42d4d7 | 2854 | Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS); |
sepp_nepp | 11:c6f95a42d4d7 | 2855 | // NB: return value was previously only passed to logging macro,but did not get passed back |
sepp_nepp | 11:c6f95a42d4d7 | 2856 | |
sepp_nepp | 11:c6f95a42d4d7 | 2857 | if (ErrState==VL53L0X_OK) { Range_start_continuous_mode(); } |
nikapov | 0:a1a69d32f310 | 2858 | } |
nikapov | 0:a1a69d32f310 | 2859 | |
sepp_nepp | 11:c6f95a42d4d7 | 2860 | |
sepp_nepp | 11:c6f95a42d4d7 | 2861 | VL53L0X_Error VL53L0X::Start_Measurement(TOperatingMode operating_mode, void (*fptr)(void)) |
sepp_nepp | 11:c6f95a42d4d7 | 2862 | { uint8_t VhvSettings; |
nikapov | 0:a1a69d32f310 | 2863 | uint8_t PhaseCal; |
sepp_nepp | 7:41cbc431e1f4 | 2864 | // *** from mass market cube expansion v1.1,ranging with satellites. |
sepp_nepp | 7:41cbc431e1f4 | 2865 | // default settings,for normal range. |
sepp_nepp | 11:c6f95a42d4d7 | 2866 | TFP1616 signalLimit = (TFP1616)(0.25 * 65536); |
sepp_nepp | 11:c6f95a42d4d7 | 2867 | TFP1616 sigmaLimit = (TFP1616)(18 * 65536); |
nikapov | 0:a1a69d32f310 | 2868 | uint32_t timingBudget = 33000; |
nikapov | 0:a1a69d32f310 | 2869 | uint8_t preRangeVcselPeriod = 14; |
nikapov | 0:a1a69d32f310 | 2870 | uint8_t finalRangeVcselPeriod = 10; |
nikapov | 0:a1a69d32f310 | 2871 | |
sepp_nepp | 11:c6f95a42d4d7 | 2872 | switch (operating_mode) { |
sepp_nepp | 11:c6f95a42d4d7 | 2873 | case op_INT: |
sepp_nepp | 11:c6f95a42d4d7 | 2874 | if (_gpio1Int == NULL) { ErrState=1; return ErrState; } |
sepp_nepp | 11:c6f95a42d4d7 | 2875 | Stop_Measurement(); // it is safer to do this while sensor is stopped |
sepp_nepp | 11:c6f95a42d4d7 | 2876 | |
sepp_nepp | 11:c6f95a42d4d7 | 2877 | Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, |
sepp_nepp | 11:c6f95a42d4d7 | 2878 | GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH); |
sepp_nepp | 11:c6f95a42d4d7 | 2879 | |
sepp_nepp | 11:c6f95a42d4d7 | 2880 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2881 | { Attach_interrupt_measure_detection_irq(fptr); |
sepp_nepp | 11:c6f95a42d4d7 | 2882 | Enable_interrupt_measure_detection_irq(); } |
sepp_nepp | 11:c6f95a42d4d7 | 2883 | |
sepp_nepp | 11:c6f95a42d4d7 | 2884 | Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS); |
sepp_nepp | 11:c6f95a42d4d7 | 2885 | // NB: return value was previously only passed to logging macro, but did not get passed back |
sepp_nepp | 11:c6f95a42d4d7 | 2886 | |
sepp_nepp | 11:c6f95a42d4d7 | 2887 | // Setup in continuous ranging mode |
sepp_nepp | 11:c6f95a42d4d7 | 2888 | Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); |
sepp_nepp | 11:c6f95a42d4d7 | 2889 | Start_Measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2890 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2891 | |
sepp_nepp | 11:c6f95a42d4d7 | 2892 | case op_single_shot_poll: |
sepp_nepp | 11:c6f95a42d4d7 | 2893 | // singelshot,polled ranging; no need to do this when we use VL53L0X_PerformSingleRangingMeasurement |
sepp_nepp | 11:c6f95a42d4d7 | 2894 | Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode |
nikapov | 0:a1a69d32f310 | 2895 | |
nikapov | 0:a1a69d32f310 | 2896 | // Enable/Disable Sigma and Signal check |
sepp_nepp | 11:c6f95a42d4d7 | 2897 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2898 | { Set_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,1); } |
sepp_nepp | 11:c6f95a42d4d7 | 2899 | |
sepp_nepp | 11:c6f95a42d4d7 | 2900 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2901 | { Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); } |
sepp_nepp | 11:c6f95a42d4d7 | 2902 | |
sepp_nepp | 11:c6f95a42d4d7 | 2903 | |
sepp_nepp | 11:c6f95a42d4d7 | 2904 | /* Ranging configuration */ |
sepp_nepp | 11:c6f95a42d4d7 | 2905 | // *** from mass market cube expansion v1.1,ranging with satellites. |
sepp_nepp | 11:c6f95a42d4d7 | 2906 | // switch(rangingConfig) { |
sepp_nepp | 11:c6f95a42d4d7 | 2907 | // case LONG_RANGE: |
sepp_nepp | 11:c6f95a42d4d7 | 2908 | signalLimit = (TFP1616)(0.1 * 65536); |
sepp_nepp | 11:c6f95a42d4d7 | 2909 | sigmaLimit = (TFP1616)(60 * 65536); |
nikapov | 0:a1a69d32f310 | 2910 | timingBudget = 33000; |
nikapov | 0:a1a69d32f310 | 2911 | preRangeVcselPeriod = 18; |
nikapov | 0:a1a69d32f310 | 2912 | finalRangeVcselPeriod = 14; |
nikapov | 0:a1a69d32f310 | 2913 | /* break; |
nikapov | 0:a1a69d32f310 | 2914 | case HIGH_ACCURACY: |
sepp_nepp | 11:c6f95a42d4d7 | 2915 | signalLimit = (TFP1616)(0.25*65536); |
sepp_nepp | 11:c6f95a42d4d7 | 2916 | sigmaLimit = (TFP1616)(18*65536); |
nikapov | 0:a1a69d32f310 | 2917 | timingBudget = 200000; |
nikapov | 0:a1a69d32f310 | 2918 | preRangeVcselPeriod = 14; |
nikapov | 0:a1a69d32f310 | 2919 | finalRangeVcselPeriod = 10; |
nikapov | 0:a1a69d32f310 | 2920 | break; |
nikapov | 0:a1a69d32f310 | 2921 | case HIGH_SPEED: |
sepp_nepp | 11:c6f95a42d4d7 | 2922 | signalLimit = (TFP1616)(0.25*65536); |
sepp_nepp | 11:c6f95a42d4d7 | 2923 | sigmaLimit = (TFP1616)(32*65536); |
nikapov | 0:a1a69d32f310 | 2924 | timingBudget = 20000; |
nikapov | 0:a1a69d32f310 | 2925 | preRangeVcselPeriod = 14; |
nikapov | 0:a1a69d32f310 | 2926 | finalRangeVcselPeriod = 10; |
nikapov | 0:a1a69d32f310 | 2927 | break; |
nikapov | 0:a1a69d32f310 | 2928 | default: |
nikapov | 0:a1a69d32f310 | 2929 | debug_printf("Not Supported"); |
nikapov | 0:a1a69d32f310 | 2930 | } |
nikapov | 0:a1a69d32f310 | 2931 | */ |
nikapov | 0:a1a69d32f310 | 2932 | |
sepp_nepp | 11:c6f95a42d4d7 | 2933 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2934 | { Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,signalLimit);} |
sepp_nepp | 11:c6f95a42d4d7 | 2935 | |
sepp_nepp | 11:c6f95a42d4d7 | 2936 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2937 | { Set_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,sigmaLimit);} |
sepp_nepp | 11:c6f95a42d4d7 | 2938 | |
sepp_nepp | 11:c6f95a42d4d7 | 2939 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2940 | { Set_Measure_Time_Budget_us(timingBudget);} |
sepp_nepp | 11:c6f95a42d4d7 | 2941 | |
sepp_nepp | 11:c6f95a42d4d7 | 2942 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2943 | { Set_vcsel_PPeriod(VL53L0X_VCSEL_PRE_RANGE,preRangeVcselPeriod); } |
sepp_nepp | 11:c6f95a42d4d7 | 2944 | |
sepp_nepp | 11:c6f95a42d4d7 | 2945 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2946 | { Set_vcsel_PPeriod(VL53L0X_VCSEL_FINAL_RANGE,finalRangeVcselPeriod);} |
sepp_nepp | 11:c6f95a42d4d7 | 2947 | |
sepp_nepp | 11:c6f95a42d4d7 | 2948 | if (ErrState == VL53L0X_OK) |
sepp_nepp | 11:c6f95a42d4d7 | 2949 | { Perf_Ref_calibration(&VhvSettings,&PhaseCal,1); } |
sepp_nepp | 11:c6f95a42d4d7 | 2950 | break; |
sepp_nepp | 11:c6f95a42d4d7 | 2951 | case op_poll: // Setup in continuous ranging mode |
sepp_nepp | 11:c6f95a42d4d7 | 2952 | Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); |
sepp_nepp | 11:c6f95a42d4d7 | 2953 | Start_Measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2954 | } // switch |
sepp_nepp | 11:c6f95a42d4d7 | 2955 | return ErrState; |
sepp_nepp | 11:c6f95a42d4d7 | 2956 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2957 | |
sepp_nepp | 11:c6f95a42d4d7 | 2958 | VL53L0X_Error VL53L0X::Stop_Measurement(TOperatingMode operating_mode) |
sepp_nepp | 11:c6f95a42d4d7 | 2959 | { if ((ErrState == VL53L0X_OK) & |
sepp_nepp | 11:c6f95a42d4d7 | 2960 | (operating_mode == op_INT || operating_mode == op_poll) ) |
sepp_nepp | 11:c6f95a42d4d7 | 2961 | { // only stop if in one of the continuous modes !!!! |
sepp_nepp | 11:c6f95a42d4d7 | 2962 | Stop_Measurement(); |
sepp_nepp | 11:c6f95a42d4d7 | 2963 | Wait_Stop_Completed(); |
sepp_nepp | 11:c6f95a42d4d7 | 2964 | Clear_interrupt_mask( REG_SYSINT_GPIO_NEW_SAMPLE_READY); |
sepp_nepp | 11:c6f95a42d4d7 | 2965 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2966 | return ErrState; |
sepp_nepp | 11:c6f95a42d4d7 | 2967 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2968 | |
sepp_nepp | 11:c6f95a42d4d7 | 2969 | TRangeResults VL53L0X::Handle_irq(TOperatingMode operating_mode) |
sepp_nepp | 11:c6f95a42d4d7 | 2970 | { TRangeResults RangeResults; |
sepp_nepp | 11:c6f95a42d4d7 | 2971 | RangeResults = Get_Measurement(operating_mode); |
sepp_nepp | 11:c6f95a42d4d7 | 2972 | Enable_interrupt_measure_detection_irq(); |
sepp_nepp | 11:c6f95a42d4d7 | 2973 | return RangeResults; |
sepp_nepp | 11:c6f95a42d4d7 | 2974 | } |
sepp_nepp | 11:c6f95a42d4d7 | 2975 | |
sepp_nepp | 11:c6f95a42d4d7 | 2976 | /****************** Private device functions *************************/ |
sepp_nepp | 11:c6f95a42d4d7 | 2977 | |
sepp_nepp | 11:c6f95a42d4d7 | 2978 | void VL53L0X::Wait_read_strobe() |
sepp_nepp | 11:c6f95a42d4d7 | 2979 | { uint32_t loop_nb = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 2980 | |
sepp_nepp | 11:c6f95a42d4d7 | 2981 | Write_Byte(0x83,0x00); // set strobe register to 0 |
sepp_nepp | 11:c6f95a42d4d7 | 2982 | |
sepp_nepp | 11:c6f95a42d4d7 | 2983 | /* polling while no error, no strobe, and not reached max number of loop to avoid deadlock*/ |
sepp_nepp | 11:c6f95a42d4d7 | 2984 | while ((ErrState == VL53L0X_OK) && (Read_Byte(0x83) == 0x00) ) |
sepp_nepp | 11:c6f95a42d4d7 | 2985 | { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) |
sepp_nepp | 11:c6f95a42d4d7 | 2986 | { ErrState = VL53L0X_ERROR_TIME_OUT; } } |
sepp_nepp | 11:c6f95a42d4d7 | 2987 | |
sepp_nepp | 11:c6f95a42d4d7 | 2988 | Write_Byte(0x83,0x01); // set strobe register back to 1 'manually' |
nikapov | 0:a1a69d32f310 | 2989 | } |
nikapov | 0:a1a69d32f310 | 2990 | |
nikapov | 0:a1a69d32f310 | 2991 | /******************************************************************************/ |
sepp_nepp | 11:c6f95a42d4d7 | 2992 | /****************** Small Service and wrapper functions **********************/ |
sepp_nepp | 11:c6f95a42d4d7 | 2993 | /******************************************************************************/ |
sepp_nepp | 11:c6f95a42d4d7 | 2994 | uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10,REF_ARRAY_SPAD_5, |
sepp_nepp | 11:c6f95a42d4d7 | 2995 | REF_ARRAY_SPAD_0,REF_ARRAY_SPAD_5 }; |
sepp_nepp | 11:c6f95a42d4d7 | 2996 | |
sepp_nepp | 11:c6f95a42d4d7 | 2997 | |
sepp_nepp | 11:c6f95a42d4d7 | 2998 | uint32_t VL53L0X::Decode_timeout(uint16_t encoded_timeout) |
sepp_nepp | 11:c6f95a42d4d7 | 2999 | { /*Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 */ |
sepp_nepp | 11:c6f95a42d4d7 | 3000 | return ((uint32_t) (encoded_timeout & 0x00FF) |
sepp_nepp | 11:c6f95a42d4d7 | 3001 | << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1; |
sepp_nepp | 11:c6f95a42d4d7 | 3002 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3003 | |
sepp_nepp | 11:c6f95a42d4d7 | 3004 | uint8_t VL53L0X::Is_ApertureSPAD(uint32_t SPAD_index) |
sepp_nepp | 11:c6f95a42d4d7 | 3005 | { /* This function reports if a given SPAD index is an aperture SPAD by |
sepp_nepp | 11:c6f95a42d4d7 | 3006 | * deriving the quadrant = SPAD_index >> 6. */ |
sepp_nepp | 11:c6f95a42d4d7 | 3007 | if (refArrayQuadrants[SPAD_index >> 6] == REF_ARRAY_SPAD_0) |
sepp_nepp | 11:c6f95a42d4d7 | 3008 | { return 0; } else { return 1; } |
sepp_nepp | 11:c6f95a42d4d7 | 3009 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3010 | |
sepp_nepp | 11:c6f95a42d4d7 | 3011 | /******************************************************************************/ |
sepp_nepp | 11:c6f95a42d4d7 | 3012 | /****************** Write and read functions from I2C *************************/ |
sepp_nepp | 11:c6f95a42d4d7 | 3013 | /******************************************************************************/ |
sepp_nepp | 11:c6f95a42d4d7 | 3014 | |
sepp_nepp | 12:aa177f0e4c10 | 3015 | |
sepp_nepp | 12:aa177f0e4c10 | 3016 | uint32_t VL53L0X::Get_NVM_DWord(uint8_t NVM_Address) |
sepp_nepp | 12:aa177f0e4c10 | 3017 | { Write_Byte(0x94,NVM_Address); |
sepp_nepp | 12:aa177f0e4c10 | 3018 | Wait_read_strobe(); |
sepp_nepp | 12:aa177f0e4c10 | 3019 | return Read_DWord(0x90); |
sepp_nepp | 12:aa177f0e4c10 | 3020 | } |
sepp_nepp | 12:aa177f0e4c10 | 3021 | uint16_t VL53L0X::Get_NVM_Word(uint8_t NVM_Address) |
sepp_nepp | 12:aa177f0e4c10 | 3022 | { Write_Byte(0x94,NVM_Address); |
sepp_nepp | 12:aa177f0e4c10 | 3023 | Wait_read_strobe(); |
sepp_nepp | 12:aa177f0e4c10 | 3024 | return Read_Word(0x90); |
sepp_nepp | 12:aa177f0e4c10 | 3025 | } |
sepp_nepp | 12:aa177f0e4c10 | 3026 | uint8_t VL53L0X::Get_NVM_Byte(uint8_t NVM_Address) |
sepp_nepp | 12:aa177f0e4c10 | 3027 | { Write_Byte(0x94,NVM_Address); |
sepp_nepp | 12:aa177f0e4c10 | 3028 | Wait_read_strobe(); |
sepp_nepp | 12:aa177f0e4c10 | 3029 | return Read_Byte(0x90); |
sepp_nepp | 12:aa177f0e4c10 | 3030 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3031 | void VL53L0X::Write_Byte(uint8_t index, uint8_t data) |
sepp_nepp | 11:c6f95a42d4d7 | 3032 | { I2c_Write(index,&data,1); } |
sepp_nepp | 11:c6f95a42d4d7 | 3033 | |
sepp_nepp | 11:c6f95a42d4d7 | 3034 | void VL53L0X::Write_Word(uint8_t index,uint16_t data) |
sepp_nepp | 11:c6f95a42d4d7 | 3035 | { uint8_t buffer[2]; |
sepp_nepp | 11:c6f95a42d4d7 | 3036 | buffer[0] = data >> 8; |
sepp_nepp | 11:c6f95a42d4d7 | 3037 | buffer[1] = data & 0x00FF; |
sepp_nepp | 11:c6f95a42d4d7 | 3038 | I2c_Write(index,(uint8_t *)buffer,2); |
sepp_nepp | 11:c6f95a42d4d7 | 3039 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3040 | |
sepp_nepp | 11:c6f95a42d4d7 | 3041 | void VL53L0X::Write_DWord(uint8_t index, uint32_t data) |
sepp_nepp | 11:c6f95a42d4d7 | 3042 | { uint8_t buffer[4]; |
sepp_nepp | 11:c6f95a42d4d7 | 3043 | buffer[0] = (data >> 24) & 0xFF; |
sepp_nepp | 11:c6f95a42d4d7 | 3044 | buffer[1] = (data >> 16) & 0xFF; |
sepp_nepp | 11:c6f95a42d4d7 | 3045 | buffer[2] = (data >> 8) & 0xFF; |
sepp_nepp | 11:c6f95a42d4d7 | 3046 | buffer[3] = (data >> 0) & 0xFF; |
sepp_nepp | 11:c6f95a42d4d7 | 3047 | I2c_Write(index,(uint8_t *)buffer,4); |
sepp_nepp | 11:c6f95a42d4d7 | 3048 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3049 | |
sepp_nepp | 11:c6f95a42d4d7 | 3050 | uint8_t VL53L0X::Read_Byte(uint8_t index) |
sepp_nepp | 11:c6f95a42d4d7 | 3051 | { uint8_t result; |
sepp_nepp | 11:c6f95a42d4d7 | 3052 | I2c_Read(index,&result,1); |
sepp_nepp | 11:c6f95a42d4d7 | 3053 | return result; |
sepp_nepp | 11:c6f95a42d4d7 | 3054 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3055 | |
sepp_nepp | 11:c6f95a42d4d7 | 3056 | uint16_t VL53L0X::Read_Word(uint8_t index) |
sepp_nepp | 11:c6f95a42d4d7 | 3057 | { uint8_t buffer[2] = {0,0}; |
sepp_nepp | 11:c6f95a42d4d7 | 3058 | I2c_Read(index, &buffer[0], 2); |
sepp_nepp | 11:c6f95a42d4d7 | 3059 | return (buffer[0] << 8) + buffer[1]; |
sepp_nepp | 11:c6f95a42d4d7 | 3060 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3061 | |
sepp_nepp | 11:c6f95a42d4d7 | 3062 | uint32_t VL53L0X::Read_DWord(uint8_t index) |
sepp_nepp | 11:c6f95a42d4d7 | 3063 | { uint8_t buffer[4] = {0,0,0,0}; |
sepp_nepp | 11:c6f95a42d4d7 | 3064 | I2c_Read(index,buffer,4); |
sepp_nepp | 11:c6f95a42d4d7 | 3065 | return (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; |
sepp_nepp | 11:c6f95a42d4d7 | 3066 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3067 | |
sepp_nepp | 11:c6f95a42d4d7 | 3068 | void VL53L0X::Register_BitMask(uint8_t index,uint8_t and_mask,uint8_t or_mask) |
sepp_nepp | 11:c6f95a42d4d7 | 3069 | { uint8_t buffer = 0; |
sepp_nepp | 11:c6f95a42d4d7 | 3070 | /* read data direct onto buffer */ |
sepp_nepp | 11:c6f95a42d4d7 | 3071 | I2c_Read(index,&buffer,1); |
sepp_nepp | 11:c6f95a42d4d7 | 3072 | if (ErrState==VL53L0X_OK) { |
sepp_nepp | 11:c6f95a42d4d7 | 3073 | buffer = (buffer & and_mask) | or_mask; |
sepp_nepp | 11:c6f95a42d4d7 | 3074 | I2c_Write(index,&buffer,(uint8_t)1); |
sepp_nepp | 11:c6f95a42d4d7 | 3075 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3076 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3077 | |
sepp_nepp | 11:c6f95a42d4d7 | 3078 | /** |
sepp_nepp | 11:c6f95a42d4d7 | 3079 | * @brief Writes a buffer towards the I2C peripheral device. |
sepp_nepp | 11:c6f95a42d4d7 | 3080 | * @param RegisterAddr specifies the internal address register |
sepp_nepp | 11:c6f95a42d4d7 | 3081 | * @param p_data pointer to the byte-array data to send |
sepp_nepp | 11:c6f95a42d4d7 | 3082 | * where to start writing to (must be correctly masked). |
sepp_nepp | 11:c6f95a42d4d7 | 3083 | * @param NumByteToWrite number of bytes to be written. |
sepp_nepp | 11:c6f95a42d4d7 | 3084 | * @note On some devices if NumByteToWrite is greater |
sepp_nepp | 11:c6f95a42d4d7 | 3085 | * than one, the RegisterAddr must be masked correctly! */ |
sepp_nepp | 11:c6f95a42d4d7 | 3086 | void VL53L0X::I2c_Write( uint8_t RegisterAddr, uint8_t *p_data, |
sepp_nepp | 11:c6f95a42d4d7 | 3087 | uint16_t NumByteToWrite ) |
sepp_nepp | 11:c6f95a42d4d7 | 3088 | { int ret; |
sepp_nepp | 11:c6f95a42d4d7 | 3089 | uint8_t tmp[TEMP_BUF_SIZE]; |
sepp_nepp | 11:c6f95a42d4d7 | 3090 | |
sepp_nepp | 11:c6f95a42d4d7 | 3091 | if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 3092 | |
sepp_nepp | 11:c6f95a42d4d7 | 3093 | if (NumByteToWrite >= TEMP_BUF_SIZE) |
sepp_nepp | 11:c6f95a42d4d7 | 3094 | {ErrState = VL53L0X_ERROR_I2C_BUF_OVERFLOW; return; }; |
sepp_nepp | 11:c6f95a42d4d7 | 3095 | |
sepp_nepp | 11:c6f95a42d4d7 | 3096 | /* First, send device address. Then, send data and terminate with STOP condition */ |
sepp_nepp | 11:c6f95a42d4d7 | 3097 | tmp[0] = RegisterAddr; |
sepp_nepp | 11:c6f95a42d4d7 | 3098 | memcpy(tmp+1, p_data, NumByteToWrite); |
sepp_nepp | 11:c6f95a42d4d7 | 3099 | ret = _dev_i2c->write(I2cDevAddr, (const char*)tmp, NumByteToWrite+1, false); |
sepp_nepp | 11:c6f95a42d4d7 | 3100 | |
sepp_nepp | 11:c6f95a42d4d7 | 3101 | if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; } |
sepp_nepp | 11:c6f95a42d4d7 | 3102 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3103 | |
sepp_nepp | 11:c6f95a42d4d7 | 3104 | /**@brief Reads a buffer from the I2C peripheral device. |
sepp_nepp | 11:c6f95a42d4d7 | 3105 | * @param pBuffer pointer to the byte-array to read data in to |
sepp_nepp | 11:c6f95a42d4d7 | 3106 | * @param I2cDevAddr specifies the peripheral device slave address. |
sepp_nepp | 11:c6f95a42d4d7 | 3107 | * @param RegisterAddr specifies the internal address register |
sepp_nepp | 11:c6f95a42d4d7 | 3108 | * where to start reading from (must be correctly masked). |
sepp_nepp | 11:c6f95a42d4d7 | 3109 | * @param NumByteToRead number of bytes to be read, maximum VL53L0X_MAX_I2C_XFER_SIZE |
sepp_nepp | 11:c6f95a42d4d7 | 3110 | * @note On some devices if NumByteToWrite is greater |
sepp_nepp | 11:c6f95a42d4d7 | 3111 | * than one, the RegisterAddr must be masked correctly! |
sepp_nepp | 11:c6f95a42d4d7 | 3112 | */ |
sepp_nepp | 11:c6f95a42d4d7 | 3113 | void VL53L0X::I2c_Read(uint8_t RegisterAddr, uint8_t *p_data, |
sepp_nepp | 11:c6f95a42d4d7 | 3114 | uint16_t NumByteToRead) |
sepp_nepp | 11:c6f95a42d4d7 | 3115 | { int ret; |
sepp_nepp | 11:c6f95a42d4d7 | 3116 | if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State, return value undefined!!!! |
sepp_nepp | 11:c6f95a42d4d7 | 3117 | |
sepp_nepp | 11:c6f95a42d4d7 | 3118 | /* Send device address, without STOP condition */ |
sepp_nepp | 11:c6f95a42d4d7 | 3119 | ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true); |
sepp_nepp | 11:c6f95a42d4d7 | 3120 | if(!ret) /* Read data, with STOP condition */ |
sepp_nepp | 11:c6f95a42d4d7 | 3121 | { ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); } |
sepp_nepp | 11:c6f95a42d4d7 | 3122 | |
sepp_nepp | 11:c6f95a42d4d7 | 3123 | if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; } |
sepp_nepp | 11:c6f95a42d4d7 | 3124 | } |
sepp_nepp | 11:c6f95a42d4d7 | 3125 | |
sepp_nepp | 11:c6f95a42d4d7 | 3126 | /******************************************************************************/ |