Jim Carver
Workshop example
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Embed:
(wiki syntax)
Show/hide line numbers
VL53L0X.cpp
00001 /** 00002 ****************************************************************************** 00003 * @file VL53L0X_class.cpp 00004 * @author IMG 00005 * @version V0.0.1 00006 * @date 28-June-2016 00007 * @brief Implementation file for the VL53L0X driver class 00008 ****************************************************************************** 00009 * @attention 00010 * 00011 * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> 00012 * 00013 * Redistribution and use in source and binary forms, with or without modification, 00014 * are permitted provided that the following conditions are met: 00015 * 1. Redistributions of source code must retain the above copyright notice, 00016 * this list of conditions and the following disclaimer. 00017 * 2. Redistributions in binary form must reproduce the above copyright notice, 00018 * this list of conditions and the following disclaimer in the documentation 00019 * and/or other materials provided with the distribution. 00020 * 3. Neither the name of STMicroelectronics nor the names of its contributors 00021 * may be used to endorse or promote products derived from this software 00022 * without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00025 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00026 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00027 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00028 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00029 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00030 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00032 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00033 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 ****************************************************************************** 00036 */ 00037 00038 /* Includes */ 00039 #include <stdlib.h> 00040 00041 #include "VL53L0X.h" 00042 00043 //#include "VL53L0X_api_core.h" 00044 //#include "VL53L0X_api_calibration.h" 00045 //#include "VL53L0X_api_strings.h" 00046 #include "VL53L0X_interrupt_threshold_settings.h" 00047 #include "VL53L0X_tuning.h" 00048 #include "VL53L0X_types.h" 00049 00050 00051 /****************** define for i2c configuration *******************************/ 00052 00053 #define TEMP_BUF_SIZE 64 00054 00055 /** Maximum buffer size to be used in i2c */ 00056 #define VL53L0X_MAX_I2C_XFER_SIZE 64 /* Maximum buffer size to be used in i2c */ 00057 #define VL53L0X_I2C_USER_VAR /* none but could be for a flag var to get/pass to mutex interruptible return flags and try again */ 00058 00059 00060 #define LOG_FUNCTION_START(fmt, ...) \ 00061 _LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__) 00062 #define LOG_FUNCTION_END(status, ...) \ 00063 _LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__) 00064 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 00065 _LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__) 00066 00067 #ifdef VL53L0X_LOG_ENABLE 00068 #define trace_print(level, ...) trace_print_module_function(TRACE_MODULE_API, \ 00069 level, TRACE_FUNCTION_NONE, ##__VA_ARGS__) 00070 #endif 00071 00072 #define REF_ARRAY_SPAD_0 0 00073 #define REF_ARRAY_SPAD_5 5 00074 #define REF_ARRAY_SPAD_10 10 00075 00076 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5, 00077 REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 00078 }; 00079 00080 00081 00082 00083 VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(VL53L0X_DEV dev) 00084 { 00085 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00086 uint8_t strobe; 00087 uint32_t loop_nb; 00088 LOG_FUNCTION_START(""); 00089 00090 status |= VL53L0X_write_byte(dev, 0x83, 0x00); 00091 00092 /* polling 00093 * use timeout to avoid deadlock*/ 00094 if (status == VL53L0X_ERROR_NONE) { 00095 loop_nb = 0; 00096 do { 00097 status = VL53L0X_read_byte(dev, 0x83, &strobe); 00098 if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE) { 00099 break; 00100 } 00101 00102 loop_nb = loop_nb + 1; 00103 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); 00104 00105 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 00106 status = VL53L0X_ERROR_TIME_OUT; 00107 } 00108 } 00109 00110 status |= VL53L0X_write_byte(dev, 0x83, 0x01); 00111 00112 LOG_FUNCTION_END(status); 00113 return status; 00114 } 00115 00116 VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(VL53L0X_DEV dev, uint8_t option) 00117 { 00118 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00119 uint8_t byte; 00120 uint32_t tmp_dword; 00121 uint8_t module_id; 00122 uint8_t revision; 00123 uint8_t reference_spad_count = 0; 00124 uint8_t reference_spad_type = 0; 00125 uint32_t part_uid_upper = 0; 00126 uint32_t part_uid_lower = 0; 00127 uint32_t offset_fixed1104_mm = 0; 00128 int16_t offset_micro_meters = 0; 00129 uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4; 00130 uint32_t dist_meas_fixed1104_400_mm = 0; 00131 uint32_t signal_rate_meas_fixed1104_400_mm = 0; 00132 char product_id[19]; 00133 char *product_id_tmp; 00134 uint8_t read_data_from_device_done; 00135 FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0; 00136 uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE]; 00137 int i; 00138 00139 00140 LOG_FUNCTION_START(""); 00141 00142 read_data_from_device_done = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 00143 ReadDataFromDeviceDone); 00144 00145 /* This access is done only once after that a GetDeviceInfo or 00146 * datainit is done*/ 00147 if (read_data_from_device_done != 7) { 00148 00149 status |= VL53L0X_write_byte(dev, 0x80, 0x01); 00150 status |= VL53L0X_write_byte(dev, 0xFF, 0x01); 00151 status |= VL53L0X_write_byte(dev, 0x00, 0x00); 00152 00153 status |= VL53L0X_write_byte(dev, 0xFF, 0x06); 00154 status |= VL53L0X_read_byte(dev, 0x83, &byte); 00155 status |= VL53L0X_write_byte(dev, 0x83, byte | 4); 00156 status |= VL53L0X_write_byte(dev, 0xFF, 0x07); 00157 status |= VL53L0X_write_byte(dev, 0x81, 0x01); 00158 00159 status |= VL53L0X_polling_delay(dev); 00160 00161 status |= VL53L0X_write_byte(dev, 0x80, 0x01); 00162 00163 if (((option & 1) == 1) && 00164 ((read_data_from_device_done & 1) == 0)) { 00165 status |= VL53L0X_write_byte(dev, 0x94, 0x6b); 00166 status |= VL53L0X_device_read_strobe(dev); 00167 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00168 00169 reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f); 00170 reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01); 00171 00172 status |= VL53L0X_write_byte(dev, 0x94, 0x24); 00173 status |= VL53L0X_device_read_strobe(dev); 00174 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00175 00176 00177 nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24) 00178 & 0xff); 00179 nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16) 00180 & 0xff); 00181 nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8) 00182 & 0xff); 00183 nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff); 00184 00185 status |= VL53L0X_write_byte(dev, 0x94, 0x25); 00186 status |= VL53L0X_device_read_strobe(dev); 00187 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00188 00189 nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24) 00190 & 0xff); 00191 nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16) 00192 & 0xff); 00193 } 00194 00195 if (((option & 2) == 2) && 00196 ((read_data_from_device_done & 2) == 0)) { 00197 00198 status |= VL53L0X_write_byte(dev, 0x94, 0x02); 00199 status |= VL53L0X_device_read_strobe(dev); 00200 status |= VL53L0X_read_byte(dev, 0x90, &module_id); 00201 00202 status |= VL53L0X_write_byte(dev, 0x94, 0x7B); 00203 status |= VL53L0X_device_read_strobe(dev); 00204 status |= VL53L0X_read_byte(dev, 0x90, &revision); 00205 00206 status |= VL53L0X_write_byte(dev, 0x94, 0x77); 00207 status |= VL53L0X_device_read_strobe(dev); 00208 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00209 00210 product_id[0] = (char)((tmp_dword >> 25) & 0x07f); 00211 product_id[1] = (char)((tmp_dword >> 18) & 0x07f); 00212 product_id[2] = (char)((tmp_dword >> 11) & 0x07f); 00213 product_id[3] = (char)((tmp_dword >> 4) & 0x07f); 00214 00215 byte = (uint8_t)((tmp_dword & 0x00f) << 3); 00216 00217 status |= VL53L0X_write_byte(dev, 0x94, 0x78); 00218 status |= VL53L0X_device_read_strobe(dev); 00219 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00220 00221 product_id[4] = (char)(byte + 00222 ((tmp_dword >> 29) & 0x07f)); 00223 product_id[5] = (char)((tmp_dword >> 22) & 0x07f); 00224 product_id[6] = (char)((tmp_dword >> 15) & 0x07f); 00225 product_id[7] = (char)((tmp_dword >> 8) & 0x07f); 00226 product_id[8] = (char)((tmp_dword >> 1) & 0x07f); 00227 00228 byte = (uint8_t)((tmp_dword & 0x001) << 6); 00229 00230 status |= VL53L0X_write_byte(dev, 0x94, 0x79); 00231 00232 status |= VL53L0X_device_read_strobe(dev); 00233 00234 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00235 00236 product_id[9] = (char)(byte + 00237 ((tmp_dword >> 26) & 0x07f)); 00238 product_id[10] = (char)((tmp_dword >> 19) & 0x07f); 00239 product_id[11] = (char)((tmp_dword >> 12) & 0x07f); 00240 product_id[12] = (char)((tmp_dword >> 5) & 0x07f); 00241 00242 byte = (uint8_t)((tmp_dword & 0x01f) << 2); 00243 00244 status |= VL53L0X_write_byte(dev, 0x94, 0x7A); 00245 00246 status |= VL53L0X_device_read_strobe(dev); 00247 00248 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00249 00250 product_id[13] = (char)(byte + 00251 ((tmp_dword >> 30) & 0x07f)); 00252 product_id[14] = (char)((tmp_dword >> 23) & 0x07f); 00253 product_id[15] = (char)((tmp_dword >> 16) & 0x07f); 00254 product_id[16] = (char)((tmp_dword >> 9) & 0x07f); 00255 product_id[17] = (char)((tmp_dword >> 2) & 0x07f); 00256 product_id[18] = '\0'; 00257 00258 } 00259 00260 if (((option & 4) == 4) && 00261 ((read_data_from_device_done & 4) == 0)) { 00262 00263 status |= VL53L0X_write_byte(dev, 0x94, 0x7B); 00264 status |= VL53L0X_device_read_strobe(dev); 00265 status |= VL53L0X_read_dword(dev, 0x90, &part_uid_upper); 00266 00267 status |= VL53L0X_write_byte(dev, 0x94, 0x7C); 00268 status |= VL53L0X_device_read_strobe(dev); 00269 status |= VL53L0X_read_dword(dev, 0x90, &part_uid_lower); 00270 00271 status |= VL53L0X_write_byte(dev, 0x94, 0x73); 00272 status |= VL53L0X_device_read_strobe(dev); 00273 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00274 00275 signal_rate_meas_fixed1104_400_mm = (tmp_dword & 00276 0x0000000ff) << 8; 00277 00278 status |= VL53L0X_write_byte(dev, 0x94, 0x74); 00279 status |= VL53L0X_device_read_strobe(dev); 00280 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00281 00282 signal_rate_meas_fixed1104_400_mm |= ((tmp_dword & 00283 0xff000000) >> 24); 00284 00285 status |= VL53L0X_write_byte(dev, 0x94, 0x75); 00286 status |= VL53L0X_device_read_strobe(dev); 00287 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00288 00289 dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff) 00290 << 8; 00291 00292 status |= VL53L0X_write_byte(dev, 0x94, 0x76); 00293 status |= VL53L0X_device_read_strobe(dev); 00294 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword); 00295 00296 dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000) 00297 >> 24); 00298 } 00299 00300 status |= VL53L0X_write_byte(dev, 0x81, 0x00); 00301 status |= VL53L0X_write_byte(dev, 0xFF, 0x06); 00302 status |= VL53L0X_read_byte(dev, 0x83, &byte); 00303 status |= VL53L0X_write_byte(dev, 0x83, byte & 0xfb); 00304 status |= VL53L0X_write_byte(dev, 0xFF, 0x01); 00305 status |= VL53L0X_write_byte(dev, 0x00, 0x01); 00306 00307 status |= VL53L0X_write_byte(dev, 0xFF, 0x00); 00308 status |= VL53L0X_write_byte(dev, 0x80, 0x00); 00309 } 00310 00311 if ((status == VL53L0X_ERROR_NONE) && 00312 (read_data_from_device_done != 7)) { 00313 /* Assign to variable if status is ok */ 00314 if (((option & 1) == 1) && 00315 ((read_data_from_device_done & 1) == 0)) { 00316 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00317 ReferenceSpadCount, reference_spad_count); 00318 00319 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00320 ReferenceSpadType, reference_spad_type); 00321 00322 for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) { 00323 dev->Data .SpadData .RefGoodSpadMap [i] = 00324 nvm_ref_good_spad_map[i]; 00325 } 00326 } 00327 00328 if (((option & 2) == 2) && 00329 ((read_data_from_device_done & 2) == 0)) { 00330 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00331 ModuleId, module_id); 00332 00333 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00334 Revision, revision); 00335 00336 product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 00337 ProductId); 00338 VL53L0X_COPYSTRING(product_id_tmp, product_id); 00339 00340 } 00341 00342 if (((option & 4) == 4) && 00343 ((read_data_from_device_done & 4) == 0)) { 00344 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00345 PartUIDUpper, part_uid_upper); 00346 00347 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00348 PartUIDLower, part_uid_lower); 00349 00350 signal_rate_meas_fixed400_mm_fix = 00351 VL53L0X_FIXPOINT97TOFIXPOINT1616( 00352 signal_rate_meas_fixed1104_400_mm); 00353 00354 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 00355 SignalRateMeasFixed400mm, 00356 signal_rate_meas_fixed400_mm_fix); 00357 00358 offset_micro_meters = 0; 00359 if (dist_meas_fixed1104_400_mm != 0) { 00360 offset_fixed1104_mm = 00361 dist_meas_fixed1104_400_mm - 00362 dist_meas_tgt_fixed1104_mm; 00363 offset_micro_meters = (offset_fixed1104_mm 00364 * 1000) >> 4; 00365 offset_micro_meters *= -1; 00366 } 00367 00368 PALDevDataSet(dev, 00369 Part2PartOffsetAdjustmentNVMMicroMeter, 00370 offset_micro_meters); 00371 } 00372 byte = (uint8_t)(read_data_from_device_done | option); 00373 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone, 00374 byte); 00375 } 00376 00377 LOG_FUNCTION_END(status); 00378 return status; 00379 } 00380 00381 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV dev, 00382 int32_t *p_offset_calibration_data_micro_meter) 00383 { 00384 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00385 uint16_t range_offset_register; 00386 int16_t c_max_offset = 2047; 00387 int16_t c_offset_range = 4096; 00388 00389 /* Note that offset has 10.2 format */ 00390 00391 status = VL53L0X_read_word(dev, 00392 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, 00393 &range_offset_register); 00394 00395 if (status == VL53L0X_ERROR_NONE) { 00396 range_offset_register = (range_offset_register & 0x0fff); 00397 00398 /* Apply 12 bit 2's compliment conversion */ 00399 if (range_offset_register > c_max_offset) { 00400 *p_offset_calibration_data_micro_meter = 00401 (int16_t)(range_offset_register - c_offset_range) 00402 * 250; 00403 } else { 00404 *p_offset_calibration_data_micro_meter = 00405 (int16_t)range_offset_register * 250; 00406 } 00407 00408 } 00409 00410 return status; 00411 } 00412 00413 VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV dev, 00414 int32_t *p_offset_calibration_data_micro_meter) 00415 { 00416 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00417 LOG_FUNCTION_START(""); 00418 00419 status = wrapped_VL53L0X_get_offset_calibration_data_micro_meter(dev, 00420 p_offset_calibration_data_micro_meter); 00421 00422 LOG_FUNCTION_END(status); 00423 return status; 00424 } 00425 00426 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV dev, 00427 int32_t offset_calibration_data_micro_meter) 00428 { 00429 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00430 int32_t c_max_offset_micro_meter = 511000; 00431 int32_t c_min_offset_micro_meter = -512000; 00432 int16_t c_offset_range = 4096; 00433 uint32_t encoded_offset_val; 00434 00435 LOG_FUNCTION_START(""); 00436 00437 if (offset_calibration_data_micro_meter > c_max_offset_micro_meter) { 00438 offset_calibration_data_micro_meter = c_max_offset_micro_meter; 00439 } else { 00440 if (offset_calibration_data_micro_meter < c_min_offset_micro_meter) { 00441 offset_calibration_data_micro_meter = c_min_offset_micro_meter; 00442 } 00443 } 00444 00445 /* The offset register is 10.2 format and units are mm 00446 * therefore conversion is applied by a division of 00447 * 250. 00448 */ 00449 if (offset_calibration_data_micro_meter >= 0) { 00450 encoded_offset_val = 00451 offset_calibration_data_micro_meter / 250; 00452 } else { 00453 encoded_offset_val = 00454 c_offset_range + 00455 offset_calibration_data_micro_meter / 250; 00456 } 00457 00458 status = VL53L0X_write_word(dev, 00459 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, 00460 encoded_offset_val); 00461 00462 LOG_FUNCTION_END(status); 00463 return status; 00464 } 00465 00466 VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV dev, 00467 int32_t offset_calibration_data_micro_meter) 00468 { 00469 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00470 LOG_FUNCTION_START(""); 00471 00472 status = wrapped_VL53L0X_set_offset_calibration_data_micro_meter(dev, 00473 offset_calibration_data_micro_meter); 00474 00475 LOG_FUNCTION_END(status); 00476 return status; 00477 } 00478 00479 VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(VL53L0X_DEV dev) 00480 { 00481 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00482 int32_t corrected_offset_micro_meters; 00483 int32_t current_offset_micro_meters; 00484 00485 /* if we run on this function we can read all the NVM info 00486 * used by the API */ 00487 status = VL53L0X_get_info_from_device(dev, 7); 00488 00489 /* Read back current device offset */ 00490 if (status == VL53L0X_ERROR_NONE) { 00491 status = VL53L0X_get_offset_calibration_data_micro_meter(dev, 00492 ¤t_offset_micro_meters); 00493 } 00494 00495 /* Apply Offset Adjustment derived from 400mm measurements */ 00496 if (status == VL53L0X_ERROR_NONE) { 00497 00498 /* Store initial device offset */ 00499 PALDevDataSet(dev, Part2PartOffsetNVMMicroMeter, 00500 current_offset_micro_meters); 00501 00502 corrected_offset_micro_meters = current_offset_micro_meters + 00503 (int32_t)PALDevDataGet(dev, 00504 Part2PartOffsetAdjustmentNVMMicroMeter); 00505 00506 status = VL53L0X_set_offset_calibration_data_micro_meter(dev, 00507 corrected_offset_micro_meters); 00508 00509 /* store current, adjusted offset */ 00510 if (status == VL53L0X_ERROR_NONE) { 00511 VL53L0X_SETPARAMETERFIELD(dev, RangeOffsetMicroMeters, 00512 corrected_offset_micro_meters); 00513 } 00514 } 00515 00516 return status; 00517 } 00518 00519 VL53L0X_Error VL53L0X::VL53L0X_get_device_mode(VL53L0X_DEV dev, 00520 VL53L0X_DeviceModes *p_device_mode) 00521 { 00522 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00523 LOG_FUNCTION_START(""); 00524 00525 VL53L0X_GETPARAMETERFIELD(dev, DeviceMode, *p_device_mode); 00526 00527 LOG_FUNCTION_END(status); 00528 return status; 00529 } 00530 00531 VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_milli_seconds(VL53L0X_DEV dev, 00532 uint32_t *p_inter_measurement_period_milli_seconds) 00533 { 00534 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00535 uint16_t osc_calibrate_val; 00536 uint32_t im_period_milli_seconds; 00537 00538 LOG_FUNCTION_START(""); 00539 00540 status = VL53L0X_read_word(dev, VL53L0X_REG_OSC_CALIBRATE_VAL, 00541 &osc_calibrate_val); 00542 00543 if (status == VL53L0X_ERROR_NONE) { 00544 status = VL53L0X_read_dword(dev, 00545 VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 00546 &im_period_milli_seconds); 00547 } 00548 00549 if (status == VL53L0X_ERROR_NONE) { 00550 if (osc_calibrate_val != 0) { 00551 *p_inter_measurement_period_milli_seconds = 00552 im_period_milli_seconds / osc_calibrate_val; 00553 } 00554 VL53L0X_SETPARAMETERFIELD(dev, 00555 InterMeasurementPeriodMilliSeconds, 00556 *p_inter_measurement_period_milli_seconds); 00557 } 00558 00559 LOG_FUNCTION_END(status); 00560 return status; 00561 } 00562 00563 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_mega_cps(VL53L0X_DEV dev, 00564 FixPoint1616_t *p_xtalk_compensation_rate_mega_cps) 00565 { 00566 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00567 uint16_t value; 00568 FixPoint1616_t temp_fix1616; 00569 00570 LOG_FUNCTION_START(""); 00571 00572 status = VL53L0X_read_word(dev, 00573 VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&value); 00574 if (status == VL53L0X_ERROR_NONE) { 00575 if (value == 0) { 00576 /* the Xtalk is disabled return value from memory */ 00577 VL53L0X_GETPARAMETERFIELD(dev, 00578 XTalkCompensationRateMegaCps, temp_fix1616); 00579 *p_xtalk_compensation_rate_mega_cps = temp_fix1616; 00580 VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationEnable, 00581 0); 00582 } else { 00583 temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value); 00584 *p_xtalk_compensation_rate_mega_cps = temp_fix1616; 00585 VL53L0X_SETPARAMETERFIELD(dev, 00586 XTalkCompensationRateMegaCps, temp_fix1616); 00587 VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationEnable, 00588 1); 00589 } 00590 } 00591 00592 LOG_FUNCTION_END(status); 00593 return status; 00594 } 00595 00596 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value(VL53L0X_DEV dev, uint16_t limit_check_id, 00597 FixPoint1616_t *p_limit_check_value) 00598 { 00599 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00600 uint8_t enable_zero_value = 0; 00601 uint16_t temp16; 00602 FixPoint1616_t temp_fix1616; 00603 00604 LOG_FUNCTION_START(""); 00605 00606 switch (limit_check_id) { 00607 00608 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: 00609 /* internal computation: */ 00610 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue, 00611 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp_fix1616); 00612 enable_zero_value = 0; 00613 break; 00614 00615 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: 00616 status = VL53L0X_read_word(dev, 00617 VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 00618 &temp16); 00619 if (status == VL53L0X_ERROR_NONE) { 00620 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16); 00621 } 00622 00623 00624 enable_zero_value = 1; 00625 break; 00626 00627 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: 00628 /* internal computation: */ 00629 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue, 00630 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp_fix1616); 00631 enable_zero_value = 0; 00632 break; 00633 00634 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: 00635 /* internal computation: */ 00636 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue, 00637 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, temp_fix1616); 00638 enable_zero_value = 0; 00639 break; 00640 00641 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: 00642 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: 00643 status = VL53L0X_read_word(dev, 00644 VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, 00645 &temp16); 00646 if (status == VL53L0X_ERROR_NONE) { 00647 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16); 00648 } 00649 00650 00651 enable_zero_value = 0; 00652 break; 00653 00654 default: 00655 status = VL53L0X_ERROR_INVALID_PARAMS; 00656 00657 } 00658 00659 if (status == VL53L0X_ERROR_NONE) { 00660 00661 if (enable_zero_value == 1) { 00662 00663 if (temp_fix1616 == 0) { 00664 /* disabled: return value from memory */ 00665 VL53L0X_GETARRAYPARAMETERFIELD(dev, 00666 LimitChecksValue, limit_check_id, 00667 temp_fix1616); 00668 *p_limit_check_value = temp_fix1616; 00669 VL53L0X_SETARRAYPARAMETERFIELD(dev, 00670 LimitChecksEnable, limit_check_id, 0); 00671 } else { 00672 *p_limit_check_value = temp_fix1616; 00673 VL53L0X_SETARRAYPARAMETERFIELD(dev, 00674 LimitChecksValue, limit_check_id, 00675 temp_fix1616); 00676 VL53L0X_SETARRAYPARAMETERFIELD(dev, 00677 LimitChecksEnable, limit_check_id, 1); 00678 } 00679 } else { 00680 *p_limit_check_value = temp_fix1616; 00681 } 00682 } 00683 00684 LOG_FUNCTION_END(status); 00685 return status; 00686 00687 } 00688 00689 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id, 00690 uint8_t *p_limit_check_enable) 00691 { 00692 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00693 uint8_t temp8; 00694 00695 LOG_FUNCTION_START(""); 00696 00697 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { 00698 status = VL53L0X_ERROR_INVALID_PARAMS; 00699 *p_limit_check_enable = 0; 00700 } else { 00701 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksEnable, 00702 limit_check_id, temp8); 00703 *p_limit_check_enable = temp8; 00704 } 00705 00706 LOG_FUNCTION_END(status); 00707 return status; 00708 } 00709 00710 VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(VL53L0X_DEV dev, 00711 uint8_t *p_wrap_around_check_enable) 00712 { 00713 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00714 uint8_t data; 00715 00716 LOG_FUNCTION_START(""); 00717 00718 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data); 00719 if (status == VL53L0X_ERROR_NONE) { 00720 PALDevDataSet(dev, SequenceConfig, data); 00721 if (data & (0x01 << 7)) { 00722 *p_wrap_around_check_enable = 0x01; 00723 } else { 00724 *p_wrap_around_check_enable = 0x00; 00725 } 00726 } 00727 if (status == VL53L0X_ERROR_NONE) { 00728 VL53L0X_SETPARAMETERFIELD(dev, WrapAroundCheckEnable, 00729 *p_wrap_around_check_enable); 00730 } 00731 00732 LOG_FUNCTION_END(status); 00733 return status; 00734 } 00735 00736 VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_DEV dev, 00737 VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config, 00738 uint8_t *p_sequence_step_enabled) 00739 { 00740 VL53L0X_Error Status = VL53L0X_ERROR_NONE; 00741 *p_sequence_step_enabled = 0; 00742 LOG_FUNCTION_START(""); 00743 00744 switch (sequence_step_id) { 00745 case VL53L0X_SEQUENCESTEP_TCC: 00746 *p_sequence_step_enabled = (sequence_config & 0x10) >> 4; 00747 break; 00748 case VL53L0X_SEQUENCESTEP_DSS: 00749 *p_sequence_step_enabled = (sequence_config & 0x08) >> 3; 00750 break; 00751 case VL53L0X_SEQUENCESTEP_MSRC: 00752 *p_sequence_step_enabled = (sequence_config & 0x04) >> 2; 00753 break; 00754 case VL53L0X_SEQUENCESTEP_PRE_RANGE: 00755 *p_sequence_step_enabled = (sequence_config & 0x40) >> 6; 00756 break; 00757 case VL53L0X_SEQUENCESTEP_FINAL_RANGE: 00758 *p_sequence_step_enabled = (sequence_config & 0x80) >> 7; 00759 break; 00760 default: 00761 Status = VL53L0X_ERROR_INVALID_PARAMS; 00762 } 00763 00764 LOG_FUNCTION_END(status); 00765 return Status; 00766 } 00767 00768 VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_DEV dev, 00769 VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps) 00770 { 00771 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00772 uint8_t sequence_config = 0; 00773 LOG_FUNCTION_START(""); 00774 00775 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 00776 &sequence_config); 00777 00778 if (status == VL53L0X_ERROR_NONE) { 00779 status = sequence_step_enabled(dev, 00780 VL53L0X_SEQUENCESTEP_TCC, sequence_config, 00781 &p_scheduler_sequence_steps->TccOn); 00782 } 00783 if (status == VL53L0X_ERROR_NONE) { 00784 status = sequence_step_enabled(dev, 00785 VL53L0X_SEQUENCESTEP_DSS, sequence_config, 00786 &p_scheduler_sequence_steps->DssOn); 00787 } 00788 if (status == VL53L0X_ERROR_NONE) { 00789 status = sequence_step_enabled(dev, 00790 VL53L0X_SEQUENCESTEP_MSRC, sequence_config, 00791 &p_scheduler_sequence_steps->MsrcOn); 00792 } 00793 if (status == VL53L0X_ERROR_NONE) { 00794 status = sequence_step_enabled(dev, 00795 VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config, 00796 &p_scheduler_sequence_steps->PreRangeOn); 00797 } 00798 if (status == VL53L0X_ERROR_NONE) { 00799 status = sequence_step_enabled(dev, 00800 VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config, 00801 &p_scheduler_sequence_steps->FinalRangeOn); 00802 } 00803 00804 LOG_FUNCTION_END(status); 00805 return status; 00806 } 00807 00808 uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg) 00809 { 00810 /*! 00811 * Converts the encoded VCSEL period register value into the real 00812 * period in PLL clocks 00813 */ 00814 00815 uint8_t vcsel_period_pclks = 0; 00816 00817 vcsel_period_pclks = (vcsel_period_reg + 1) << 1; 00818 00819 return vcsel_period_pclks; 00820 } 00821 00822 uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks) 00823 { 00824 /*! 00825 * Converts the encoded VCSEL period register value into the real period 00826 * in PLL clocks 00827 */ 00828 00829 uint8_t vcsel_period_reg = 0; 00830 00831 vcsel_period_reg = (vcsel_period_pclks >> 1) - 1; 00832 00833 return vcsel_period_reg; 00834 } 00835 00836 00837 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV dev, 00838 VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk) 00839 { 00840 VL53L0X_Error status = VL53L0X_ERROR_NONE; 00841 uint8_t vcsel_period_reg; 00842 uint8_t min_pre_vcsel_period_pclk = 12; 00843 uint8_t max_pre_vcsel_period_pclk = 18; 00844 uint8_t min_final_vcsel_period_pclk = 8; 00845 uint8_t max_final_vcsel_period_pclk = 14; 00846 uint32_t measurement_timing_budget_micro_seconds; 00847 uint32_t final_range_timeout_micro_seconds; 00848 uint32_t pre_range_timeout_micro_seconds; 00849 uint32_t msrc_timeout_micro_seconds; 00850 uint8_t phase_cal_int = 0; 00851 00852 /* Check if valid clock period requested */ 00853 00854 if ((vcsel_pulse_period_pclk % 2) != 0) { 00855 /* Value must be an even number */ 00856 status = VL53L0X_ERROR_INVALID_PARAMS; 00857 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE && 00858 (vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk || 00859 vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) { 00860 status = VL53L0X_ERROR_INVALID_PARAMS; 00861 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE && 00862 (vcsel_pulse_period_pclk < min_final_vcsel_period_pclk || 00863 vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) { 00864 00865 status = VL53L0X_ERROR_INVALID_PARAMS; 00866 } 00867 00868 /* Apply specific settings for the requested clock period */ 00869 00870 if (status != VL53L0X_ERROR_NONE) { 00871 return status; 00872 } 00873 00874 00875 if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) { 00876 00877 /* Set phase check limits */ 00878 if (vcsel_pulse_period_pclk == 12) { 00879 00880 status = VL53L0X_write_byte(dev, 00881 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 00882 0x18); 00883 status = VL53L0X_write_byte(dev, 00884 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 00885 0x08); 00886 } else if (vcsel_pulse_period_pclk == 14) { 00887 00888 status = VL53L0X_write_byte(dev, 00889 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 00890 0x30); 00891 status = VL53L0X_write_byte(dev, 00892 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 00893 0x08); 00894 } else if (vcsel_pulse_period_pclk == 16) { 00895 00896 status = VL53L0X_write_byte(dev, 00897 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 00898 0x40); 00899 status = VL53L0X_write_byte(dev, 00900 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 00901 0x08); 00902 } else if (vcsel_pulse_period_pclk == 18) { 00903 00904 status = VL53L0X_write_byte(dev, 00905 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 00906 0x50); 00907 status = VL53L0X_write_byte(dev, 00908 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 00909 0x08); 00910 } 00911 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) { 00912 00913 if (vcsel_pulse_period_pclk == 8) { 00914 00915 status = VL53L0X_write_byte(dev, 00916 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 00917 0x10); 00918 status = VL53L0X_write_byte(dev, 00919 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 00920 0x08); 00921 00922 status |= VL53L0X_write_byte(dev, 00923 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); 00924 status |= VL53L0X_write_byte(dev, 00925 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); 00926 00927 status |= VL53L0X_write_byte(dev, 0xff, 0x01); 00928 status |= VL53L0X_write_byte(dev, 00929 VL53L0X_REG_ALGO_PHASECAL_LIM, 00930 0x30); 00931 status |= VL53L0X_write_byte(dev, 0xff, 0x00); 00932 } else if (vcsel_pulse_period_pclk == 10) { 00933 00934 status = VL53L0X_write_byte(dev, 00935 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 00936 0x28); 00937 status = VL53L0X_write_byte(dev, 00938 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 00939 0x08); 00940 00941 status |= VL53L0X_write_byte(dev, 00942 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); 00943 status |= VL53L0X_write_byte(dev, 00944 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); 00945 00946 status |= VL53L0X_write_byte(dev, 0xff, 0x01); 00947 status |= VL53L0X_write_byte(dev, 00948 VL53L0X_REG_ALGO_PHASECAL_LIM, 00949 0x20); 00950 status |= VL53L0X_write_byte(dev, 0xff, 0x00); 00951 } else if (vcsel_pulse_period_pclk == 12) { 00952 00953 status = VL53L0X_write_byte(dev, 00954 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 00955 0x38); 00956 status = VL53L0X_write_byte(dev, 00957 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 00958 0x08); 00959 00960 status |= VL53L0X_write_byte(dev, 00961 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); 00962 status |= VL53L0X_write_byte(dev, 00963 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); 00964 00965 status |= VL53L0X_write_byte(dev, 0xff, 0x01); 00966 status |= VL53L0X_write_byte(dev, 00967 VL53L0X_REG_ALGO_PHASECAL_LIM, 00968 0x20); 00969 status |= VL53L0X_write_byte(dev, 0xff, 0x00); 00970 } else if (vcsel_pulse_period_pclk == 14) { 00971 00972 status = VL53L0X_write_byte(dev, 00973 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 00974 0x048); 00975 status = VL53L0X_write_byte(dev, 00976 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 00977 0x08); 00978 00979 status |= VL53L0X_write_byte(dev, 00980 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); 00981 status |= VL53L0X_write_byte(dev, 00982 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); 00983 00984 status |= VL53L0X_write_byte(dev, 0xff, 0x01); 00985 status |= VL53L0X_write_byte(dev, 00986 VL53L0X_REG_ALGO_PHASECAL_LIM, 00987 0x20); 00988 status |= VL53L0X_write_byte(dev, 0xff, 0x00); 00989 } 00990 } 00991 00992 00993 /* Re-calculate and apply timeouts, in macro periods */ 00994 00995 if (status == VL53L0X_ERROR_NONE) { 00996 vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t) 00997 vcsel_pulse_period_pclk); 00998 00999 /* When the VCSEL period for the pre or final range is changed, 01000 * the corresponding timeout must be read from the device using 01001 * the current VCSEL period, then the new VCSEL period can be 01002 * applied. The timeout then must be written back to the device 01003 * using the new VCSEL period. 01004 * 01005 * For the MSRC timeout, the same applies - this timeout being 01006 * dependant on the pre-range vcsel period. 01007 */ 01008 switch (vcsel_period_type) { 01009 case VL53L0X_VCSEL_PERIOD_PRE_RANGE: 01010 status = get_sequence_step_timeout(dev, 01011 VL53L0X_SEQUENCESTEP_PRE_RANGE, 01012 &pre_range_timeout_micro_seconds); 01013 01014 if (status == VL53L0X_ERROR_NONE) 01015 status = get_sequence_step_timeout(dev, 01016 VL53L0X_SEQUENCESTEP_MSRC, 01017 &msrc_timeout_micro_seconds); 01018 01019 if (status == VL53L0X_ERROR_NONE) 01020 status = VL53L0X_write_byte(dev, 01021 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, 01022 vcsel_period_reg); 01023 01024 01025 if (status == VL53L0X_ERROR_NONE) 01026 status = set_sequence_step_timeout(dev, 01027 VL53L0X_SEQUENCESTEP_PRE_RANGE, 01028 pre_range_timeout_micro_seconds); 01029 01030 01031 if (status == VL53L0X_ERROR_NONE) 01032 status = set_sequence_step_timeout(dev, 01033 VL53L0X_SEQUENCESTEP_MSRC, 01034 msrc_timeout_micro_seconds); 01035 01036 VL53L0X_SETDEVICESPECIFICPARAMETER( 01037 dev, 01038 PreRangeVcselPulsePeriod, 01039 vcsel_pulse_period_pclk); 01040 break; 01041 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: 01042 status = get_sequence_step_timeout(dev, 01043 VL53L0X_SEQUENCESTEP_FINAL_RANGE, 01044 &final_range_timeout_micro_seconds); 01045 01046 if (status == VL53L0X_ERROR_NONE) 01047 status = VL53L0X_write_byte(dev, 01048 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, 01049 vcsel_period_reg); 01050 01051 01052 if (status == VL53L0X_ERROR_NONE) 01053 status = set_sequence_step_timeout(dev, 01054 VL53L0X_SEQUENCESTEP_FINAL_RANGE, 01055 final_range_timeout_micro_seconds); 01056 01057 VL53L0X_SETDEVICESPECIFICPARAMETER( 01058 dev, 01059 FinalRangeVcselPulsePeriod, 01060 vcsel_pulse_period_pclk); 01061 break; 01062 default: 01063 status = VL53L0X_ERROR_INVALID_PARAMS; 01064 } 01065 } 01066 01067 /* Finally, the timing budget must be re-applied */ 01068 if (status == VL53L0X_ERROR_NONE) { 01069 VL53L0X_GETPARAMETERFIELD(dev, 01070 MeasurementTimingBudgetMicroSeconds, 01071 measurement_timing_budget_micro_seconds); 01072 01073 status = VL53L0X_set_measurement_timing_budget_micro_seconds(dev, 01074 measurement_timing_budget_micro_seconds); 01075 } 01076 01077 /* Perform the phase calibration. This is needed after changing on 01078 * vcsel period. 01079 * get_data_enable = 0, restore_config = 1 */ 01080 if (status == VL53L0X_ERROR_NONE) 01081 status = VL53L0X_perform_phase_calibration( 01082 dev, &phase_cal_int, 0, 1); 01083 01084 return status; 01085 } 01086 01087 VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV dev, 01088 VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period) 01089 { 01090 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01091 LOG_FUNCTION_START(""); 01092 01093 status = wrapped_VL53L0X_set_vcsel_pulse_period(dev, vcsel_period_type, 01094 vcsel_pulse_period); 01095 01096 LOG_FUNCTION_END(status); 01097 return status; 01098 } 01099 01100 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV dev, 01101 VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk) 01102 { 01103 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01104 uint8_t vcsel_period_reg; 01105 01106 switch (vcsel_period_type) { 01107 case VL53L0X_VCSEL_PERIOD_PRE_RANGE: 01108 status = VL53L0X_read_byte(dev, 01109 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, 01110 &vcsel_period_reg); 01111 break; 01112 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: 01113 status = VL53L0X_read_byte(dev, 01114 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, 01115 &vcsel_period_reg); 01116 break; 01117 default: 01118 status = VL53L0X_ERROR_INVALID_PARAMS; 01119 } 01120 01121 if (status == VL53L0X_ERROR_NONE) 01122 *p_vcsel_pulse_period_pclk = 01123 VL53L0X_decode_vcsel_period(vcsel_period_reg); 01124 01125 return status; 01126 } 01127 01128 VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV dev, 01129 VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk) 01130 { 01131 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01132 LOG_FUNCTION_START(""); 01133 01134 status = wrapped_VL53L0X_get_vcsel_pulse_period(dev, vcsel_period_type, 01135 p_vcsel_pulse_period_pclk); 01136 01137 LOG_FUNCTION_END(status); 01138 return status; 01139 } 01140 01141 uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout) 01142 { 01143 /*! 01144 * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 01145 */ 01146 01147 uint32_t timeout_macro_clks = 0; 01148 01149 timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF) 01150 << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1; 01151 01152 return timeout_macro_clks; 01153 } 01154 01155 uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(VL53L0X_DEV dev, uint8_t vcsel_period_pclks) 01156 { 01157 uint64_t pll_period_ps; 01158 uint32_t macro_period_vclks; 01159 uint32_t macro_period_ps; 01160 01161 LOG_FUNCTION_START(""); 01162 01163 /* The above calculation will produce rounding errors, 01164 therefore set fixed value 01165 */ 01166 pll_period_ps = 1655; 01167 01168 macro_period_vclks = 2304; 01169 macro_period_ps = (uint32_t)(macro_period_vclks 01170 * vcsel_period_pclks * pll_period_ps); 01171 01172 LOG_FUNCTION_END(""); 01173 return macro_period_ps; 01174 } 01175 01176 /* To convert register value into us */ 01177 uint32_t VL53L0X::VL53L0X_calc_timeout_us(VL53L0X_DEV dev, 01178 uint16_t timeout_period_mclks, 01179 uint8_t vcsel_period_pclks) 01180 { 01181 uint32_t macro_period_ps; 01182 uint32_t macro_period_ns; 01183 uint32_t actual_timeout_period_us = 0; 01184 01185 macro_period_ps = VL53L0X_calc_macro_period_ps(dev, vcsel_period_pclks); 01186 macro_period_ns = (macro_period_ps + 500) / 1000; 01187 01188 actual_timeout_period_us = 01189 ((timeout_period_mclks * macro_period_ns) + 500) / 1000; 01190 01191 return actual_timeout_period_us; 01192 } 01193 01194 VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV dev, 01195 VL53L0X_SequenceStepId sequence_step_id, 01196 uint32_t *p_time_out_micro_secs) 01197 { 01198 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01199 uint8_t current_vcsel_pulse_period_p_clk; 01200 uint8_t encoded_time_out_byte = 0; 01201 uint32_t timeout_micro_seconds = 0; 01202 uint16_t pre_range_encoded_time_out = 0; 01203 uint16_t msrc_time_out_m_clks; 01204 uint16_t pre_range_time_out_m_clks; 01205 uint16_t final_range_time_out_m_clks = 0; 01206 uint16_t final_range_encoded_time_out; 01207 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 01208 01209 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) || 01210 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) || 01211 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) { 01212 01213 status = VL53L0X_get_vcsel_pulse_period(dev, 01214 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01215 ¤t_vcsel_pulse_period_p_clk); 01216 if (status == VL53L0X_ERROR_NONE) { 01217 status = VL53L0X_read_byte(dev, 01218 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, 01219 &encoded_time_out_byte); 01220 } 01221 msrc_time_out_m_clks = VL53L0X_decode_timeout(encoded_time_out_byte); 01222 01223 timeout_micro_seconds = VL53L0X_calc_timeout_us(dev, 01224 msrc_time_out_m_clks, 01225 current_vcsel_pulse_period_p_clk); 01226 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { 01227 /* Retrieve PRE-RANGE VCSEL Period */ 01228 status = VL53L0X_get_vcsel_pulse_period(dev, 01229 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01230 ¤t_vcsel_pulse_period_p_clk); 01231 01232 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ 01233 if (status == VL53L0X_ERROR_NONE) { 01234 01235 /* Retrieve PRE-RANGE VCSEL Period */ 01236 status = VL53L0X_get_vcsel_pulse_period(dev, 01237 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01238 ¤t_vcsel_pulse_period_p_clk); 01239 01240 if (status == VL53L0X_ERROR_NONE) { 01241 status = VL53L0X_read_word(dev, 01242 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 01243 &pre_range_encoded_time_out); 01244 } 01245 01246 pre_range_time_out_m_clks = VL53L0X_decode_timeout( 01247 pre_range_encoded_time_out); 01248 01249 timeout_micro_seconds = VL53L0X_calc_timeout_us(dev, 01250 pre_range_time_out_m_clks, 01251 current_vcsel_pulse_period_p_clk); 01252 } 01253 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { 01254 01255 VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps); 01256 pre_range_time_out_m_clks = 0; 01257 01258 if (scheduler_sequence_steps.PreRangeOn) { 01259 /* Retrieve PRE-RANGE VCSEL Period */ 01260 status = VL53L0X_get_vcsel_pulse_period(dev, 01261 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01262 ¤t_vcsel_pulse_period_p_clk); 01263 01264 /* Retrieve PRE-RANGE Timeout in Macro periods 01265 * (MCLKS) */ 01266 if (status == VL53L0X_ERROR_NONE) { 01267 status = VL53L0X_read_word(dev, 01268 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 01269 &pre_range_encoded_time_out); 01270 pre_range_time_out_m_clks = VL53L0X_decode_timeout( 01271 pre_range_encoded_time_out); 01272 } 01273 } 01274 01275 if (status == VL53L0X_ERROR_NONE) { 01276 /* Retrieve FINAL-RANGE VCSEL Period */ 01277 status = VL53L0X_get_vcsel_pulse_period(dev, 01278 VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 01279 ¤t_vcsel_pulse_period_p_clk); 01280 } 01281 01282 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */ 01283 if (status == VL53L0X_ERROR_NONE) { 01284 status = VL53L0X_read_word(dev, 01285 VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 01286 &final_range_encoded_time_out); 01287 final_range_time_out_m_clks = VL53L0X_decode_timeout( 01288 final_range_encoded_time_out); 01289 } 01290 01291 final_range_time_out_m_clks -= pre_range_time_out_m_clks; 01292 timeout_micro_seconds = VL53L0X_calc_timeout_us(dev, 01293 final_range_time_out_m_clks, 01294 current_vcsel_pulse_period_p_clk); 01295 } 01296 01297 *p_time_out_micro_secs = timeout_micro_seconds; 01298 01299 return status; 01300 } 01301 01302 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev, 01303 uint32_t *p_measurement_timing_budget_micro_seconds) 01304 { 01305 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01306 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 01307 uint32_t final_range_timeout_micro_seconds; 01308 uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000; 01309 uint32_t start_overhead_micro_seconds = 1910; 01310 uint32_t end_overhead_micro_seconds = 960; 01311 uint32_t msrc_overhead_micro_seconds = 660; 01312 uint32_t tcc_overhead_micro_seconds = 590; 01313 uint32_t dss_overhead_micro_seconds = 690; 01314 uint32_t pre_range_overhead_micro_seconds = 660; 01315 uint32_t final_range_overhead_micro_seconds = 550; 01316 uint32_t pre_range_timeout_micro_seconds = 0; 01317 01318 LOG_FUNCTION_START(""); 01319 01320 /* Start and end overhead times always present */ 01321 *p_measurement_timing_budget_micro_seconds 01322 = start_overhead_micro_seconds + end_overhead_micro_seconds; 01323 01324 status = VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps); 01325 01326 if (status != VL53L0X_ERROR_NONE) { 01327 LOG_FUNCTION_END(status); 01328 return status; 01329 } 01330 01331 01332 if (scheduler_sequence_steps.TccOn || 01333 scheduler_sequence_steps.MsrcOn || 01334 scheduler_sequence_steps.DssOn) { 01335 01336 status = get_sequence_step_timeout(dev, 01337 VL53L0X_SEQUENCESTEP_MSRC, 01338 &msrc_dcc_tcc_timeout_micro_seconds); 01339 01340 if (status == VL53L0X_ERROR_NONE) { 01341 if (scheduler_sequence_steps.TccOn) { 01342 *p_measurement_timing_budget_micro_seconds += 01343 msrc_dcc_tcc_timeout_micro_seconds + 01344 tcc_overhead_micro_seconds; 01345 } 01346 01347 if (scheduler_sequence_steps.DssOn) { 01348 *p_measurement_timing_budget_micro_seconds += 01349 2 * (msrc_dcc_tcc_timeout_micro_seconds + 01350 dss_overhead_micro_seconds); 01351 } else if (scheduler_sequence_steps.MsrcOn) { 01352 *p_measurement_timing_budget_micro_seconds += 01353 msrc_dcc_tcc_timeout_micro_seconds + 01354 msrc_overhead_micro_seconds; 01355 } 01356 } 01357 } 01358 01359 if (status == VL53L0X_ERROR_NONE) { 01360 if (scheduler_sequence_steps.PreRangeOn) { 01361 status = get_sequence_step_timeout(dev, 01362 VL53L0X_SEQUENCESTEP_PRE_RANGE, 01363 &pre_range_timeout_micro_seconds); 01364 *p_measurement_timing_budget_micro_seconds += 01365 pre_range_timeout_micro_seconds + 01366 pre_range_overhead_micro_seconds; 01367 } 01368 } 01369 01370 if (status == VL53L0X_ERROR_NONE) { 01371 if (scheduler_sequence_steps.FinalRangeOn) { 01372 status = get_sequence_step_timeout(dev, 01373 VL53L0X_SEQUENCESTEP_FINAL_RANGE, 01374 &final_range_timeout_micro_seconds); 01375 *p_measurement_timing_budget_micro_seconds += 01376 (final_range_timeout_micro_seconds + 01377 final_range_overhead_micro_seconds); 01378 } 01379 } 01380 01381 if (status == VL53L0X_ERROR_NONE) { 01382 VL53L0X_SETPARAMETERFIELD(dev, 01383 MeasurementTimingBudgetMicroSeconds, 01384 *p_measurement_timing_budget_micro_seconds); 01385 } 01386 01387 LOG_FUNCTION_END(status); 01388 return status; 01389 } 01390 01391 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev, 01392 uint32_t *p_measurement_timing_budget_micro_seconds) 01393 { 01394 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01395 LOG_FUNCTION_START(""); 01396 01397 status = wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(dev, 01398 p_measurement_timing_budget_micro_seconds); 01399 01400 LOG_FUNCTION_END(status); 01401 return status; 01402 } 01403 01404 VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters(VL53L0X_DEV dev, 01405 VL53L0X_DeviceParameters_t *p_device_parameters) 01406 { 01407 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01408 int i; 01409 01410 LOG_FUNCTION_START(""); 01411 01412 status = VL53L0X_get_device_mode(dev, &(p_device_parameters->DeviceMode )); 01413 01414 if (status == VL53L0X_ERROR_NONE) 01415 status = VL53L0X_get_inter_measurement_period_milli_seconds(dev, 01416 &(p_device_parameters->InterMeasurementPeriodMilliSeconds )); 01417 01418 01419 if (status == VL53L0X_ERROR_NONE) { 01420 p_device_parameters->XTalkCompensationEnable = 0; 01421 } 01422 01423 if (status == VL53L0X_ERROR_NONE) 01424 status = VL53L0X_get_x_talk_compensation_rate_mega_cps(dev, 01425 &(p_device_parameters->XTalkCompensationRateMegaCps )); 01426 01427 01428 if (status == VL53L0X_ERROR_NONE) 01429 status = VL53L0X_get_offset_calibration_data_micro_meter(dev, 01430 &(p_device_parameters->RangeOffsetMicroMeters )); 01431 01432 01433 if (status == VL53L0X_ERROR_NONE) { 01434 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { 01435 /* get first the values, then the enables. 01436 * VL53L0X_GetLimitCheckValue will modify the enable 01437 * flags 01438 */ 01439 if (status == VL53L0X_ERROR_NONE) { 01440 status |= VL53L0X_get_limit_check_value(dev, i, 01441 &(p_device_parameters->LimitChecksValue [i])); 01442 } else { 01443 break; 01444 } 01445 if (status == VL53L0X_ERROR_NONE) { 01446 status |= VL53L0X_get_limit_check_enable(dev, i, 01447 &(p_device_parameters->LimitChecksEnable [i])); 01448 } else { 01449 break; 01450 } 01451 } 01452 } 01453 01454 if (status == VL53L0X_ERROR_NONE) { 01455 status = VL53L0X_get_wrap_around_check_enable(dev, 01456 &(p_device_parameters->WrapAroundCheckEnable )); 01457 } 01458 01459 /* Need to be done at the end as it uses VCSELPulsePeriod */ 01460 if (status == VL53L0X_ERROR_NONE) { 01461 status = VL53L0X_get_measurement_timing_budget_micro_seconds(dev, 01462 &(p_device_parameters->MeasurementTimingBudgetMicroSeconds )); 01463 } 01464 01465 LOG_FUNCTION_END(status); 01466 return status; 01467 } 01468 01469 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value(VL53L0X_DEV dev, uint16_t limit_check_id, 01470 FixPoint1616_t limit_check_value) 01471 { 01472 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01473 uint8_t temp8; 01474 01475 LOG_FUNCTION_START(""); 01476 01477 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksEnable, limit_check_id, 01478 temp8); 01479 01480 if (temp8 == 0) { /* disabled write only internal value */ 01481 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue, 01482 limit_check_id, limit_check_value); 01483 } else { 01484 01485 switch (limit_check_id) { 01486 01487 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: 01488 /* internal computation: */ 01489 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue, 01490 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 01491 limit_check_value); 01492 break; 01493 01494 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: 01495 01496 status = VL53L0X_write_word(dev, 01497 VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 01498 VL53L0X_FIXPOINT1616TOFIXPOINT97( 01499 limit_check_value)); 01500 01501 break; 01502 01503 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: 01504 01505 /* internal computation: */ 01506 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue, 01507 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 01508 limit_check_value); 01509 01510 break; 01511 01512 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: 01513 01514 /* internal computation: */ 01515 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue, 01516 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 01517 limit_check_value); 01518 01519 break; 01520 01521 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: 01522 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: 01523 01524 status = VL53L0X_write_word(dev, 01525 VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, 01526 VL53L0X_FIXPOINT1616TOFIXPOINT97( 01527 limit_check_value)); 01528 01529 break; 01530 01531 default: 01532 status = VL53L0X_ERROR_INVALID_PARAMS; 01533 01534 } 01535 01536 if (status == VL53L0X_ERROR_NONE) { 01537 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue, 01538 limit_check_id, limit_check_value); 01539 } 01540 } 01541 01542 LOG_FUNCTION_END(status); 01543 return status; 01544 } 01545 01546 VL53L0X_Error VL53L0X::VL53L0X_data_init(VL53L0X_DEV dev) 01547 { 01548 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01549 VL53L0X_DeviceParameters_t CurrentParameters; 01550 int i; 01551 uint8_t StopVariable; 01552 01553 LOG_FUNCTION_START(""); 01554 01555 /* by default the I2C is running at 1V8 if you want to change it you 01556 * need to include this define at compilation level. */ 01557 #ifdef USE_I2C_2V8 01558 Status = VL53L0X_UpdateByte(Dev, 01559 VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 01560 0xFE, 01561 0x01); 01562 #endif 01563 01564 /* Set I2C standard mode */ 01565 if (status == VL53L0X_ERROR_NONE) { 01566 status = VL53L0X_write_byte(dev, 0x88, 0x00); 01567 } 01568 01569 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone, 0); 01570 01571 #ifdef USE_IQC_STATION 01572 if (Status == VL53L0X_ERROR_NONE) { 01573 Status = VL53L0X_apply_offset_adjustment(Dev); 01574 } 01575 #endif 01576 01577 /* Default value is 1000 for Linearity Corrective Gain */ 01578 PALDevDataSet(dev, LinearityCorrectiveGain, 1000); 01579 01580 /* Dmax default Parameter */ 01581 PALDevDataSet(dev, DmaxCalRangeMilliMeter, 400); 01582 PALDevDataSet(dev, DmaxCalSignalRateRtnMegaCps, 01583 (FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/ 01584 01585 /* Set Default static parameters 01586 *set first temporary values 9.44MHz * 65536 = 618660 */ 01587 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz, 618660); 01588 01589 /* Set Default XTalkCompensationRateMegaCps to 0 */ 01590 VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps, 0); 01591 01592 /* Get default parameters */ 01593 status = VL53L0X_get_device_parameters(dev, &CurrentParameters); 01594 if (status == VL53L0X_ERROR_NONE) { 01595 /* initialize PAL values */ 01596 CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; 01597 CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED; 01598 PALDevDataSet(dev, CurrentParameters, CurrentParameters); 01599 } 01600 01601 /* Sigma estimator variable */ 01602 PALDevDataSet(dev, SigmaEstRefArray, 100); 01603 PALDevDataSet(dev, SigmaEstEffPulseWidth, 900); 01604 PALDevDataSet(dev, SigmaEstEffAmbWidth, 500); 01605 PALDevDataSet(dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */ 01606 01607 /* Use internal default settings */ 01608 PALDevDataSet(dev, UseInternalTuningSettings, 1); 01609 01610 status |= VL53L0X_write_byte(dev, 0x80, 0x01); 01611 status |= VL53L0X_write_byte(dev, 0xFF, 0x01); 01612 status |= VL53L0X_write_byte(dev, 0x00, 0x00); 01613 status |= VL53L0X_read_byte(dev, 0x91, &StopVariable); 01614 PALDevDataSet(dev, StopVariable, StopVariable); 01615 status |= VL53L0X_write_byte(dev, 0x00, 0x01); 01616 status |= VL53L0X_write_byte(dev, 0xFF, 0x00); 01617 status |= VL53L0X_write_byte(dev, 0x80, 0x00); 01618 01619 /* Enable all check */ 01620 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { 01621 if (status == VL53L0X_ERROR_NONE) { 01622 status |= VL53L0X_set_limit_check_enable(dev, i, 1); 01623 } else { 01624 break; 01625 } 01626 01627 } 01628 01629 /* Disable the following checks */ 01630 if (status == VL53L0X_ERROR_NONE) 01631 status = VL53L0X_set_limit_check_enable(dev, 01632 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0); 01633 01634 if (status == VL53L0X_ERROR_NONE) 01635 status = VL53L0X_set_limit_check_enable(dev, 01636 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0); 01637 01638 if (status == VL53L0X_ERROR_NONE) 01639 status = VL53L0X_set_limit_check_enable(dev, 01640 VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0); 01641 01642 if (status == VL53L0X_ERROR_NONE) 01643 status = VL53L0X_set_limit_check_enable(dev, 01644 VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0); 01645 01646 /* Limit default values */ 01647 if (status == VL53L0X_ERROR_NONE) { 01648 status = VL53L0X_set_limit_check_value(dev, 01649 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 01650 (FixPoint1616_t)(18 * 65536)); 01651 } 01652 if (status == VL53L0X_ERROR_NONE) { 01653 status = VL53L0X_set_limit_check_value(dev, 01654 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 01655 (FixPoint1616_t)(25 * 65536 / 100)); 01656 /* 0.25 * 65536 */ 01657 } 01658 01659 if (status == VL53L0X_ERROR_NONE) { 01660 status = VL53L0X_set_limit_check_value(dev, 01661 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 01662 (FixPoint1616_t)(35 * 65536)); 01663 } 01664 01665 if (status == VL53L0X_ERROR_NONE) { 01666 status = VL53L0X_set_limit_check_value(dev, 01667 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 01668 (FixPoint1616_t)(0 * 65536)); 01669 } 01670 01671 if (status == VL53L0X_ERROR_NONE) { 01672 01673 PALDevDataSet(dev, SequenceConfig, 0xFF); 01674 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 01675 0xFF); 01676 01677 /* Set PAL state to tell that we are waiting for call to 01678 * VL53L0X_StaticInit */ 01679 PALDevDataSet(dev, PalState, VL53L0X_STATE_WAIT_STATICINIT); 01680 } 01681 01682 if (status == VL53L0X_ERROR_NONE) { 01683 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 0); 01684 } 01685 01686 01687 LOG_FUNCTION_END(status); 01688 return status; 01689 } 01690 01691 VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV dev, 01692 uint8_t *revision, 01693 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info) 01694 { 01695 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01696 uint8_t module_id_int; 01697 char *product_id_tmp; 01698 01699 LOG_FUNCTION_START(""); 01700 01701 status = VL53L0X_get_info_from_device(dev, 2); 01702 01703 if (status == VL53L0X_ERROR_NONE) { 01704 module_id_int = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, ModuleId); 01705 01706 if (module_id_int == 0) { 01707 *revision = 0; 01708 VL53L0X_COPYSTRING(p_VL53L0X_device_info->ProductId , ""); 01709 } else { 01710 *revision = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, Revision); 01711 product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 01712 ProductId); 01713 VL53L0X_COPYSTRING(p_VL53L0X_device_info->ProductId , product_id_tmp); 01714 } 01715 } 01716 01717 LOG_FUNCTION_END(status); 01718 return status; 01719 } 01720 01721 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_device_info(VL53L0X_DEV dev, 01722 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info) 01723 { 01724 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01725 uint8_t revision_id; 01726 uint8_t revision; 01727 01728 status = VL53L0X_check_part_used(dev, &revision, p_VL53L0X_device_info); 01729 01730 if (status == VL53L0X_ERROR_NONE) { 01731 if (revision == 0) { 01732 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name , 01733 VL53L0X_STRING_DEVICE_INFO_NAME_TS0); 01734 } else if ((revision <= 34) && (revision != 32)) { 01735 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name , 01736 VL53L0X_STRING_DEVICE_INFO_NAME_TS1); 01737 } else if (revision < 39) { 01738 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name , 01739 VL53L0X_STRING_DEVICE_INFO_NAME_TS2); 01740 } else { 01741 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name , 01742 VL53L0X_STRING_DEVICE_INFO_NAME_ES1); 01743 } 01744 01745 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Type , 01746 VL53L0X_STRING_DEVICE_INFO_TYPE); 01747 01748 } 01749 01750 if (status == VL53L0X_ERROR_NONE) { 01751 status = VL53L0X_read_byte(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, 01752 &p_VL53L0X_device_info->ProductType ); 01753 } 01754 01755 if (status == VL53L0X_ERROR_NONE) { 01756 status = VL53L0X_read_byte(dev, 01757 VL53L0X_REG_IDENTIFICATION_REVISION_ID, 01758 &revision_id); 01759 p_VL53L0X_device_info->ProductRevisionMajor = 1; 01760 p_VL53L0X_device_info->ProductRevisionMinor = 01761 (revision_id & 0xF0) >> 4; 01762 } 01763 01764 return status; 01765 } 01766 01767 VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV dev, 01768 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info) 01769 { 01770 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01771 LOG_FUNCTION_START(""); 01772 01773 status = wrapped_VL53L0X_get_device_info(dev, p_VL53L0X_device_info); 01774 01775 LOG_FUNCTION_END(status); 01776 return status; 01777 } 01778 01779 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(VL53L0X_DEV dev, 01780 uint32_t *p_interrupt_mask_status) 01781 { 01782 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01783 uint8_t byte; 01784 LOG_FUNCTION_START(""); 01785 01786 status = VL53L0X_read_byte(dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte); 01787 *p_interrupt_mask_status = byte & 0x07; 01788 01789 if (byte & 0x18) { 01790 status = VL53L0X_ERROR_RANGE_ERROR; 01791 } 01792 01793 LOG_FUNCTION_END(status); 01794 return status; 01795 } 01796 01797 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(VL53L0X_DEV dev, 01798 uint8_t *p_measurement_data_ready) 01799 { 01800 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01801 uint8_t sys_range_status_register; 01802 uint8_t interrupt_config; 01803 uint32_t interrupt_mask; 01804 LOG_FUNCTION_START(""); 01805 01806 interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 01807 Pin0GpioFunctionality); 01808 01809 if (interrupt_config == 01810 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { 01811 status = VL53L0X_get_interrupt_mask_status(dev, &interrupt_mask); 01812 if (interrupt_mask == 01813 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { 01814 *p_measurement_data_ready = 1; 01815 } else { 01816 *p_measurement_data_ready = 0; 01817 } 01818 } else { 01819 status = VL53L0X_read_byte(dev, VL53L0X_REG_RESULT_RANGE_STATUS, 01820 &sys_range_status_register); 01821 if (status == VL53L0X_ERROR_NONE) { 01822 if (sys_range_status_register & 0x01) { 01823 *p_measurement_data_ready = 1; 01824 } else { 01825 *p_measurement_data_ready = 0; 01826 } 01827 } 01828 } 01829 01830 LOG_FUNCTION_END(status); 01831 return status; 01832 } 01833 01834 VL53L0X_Error VL53L0X::VL53L0X_polling_delay(VL53L0X_DEV dev) 01835 { 01836 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01837 01838 // do nothing 01839 VL53L0X_OsDelay(); 01840 return status; 01841 } 01842 01843 VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV dev) 01844 { 01845 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01846 uint8_t new_data_ready = 0; 01847 uint32_t loop_nb; 01848 01849 LOG_FUNCTION_START(""); 01850 01851 loop_nb = 0; 01852 01853 do { 01854 status = VL53L0X_get_measurement_data_ready(dev, &new_data_ready); 01855 if (status != 0) { 01856 break; /* the error is set */ 01857 } 01858 01859 if (new_data_ready == 1) { 01860 break; /* done note that status == 0 */ 01861 } 01862 01863 loop_nb++; 01864 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 01865 status = VL53L0X_ERROR_TIME_OUT; 01866 break; 01867 } 01868 01869 VL53L0X_polling_delay(dev); 01870 } while (1); 01871 01872 LOG_FUNCTION_END(status); 01873 01874 return status; 01875 } 01876 01877 /* Group PAL Interrupt Functions */ 01878 VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask(VL53L0X_DEV dev, uint32_t interrupt_mask) 01879 { 01880 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01881 uint8_t loop_count; 01882 uint8_t byte; 01883 LOG_FUNCTION_START(""); 01884 01885 /* clear bit 0 range interrupt, bit 1 error interrupt */ 01886 loop_count = 0; 01887 do { 01888 status = VL53L0X_write_byte(dev, 01889 VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01); 01890 status |= VL53L0X_write_byte(dev, 01891 VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00); 01892 status |= VL53L0X_read_byte(dev, 01893 VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte); 01894 loop_count++; 01895 } while (((byte & 0x07) != 0x00) 01896 && (loop_count < 3) 01897 && (status == VL53L0X_ERROR_NONE)); 01898 01899 01900 if (loop_count >= 3) { 01901 status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED; 01902 } 01903 01904 LOG_FUNCTION_END(status); 01905 return status; 01906 } 01907 01908 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(VL53L0X_DEV dev, 01909 uint8_t vhv_init_byte) 01910 { 01911 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01912 01913 if (status == VL53L0X_ERROR_NONE) { 01914 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 01915 VL53L0X_REG_SYSRANGE_MODE_START_STOP | 01916 vhv_init_byte); 01917 } 01918 01919 if (status == VL53L0X_ERROR_NONE) { 01920 status = VL53L0X_measurement_poll_for_completion(dev); 01921 } 01922 01923 if (status == VL53L0X_ERROR_NONE) { 01924 status = VL53L0X_clear_interrupt_mask(dev, 0); 01925 } 01926 01927 if (status == VL53L0X_ERROR_NONE) { 01928 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x00); 01929 } 01930 01931 return status; 01932 } 01933 01934 VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(VL53L0X_DEV dev, uint8_t read_not_write, 01935 uint8_t vhv_settings, uint8_t phase_cal, 01936 uint8_t *p_vhv_settings, uint8_t *p_phase_cal, 01937 const uint8_t vhv_enable, const uint8_t phase_enable) 01938 { 01939 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01940 uint8_t phase_calint = 0; 01941 01942 /* Read VHV from device */ 01943 status |= VL53L0X_write_byte(dev, 0xFF, 0x01); 01944 status |= VL53L0X_write_byte(dev, 0x00, 0x00); 01945 status |= VL53L0X_write_byte(dev, 0xFF, 0x00); 01946 01947 if (read_not_write) { 01948 if (vhv_enable) { 01949 status |= VL53L0X_read_byte(dev, 0xCB, p_vhv_settings); 01950 } 01951 if (phase_enable) { 01952 status |= VL53L0X_read_byte(dev, 0xEE, &phase_calint); 01953 } 01954 } else { 01955 if (vhv_enable) { 01956 status |= VL53L0X_write_byte(dev, 0xCB, vhv_settings); 01957 } 01958 if (phase_enable) { 01959 status |= VL53L0X_update_byte(dev, 0xEE, 0x80, phase_cal); 01960 } 01961 } 01962 01963 status |= VL53L0X_write_byte(dev, 0xFF, 0x01); 01964 status |= VL53L0X_write_byte(dev, 0x00, 0x01); 01965 status |= VL53L0X_write_byte(dev, 0xFF, 0x00); 01966 01967 *p_phase_cal = (uint8_t)(phase_calint & 0xEF); 01968 01969 return status; 01970 } 01971 01972 VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(VL53L0X_DEV dev, 01973 uint8_t *p_vhv_settings, const uint8_t get_data_enable, 01974 const uint8_t restore_config) 01975 { 01976 VL53L0X_Error status = VL53L0X_ERROR_NONE; 01977 uint8_t sequence_config = 0; 01978 uint8_t vhv_settings = 0; 01979 uint8_t phase_cal = 0; 01980 uint8_t phase_cal_int = 0; 01981 01982 /* store the value of the sequence config, 01983 * this will be reset before the end of the function 01984 */ 01985 01986 if (restore_config) { 01987 sequence_config = PALDevDataGet(dev, SequenceConfig); 01988 } 01989 01990 /* Run VHV */ 01991 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01); 01992 01993 if (status == VL53L0X_ERROR_NONE) { 01994 status = VL53L0X_perform_single_ref_calibration(dev, 0x40); 01995 } 01996 01997 /* Read VHV from device */ 01998 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { 01999 status = VL53L0X_ref_calibration_io(dev, 1, 02000 vhv_settings, phase_cal, /* Not used here */ 02001 p_vhv_settings, &phase_cal_int, 02002 1, 0); 02003 } else { 02004 *p_vhv_settings = 0; 02005 } 02006 02007 02008 if ((status == VL53L0X_ERROR_NONE) && restore_config) { 02009 /* restore the previous Sequence Config */ 02010 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 02011 sequence_config); 02012 if (status == VL53L0X_ERROR_NONE) { 02013 PALDevDataSet(dev, SequenceConfig, sequence_config); 02014 } 02015 02016 } 02017 02018 return status; 02019 } 02020 02021 VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(VL53L0X_DEV dev, 02022 uint8_t *p_phase_cal, const uint8_t get_data_enable, 02023 const uint8_t restore_config) 02024 { 02025 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02026 uint8_t sequence_config = 0; 02027 uint8_t vhv_settings = 0; 02028 uint8_t phase_cal = 0; 02029 uint8_t vhv_settingsint; 02030 02031 /* store the value of the sequence config, 02032 * this will be reset before the end of the function 02033 */ 02034 02035 if (restore_config) { 02036 sequence_config = PALDevDataGet(dev, SequenceConfig); 02037 } 02038 02039 /* Run PhaseCal */ 02040 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02); 02041 02042 if (status == VL53L0X_ERROR_NONE) { 02043 status = VL53L0X_perform_single_ref_calibration(dev, 0x0); 02044 } 02045 02046 /* Read PhaseCal from device */ 02047 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { 02048 status = VL53L0X_ref_calibration_io(dev, 1, 02049 vhv_settings, phase_cal, /* Not used here */ 02050 &vhv_settingsint, p_phase_cal, 02051 0, 1); 02052 } else { 02053 *p_phase_cal = 0; 02054 } 02055 02056 02057 if ((status == VL53L0X_ERROR_NONE) && restore_config) { 02058 /* restore the previous Sequence Config */ 02059 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 02060 sequence_config); 02061 if (status == VL53L0X_ERROR_NONE) { 02062 PALDevDataSet(dev, SequenceConfig, sequence_config); 02063 } 02064 02065 } 02066 02067 return status; 02068 } 02069 02070 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV dev, 02071 uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable) 02072 { 02073 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02074 uint8_t sequence_config = 0; 02075 02076 /* store the value of the sequence config, 02077 * this will be reset before the end of the function 02078 */ 02079 02080 sequence_config = PALDevDataGet(dev, SequenceConfig); 02081 02082 /* In the following function we don't save the config to optimize 02083 * writes on device. Config is saved and restored only once. */ 02084 status = VL53L0X_perform_vhv_calibration( 02085 dev, p_vhv_settings, get_data_enable, 0); 02086 02087 if (status == VL53L0X_ERROR_NONE) { 02088 status = VL53L0X_perform_phase_calibration( 02089 dev, p_phase_cal, get_data_enable, 0); 02090 } 02091 02092 02093 if (status == VL53L0X_ERROR_NONE) { 02094 /* restore the previous Sequence Config */ 02095 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 02096 sequence_config); 02097 if (status == VL53L0X_ERROR_NONE) { 02098 PALDevDataSet(dev, SequenceConfig, sequence_config); 02099 } 02100 02101 } 02102 02103 return status; 02104 } 02105 02106 void VL53L0X::get_next_good_spad(uint8_t good_spad_array[], uint32_t size, 02107 uint32_t curr, int32_t *p_next) 02108 { 02109 uint32_t start_index; 02110 uint32_t fine_offset; 02111 uint32_t c_spads_per_byte = 8; 02112 uint32_t coarse_index; 02113 uint32_t fine_index; 02114 uint8_t data_byte; 02115 uint8_t success = 0; 02116 02117 /* 02118 * Starting with the current good spad, loop through the array to find 02119 * the next. i.e. the next bit set in the sequence. 02120 * 02121 * The coarse index is the byte index of the array and the fine index is 02122 * the index of the bit within each byte. 02123 */ 02124 02125 *p_next = -1; 02126 02127 start_index = curr / c_spads_per_byte; 02128 fine_offset = curr % c_spads_per_byte; 02129 02130 for (coarse_index = start_index; ((coarse_index < size) && !success); 02131 coarse_index++) { 02132 fine_index = 0; 02133 data_byte = good_spad_array[coarse_index]; 02134 02135 if (coarse_index == start_index) { 02136 /* locate the bit position of the provided current 02137 * spad bit before iterating */ 02138 data_byte >>= fine_offset; 02139 fine_index = fine_offset; 02140 } 02141 02142 while (fine_index < c_spads_per_byte) { 02143 if ((data_byte & 0x1) == 1) { 02144 success = 1; 02145 *p_next = coarse_index * c_spads_per_byte + fine_index; 02146 break; 02147 } 02148 data_byte >>= 1; 02149 fine_index++; 02150 } 02151 } 02152 } 02153 02154 uint8_t VL53L0X::is_aperture(uint32_t spad_index) 02155 { 02156 /* 02157 * This function reports if a given spad index is an aperture SPAD by 02158 * deriving the quadrant. 02159 */ 02160 uint32_t quadrant; 02161 uint8_t is_aperture = 1; 02162 quadrant = spad_index >> 6; 02163 if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) { 02164 is_aperture = 0; 02165 } 02166 02167 return is_aperture; 02168 } 02169 02170 VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[], uint32_t size, 02171 uint32_t spad_index) 02172 { 02173 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02174 uint32_t c_spads_per_byte = 8; 02175 uint32_t coarse_index; 02176 uint32_t fine_index; 02177 02178 coarse_index = spad_index / c_spads_per_byte; 02179 fine_index = spad_index % c_spads_per_byte; 02180 if (coarse_index >= size) { 02181 status = VL53L0X_ERROR_REF_SPAD_INIT; 02182 } else { 02183 spad_array[coarse_index] |= (1 << fine_index); 02184 } 02185 02186 return status; 02187 } 02188 02189 VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array) 02190 { 02191 VL53L0X_Error status = VL53L0X_write_multi(dev, 02192 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 02193 p_ref_spad_array, 6); 02194 02195 return status; 02196 } 02197 02198 VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array) 02199 { 02200 VL53L0X_Error status = VL53L0X_read_multi(dev, 02201 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 02202 p_ref_spad_array, 02203 6); 02204 // VL53L0X_Error status = VL53L0X_ERROR_NONE; 02205 // uint8_t count=0; 02206 02207 // for (count = 0; count < 6; count++) 02208 // status = VL53L0X_RdByte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]); 02209 return status; 02210 } 02211 02212 VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV dev, 02213 uint8_t aperture_spads, 02214 uint8_t good_spad_array[], 02215 uint8_t spad_array[], 02216 uint32_t size, 02217 uint32_t start, 02218 uint32_t offset, 02219 uint32_t spad_count, 02220 uint32_t *p_last_spad) 02221 { 02222 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02223 uint32_t index; 02224 uint32_t i; 02225 int32_t next_good_spad = offset; 02226 uint32_t current_spad; 02227 uint8_t check_spad_array[6]; 02228 02229 /* 02230 * This function takes in a spad array which may or may not have SPADS 02231 * already enabled and appends from a given offset a requested number 02232 * of new SPAD enables. The 'good spad map' is applied to 02233 * determine the next SPADs to enable. 02234 * 02235 * This function applies to only aperture or only non-aperture spads. 02236 * Checks are performed to ensure this. 02237 */ 02238 02239 current_spad = offset; 02240 for (index = 0; index < spad_count; index++) { 02241 get_next_good_spad(good_spad_array, size, current_spad, 02242 &next_good_spad); 02243 02244 if (next_good_spad == -1) { 02245 status = VL53L0X_ERROR_REF_SPAD_INIT; 02246 break; 02247 } 02248 02249 /* Confirm that the next good SPAD is non-aperture */ 02250 if (is_aperture(start + next_good_spad) != aperture_spads) { 02251 /* if we can't get the required number of good aperture 02252 * spads from the current quadrant then this is an error 02253 */ 02254 status = VL53L0X_ERROR_REF_SPAD_INIT; 02255 break; 02256 } 02257 current_spad = (uint32_t)next_good_spad; 02258 enable_spad_bit(spad_array, size, current_spad); 02259 current_spad++; 02260 } 02261 *p_last_spad = current_spad; 02262 02263 if (status == VL53L0X_ERROR_NONE) { 02264 status = set_ref_spad_map(dev, spad_array); 02265 } 02266 02267 02268 if (status == VL53L0X_ERROR_NONE) { 02269 status = get_ref_spad_map(dev, check_spad_array); 02270 02271 i = 0; 02272 02273 /* Compare spad maps. If not equal report error. */ 02274 while (i < size) { 02275 if (spad_array[i] != check_spad_array[i]) { 02276 status = VL53L0X_ERROR_REF_SPAD_INIT; 02277 break; 02278 } 02279 i++; 02280 } 02281 } 02282 return status; 02283 } 02284 02285 VL53L0X_Error VL53L0X::VL53L0X_set_device_mode(VL53L0X_DEV dev, VL53L0X_DeviceModes device_mode) 02286 { 02287 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02288 02289 LOG_FUNCTION_START("%d", (int)DeviceMode); 02290 02291 switch (device_mode) { 02292 case VL53L0X_DEVICEMODE_SINGLE_RANGING: 02293 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: 02294 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: 02295 case VL53L0X_DEVICEMODE_GPIO_DRIVE: 02296 case VL53L0X_DEVICEMODE_GPIO_OSC: 02297 /* Supported modes */ 02298 VL53L0X_SETPARAMETERFIELD(dev, DeviceMode, device_mode); 02299 break; 02300 default: 02301 /* Unsupported mode */ 02302 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; 02303 } 02304 02305 LOG_FUNCTION_END(status); 02306 return status; 02307 } 02308 02309 VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DEV dev, 02310 VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low, 02311 FixPoint1616_t threshold_high) 02312 { 02313 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02314 uint16_t threshold16; 02315 LOG_FUNCTION_START(""); 02316 02317 /* no dependency on DeviceMode for Ewok */ 02318 /* Need to divide by 2 because the FW will apply a x2 */ 02319 threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff); 02320 status = VL53L0X_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16); 02321 02322 if (status == VL53L0X_ERROR_NONE) { 02323 /* Need to divide by 2 because the FW will apply a x2 */ 02324 threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff); 02325 status = VL53L0X_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH, 02326 threshold16); 02327 } 02328 02329 LOG_FUNCTION_END(status); 02330 return status; 02331 } 02332 02333 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DEV dev, 02334 VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low, 02335 FixPoint1616_t *p_threshold_high) 02336 { 02337 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02338 uint16_t threshold16; 02339 LOG_FUNCTION_START(""); 02340 02341 /* no dependency on DeviceMode for Ewok */ 02342 02343 status = VL53L0X_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16); 02344 /* Need to multiply by 2 because the FW will apply a x2 */ 02345 *p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17); 02346 02347 if (status == VL53L0X_ERROR_NONE) { 02348 status = VL53L0X_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH, 02349 &threshold16); 02350 /* Need to multiply by 2 because the FW will apply a x2 */ 02351 *p_threshold_high = 02352 (FixPoint1616_t)((0x00fff & threshold16) << 17); 02353 } 02354 02355 LOG_FUNCTION_END(status); 02356 return status; 02357 } 02358 02359 VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV dev, 02360 uint8_t *p_tuning_setting_buffer) 02361 { 02362 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02363 int i; 02364 int index; 02365 uint8_t msb; 02366 uint8_t lsb; 02367 uint8_t select_param; 02368 uint8_t number_of_writes; 02369 uint8_t address; 02370 uint8_t local_buffer[4]; /* max */ 02371 uint16_t temp16; 02372 02373 LOG_FUNCTION_START(""); 02374 02375 index = 0; 02376 02377 while ((*(p_tuning_setting_buffer + index) != 0) && 02378 (status == VL53L0X_ERROR_NONE)) { 02379 number_of_writes = *(p_tuning_setting_buffer + index); 02380 index++; 02381 if (number_of_writes == 0xFF) { 02382 /* internal parameters */ 02383 select_param = *(p_tuning_setting_buffer + index); 02384 index++; 02385 switch (select_param) { 02386 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */ 02387 msb = *(p_tuning_setting_buffer + index); 02388 index++; 02389 lsb = *(p_tuning_setting_buffer + index); 02390 index++; 02391 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02392 PALDevDataSet(dev, SigmaEstRefArray, temp16); 02393 break; 02394 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */ 02395 msb = *(p_tuning_setting_buffer + index); 02396 index++; 02397 lsb = *(p_tuning_setting_buffer + index); 02398 index++; 02399 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02400 PALDevDataSet(dev, SigmaEstEffPulseWidth, 02401 temp16); 02402 break; 02403 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */ 02404 msb = *(p_tuning_setting_buffer + index); 02405 index++; 02406 lsb = *(p_tuning_setting_buffer + index); 02407 index++; 02408 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02409 PALDevDataSet(dev, SigmaEstEffAmbWidth, temp16); 02410 break; 02411 case 3: /* uint16_t targetRefRate -> 2 bytes */ 02412 msb = *(p_tuning_setting_buffer + index); 02413 index++; 02414 lsb = *(p_tuning_setting_buffer + index); 02415 index++; 02416 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02417 PALDevDataSet(dev, targetRefRate, temp16); 02418 break; 02419 default: /* invalid parameter */ 02420 status = VL53L0X_ERROR_INVALID_PARAMS; 02421 } 02422 02423 } else if (number_of_writes <= 4) { 02424 address = *(p_tuning_setting_buffer + index); 02425 index++; 02426 02427 for (i = 0; i < number_of_writes; i++) { 02428 local_buffer[i] = *(p_tuning_setting_buffer + 02429 index); 02430 index++; 02431 } 02432 02433 status = VL53L0X_write_multi(dev, address, local_buffer, 02434 number_of_writes); 02435 02436 } else { 02437 status = VL53L0X_ERROR_INVALID_PARAMS; 02438 } 02439 } 02440 02441 LOG_FUNCTION_END(status); 02442 return status; 02443 } 02444 02445 VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(VL53L0X_DEV dev, 02446 uint8_t start_not_stopflag) 02447 { 02448 uint8_t interrupt_config; 02449 FixPoint1616_t threshold_low; 02450 FixPoint1616_t threshold_high; 02451 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02452 02453 interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 02454 Pin0GpioFunctionality); 02455 02456 if ((interrupt_config == 02457 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) || 02458 (interrupt_config == 02459 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) || 02460 (interrupt_config == 02461 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) { 02462 02463 status = VL53L0X_get_interrupt_thresholds(dev, 02464 VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 02465 &threshold_low, &threshold_high); 02466 02467 if (((threshold_low > 255 * 65536) || 02468 (threshold_high > 255 * 65536)) && 02469 (status == VL53L0X_ERROR_NONE)) { 02470 02471 if (start_not_stopflag != 0) { 02472 status = VL53L0X_load_tuning_settings(dev, 02473 InterruptThresholdSettings); 02474 } else { 02475 status |= VL53L0X_write_byte(dev, 0xFF, 0x04); 02476 status |= VL53L0X_write_byte(dev, 0x70, 0x00); 02477 status |= VL53L0X_write_byte(dev, 0xFF, 0x00); 02478 status |= VL53L0X_write_byte(dev, 0x80, 0x00); 02479 } 02480 02481 } 02482 02483 02484 } 02485 02486 return status; 02487 02488 } 02489 02490 VL53L0X_Error VL53L0X::VL53L0X_start_measurement(VL53L0X_DEV dev) 02491 { 02492 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02493 VL53L0X_DeviceModes device_mode; 02494 uint8_t byte; 02495 uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP; 02496 uint32_t loop_nb; 02497 LOG_FUNCTION_START(""); 02498 02499 /* Get Current DeviceMode */ 02500 VL53L0X_get_device_mode(dev, &device_mode); 02501 02502 status = VL53L0X_write_byte(dev, 0x80, 0x01); 02503 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 02504 status = VL53L0X_write_byte(dev, 0x00, 0x00); 02505 status = VL53L0X_write_byte(dev, 0x91, PALDevDataGet(dev, StopVariable)); 02506 status = VL53L0X_write_byte(dev, 0x00, 0x01); 02507 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 02508 status = VL53L0X_write_byte(dev, 0x80, 0x00); 02509 02510 switch (device_mode) { 02511 case VL53L0X_DEVICEMODE_SINGLE_RANGING: 02512 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x01); 02513 02514 byte = start_stop_byte; 02515 if (status == VL53L0X_ERROR_NONE) { 02516 /* Wait until start bit has been cleared */ 02517 loop_nb = 0; 02518 do { 02519 if (loop_nb > 0) 02520 status = VL53L0X_read_byte(dev, 02521 VL53L0X_REG_SYSRANGE_START, &byte); 02522 loop_nb = loop_nb + 1; 02523 } while (((byte & start_stop_byte) == start_stop_byte) 02524 && (status == VL53L0X_ERROR_NONE) 02525 && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP)); 02526 02527 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 02528 status = VL53L0X_ERROR_TIME_OUT; 02529 } 02530 02531 } 02532 02533 break; 02534 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: 02535 /* Back-to-back mode */ 02536 02537 /* Check if need to apply interrupt settings */ 02538 if (status == VL53L0X_ERROR_NONE) { 02539 status = VL53L0X_check_and_load_interrupt_settings(dev, 1); 02540 } 02541 02542 status = VL53L0X_write_byte(dev, 02543 VL53L0X_REG_SYSRANGE_START, 02544 VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK); 02545 if (status == VL53L0X_ERROR_NONE) { 02546 /* Set PAL State to Running */ 02547 PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING); 02548 } 02549 break; 02550 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: 02551 /* Continuous mode */ 02552 /* Check if need to apply interrupt settings */ 02553 if (status == VL53L0X_ERROR_NONE) { 02554 status = VL53L0X_check_and_load_interrupt_settings(dev, 1); 02555 } 02556 02557 status = VL53L0X_write_byte(dev, 02558 VL53L0X_REG_SYSRANGE_START, 02559 VL53L0X_REG_SYSRANGE_MODE_TIMED); 02560 02561 if (status == VL53L0X_ERROR_NONE) { 02562 /* Set PAL State to Running */ 02563 PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING); 02564 } 02565 break; 02566 default: 02567 /* Selected mode not supported */ 02568 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; 02569 } 02570 02571 02572 LOG_FUNCTION_END(status); 02573 return status; 02574 } 02575 02576 /* Group PAL Measurement Functions */ 02577 VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement(VL53L0X_DEV dev) 02578 { 02579 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02580 VL53L0X_DeviceModes device_mode; 02581 02582 LOG_FUNCTION_START(""); 02583 02584 /* Get Current DeviceMode */ 02585 status = VL53L0X_get_device_mode(dev, &device_mode); 02586 02587 /* Start immediately to run a single ranging measurement in case of 02588 * single ranging or single histogram */ 02589 if (status == VL53L0X_ERROR_NONE 02590 && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) { 02591 status = VL53L0X_start_measurement(dev); 02592 } 02593 02594 02595 if (status == VL53L0X_ERROR_NONE) { 02596 status = VL53L0X_measurement_poll_for_completion(dev); 02597 } 02598 02599 02600 /* Change PAL State in case of single ranging or single histogram */ 02601 if (status == VL53L0X_ERROR_NONE 02602 && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) { 02603 PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE); 02604 } 02605 02606 02607 LOG_FUNCTION_END(status); 02608 return status; 02609 } 02610 02611 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(VL53L0X_DEV dev, 02612 uint8_t *p_x_talk_compensation_enable) 02613 { 02614 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02615 uint8_t temp8; 02616 LOG_FUNCTION_START(""); 02617 02618 VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable, temp8); 02619 *p_x_talk_compensation_enable = temp8; 02620 02621 LOG_FUNCTION_END(status); 02622 return status; 02623 } 02624 02625 VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV dev, 02626 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 02627 FixPoint1616_t *p_total_xtalk_rate_mcps) 02628 { 02629 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02630 02631 uint8_t xtalk_comp_enable; 02632 FixPoint1616_t total_xtalk_mega_cps; 02633 FixPoint1616_t xtalk_per_spad_mega_cps; 02634 02635 *p_total_xtalk_rate_mcps = 0; 02636 02637 status = VL53L0X_get_x_talk_compensation_enable(dev, &xtalk_comp_enable); 02638 if (status == VL53L0X_ERROR_NONE) { 02639 02640 if (xtalk_comp_enable) { 02641 02642 VL53L0X_GETPARAMETERFIELD( 02643 dev, 02644 XTalkCompensationRateMegaCps, 02645 xtalk_per_spad_mega_cps); 02646 02647 /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */ 02648 total_xtalk_mega_cps = 02649 p_ranging_measurement_data->EffectiveSpadRtnCount * 02650 xtalk_per_spad_mega_cps; 02651 02652 /* FixPoint0824 >> 8 = FixPoint1616 */ 02653 *p_total_xtalk_rate_mcps = 02654 (total_xtalk_mega_cps + 0x80) >> 8; 02655 } 02656 } 02657 02658 return status; 02659 } 02660 02661 VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV dev, 02662 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 02663 FixPoint1616_t *p_total_signal_rate_mcps) 02664 { 02665 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02666 FixPoint1616_t total_xtalk_mega_cps; 02667 02668 LOG_FUNCTION_START(""); 02669 02670 *p_total_signal_rate_mcps = 02671 p_ranging_measurement_data->SignalRateRtnMegaCps; 02672 02673 status = VL53L0X_get_total_xtalk_rate( 02674 dev, p_ranging_measurement_data, &total_xtalk_mega_cps); 02675 02676 if (status == VL53L0X_ERROR_NONE) { 02677 *p_total_signal_rate_mcps += total_xtalk_mega_cps; 02678 } 02679 02680 return status; 02681 } 02682 02683 /* To convert ms into register value */ 02684 uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(VL53L0X_DEV dev, 02685 uint32_t timeout_period_us, 02686 uint8_t vcsel_period_pclks) 02687 { 02688 uint32_t macro_period_ps; 02689 uint32_t macro_period_ns; 02690 uint32_t timeout_period_mclks = 0; 02691 02692 macro_period_ps = VL53L0X_calc_macro_period_ps(dev, vcsel_period_pclks); 02693 macro_period_ns = (macro_period_ps + 500) / 1000; 02694 02695 timeout_period_mclks = 02696 (uint32_t)(((timeout_period_us * 1000) 02697 + (macro_period_ns / 2)) / macro_period_ns); 02698 02699 return timeout_period_mclks; 02700 } 02701 02702 uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num) 02703 { 02704 /* 02705 * Implements an integer square root 02706 * 02707 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots 02708 */ 02709 02710 uint32_t res = 0; 02711 uint32_t bit = 1 << 30; 02712 /* The second-to-top bit is set: 02713 * 1 << 14 for 16-bits, 1 << 30 for 32 bits */ 02714 02715 /* "bit" starts at the highest power of four <= the argument. */ 02716 while (bit > num) { 02717 bit >>= 2; 02718 } 02719 02720 02721 while (bit != 0) { 02722 if (num >= res + bit) { 02723 num -= res + bit; 02724 res = (res >> 1) + bit; 02725 } else { 02726 res >>= 1; 02727 } 02728 02729 bit >>= 2; 02730 } 02731 02732 return res; 02733 } 02734 02735 VL53L0X_Error VL53L0X::VL53L0X_calc_dmax( 02736 VL53L0X_DEV dev, 02737 FixPoint1616_t total_signal_rate_mcps, 02738 FixPoint1616_t total_corr_signal_rate_mcps, 02739 FixPoint1616_t pw_mult, 02740 uint32_t sigma_estimate_p1, 02741 FixPoint1616_t sigma_estimate_p2, 02742 uint32_t peak_vcsel_duration_us, 02743 uint32_t *pd_max_mm) 02744 { 02745 const uint32_t c_sigma_limit = 18; 02746 const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */ 02747 const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */ 02748 const uint32_t c_amb_eff_width_sigma_est_ns = 6; 02749 const uint32_t c_amb_eff_width_d_max_ns = 7; 02750 uint32_t dmax_cal_range_mm; 02751 FixPoint1616_t dmax_cal_signal_rate_rtn_mcps; 02752 FixPoint1616_t min_signal_needed; 02753 FixPoint1616_t min_signal_needed_p1; 02754 FixPoint1616_t min_signal_needed_p2; 02755 FixPoint1616_t min_signal_needed_p3; 02756 FixPoint1616_t min_signal_needed_p4; 02757 FixPoint1616_t sigma_limit_tmp; 02758 FixPoint1616_t sigma_est_sq_tmp; 02759 FixPoint1616_t signal_limit_tmp; 02760 FixPoint1616_t signal_at0_mm; 02761 FixPoint1616_t dmax_dark; 02762 FixPoint1616_t dmax_ambient; 02763 FixPoint1616_t dmax_dark_tmp; 02764 FixPoint1616_t sigma_est_p2_tmp; 02765 uint32_t signal_rate_temp_mcps; 02766 02767 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02768 02769 LOG_FUNCTION_START(""); 02770 02771 dmax_cal_range_mm = 02772 PALDevDataGet(dev, DmaxCalRangeMilliMeter); 02773 02774 dmax_cal_signal_rate_rtn_mcps = 02775 PALDevDataGet(dev, DmaxCalSignalRateRtnMegaCps); 02776 02777 /* uint32 * FixPoint1616 = FixPoint1616 */ 02778 signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps; 02779 02780 /* FixPoint1616 >> 8 = FixPoint2408 */ 02781 signal_at0_mm = (signal_at0_mm + 0x80) >> 8; 02782 signal_at0_mm *= dmax_cal_range_mm; 02783 02784 min_signal_needed_p1 = 0; 02785 if (total_corr_signal_rate_mcps > 0) { 02786 02787 /* Shift by 10 bits to increase resolution prior to the 02788 * division */ 02789 signal_rate_temp_mcps = total_signal_rate_mcps << 10; 02790 02791 /* Add rounding value prior to division */ 02792 min_signal_needed_p1 = signal_rate_temp_mcps + 02793 (total_corr_signal_rate_mcps / 2); 02794 02795 /* FixPoint0626/FixPoint1616 = FixPoint2210 */ 02796 min_signal_needed_p1 /= total_corr_signal_rate_mcps; 02797 02798 /* Apply a factored version of the speed of light. 02799 Correction to be applied at the end */ 02800 min_signal_needed_p1 *= 3; 02801 02802 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */ 02803 min_signal_needed_p1 *= min_signal_needed_p1; 02804 02805 /* FixPoint1220 >> 16 = FixPoint2804 */ 02806 min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16; 02807 } 02808 02809 min_signal_needed_p2 = pw_mult * sigma_estimate_p1; 02810 02811 /* FixPoint1616 >> 16 = uint32 */ 02812 min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16; 02813 02814 /* uint32 * uint32 = uint32 */ 02815 min_signal_needed_p2 *= min_signal_needed_p2; 02816 02817 /* Check sigmaEstimateP2 02818 * If this value is too high there is not enough signal rate 02819 * to calculate dmax value so set a suitable value to ensure 02820 * a very small dmax. 02821 */ 02822 sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16; 02823 sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) / 02824 c_amb_eff_width_sigma_est_ns; 02825 sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns; 02826 02827 if (sigma_est_p2_tmp > 0xffff) { 02828 min_signal_needed_p3 = 0xfff00000; 02829 } else { 02830 02831 /* DMAX uses a different ambient width from sigma, so apply 02832 * correction. 02833 * Perform division before multiplication to prevent overflow. 02834 */ 02835 sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) / 02836 c_amb_eff_width_sigma_est_ns; 02837 sigma_estimate_p2 *= c_amb_eff_width_d_max_ns; 02838 02839 /* FixPoint1616 >> 16 = uint32 */ 02840 min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16; 02841 02842 min_signal_needed_p3 *= min_signal_needed_p3; 02843 02844 } 02845 02846 /* FixPoint1814 / uint32 = FixPoint1814 */ 02847 sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000; 02848 02849 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */ 02850 sigma_limit_tmp *= sigma_limit_tmp; 02851 02852 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ 02853 sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref; 02854 02855 /* FixPoint3232 >> 4 = FixPoint0428 */ 02856 sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4; 02857 02858 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */ 02859 sigma_limit_tmp -= sigma_est_sq_tmp; 02860 02861 /* uint32_t * FixPoint0428 = FixPoint0428 */ 02862 min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp; 02863 02864 /* FixPoint0428 >> 14 = FixPoint1814 */ 02865 min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14; 02866 02867 /* uint32 + uint32 = uint32 */ 02868 min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3); 02869 02870 /* uint32 / uint32 = uint32 */ 02871 min_signal_needed += (peak_vcsel_duration_us / 2); 02872 min_signal_needed /= peak_vcsel_duration_us; 02873 02874 /* uint32 << 14 = FixPoint1814 */ 02875 min_signal_needed <<= 14; 02876 02877 /* FixPoint1814 / FixPoint1814 = uint32 */ 02878 min_signal_needed += (min_signal_needed_p4 / 2); 02879 min_signal_needed /= min_signal_needed_p4; 02880 02881 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/ 02882 min_signal_needed *= min_signal_needed_p1; 02883 02884 /* Apply correction by dividing by 1000000. 02885 * This assumes 10E16 on the numerator of the equation 02886 * and 10E-22 on the denominator. 02887 * We do this because 32bit fix point calculation can't 02888 * handle the larger and smaller elements of this equation, 02889 * i.e. speed of light and pulse widths. 02890 */ 02891 min_signal_needed = (min_signal_needed + 500) / 1000; 02892 min_signal_needed <<= 4; 02893 02894 min_signal_needed = (min_signal_needed + 500) / 1000; 02895 02896 /* FixPoint1616 >> 8 = FixPoint2408 */ 02897 signal_limit_tmp = (c_signal_limit + 0x80) >> 8; 02898 02899 /* FixPoint2408/FixPoint2408 = uint32 */ 02900 if (signal_limit_tmp != 0) { 02901 dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2)) 02902 / signal_limit_tmp; 02903 } else { 02904 dmax_dark_tmp = 0; 02905 } 02906 02907 dmax_dark = VL53L0X_isqrt(dmax_dark_tmp); 02908 02909 /* FixPoint2408/FixPoint2408 = uint32 */ 02910 if (min_signal_needed != 0) { 02911 dmax_ambient = (signal_at0_mm + min_signal_needed / 2) 02912 / min_signal_needed; 02913 } else { 02914 dmax_ambient = 0; 02915 } 02916 02917 dmax_ambient = VL53L0X_isqrt(dmax_ambient); 02918 02919 *pd_max_mm = dmax_dark; 02920 if (dmax_dark > dmax_ambient) { 02921 *pd_max_mm = dmax_ambient; 02922 } 02923 02924 LOG_FUNCTION_END(status); 02925 02926 return status; 02927 } 02928 02929 VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV dev, 02930 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 02931 FixPoint1616_t *p_sigma_estimate, 02932 uint32_t *p_dmax_mm) 02933 { 02934 /* Expressed in 100ths of a ns, i.e. centi-ns */ 02935 const uint32_t c_pulse_effective_width_centi_ns = 800; 02936 /* Expressed in 100ths of a ns, i.e. centi-ns */ 02937 const uint32_t c_ambient_effective_width_centi_ns = 600; 02938 const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */ 02939 const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */ 02940 const FixPoint1616_t c_sigma_est_max = 0x028F87AE; 02941 const FixPoint1616_t c_sigma_est_rtn_max = 0xF000; 02942 const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 / 02943 c_ambient_effective_width_centi_ns; 02944 /* Time Of Flight per mm (6.6 pico secs) */ 02945 const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A; 02946 const uint32_t c_16bit_rounding_param = 0x00008000; 02947 const FixPoint1616_t c_max_x_talk_kcps = 0x00320000; 02948 const uint32_t c_pll_period_ps = 1655; 02949 02950 uint32_t vcsel_total_events_rtn; 02951 uint32_t final_range_timeout_micro_secs; 02952 uint32_t pre_range_timeout_micro_secs; 02953 uint32_t final_range_integration_time_milli_secs; 02954 FixPoint1616_t sigma_estimate_p1; 02955 FixPoint1616_t sigma_estimate_p2; 02956 FixPoint1616_t sigma_estimate_p3; 02957 FixPoint1616_t delta_t_ps; 02958 FixPoint1616_t pw_mult; 02959 FixPoint1616_t sigma_est_rtn; 02960 FixPoint1616_t sigma_estimate; 02961 FixPoint1616_t x_talk_correction; 02962 FixPoint1616_t ambient_rate_kcps; 02963 FixPoint1616_t peak_signal_rate_kcps; 02964 FixPoint1616_t x_talk_comp_rate_mcps; 02965 uint32_t x_talk_comp_rate_kcps; 02966 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02967 FixPoint1616_t diff1_mcps; 02968 FixPoint1616_t diff2_mcps; 02969 FixPoint1616_t sqr1; 02970 FixPoint1616_t sqr2; 02971 FixPoint1616_t sqr_sum; 02972 FixPoint1616_t sqrt_result_centi_ns; 02973 FixPoint1616_t sqrt_result; 02974 FixPoint1616_t total_signal_rate_mcps; 02975 FixPoint1616_t corrected_signal_rate_mcps; 02976 FixPoint1616_t sigma_est_ref; 02977 uint32_t vcsel_width; 02978 uint32_t final_range_macro_pclks; 02979 uint32_t pre_range_macro_pclks; 02980 uint32_t peak_vcsel_duration_us; 02981 uint8_t final_range_vcsel_pclks; 02982 uint8_t pre_range_vcsel_pclks; 02983 /*! \addtogroup calc_sigma_estimate 02984 * @{ 02985 * 02986 * Estimates the range sigma 02987 */ 02988 02989 LOG_FUNCTION_START(""); 02990 02991 VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps, 02992 x_talk_comp_rate_mcps); 02993 02994 /* 02995 * We work in kcps rather than mcps as this helps keep within the 02996 * confines of the 32 Fix1616 type. 02997 */ 02998 02999 ambient_rate_kcps = 03000 (p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16; 03001 03002 corrected_signal_rate_mcps = 03003 p_ranging_measurement_data->SignalRateRtnMegaCps; 03004 03005 03006 status = VL53L0X_get_total_signal_rate( 03007 dev, p_ranging_measurement_data, &total_signal_rate_mcps); 03008 status = VL53L0X_get_total_xtalk_rate( 03009 dev, p_ranging_measurement_data, &x_talk_comp_rate_mcps); 03010 03011 03012 /* Signal rate measurement provided by device is the 03013 * peak signal rate, not average. 03014 */ 03015 peak_signal_rate_kcps = (total_signal_rate_mcps * 1000); 03016 peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16; 03017 03018 x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000; 03019 03020 if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) { 03021 x_talk_comp_rate_kcps = c_max_x_talk_kcps; 03022 } 03023 03024 if (status == VL53L0X_ERROR_NONE) { 03025 03026 /* Calculate final range macro periods */ 03027 final_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER( 03028 dev, FinalRangeTimeoutMicroSecs); 03029 03030 final_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER( 03031 dev, FinalRangeVcselPulsePeriod); 03032 03033 final_range_macro_pclks = VL53L0X_calc_timeout_mclks( 03034 dev, final_range_timeout_micro_secs, final_range_vcsel_pclks); 03035 03036 /* Calculate pre-range macro periods */ 03037 pre_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER( 03038 dev, PreRangeTimeoutMicroSecs); 03039 03040 pre_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER( 03041 dev, PreRangeVcselPulsePeriod); 03042 03043 pre_range_macro_pclks = VL53L0X_calc_timeout_mclks( 03044 dev, pre_range_timeout_micro_secs, pre_range_vcsel_pclks); 03045 03046 vcsel_width = 3; 03047 if (final_range_vcsel_pclks == 8) { 03048 vcsel_width = 2; 03049 } 03050 03051 03052 peak_vcsel_duration_us = vcsel_width * 2048 * 03053 (pre_range_macro_pclks + final_range_macro_pclks); 03054 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; 03055 peak_vcsel_duration_us *= c_pll_period_ps; 03056 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; 03057 03058 /* Fix1616 >> 8 = Fix2408 */ 03059 total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8; 03060 03061 /* Fix2408 * uint32 = Fix2408 */ 03062 vcsel_total_events_rtn = total_signal_rate_mcps * 03063 peak_vcsel_duration_us; 03064 03065 /* Fix2408 >> 8 = uint32 */ 03066 vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8; 03067 03068 /* Fix2408 << 8 = Fix1616 = */ 03069 total_signal_rate_mcps <<= 8; 03070 } 03071 03072 if (status != VL53L0X_ERROR_NONE) { 03073 LOG_FUNCTION_END(status); 03074 return status; 03075 } 03076 03077 if (peak_signal_rate_kcps == 0) { 03078 *p_sigma_estimate = c_sigma_est_max; 03079 PALDevDataSet(dev, SigmaEstimate, c_sigma_est_max); 03080 *p_dmax_mm = 0; 03081 } else { 03082 if (vcsel_total_events_rtn < 1) { 03083 vcsel_total_events_rtn = 1; 03084 } 03085 03086 sigma_estimate_p1 = c_pulse_effective_width_centi_ns; 03087 03088 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */ 03089 sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps; 03090 if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) { 03091 /* Clip to prevent overflow. Will ensure safe 03092 * max result. */ 03093 sigma_estimate_p2 = c_amb_to_signal_ratio_max; 03094 } 03095 sigma_estimate_p2 *= c_ambient_effective_width_centi_ns; 03096 03097 sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12); 03098 03099 /* uint32 * FixPoint1616 = FixPoint1616 */ 03100 delta_t_ps = p_ranging_measurement_data->RangeMilliMeter * 03101 c_tof_per_mm_ps; 03102 03103 /* 03104 * vcselRate - xtalkCompRate 03105 * (uint32 << 16) - FixPoint1616 = FixPoint1616. 03106 * Divide result by 1000 to convert to mcps. 03107 * 500 is added to ensure rounding when integer division 03108 * truncates. 03109 */ 03110 diff1_mcps = (((peak_signal_rate_kcps << 16) - 03111 2 * x_talk_comp_rate_kcps) + 500) / 1000; 03112 03113 /* vcselRate + xtalkCompRate */ 03114 diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000; 03115 03116 /* Shift by 8 bits to increase resolution prior to the 03117 * division */ 03118 diff1_mcps <<= 8; 03119 03120 /* FixPoint0824/FixPoint1616 = FixPoint2408 */ 03121 // xTalkCorrection = abs(diff1_mcps/diff2_mcps); 03122 // abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway! 03123 x_talk_correction = diff1_mcps / diff2_mcps; 03124 03125 /* FixPoint2408 << 8 = FixPoint1616 */ 03126 x_talk_correction <<= 8; 03127 03128 if (p_ranging_measurement_data->RangeStatus != 0) { 03129 pw_mult = 1 << 16; 03130 } else { 03131 /* FixPoint1616/uint32 = FixPoint1616 */ 03132 pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */ 03133 03134 /* 03135 * FixPoint1616 * FixPoint1616 = FixPoint3232, however both 03136 * values are small enough such that32 bits will not be 03137 * exceeded. 03138 */ 03139 pw_mult *= ((1 << 16) - x_talk_correction); 03140 03141 /* (FixPoint3232 >> 16) = FixPoint1616 */ 03142 pw_mult = (pw_mult + c_16bit_rounding_param) >> 16; 03143 03144 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */ 03145 pw_mult += (1 << 16); 03146 03147 /* 03148 * At this point the value will be 1.xx, therefore if we square 03149 * the value this will exceed 32 bits. To address this perform 03150 * a single shift to the right before the multiplication. 03151 */ 03152 pw_mult >>= 1; 03153 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */ 03154 pw_mult = pw_mult * pw_mult; 03155 03156 /* (FixPoint3430 >> 14) = Fix1616 */ 03157 pw_mult >>= 14; 03158 } 03159 03160 /* FixPoint1616 * uint32 = FixPoint1616 */ 03161 sqr1 = pw_mult * sigma_estimate_p1; 03162 03163 /* (FixPoint1616 >> 16) = FixPoint3200 */ 03164 sqr1 = (sqr1 + 0x8000) >> 16; 03165 03166 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ 03167 sqr1 *= sqr1; 03168 03169 sqr2 = sigma_estimate_p2; 03170 03171 /* (FixPoint1616 >> 16) = FixPoint3200 */ 03172 sqr2 = (sqr2 + 0x8000) >> 16; 03173 03174 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ 03175 sqr2 *= sqr2; 03176 03177 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */ 03178 sqr_sum = sqr1 + sqr2; 03179 03180 /* SQRT(FixPoin6400) = FixPoint3200 */ 03181 sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum); 03182 03183 /* (FixPoint3200 << 16) = FixPoint1616 */ 03184 sqrt_result_centi_ns <<= 16; 03185 03186 /* 03187 * Note that the Speed Of Light is expressed in um per 1E-10 03188 * seconds (2997) Therefore to get mm/ns we have to divide by 03189 * 10000 03190 */ 03191 sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) / 03192 sigma_estimate_p3); 03193 sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR; 03194 03195 /* Add 5000 before dividing by 10000 to ensure rounding. */ 03196 sigma_est_rtn += 5000; 03197 sigma_est_rtn /= 10000; 03198 03199 if (sigma_est_rtn > c_sigma_est_rtn_max) { 03200 /* Clip to prevent overflow. Will ensure safe 03201 * max result. */ 03202 sigma_est_rtn = c_sigma_est_rtn_max; 03203 } 03204 final_range_integration_time_milli_secs = 03205 (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000; 03206 03207 /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range) 03208 * sqrt(FixPoint1616/int) = FixPoint2408) 03209 */ 03210 sigma_est_ref = 03211 VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs + 03212 final_range_integration_time_milli_secs / 2) / 03213 final_range_integration_time_milli_secs); 03214 03215 /* FixPoint2408 << 8 = FixPoint1616 */ 03216 sigma_est_ref <<= 8; 03217 sigma_est_ref = (sigma_est_ref + 500) / 1000; 03218 03219 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ 03220 sqr1 = sigma_est_rtn * sigma_est_rtn; 03221 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ 03222 sqr2 = sigma_est_ref * sigma_est_ref; 03223 03224 /* sqrt(FixPoint3232) = FixPoint1616 */ 03225 sqrt_result = VL53L0X_isqrt((sqr1 + sqr2)); 03226 /* 03227 * Note that the Shift by 4 bits increases resolution prior to 03228 * the sqrt, therefore the result must be shifted by 2 bits to 03229 * the right to revert back to the FixPoint1616 format. 03230 */ 03231 03232 sigma_estimate = 1000 * sqrt_result; 03233 03234 if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) || 03235 (sigma_estimate > c_sigma_est_max)) { 03236 sigma_estimate = c_sigma_est_max; 03237 } 03238 03239 *p_sigma_estimate = (uint32_t)(sigma_estimate); 03240 PALDevDataSet(dev, SigmaEstimate, *p_sigma_estimate); 03241 status = VL53L0X_calc_dmax( 03242 dev, 03243 total_signal_rate_mcps, 03244 corrected_signal_rate_mcps, 03245 pw_mult, 03246 sigma_estimate_p1, 03247 sigma_estimate_p2, 03248 peak_vcsel_duration_us, 03249 p_dmax_mm); 03250 } 03251 03252 LOG_FUNCTION_END(status); 03253 return status; 03254 } 03255 03256 VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV dev, 03257 uint8_t device_range_status, 03258 FixPoint1616_t signal_rate, 03259 uint16_t effective_spad_rtn_count, 03260 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 03261 uint8_t *p_pal_range_status) 03262 { 03263 VL53L0X_Error status = VL53L0X_ERROR_NONE; 03264 uint8_t none_flag; 03265 uint8_t sigma_limitflag = 0; 03266 uint8_t signal_ref_clipflag = 0; 03267 uint8_t range_ignore_thresholdflag = 0; 03268 uint8_t sigma_limit_check_enable = 0; 03269 uint8_t signal_rate_final_range_limit_check_enable = 0; 03270 uint8_t signal_ref_clip_limit_check_enable = 0; 03271 uint8_t range_ignore_threshold_limit_check_enable = 0; 03272 FixPoint1616_t sigma_estimate; 03273 FixPoint1616_t sigma_limit_value; 03274 FixPoint1616_t signal_ref_clip_value; 03275 FixPoint1616_t range_ignore_threshold_value; 03276 FixPoint1616_t signal_rate_per_spad; 03277 uint8_t device_range_status_internal = 0; 03278 uint16_t tmp_word = 0; 03279 uint8_t temp8; 03280 uint32_t dmax_mm = 0; 03281 FixPoint1616_t last_signal_ref_mcps; 03282 03283 LOG_FUNCTION_START(""); 03284 03285 03286 /* 03287 * VL53L0X has a good ranging when the value of the 03288 * DeviceRangeStatus = 11. This function will replace the value 0 with 03289 * the value 11 in the DeviceRangeStatus. 03290 * In addition, the SigmaEstimator is not included in the VL53L0X 03291 * DeviceRangeStatus, this will be added in the PalRangeStatus. 03292 */ 03293 03294 device_range_status_internal = ((device_range_status & 0x78) >> 3); 03295 03296 if (device_range_status_internal == 0 || 03297 device_range_status_internal == 5 || 03298 device_range_status_internal == 7 || 03299 device_range_status_internal == 12 || 03300 device_range_status_internal == 13 || 03301 device_range_status_internal == 14 || 03302 device_range_status_internal == 15 03303 ) { 03304 none_flag = 1; 03305 } else { 03306 none_flag = 0; 03307 } 03308 03309 /* 03310 * Check if Sigma limit is enabled, if yes then do comparison with limit 03311 * value and put the result back into pPalRangeStatus. 03312 */ 03313 if (status == VL53L0X_ERROR_NONE) { 03314 status = VL53L0X_get_limit_check_enable(dev, 03315 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 03316 &sigma_limit_check_enable); 03317 } 03318 03319 if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) { 03320 /* 03321 * compute the Sigma and check with limit 03322 */ 03323 status = VL53L0X_calc_sigma_estimate( 03324 dev, 03325 p_ranging_measurement_data, 03326 &sigma_estimate, 03327 &dmax_mm); 03328 if (status == VL53L0X_ERROR_NONE) { 03329 p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm; 03330 } 03331 03332 if (status == VL53L0X_ERROR_NONE) { 03333 status = VL53L0X_get_limit_check_value(dev, 03334 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 03335 &sigma_limit_value); 03336 03337 if ((sigma_limit_value > 0) && 03338 (sigma_estimate > sigma_limit_value)) { 03339 /* Limit Fail */ 03340 sigma_limitflag = 1; 03341 } 03342 } 03343 } 03344 03345 /* 03346 * Check if Signal ref clip limit is enabled, if yes then do comparison 03347 * with limit value and put the result back into pPalRangeStatus. 03348 */ 03349 if (status == VL53L0X_ERROR_NONE) { 03350 status = VL53L0X_get_limit_check_enable(dev, 03351 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 03352 &signal_ref_clip_limit_check_enable); 03353 } 03354 03355 if ((signal_ref_clip_limit_check_enable != 0) && 03356 (status == VL53L0X_ERROR_NONE)) { 03357 03358 status = VL53L0X_get_limit_check_value(dev, 03359 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 03360 &signal_ref_clip_value); 03361 03362 /* Read LastSignalRefMcps from device */ 03363 if (status == VL53L0X_ERROR_NONE) { 03364 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 03365 } 03366 03367 if (status == VL53L0X_ERROR_NONE) { 03368 status = VL53L0X_read_word(dev, 03369 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, 03370 &tmp_word); 03371 } 03372 03373 if (status == VL53L0X_ERROR_NONE) { 03374 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 03375 } 03376 03377 last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word); 03378 PALDevDataSet(dev, LastSignalRefMcps, last_signal_ref_mcps); 03379 03380 if ((signal_ref_clip_value > 0) && 03381 (last_signal_ref_mcps > signal_ref_clip_value)) { 03382 /* Limit Fail */ 03383 signal_ref_clipflag = 1; 03384 } 03385 } 03386 03387 /* 03388 * Check if Signal ref clip limit is enabled, if yes then do comparison 03389 * with limit value and put the result back into pPalRangeStatus. 03390 * EffectiveSpadRtnCount has a format 8.8 03391 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL 03392 */ 03393 if (status == VL53L0X_ERROR_NONE) { 03394 status = VL53L0X_get_limit_check_enable(dev, 03395 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 03396 &range_ignore_threshold_limit_check_enable); 03397 } 03398 03399 if ((range_ignore_threshold_limit_check_enable != 0) && 03400 (status == VL53L0X_ERROR_NONE)) { 03401 03402 /* Compute the signal rate per spad */ 03403 if (effective_spad_rtn_count == 0) { 03404 signal_rate_per_spad = 0; 03405 } else { 03406 signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate) 03407 / effective_spad_rtn_count); 03408 } 03409 03410 status = VL53L0X_get_limit_check_value(dev, 03411 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 03412 &range_ignore_threshold_value); 03413 03414 if ((range_ignore_threshold_value > 0) && 03415 (signal_rate_per_spad < range_ignore_threshold_value)) { 03416 /* Limit Fail add 2^6 to range status */ 03417 range_ignore_thresholdflag = 1; 03418 } 03419 } 03420 03421 if (status == VL53L0X_ERROR_NONE) { 03422 if (none_flag == 1) { 03423 *p_pal_range_status = 255; /* NONE */ 03424 } else if (device_range_status_internal == 1 || 03425 device_range_status_internal == 2 || 03426 device_range_status_internal == 3) { 03427 *p_pal_range_status = 5; /* HW fail */ 03428 } else if (device_range_status_internal == 6 || 03429 device_range_status_internal == 9) { 03430 *p_pal_range_status = 4; /* Phase fail */ 03431 } else if (device_range_status_internal == 8 || 03432 device_range_status_internal == 10 || 03433 signal_ref_clipflag == 1) { 03434 *p_pal_range_status = 3; /* Min range */ 03435 } else if (device_range_status_internal == 4 || 03436 range_ignore_thresholdflag == 1) { 03437 *p_pal_range_status = 2; /* Signal Fail */ 03438 } else if (sigma_limitflag == 1) { 03439 *p_pal_range_status = 1; /* Sigma Fail */ 03440 } else { 03441 *p_pal_range_status = 0; /* Range Valid */ 03442 } 03443 } 03444 03445 /* DMAX only relevant during range error */ 03446 if (*p_pal_range_status == 0) { 03447 p_ranging_measurement_data->RangeDMaxMilliMeter = 0; 03448 } 03449 03450 /* fill the Limit Check Status */ 03451 03452 status = VL53L0X_get_limit_check_enable(dev, 03453 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 03454 &signal_rate_final_range_limit_check_enable); 03455 03456 if (status == VL53L0X_ERROR_NONE) { 03457 if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1)) { 03458 temp8 = 1; 03459 } else { 03460 temp8 = 0; 03461 } 03462 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus, 03463 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp8); 03464 03465 if ((device_range_status_internal == 4) || 03466 (signal_rate_final_range_limit_check_enable == 0)) { 03467 temp8 = 1; 03468 } else { 03469 temp8 = 0; 03470 } 03471 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus, 03472 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 03473 temp8); 03474 03475 if ((signal_ref_clip_limit_check_enable == 0) || 03476 (signal_ref_clipflag == 1)) { 03477 temp8 = 1; 03478 } else { 03479 temp8 = 0; 03480 } 03481 03482 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus, 03483 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp8); 03484 03485 if ((range_ignore_threshold_limit_check_enable == 0) || 03486 (range_ignore_thresholdflag == 1)) { 03487 temp8 = 1; 03488 } else { 03489 temp8 = 0; 03490 } 03491 03492 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus, 03493 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 03494 temp8); 03495 } 03496 03497 LOG_FUNCTION_END(status); 03498 return status; 03499 03500 } 03501 03502 VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_DEV dev, 03503 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data) 03504 { 03505 VL53L0X_Error status = VL53L0X_ERROR_NONE; 03506 uint8_t device_range_status; 03507 uint8_t range_fractional_enable; 03508 uint8_t pal_range_status; 03509 uint8_t x_talk_compensation_enable; 03510 uint16_t ambient_rate; 03511 FixPoint1616_t signal_rate; 03512 uint16_t x_talk_compensation_rate_mega_cps; 03513 uint16_t effective_spad_rtn_count; 03514 uint16_t tmpuint16; 03515 uint16_t xtalk_range_milli_meter; 03516 uint16_t linearity_corrective_gain; 03517 uint8_t localBuffer[12]; 03518 VL53L0X_RangingMeasurementData_t last_range_data_buffer; 03519 03520 LOG_FUNCTION_START(""); 03521 03522 /* 03523 * use multi read even if some registers are not useful, result will 03524 * be more efficient 03525 * start reading at 0x14 dec20 03526 * end reading at 0x21 dec33 total 14 bytes to read 03527 */ 03528 status = VL53L0X_read_multi(dev, 0x14, localBuffer, 12); 03529 03530 if (status == VL53L0X_ERROR_NONE) { 03531 03532 p_ranging_measurement_data->ZoneId = 0; /* Only one zone */ 03533 p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */ 03534 03535 tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]); 03536 /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional 03537 *(format 11.2) else no fractional 03538 */ 03539 03540 p_ranging_measurement_data->MeasurementTimeUsec = 0; 03541 03542 signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616( 03543 VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6])); 03544 /* peak_signal_count_rate_rtn_mcps */ 03545 p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate; 03546 03547 ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]); 03548 p_ranging_measurement_data->AmbientRateRtnMegaCps = 03549 VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate); 03550 03551 effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], 03552 localBuffer[2]); 03553 /* EffectiveSpadRtnCount is 8.8 format */ 03554 p_ranging_measurement_data->EffectiveSpadRtnCount = 03555 effective_spad_rtn_count; 03556 03557 device_range_status = localBuffer[0]; 03558 03559 /* Get Linearity Corrective Gain */ 03560 linearity_corrective_gain = PALDevDataGet(dev, 03561 LinearityCorrectiveGain); 03562 03563 /* Get ranging configuration */ 03564 range_fractional_enable = PALDevDataGet(dev, 03565 RangeFractionalEnable); 03566 03567 if (linearity_corrective_gain != 1000) { 03568 03569 tmpuint16 = (uint16_t)((linearity_corrective_gain 03570 * tmpuint16 + 500) / 1000); 03571 03572 /* Implement Xtalk */ 03573 VL53L0X_GETPARAMETERFIELD(dev, 03574 XTalkCompensationRateMegaCps, 03575 x_talk_compensation_rate_mega_cps); 03576 VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable, 03577 x_talk_compensation_enable); 03578 03579 if (x_talk_compensation_enable) { 03580 03581 if ((signal_rate 03582 - ((x_talk_compensation_rate_mega_cps 03583 * effective_spad_rtn_count) >> 8)) 03584 <= 0) { 03585 if (range_fractional_enable) { 03586 xtalk_range_milli_meter = 8888; 03587 } else { 03588 xtalk_range_milli_meter = 8888 << 2; 03589 } 03590 } else { 03591 xtalk_range_milli_meter = 03592 (tmpuint16 * signal_rate) 03593 / (signal_rate 03594 - ((x_talk_compensation_rate_mega_cps 03595 * effective_spad_rtn_count) 03596 >> 8)); 03597 } 03598 03599 tmpuint16 = xtalk_range_milli_meter; 03600 } 03601 03602 } 03603 03604 if (range_fractional_enable) { 03605 p_ranging_measurement_data->RangeMilliMeter = 03606 (uint16_t)((tmpuint16) >> 2); 03607 p_ranging_measurement_data->RangeFractionalPart = 03608 (uint8_t)((tmpuint16 & 0x03) << 6); 03609 } else { 03610 p_ranging_measurement_data->RangeMilliMeter = tmpuint16; 03611 p_ranging_measurement_data->RangeFractionalPart = 0; 03612 } 03613 03614 /* 03615 * For a standard definition of RangeStatus, this should 03616 * return 0 in case of good result after a ranging 03617 * The range status depends on the device so call a device 03618 * specific function to obtain the right Status. 03619 */ 03620 status |= VL53L0X_get_pal_range_status(dev, device_range_status, 03621 signal_rate, effective_spad_rtn_count, 03622 p_ranging_measurement_data, &pal_range_status); 03623 03624 if (status == VL53L0X_ERROR_NONE) { 03625 p_ranging_measurement_data->RangeStatus = pal_range_status; 03626 } 03627 03628 } 03629 03630 if (status == VL53L0X_ERROR_NONE) { 03631 /* Copy last read data into Dev buffer */ 03632 last_range_data_buffer = PALDevDataGet(dev, LastRangeMeasure); 03633 03634 last_range_data_buffer.RangeMilliMeter = 03635 p_ranging_measurement_data->RangeMilliMeter; 03636 last_range_data_buffer.RangeFractionalPart = 03637 p_ranging_measurement_data->RangeFractionalPart; 03638 last_range_data_buffer.RangeDMaxMilliMeter = 03639 p_ranging_measurement_data->RangeDMaxMilliMeter; 03640 last_range_data_buffer.MeasurementTimeUsec = 03641 p_ranging_measurement_data->MeasurementTimeUsec; 03642 last_range_data_buffer.SignalRateRtnMegaCps = 03643 p_ranging_measurement_data->SignalRateRtnMegaCps; 03644 last_range_data_buffer.AmbientRateRtnMegaCps = 03645 p_ranging_measurement_data->AmbientRateRtnMegaCps; 03646 last_range_data_buffer.EffectiveSpadRtnCount = 03647 p_ranging_measurement_data->EffectiveSpadRtnCount; 03648 last_range_data_buffer.RangeStatus = 03649 p_ranging_measurement_data->RangeStatus; 03650 03651 PALDevDataSet(dev, LastRangeMeasure, last_range_data_buffer); 03652 } 03653 03654 LOG_FUNCTION_END(status); 03655 return status; 03656 } 03657 03658 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_DEV dev, 03659 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data) 03660 { 03661 VL53L0X_Error status = VL53L0X_ERROR_NONE; 03662 03663 LOG_FUNCTION_START(""); 03664 03665 /* This function will do a complete single ranging 03666 * Here we fix the mode! */ 03667 status = VL53L0X_set_device_mode(dev, VL53L0X_DEVICEMODE_SINGLE_RANGING); 03668 03669 if (status == VL53L0X_ERROR_NONE) { 03670 status = VL53L0X_perform_single_measurement(dev); 03671 } 03672 03673 if (status == VL53L0X_ERROR_NONE) { 03674 status = VL53L0X_get_ranging_measurement_data(dev, 03675 p_ranging_measurement_data); 03676 } 03677 03678 if (status == VL53L0X_ERROR_NONE) { 03679 status = VL53L0X_clear_interrupt_mask(dev, 0); 03680 } 03681 03682 LOG_FUNCTION_END(status); 03683 return status; 03684 } 03685 03686 VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV dev, 03687 uint16_t *p_ref_signal_rate) 03688 { 03689 VL53L0X_Error status = VL53L0X_ERROR_NONE; 03690 VL53L0X_RangingMeasurementData_t ranging_measurement_data; 03691 03692 uint8_t sequence_config = 0; 03693 03694 /* store the value of the sequence config, 03695 * this will be reset before the end of the function 03696 */ 03697 03698 sequence_config = PALDevDataGet(dev, SequenceConfig); 03699 03700 /* 03701 * This function performs a reference signal rate measurement. 03702 */ 03703 if (status == VL53L0X_ERROR_NONE) { 03704 status = VL53L0X_write_byte(dev, 03705 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0); 03706 } 03707 03708 if (status == VL53L0X_ERROR_NONE) { 03709 status = VL53L0X_perform_single_ranging_measurement(dev, 03710 &ranging_measurement_data); 03711 } 03712 03713 if (status == VL53L0X_ERROR_NONE) { 03714 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 03715 } 03716 03717 if (status == VL53L0X_ERROR_NONE) { 03718 status = VL53L0X_read_word(dev, 03719 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, 03720 p_ref_signal_rate); 03721 } 03722 03723 if (status == VL53L0X_ERROR_NONE) { 03724 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 03725 } 03726 03727 if (status == VL53L0X_ERROR_NONE) { 03728 /* restore the previous Sequence Config */ 03729 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 03730 sequence_config); 03731 if (status == VL53L0X_ERROR_NONE) { 03732 PALDevDataSet(dev, SequenceConfig, sequence_config); 03733 } 03734 } 03735 03736 return status; 03737 } 03738 03739 VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(VL53L0X_DEV dev, 03740 uint32_t *ref_spad_count, 03741 uint8_t *is_aperture_spads) 03742 { 03743 VL53L0X_Error status = VL53L0X_ERROR_NONE; 03744 uint8_t last_spad_array[6]; 03745 uint8_t start_select = 0xB4; 03746 uint32_t minimum_spad_count = 3; 03747 uint32_t max_spad_count = 44; 03748 uint32_t current_spad_index = 0; 03749 uint32_t last_spad_index = 0; 03750 int32_t next_good_spad = 0; 03751 uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */ 03752 uint16_t peak_signal_rate_ref; 03753 uint32_t need_apt_spads = 0; 03754 uint32_t index = 0; 03755 uint32_t spad_array_size = 6; 03756 uint32_t signal_rate_diff = 0; 03757 uint32_t last_signal_rate_diff = 0; 03758 uint8_t complete = 0; 03759 uint8_t vhv_settings = 0; 03760 uint8_t phase_cal = 0; 03761 uint32_t ref_spad_count_int = 0; 03762 uint8_t is_aperture_spads_int = 0; 03763 03764 /* 03765 * The reference SPAD initialization procedure determines the minimum 03766 * amount of reference spads to be enables to achieve a target reference 03767 * signal rate and should be performed once during initialization. 03768 * 03769 * Either aperture or non-aperture spads are applied but never both. 03770 * Firstly non-aperture spads are set, begining with 5 spads, and 03771 * increased one spad at a time until the closest measurement to the 03772 * target rate is achieved. 03773 * 03774 * If the target rate is exceeded when 5 non-aperture spads are enabled, 03775 * initialization is performed instead with aperture spads. 03776 * 03777 * When setting spads, a 'Good Spad Map' is applied. 03778 * 03779 * This procedure operates within a SPAD window of interest of a maximum 03780 * 44 spads. 03781 * The start point is currently fixed to 180, which lies towards the end 03782 * of the non-aperture quadrant and runs in to the adjacent aperture 03783 * quadrant. 03784 */ 03785 target_ref_rate = PALDevDataGet(dev, targetRefRate); 03786 03787 /* 03788 * Initialize Spad arrays. 03789 * Currently the good spad map is initialised to 'All good'. 03790 * This is a short term implementation. The good spad map will be 03791 * provided as an input. 03792 * Note that there are 6 bytes. Only the first 44 bits will be used to 03793 * represent spads. 03794 */ 03795 for (index = 0; index < spad_array_size; index++) { 03796 dev->Data .SpadData .RefSpadEnables [index] = 0; 03797 } 03798 03799 03800 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 03801 03802 if (status == VL53L0X_ERROR_NONE) { 03803 status = VL53L0X_write_byte(dev, 03804 VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); 03805 } 03806 03807 if (status == VL53L0X_ERROR_NONE) { 03808 status = VL53L0X_write_byte(dev, 03809 VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); 03810 } 03811 03812 if (status == VL53L0X_ERROR_NONE) { 03813 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 03814 } 03815 03816 if (status == VL53L0X_ERROR_NONE) { 03817 status = VL53L0X_write_byte(dev, 03818 VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, 03819 start_select); 03820 } 03821 03822 if (status == VL53L0X_ERROR_NONE) { 03823 status = VL53L0X_write_byte(dev, 03824 VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0); 03825 } 03826 03827 /* Perform ref calibration */ 03828 if (status == VL53L0X_ERROR_NONE) { 03829 status = VL53L0X_perform_ref_calibration(dev, &vhv_settings, 03830 &phase_cal, 0); 03831 } 03832 03833 if (status == VL53L0X_ERROR_NONE) { 03834 /* Enable Minimum NON-APERTURE Spads */ 03835 current_spad_index = 0; 03836 last_spad_index = current_spad_index; 03837 need_apt_spads = 0; 03838 status = enable_ref_spads(dev, 03839 need_apt_spads, 03840 dev->Data .SpadData .RefGoodSpadMap , 03841 dev->Data .SpadData .RefSpadEnables , 03842 spad_array_size, 03843 start_select, 03844 current_spad_index, 03845 minimum_spad_count, 03846 &last_spad_index); 03847 } 03848 03849 if (status == VL53L0X_ERROR_NONE) { 03850 current_spad_index = last_spad_index; 03851 03852 status = perform_ref_signal_measurement(dev, 03853 &peak_signal_rate_ref); 03854 if ((status == VL53L0X_ERROR_NONE) && 03855 (peak_signal_rate_ref > target_ref_rate)) { 03856 /* Signal rate measurement too high, 03857 * switch to APERTURE SPADs */ 03858 03859 for (index = 0; index < spad_array_size; index++) { 03860 dev->Data .SpadData .RefSpadEnables [index] = 0; 03861 } 03862 03863 03864 /* Increment to the first APERTURE spad */ 03865 while ((is_aperture(start_select + current_spad_index) 03866 == 0) && (current_spad_index < max_spad_count)) { 03867 current_spad_index++; 03868 } 03869 03870 need_apt_spads = 1; 03871 03872 status = enable_ref_spads(dev, 03873 need_apt_spads, 03874 dev->Data .SpadData .RefGoodSpadMap , 03875 dev->Data .SpadData .RefSpadEnables , 03876 spad_array_size, 03877 start_select, 03878 current_spad_index, 03879 minimum_spad_count, 03880 &last_spad_index); 03881 03882 if (status == VL53L0X_ERROR_NONE) { 03883 current_spad_index = last_spad_index; 03884 status = perform_ref_signal_measurement(dev, 03885 &peak_signal_rate_ref); 03886 03887 if ((status == VL53L0X_ERROR_NONE) && 03888 (peak_signal_rate_ref > target_ref_rate)) { 03889 /* Signal rate still too high after 03890 * setting the minimum number of 03891 * APERTURE spads. Can do no more 03892 * therefore set the min number of 03893 * aperture spads as the result. 03894 */ 03895 is_aperture_spads_int = 1; 03896 ref_spad_count_int = minimum_spad_count; 03897 } 03898 } 03899 } else { 03900 need_apt_spads = 0; 03901 } 03902 } 03903 03904 if ((status == VL53L0X_ERROR_NONE) && 03905 (peak_signal_rate_ref < target_ref_rate)) { 03906 /* At this point, the minimum number of either aperture 03907 * or non-aperture spads have been set. Proceed to add 03908 * spads and perform measurements until the target 03909 * reference is reached. 03910 */ 03911 is_aperture_spads_int = need_apt_spads; 03912 ref_spad_count_int = minimum_spad_count; 03913 03914 memcpy(last_spad_array, dev->Data .SpadData .RefSpadEnables , 03915 spad_array_size); 03916 last_signal_rate_diff = abs(peak_signal_rate_ref - 03917 target_ref_rate); 03918 complete = 0; 03919 03920 while (!complete) { 03921 get_next_good_spad( 03922 dev->Data .SpadData .RefGoodSpadMap , 03923 spad_array_size, current_spad_index, 03924 &next_good_spad); 03925 03926 if (next_good_spad == -1) { 03927 status = VL53L0X_ERROR_REF_SPAD_INIT; 03928 break; 03929 } 03930 03931 /* Cannot combine Aperture and Non-Aperture spads, so 03932 * ensure the current spad is of the correct type. 03933 */ 03934 if (is_aperture((uint32_t)start_select + next_good_spad) != 03935 need_apt_spads) { 03936 /* At this point we have enabled the maximum 03937 * number of Aperture spads. 03938 */ 03939 complete = 1; 03940 break; 03941 } 03942 03943 (ref_spad_count_int)++; 03944 03945 current_spad_index = next_good_spad; 03946 status = enable_spad_bit( 03947 dev->Data .SpadData .RefSpadEnables , 03948 spad_array_size, current_spad_index); 03949 03950 if (status == VL53L0X_ERROR_NONE) { 03951 current_spad_index++; 03952 /* Proceed to apply the additional spad and 03953 * perform measurement. */ 03954 status = set_ref_spad_map(dev, 03955 dev->Data .SpadData .RefSpadEnables ); 03956 } 03957 03958 if (status != VL53L0X_ERROR_NONE) { 03959 break; 03960 } 03961 03962 status = perform_ref_signal_measurement(dev, 03963 &peak_signal_rate_ref); 03964 03965 if (status != VL53L0X_ERROR_NONE) { 03966 break; 03967 } 03968 03969 signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate); 03970 03971 if (peak_signal_rate_ref > target_ref_rate) { 03972 /* Select the spad map that provides the 03973 * measurement closest to the target rate, 03974 * either above or below it. 03975 */ 03976 if (signal_rate_diff > last_signal_rate_diff) { 03977 /* Previous spad map produced a closer 03978 * measurement, so choose this. */ 03979 status = set_ref_spad_map(dev, 03980 last_spad_array); 03981 memcpy( 03982 dev->Data .SpadData .RefSpadEnables , 03983 last_spad_array, spad_array_size); 03984 03985 (ref_spad_count_int)--; 03986 } 03987 complete = 1; 03988 } else { 03989 /* Continue to add spads */ 03990 last_signal_rate_diff = signal_rate_diff; 03991 memcpy(last_spad_array, 03992 dev->Data .SpadData .RefSpadEnables , 03993 spad_array_size); 03994 } 03995 03996 } /* while */ 03997 } 03998 03999 if (status == VL53L0X_ERROR_NONE) { 04000 *ref_spad_count = ref_spad_count_int; 04001 *is_aperture_spads = is_aperture_spads_int; 04002 04003 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1); 04004 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04005 ReferenceSpadCount, (uint8_t)(*ref_spad_count)); 04006 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04007 ReferenceSpadType, *is_aperture_spads); 04008 } 04009 04010 return status; 04011 } 04012 04013 VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(VL53L0X_DEV dev, 04014 uint32_t count, uint8_t is_aperture_spads) 04015 { 04016 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04017 uint32_t current_spad_index = 0; 04018 uint8_t start_select = 0xB4; 04019 uint32_t spad_array_size = 6; 04020 uint32_t max_spad_count = 44; 04021 uint32_t last_spad_index; 04022 uint32_t index; 04023 04024 /* 04025 * This function applies a requested number of reference spads, either 04026 * aperture or 04027 * non-aperture, as requested. 04028 * The good spad map will be applied. 04029 */ 04030 04031 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 04032 04033 if (status == VL53L0X_ERROR_NONE) { 04034 status = VL53L0X_write_byte(dev, 04035 VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); 04036 } 04037 04038 if (status == VL53L0X_ERROR_NONE) { 04039 status = VL53L0X_write_byte(dev, 04040 VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); 04041 } 04042 04043 if (status == VL53L0X_ERROR_NONE) { 04044 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 04045 } 04046 04047 if (status == VL53L0X_ERROR_NONE) { 04048 status = VL53L0X_write_byte(dev, 04049 VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, 04050 start_select); 04051 } 04052 04053 for (index = 0; index < spad_array_size; index++) { 04054 dev->Data .SpadData .RefSpadEnables [index] = 0; 04055 } 04056 04057 if (is_aperture_spads) { 04058 /* Increment to the first APERTURE spad */ 04059 while ((is_aperture(start_select + current_spad_index) == 0) && 04060 (current_spad_index < max_spad_count)) { 04061 current_spad_index++; 04062 } 04063 } 04064 status = enable_ref_spads(dev, 04065 is_aperture_spads, 04066 dev->Data .SpadData .RefGoodSpadMap , 04067 dev->Data .SpadData .RefSpadEnables , 04068 spad_array_size, 04069 start_select, 04070 current_spad_index, 04071 count, 04072 &last_spad_index); 04073 04074 if (status == VL53L0X_ERROR_NONE) { 04075 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1); 04076 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04077 ReferenceSpadCount, (uint8_t)(count)); 04078 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04079 ReferenceSpadType, is_aperture_spads); 04080 } 04081 04082 return status; 04083 } 04084 04085 VL53L0X_Error VL53L0X::VL53L0X_wait_device_booted(VL53L0X_DEV dev) 04086 { 04087 VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED; 04088 LOG_FUNCTION_START(""); 04089 04090 /* not implemented on VL53L0X */ 04091 04092 LOG_FUNCTION_END(status); 04093 return status; 04094 } 04095 04096 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV dev, uint8_t *p_vhv_settings, 04097 uint8_t *p_phase_cal) 04098 { 04099 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04100 LOG_FUNCTION_START(""); 04101 04102 status = VL53L0X_perform_ref_calibration(dev, p_vhv_settings, 04103 p_phase_cal, 1); 04104 04105 LOG_FUNCTION_END(status); 04106 return status; 04107 } 04108 04109 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(VL53L0X_DEV dev, 04110 uint32_t *ref_spad_count, uint8_t *is_aperture_spads) 04111 { 04112 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04113 LOG_FUNCTION_START(""); 04114 04115 status = wrapped_VL53L0X_perform_ref_spad_management(dev, ref_spad_count, 04116 is_aperture_spads); 04117 04118 LOG_FUNCTION_END(status); 04119 04120 return status; 04121 } 04122 04123 /* Group PAL Init Functions */ 04124 VL53L0X_Error VL53L0X::VL53L0X_set_device_address(VL53L0X_DEV dev, uint8_t device_address) 04125 { 04126 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04127 LOG_FUNCTION_START(""); 04128 04129 status = VL53L0X_write_byte(dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, 04130 device_address / 2); 04131 04132 LOG_FUNCTION_END(status); 04133 return status; 04134 } 04135 04136 VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config(VL53L0X_DEV dev, uint8_t pin, 04137 VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality, 04138 VL53L0X_InterruptPolarity polarity) 04139 { 04140 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04141 uint8_t data; 04142 04143 LOG_FUNCTION_START(""); 04144 04145 if (pin != 0) { 04146 status = VL53L0X_ERROR_GPIO_NOT_EXISTING; 04147 } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) { 04148 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) { 04149 data = 0x10; 04150 } else { 04151 data = 1; 04152 } 04153 04154 status = VL53L0X_write_byte(dev, 04155 VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data); 04156 04157 } else { 04158 if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) { 04159 04160 status |= VL53L0X_write_byte(dev, 0xff, 0x01); 04161 status |= VL53L0X_write_byte(dev, 0x00, 0x00); 04162 04163 status |= VL53L0X_write_byte(dev, 0xff, 0x00); 04164 status |= VL53L0X_write_byte(dev, 0x80, 0x01); 04165 status |= VL53L0X_write_byte(dev, 0x85, 0x02); 04166 04167 status |= VL53L0X_write_byte(dev, 0xff, 0x04); 04168 status |= VL53L0X_write_byte(dev, 0xcd, 0x00); 04169 status |= VL53L0X_write_byte(dev, 0xcc, 0x11); 04170 04171 status |= VL53L0X_write_byte(dev, 0xff, 0x07); 04172 status |= VL53L0X_write_byte(dev, 0xbe, 0x00); 04173 04174 status |= VL53L0X_write_byte(dev, 0xff, 0x06); 04175 status |= VL53L0X_write_byte(dev, 0xcc, 0x09); 04176 04177 status |= VL53L0X_write_byte(dev, 0xff, 0x00); 04178 status |= VL53L0X_write_byte(dev, 0xff, 0x01); 04179 status |= VL53L0X_write_byte(dev, 0x00, 0x00); 04180 04181 } else { 04182 04183 if (status == VL53L0X_ERROR_NONE) { 04184 switch (functionality) { 04185 case VL53L0X_GPIOFUNCTIONALITY_OFF: 04186 data = 0x00; 04187 break; 04188 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW: 04189 data = 0x01; 04190 break; 04191 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH: 04192 data = 0x02; 04193 break; 04194 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT: 04195 data = 0x03; 04196 break; 04197 case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY: 04198 data = 0x04; 04199 break; 04200 default: 04201 status = 04202 VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED; 04203 } 04204 } 04205 04206 if (status == VL53L0X_ERROR_NONE) { 04207 status = VL53L0X_write_byte(dev, 04208 VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data); 04209 } 04210 04211 if (status == VL53L0X_ERROR_NONE) { 04212 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) { 04213 data = 0; 04214 } else { 04215 data = (uint8_t)(1 << 4); 04216 } 04217 status = VL53L0X_update_byte(dev, 04218 VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data); 04219 } 04220 04221 if (status == VL53L0X_ERROR_NONE) { 04222 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04223 Pin0GpioFunctionality, functionality); 04224 } 04225 04226 if (status == VL53L0X_ERROR_NONE) { 04227 status = VL53L0X_clear_interrupt_mask(dev, 0); 04228 } 04229 } 04230 } 04231 LOG_FUNCTION_END(status); 04232 return status; 04233 } 04234 04235 VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable(VL53L0X_DEV dev, uint8_t *p_enabled) 04236 { 04237 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04238 LOG_FUNCTION_START(""); 04239 04240 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled); 04241 04242 if (status == VL53L0X_ERROR_NONE) { 04243 *p_enabled = (*p_enabled & 1); 04244 } 04245 04246 LOG_FUNCTION_END(status); 04247 return status; 04248 } 04249 04250 uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks) 04251 { 04252 /*! 04253 * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format 04254 */ 04255 04256 uint16_t encoded_timeout = 0; 04257 uint32_t ls_byte = 0; 04258 uint16_t ms_byte = 0; 04259 04260 if (timeout_macro_clks > 0) { 04261 ls_byte = timeout_macro_clks - 1; 04262 04263 while ((ls_byte & 0xFFFFFF00) > 0) { 04264 ls_byte = ls_byte >> 1; 04265 ms_byte++; 04266 } 04267 04268 encoded_timeout = (ms_byte << 8) 04269 + (uint16_t)(ls_byte & 0x000000FF); 04270 } 04271 04272 return encoded_timeout; 04273 04274 } 04275 04276 VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV dev, 04277 VL53L0X_SequenceStepId sequence_step_id, 04278 uint32_t timeout_micro_secs) 04279 { 04280 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04281 uint8_t current_vcsel_pulse_period_p_clk; 04282 uint8_t msrc_encoded_time_out; 04283 uint16_t pre_range_encoded_time_out; 04284 uint16_t pre_range_time_out_m_clks; 04285 uint16_t msrc_range_time_out_m_clks; 04286 uint32_t final_range_time_out_m_clks; 04287 uint16_t final_range_encoded_time_out; 04288 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 04289 04290 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) || 04291 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) || 04292 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) { 04293 04294 status = VL53L0X_get_vcsel_pulse_period(dev, 04295 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 04296 ¤t_vcsel_pulse_period_p_clk); 04297 04298 if (status == VL53L0X_ERROR_NONE) { 04299 msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(dev, 04300 timeout_micro_secs, 04301 (uint8_t)current_vcsel_pulse_period_p_clk); 04302 04303 if (msrc_range_time_out_m_clks > 256) { 04304 msrc_encoded_time_out = 255; 04305 } else { 04306 msrc_encoded_time_out = 04307 (uint8_t)msrc_range_time_out_m_clks - 1; 04308 } 04309 04310 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04311 LastEncodedTimeout, 04312 msrc_encoded_time_out); 04313 } 04314 04315 if (status == VL53L0X_ERROR_NONE) { 04316 status = VL53L0X_write_byte(dev, 04317 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, 04318 msrc_encoded_time_out); 04319 } 04320 } else { 04321 04322 if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { 04323 04324 if (status == VL53L0X_ERROR_NONE) { 04325 status = VL53L0X_get_vcsel_pulse_period(dev, 04326 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 04327 ¤t_vcsel_pulse_period_p_clk); 04328 pre_range_time_out_m_clks = 04329 VL53L0X_calc_timeout_mclks(dev, 04330 timeout_micro_secs, 04331 (uint8_t)current_vcsel_pulse_period_p_clk); 04332 pre_range_encoded_time_out = VL53L0X_encode_timeout( 04333 pre_range_time_out_m_clks); 04334 04335 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, 04336 LastEncodedTimeout, 04337 pre_range_encoded_time_out); 04338 } 04339 04340 if (status == VL53L0X_ERROR_NONE) { 04341 status = VL53L0X_write_word(dev, 04342 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 04343 pre_range_encoded_time_out); 04344 } 04345 04346 if (status == VL53L0X_ERROR_NONE) { 04347 VL53L0X_SETDEVICESPECIFICPARAMETER( 04348 dev, 04349 PreRangeTimeoutMicroSecs, 04350 timeout_micro_secs); 04351 } 04352 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { 04353 04354 /* For the final range timeout, the pre-range timeout 04355 * must be added. To do this both final and pre-range 04356 * timeouts must be expressed in macro periods MClks 04357 * because they have different vcsel periods. 04358 */ 04359 04360 VL53L0X_get_sequence_step_enables(dev, 04361 &scheduler_sequence_steps); 04362 pre_range_time_out_m_clks = 0; 04363 if (scheduler_sequence_steps.PreRangeOn) { 04364 04365 /* Retrieve PRE-RANGE VCSEL Period */ 04366 status = VL53L0X_get_vcsel_pulse_period(dev, 04367 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 04368 ¤t_vcsel_pulse_period_p_clk); 04369 04370 /* Retrieve PRE-RANGE Timeout in Macro periods 04371 * (MCLKS) */ 04372 if (status == VL53L0X_ERROR_NONE) { 04373 status = VL53L0X_read_word(dev, 0x51, 04374 &pre_range_encoded_time_out); 04375 pre_range_time_out_m_clks = 04376 VL53L0X_decode_timeout( 04377 pre_range_encoded_time_out); 04378 } 04379 } 04380 04381 /* Calculate FINAL RANGE Timeout in Macro Periods 04382 * (MCLKS) and add PRE-RANGE value 04383 */ 04384 if (status == VL53L0X_ERROR_NONE) { 04385 status = VL53L0X_get_vcsel_pulse_period(dev, 04386 VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 04387 ¤t_vcsel_pulse_period_p_clk); 04388 } 04389 if (status == VL53L0X_ERROR_NONE) { 04390 final_range_time_out_m_clks = 04391 VL53L0X_calc_timeout_mclks(dev, 04392 timeout_micro_secs, 04393 (uint8_t) current_vcsel_pulse_period_p_clk); 04394 04395 final_range_time_out_m_clks += pre_range_time_out_m_clks; 04396 04397 final_range_encoded_time_out = 04398 VL53L0X_encode_timeout(final_range_time_out_m_clks); 04399 04400 if (status == VL53L0X_ERROR_NONE) { 04401 status = VL53L0X_write_word(dev, 0x71, 04402 final_range_encoded_time_out); 04403 } 04404 04405 if (status == VL53L0X_ERROR_NONE) { 04406 VL53L0X_SETDEVICESPECIFICPARAMETER( 04407 dev, 04408 FinalRangeTimeoutMicroSecs, 04409 timeout_micro_secs); 04410 } 04411 } 04412 } else { 04413 status = VL53L0X_ERROR_INVALID_PARAMS; 04414 } 04415 04416 } 04417 return status; 04418 } 04419 04420 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev, 04421 uint32_t measurement_timing_budget_micro_seconds) 04422 { 04423 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04424 uint32_t final_range_timing_budget_micro_seconds; 04425 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 04426 uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000; 04427 uint32_t start_overhead_micro_seconds = 1910; 04428 uint32_t end_overhead_micro_seconds = 960; 04429 uint32_t msrc_overhead_micro_seconds = 660; 04430 uint32_t tcc_overhead_micro_seconds = 590; 04431 uint32_t dss_overhead_micro_seconds = 690; 04432 uint32_t pre_range_overhead_micro_seconds = 660; 04433 uint32_t final_range_overhead_micro_seconds = 550; 04434 uint32_t pre_range_timeout_micro_seconds = 0; 04435 uint32_t c_min_timing_budget_micro_seconds = 20000; 04436 uint32_t sub_timeout = 0; 04437 04438 LOG_FUNCTION_START(""); 04439 04440 if (measurement_timing_budget_micro_seconds 04441 < c_min_timing_budget_micro_seconds) { 04442 status = VL53L0X_ERROR_INVALID_PARAMS; 04443 return status; 04444 } 04445 04446 final_range_timing_budget_micro_seconds = 04447 measurement_timing_budget_micro_seconds - 04448 (start_overhead_micro_seconds + end_overhead_micro_seconds); 04449 04450 status = VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps); 04451 04452 if (status == VL53L0X_ERROR_NONE && 04453 (scheduler_sequence_steps.TccOn || 04454 scheduler_sequence_steps.MsrcOn || 04455 scheduler_sequence_steps.DssOn)) { 04456 04457 /* TCC, MSRC and DSS all share the same timeout */ 04458 status = get_sequence_step_timeout(dev, 04459 VL53L0X_SEQUENCESTEP_MSRC, 04460 &msrc_dcc_tcc_timeout_micro_seconds); 04461 04462 /* Subtract the TCC, MSRC and DSS timeouts if they are 04463 * enabled. */ 04464 04465 if (status != VL53L0X_ERROR_NONE) { 04466 return status; 04467 } 04468 04469 /* TCC */ 04470 if (scheduler_sequence_steps.TccOn) { 04471 04472 sub_timeout = msrc_dcc_tcc_timeout_micro_seconds 04473 + tcc_overhead_micro_seconds; 04474 04475 if (sub_timeout < 04476 final_range_timing_budget_micro_seconds) { 04477 final_range_timing_budget_micro_seconds -= 04478 sub_timeout; 04479 } else { 04480 /* Requested timeout too big. */ 04481 status = VL53L0X_ERROR_INVALID_PARAMS; 04482 } 04483 } 04484 04485 if (status != VL53L0X_ERROR_NONE) { 04486 LOG_FUNCTION_END(status); 04487 return status; 04488 } 04489 04490 /* DSS */ 04491 if (scheduler_sequence_steps.DssOn) { 04492 04493 sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds + 04494 dss_overhead_micro_seconds); 04495 04496 if (sub_timeout < final_range_timing_budget_micro_seconds) { 04497 final_range_timing_budget_micro_seconds 04498 -= sub_timeout; 04499 } else { 04500 /* Requested timeout too big. */ 04501 status = VL53L0X_ERROR_INVALID_PARAMS; 04502 } 04503 } else if (scheduler_sequence_steps.MsrcOn) { 04504 /* MSRC */ 04505 sub_timeout = msrc_dcc_tcc_timeout_micro_seconds + 04506 msrc_overhead_micro_seconds; 04507 04508 if (sub_timeout < final_range_timing_budget_micro_seconds) { 04509 final_range_timing_budget_micro_seconds 04510 -= sub_timeout; 04511 } else { 04512 /* Requested timeout too big. */ 04513 status = VL53L0X_ERROR_INVALID_PARAMS; 04514 } 04515 } 04516 04517 } 04518 04519 if (status != VL53L0X_ERROR_NONE) { 04520 LOG_FUNCTION_END(status); 04521 return status; 04522 } 04523 04524 if (scheduler_sequence_steps.PreRangeOn) { 04525 04526 /* Subtract the Pre-range timeout if enabled. */ 04527 04528 status = get_sequence_step_timeout(dev, 04529 VL53L0X_SEQUENCESTEP_PRE_RANGE, 04530 &pre_range_timeout_micro_seconds); 04531 04532 sub_timeout = pre_range_timeout_micro_seconds + 04533 pre_range_overhead_micro_seconds; 04534 04535 if (sub_timeout < final_range_timing_budget_micro_seconds) { 04536 final_range_timing_budget_micro_seconds -= sub_timeout; 04537 } else { 04538 /* Requested timeout too big. */ 04539 status = VL53L0X_ERROR_INVALID_PARAMS; 04540 } 04541 } 04542 04543 04544 if (status == VL53L0X_ERROR_NONE && 04545 scheduler_sequence_steps.FinalRangeOn) { 04546 04547 final_range_timing_budget_micro_seconds -= 04548 final_range_overhead_micro_seconds; 04549 04550 /* Final Range Timeout 04551 * Note that the final range timeout is determined by the timing 04552 * budget and the sum of all other timeouts within the sequence. 04553 * If there is no room for the final range timeout, then an error 04554 * will be set. Otherwise the remaining time will be applied to 04555 * the final range. 04556 */ 04557 status = set_sequence_step_timeout(dev, 04558 VL53L0X_SEQUENCESTEP_FINAL_RANGE, 04559 final_range_timing_budget_micro_seconds); 04560 04561 VL53L0X_SETPARAMETERFIELD(dev, 04562 MeasurementTimingBudgetMicroSeconds, 04563 measurement_timing_budget_micro_seconds); 04564 } 04565 04566 LOG_FUNCTION_END(status); 04567 04568 return status; 04569 } 04570 04571 VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev, 04572 uint32_t measurement_timing_budget_micro_seconds) 04573 { 04574 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04575 LOG_FUNCTION_START(""); 04576 04577 status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(dev, 04578 measurement_timing_budget_micro_seconds); 04579 04580 LOG_FUNCTION_END(status); 04581 04582 return status; 04583 } 04584 04585 VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_DEV dev, 04586 VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled) 04587 { 04588 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04589 uint8_t sequence_config = 0; 04590 uint8_t sequence_config_new = 0; 04591 uint32_t measurement_timing_budget_micro_seconds; 04592 LOG_FUNCTION_START(""); 04593 04594 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 04595 &sequence_config); 04596 04597 sequence_config_new = sequence_config; 04598 04599 if (status == VL53L0X_ERROR_NONE) { 04600 if (sequence_step_enabled == 1) { 04601 04602 /* Enable requested sequence step 04603 */ 04604 switch (sequence_step_id) { 04605 case VL53L0X_SEQUENCESTEP_TCC: 04606 sequence_config_new |= 0x10; 04607 break; 04608 case VL53L0X_SEQUENCESTEP_DSS: 04609 sequence_config_new |= 0x28; 04610 break; 04611 case VL53L0X_SEQUENCESTEP_MSRC: 04612 sequence_config_new |= 0x04; 04613 break; 04614 case VL53L0X_SEQUENCESTEP_PRE_RANGE: 04615 sequence_config_new |= 0x40; 04616 break; 04617 case VL53L0X_SEQUENCESTEP_FINAL_RANGE: 04618 sequence_config_new |= 0x80; 04619 break; 04620 default: 04621 status = VL53L0X_ERROR_INVALID_PARAMS; 04622 } 04623 } else { 04624 /* Disable requested sequence step 04625 */ 04626 switch (sequence_step_id) { 04627 case VL53L0X_SEQUENCESTEP_TCC: 04628 sequence_config_new &= 0xef; 04629 break; 04630 case VL53L0X_SEQUENCESTEP_DSS: 04631 sequence_config_new &= 0xd7; 04632 break; 04633 case VL53L0X_SEQUENCESTEP_MSRC: 04634 sequence_config_new &= 0xfb; 04635 break; 04636 case VL53L0X_SEQUENCESTEP_PRE_RANGE: 04637 sequence_config_new &= 0xbf; 04638 break; 04639 case VL53L0X_SEQUENCESTEP_FINAL_RANGE: 04640 sequence_config_new &= 0x7f; 04641 break; 04642 default: 04643 status = VL53L0X_ERROR_INVALID_PARAMS; 04644 } 04645 } 04646 } 04647 04648 if (sequence_config_new != sequence_config) { 04649 /* Apply New Setting */ 04650 if (status == VL53L0X_ERROR_NONE) { 04651 status = VL53L0X_write_byte(dev, 04652 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new); 04653 } 04654 if (status == VL53L0X_ERROR_NONE) { 04655 PALDevDataSet(dev, SequenceConfig, sequence_config_new); 04656 } 04657 04658 04659 /* Recalculate timing budget */ 04660 if (status == VL53L0X_ERROR_NONE) { 04661 VL53L0X_GETPARAMETERFIELD(dev, 04662 MeasurementTimingBudgetMicroSeconds, 04663 measurement_timing_budget_micro_seconds); 04664 04665 VL53L0X_set_measurement_timing_budget_micro_seconds(dev, 04666 measurement_timing_budget_micro_seconds); 04667 } 04668 } 04669 04670 LOG_FUNCTION_END(status); 04671 04672 return status; 04673 } 04674 04675 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id, 04676 uint8_t limit_check_enable) 04677 { 04678 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04679 FixPoint1616_t temp_fix1616 = 0; 04680 uint8_t limit_check_enable_int = 0; 04681 uint8_t limit_check_disable = 0; 04682 uint8_t temp8; 04683 04684 LOG_FUNCTION_START(""); 04685 04686 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { 04687 status = VL53L0X_ERROR_INVALID_PARAMS; 04688 } else { 04689 if (limit_check_enable == 0) { 04690 temp_fix1616 = 0; 04691 limit_check_enable_int = 0; 04692 limit_check_disable = 1; 04693 04694 } else { 04695 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue, 04696 limit_check_id, temp_fix1616); 04697 limit_check_disable = 0; 04698 /* this to be sure to have either 0 or 1 */ 04699 limit_check_enable_int = 1; 04700 } 04701 04702 switch (limit_check_id) { 04703 04704 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: 04705 /* internal computation: */ 04706 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable, 04707 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 04708 limit_check_enable_int); 04709 04710 break; 04711 04712 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: 04713 04714 status = VL53L0X_write_word(dev, 04715 VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 04716 VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616)); 04717 04718 break; 04719 04720 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: 04721 04722 /* internal computation: */ 04723 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable, 04724 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 04725 limit_check_enable_int); 04726 04727 break; 04728 04729 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: 04730 04731 /* internal computation: */ 04732 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable, 04733 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 04734 limit_check_enable_int); 04735 04736 break; 04737 04738 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: 04739 04740 temp8 = (uint8_t)(limit_check_disable << 1); 04741 status = VL53L0X_update_byte(dev, 04742 VL53L0X_REG_MSRC_CONFIG_CONTROL, 04743 0xFE, temp8); 04744 04745 break; 04746 04747 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: 04748 04749 temp8 = (uint8_t)(limit_check_disable << 4); 04750 status = VL53L0X_update_byte(dev, 04751 VL53L0X_REG_MSRC_CONFIG_CONTROL, 04752 0xEF, temp8); 04753 04754 break; 04755 04756 04757 default: 04758 status = VL53L0X_ERROR_INVALID_PARAMS; 04759 04760 } 04761 04762 } 04763 04764 if (status == VL53L0X_ERROR_NONE) { 04765 if (limit_check_enable == 0) { 04766 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable, 04767 limit_check_id, 0); 04768 } else { 04769 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable, 04770 limit_check_id, 1); 04771 } 04772 } 04773 04774 LOG_FUNCTION_END(status); 04775 return status; 04776 } 04777 04778 VL53L0X_Error VL53L0X::VL53L0X_static_init(VL53L0X_DEV dev) 04779 { 04780 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04781 VL53L0X_DeviceParameters_t current_parameters = {0}; 04782 uint8_t *p_tuning_setting_buffer; 04783 uint16_t tempword = 0; 04784 uint8_t tempbyte = 0; 04785 uint8_t use_internal_tuning_settings = 0; 04786 uint32_t count = 0; 04787 uint8_t is_aperture_spads = 0; 04788 uint32_t ref_spad_count = 0; 04789 uint8_t aperture_spads = 0; 04790 uint8_t vcsel_pulse_period_pclk; 04791 uint32_t seq_timeout_micro_secs; 04792 04793 LOG_FUNCTION_START(""); 04794 04795 status = VL53L0X_get_info_from_device(dev, 1); 04796 04797 /* set the ref spad from NVM */ 04798 count = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 04799 ReferenceSpadCount); 04800 aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, 04801 ReferenceSpadType); 04802 04803 /* NVM value invalid */ 04804 if ((aperture_spads > 1) || 04805 ((aperture_spads == 1) && (count > 32)) || 04806 ((aperture_spads == 0) && (count > 12))) { 04807 status = wrapped_VL53L0X_perform_ref_spad_management(dev, &ref_spad_count, 04808 &is_aperture_spads); 04809 } else { 04810 status = VL53L0X_set_reference_spads(dev, count, aperture_spads); 04811 } 04812 04813 04814 /* Initialize tuning settings buffer to prevent compiler warning. */ 04815 p_tuning_setting_buffer = DefaultTuningSettings; 04816 04817 if (status == VL53L0X_ERROR_NONE) { 04818 use_internal_tuning_settings = PALDevDataGet(dev, 04819 UseInternalTuningSettings); 04820 04821 if (use_internal_tuning_settings == 0) { 04822 p_tuning_setting_buffer = PALDevDataGet(dev, 04823 pTuningSettingsPointer); 04824 } else { 04825 p_tuning_setting_buffer = DefaultTuningSettings; 04826 } 04827 04828 } 04829 04830 if (status == VL53L0X_ERROR_NONE) { 04831 status = VL53L0X_load_tuning_settings(dev, p_tuning_setting_buffer); 04832 } 04833 04834 04835 /* Set interrupt config to new sample ready */ 04836 if (status == VL53L0X_ERROR_NONE) { 04837 status = VL53L0X_set_gpio_config(dev, 0, 0, 04838 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, 04839 VL53L0X_INTERRUPTPOLARITY_LOW); 04840 } 04841 04842 if (status == VL53L0X_ERROR_NONE) { 04843 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 04844 status |= VL53L0X_read_word(dev, 0x84, &tempword); 04845 status |= VL53L0X_write_byte(dev, 0xFF, 0x00); 04846 } 04847 04848 if (status == VL53L0X_ERROR_NONE) { 04849 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz, 04850 VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword)); 04851 } 04852 04853 /* After static init, some device parameters may be changed, 04854 * so update them */ 04855 if (status == VL53L0X_ERROR_NONE) { 04856 status = VL53L0X_get_device_parameters(dev, ¤t_parameters); 04857 } 04858 04859 04860 if (status == VL53L0X_ERROR_NONE) { 04861 status = VL53L0X_get_fraction_enable(dev, &tempbyte); 04862 if (status == VL53L0X_ERROR_NONE) { 04863 PALDevDataSet(dev, RangeFractionalEnable, tempbyte); 04864 } 04865 04866 } 04867 04868 if (status == VL53L0X_ERROR_NONE) { 04869 PALDevDataSet(dev, CurrentParameters, current_parameters); 04870 } 04871 04872 04873 /* read the sequence config and save it */ 04874 if (status == VL53L0X_ERROR_NONE) { 04875 status = VL53L0X_read_byte(dev, 04876 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte); 04877 if (status == VL53L0X_ERROR_NONE) { 04878 PALDevDataSet(dev, SequenceConfig, tempbyte); 04879 } 04880 } 04881 04882 /* Disable MSRC and TCC by default */ 04883 if (status == VL53L0X_ERROR_NONE) { 04884 status = VL53L0X_set_sequence_step_enable(dev, 04885 VL53L0X_SEQUENCESTEP_TCC, 0); 04886 } 04887 04888 if (status == VL53L0X_ERROR_NONE) { 04889 status = VL53L0X_set_sequence_step_enable(dev, 04890 VL53L0X_SEQUENCESTEP_MSRC, 0); 04891 } 04892 04893 /* Set PAL State to standby */ 04894 if (status == VL53L0X_ERROR_NONE) { 04895 PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE); 04896 } 04897 04898 /* Store pre-range vcsel period */ 04899 if (status == VL53L0X_ERROR_NONE) { 04900 status = VL53L0X_get_vcsel_pulse_period( 04901 dev, 04902 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 04903 &vcsel_pulse_period_pclk); 04904 } 04905 04906 if (status == VL53L0X_ERROR_NONE) { 04907 VL53L0X_SETDEVICESPECIFICPARAMETER( 04908 dev, 04909 PreRangeVcselPulsePeriod, 04910 vcsel_pulse_period_pclk); 04911 } 04912 04913 /* Store final-range vcsel period */ 04914 if (status == VL53L0X_ERROR_NONE) { 04915 status = VL53L0X_get_vcsel_pulse_period( 04916 dev, 04917 VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 04918 &vcsel_pulse_period_pclk); 04919 } 04920 04921 if (status == VL53L0X_ERROR_NONE) { 04922 VL53L0X_SETDEVICESPECIFICPARAMETER( 04923 dev, 04924 FinalRangeVcselPulsePeriod, 04925 vcsel_pulse_period_pclk); 04926 } 04927 04928 /* Store pre-range timeout */ 04929 if (status == VL53L0X_ERROR_NONE) { 04930 status = get_sequence_step_timeout( 04931 dev, 04932 VL53L0X_SEQUENCESTEP_PRE_RANGE, 04933 &seq_timeout_micro_secs); 04934 } 04935 04936 if (status == VL53L0X_ERROR_NONE) { 04937 VL53L0X_SETDEVICESPECIFICPARAMETER( 04938 dev, 04939 PreRangeTimeoutMicroSecs, 04940 seq_timeout_micro_secs); 04941 } 04942 04943 /* Store final-range timeout */ 04944 if (status == VL53L0X_ERROR_NONE) { 04945 status = get_sequence_step_timeout( 04946 dev, 04947 VL53L0X_SEQUENCESTEP_FINAL_RANGE, 04948 &seq_timeout_micro_secs); 04949 } 04950 04951 if (status == VL53L0X_ERROR_NONE) { 04952 VL53L0X_SETDEVICESPECIFICPARAMETER( 04953 dev, 04954 FinalRangeTimeoutMicroSecs, 04955 seq_timeout_micro_secs); 04956 } 04957 04958 LOG_FUNCTION_END(status); 04959 return status; 04960 } 04961 04962 04963 VL53L0X_Error VL53L0X::VL53L0X_stop_measurement(VL53L0X_DEV dev) 04964 { 04965 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04966 LOG_FUNCTION_START(""); 04967 04968 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 04969 VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT); 04970 04971 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 04972 status = VL53L0X_write_byte(dev, 0x00, 0x00); 04973 status = VL53L0X_write_byte(dev, 0x91, 0x00); 04974 status = VL53L0X_write_byte(dev, 0x00, 0x01); 04975 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 04976 04977 if (status == VL53L0X_ERROR_NONE) { 04978 /* Set PAL State to Idle */ 04979 PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE); 04980 } 04981 04982 /* Check if need to apply interrupt settings */ 04983 if (status == VL53L0X_ERROR_NONE) { 04984 status = VL53L0X_check_and_load_interrupt_settings(dev, 0); 04985 } 04986 04987 LOG_FUNCTION_END(status); 04988 return status; 04989 } 04990 04991 VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(VL53L0X_DEV dev, 04992 uint32_t *p_stop_status) 04993 { 04994 VL53L0X_Error status = VL53L0X_ERROR_NONE; 04995 uint8_t byte = 0; 04996 LOG_FUNCTION_START(""); 04997 04998 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 04999 05000 if (status == VL53L0X_ERROR_NONE) { 05001 status = VL53L0X_read_byte(dev, 0x04, &byte); 05002 } 05003 05004 if (status == VL53L0X_ERROR_NONE) { 05005 status = VL53L0X_write_byte(dev, 0xFF, 0x0); 05006 } 05007 05008 *p_stop_status = byte; 05009 05010 if (byte == 0) { 05011 status = VL53L0X_write_byte(dev, 0x80, 0x01); 05012 status = VL53L0X_write_byte(dev, 0xFF, 0x01); 05013 status = VL53L0X_write_byte(dev, 0x00, 0x00); 05014 status = VL53L0X_write_byte(dev, 0x91, 05015 PALDevDataGet(dev, StopVariable)); 05016 status = VL53L0X_write_byte(dev, 0x00, 0x01); 05017 status = VL53L0X_write_byte(dev, 0xFF, 0x00); 05018 status = VL53L0X_write_byte(dev, 0x80, 0x00); 05019 } 05020 05021 LOG_FUNCTION_END(status); 05022 return status; 05023 } 05024 05025 /****************** Write and read functions from I2C *************************/ 05026 05027 VL53L0X_Error VL53L0X::VL53L0X_write_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count) 05028 { 05029 int status; 05030 05031 status = VL53L0X_i2c_write(dev->I2cDevAddr , index, p_data, (uint16_t)count); 05032 return status; 05033 } 05034 05035 VL53L0X_Error VL53L0X::VL53L0X_read_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count) 05036 { 05037 int status; 05038 05039 if (count >= VL53L0X_MAX_I2C_XFER_SIZE) { 05040 status = VL53L0X_ERROR_INVALID_PARAMS; 05041 } 05042 05043 status = VL53L0X_i2c_read(dev->I2cDevAddr , index, p_data, (uint16_t)count); 05044 05045 return status; 05046 } 05047 05048 05049 VL53L0X_Error VL53L0X::VL53L0X_write_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t data) 05050 { 05051 int status; 05052 05053 status = VL53L0X_i2c_write(Dev->I2cDevAddr , index, &data, 1); 05054 return status; 05055 } 05056 05057 VL53L0X_Error VL53L0X::VL53L0X_write_word(VL53L0X_DEV dev, uint8_t index, uint16_t data) 05058 { 05059 int status; 05060 uint8_t buffer[2]; 05061 05062 buffer[0] = data >> 8; 05063 buffer[1] = data & 0x00FF; 05064 status = VL53L0X_i2c_write(dev->I2cDevAddr , index, (uint8_t *)buffer, 2); 05065 return status; 05066 } 05067 05068 VL53L0X_Error VL53L0X::VL53L0X_write_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t data) 05069 { 05070 int status; 05071 uint8_t buffer[4]; 05072 05073 buffer[0] = (data >> 24) & 0xFF; 05074 buffer[1] = (data >> 16) & 0xFF; 05075 buffer[2] = (data >> 8) & 0xFF; 05076 buffer[3] = (data >> 0) & 0xFF; 05077 status = VL53L0X_i2c_write(Dev->I2cDevAddr , index, (uint8_t *)buffer, 4); 05078 return status; 05079 } 05080 05081 05082 VL53L0X_Error VL53L0X::VL53L0X_read_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t *p_data) 05083 { 05084 int status; 05085 05086 status = VL53L0X_i2c_read(Dev->I2cDevAddr , index, p_data, 1); 05087 05088 if (status) { 05089 return -1; 05090 } 05091 05092 return 0; 05093 } 05094 05095 VL53L0X_Error VL53L0X::VL53L0X_read_word(VL53L0X_DEV Dev, uint8_t index, uint16_t *p_data) 05096 { 05097 int status; 05098 uint8_t buffer[2] = {0, 0}; 05099 05100 status = VL53L0X_i2c_read(Dev->I2cDevAddr , index, buffer, 2); 05101 if (!status) { 05102 *p_data = (buffer[0] << 8) + buffer[1]; 05103 } 05104 return status; 05105 05106 } 05107 05108 VL53L0X_Error VL53L0X::VL53L0X_read_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t *p_data) 05109 { 05110 int status; 05111 uint8_t buffer[4] = {0, 0, 0, 0}; 05112 05113 status = VL53L0X_i2c_read(Dev->I2cDevAddr , index, buffer, 4); 05114 if (!status) { 05115 *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; 05116 } 05117 return status; 05118 05119 } 05120 05121 VL53L0X_Error VL53L0X::VL53L0X_update_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t and_data, uint8_t or_data) 05122 { 05123 int status; 05124 uint8_t buffer = 0; 05125 05126 /* read data direct onto buffer */ 05127 status = VL53L0X_i2c_read(Dev->I2cDevAddr , index, &buffer, 1); 05128 if (!status) { 05129 buffer = (buffer & and_data) | or_data; 05130 status = VL53L0X_i2c_write(Dev->I2cDevAddr , index, &buffer, (uint8_t)1); 05131 } 05132 return status; 05133 } 05134 05135 VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data, 05136 uint16_t NumByteToWrite) 05137 { 05138 int ret; 05139 05140 ret = _dev_i2c->i2c_write(p_data, DeviceAddr, RegisterAddr, NumByteToWrite); 05141 05142 if (ret) { 05143 return -1; 05144 } 05145 return 0; 05146 } 05147 05148 VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data, 05149 uint16_t NumByteToRead) 05150 { 05151 int ret; 05152 05153 ret = _dev_i2c->i2c_read(p_data, DeviceAddr, RegisterAddr, NumByteToRead); 05154 05155 if (ret) { 05156 return -1; 05157 } 05158 return 0; 05159 } 05160 05161 int VL53L0X::read_id(uint8_t *id) 05162 { 05163 int status = 0; 05164 uint16_t rl_id = 0; 05165 05166 status = VL53L0X_read_word(_device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id); 05167 if (rl_id == 0xEEAA) { 05168 return status; 05169 } 05170 05171 return -1; 05172 } 05173 05174 05175 VL53L0X_Error VL53L0X::wait_measurement_data_ready(VL53L0X_DEV dev) 05176 { 05177 VL53L0X_Error status = VL53L0X_ERROR_NONE; 05178 uint8_t new_dat_ready = 0; 05179 uint32_t loop_nb; 05180 05181 // Wait until it finished 05182 // use timeout to avoid deadlock 05183 if (status == VL53L0X_ERROR_NONE) { 05184 loop_nb = 0; 05185 do { 05186 status = VL53L0X_get_measurement_data_ready(dev, &new_dat_ready); 05187 if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) { 05188 break; 05189 } 05190 loop_nb = loop_nb + 1; 05191 VL53L0X_polling_delay(dev); 05192 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); 05193 05194 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 05195 status = VL53L0X_ERROR_TIME_OUT; 05196 } 05197 } 05198 05199 return status; 05200 } 05201 05202 VL53L0X_Error VL53L0X::wait_stop_completed(VL53L0X_DEV dev) 05203 { 05204 VL53L0X_Error status = VL53L0X_ERROR_NONE; 05205 uint32_t stop_completed = 0; 05206 uint32_t loop_nb; 05207 05208 // Wait until it finished 05209 // use timeout to avoid deadlock 05210 if (status == VL53L0X_ERROR_NONE) { 05211 loop_nb = 0; 05212 do { 05213 status = VL53L0X_get_stop_completed_status(dev, &stop_completed); 05214 if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) { 05215 break; 05216 } 05217 loop_nb = loop_nb + 1; 05218 VL53L0X_polling_delay(dev); 05219 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); 05220 05221 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 05222 status = VL53L0X_ERROR_TIME_OUT; 05223 } 05224 05225 } 05226 05227 return status; 05228 } 05229 05230 05231 int VL53L0X::init_sensor(uint8_t new_addr) 05232 { 05233 int status; 05234 05235 VL53L0X_off(); 05236 VL53L0X_on(); 05237 05238 // status=VL53L0X_WaitDeviceBooted(Device); 05239 // if(status) 05240 // printf("WaitDeviceBooted fail\n\r"); 05241 status = is_present(); 05242 if (!status) { 05243 status = init(&_my_device); 05244 if (status != VL53L0X_ERROR_NONE) { 05245 printf("Failed to init VL53L0X sensor!\n\r"); 05246 return status; 05247 } 05248 05249 // deduce silicon version 05250 status = VL53L0X_get_device_info(&_my_device, &_device_info); 05251 05252 status = prepare(); 05253 if (status != VL53L0X_ERROR_NONE) { 05254 printf("Failed to prepare VL53L0X!\n\r"); 05255 return status; 05256 } 05257 05258 if (new_addr != VL53L0X_DEFAULT_ADDRESS) { 05259 status = set_device_address(new_addr); 05260 if (status) { 05261 printf("Failed to change I2C address!\n\r"); 05262 return status; 05263 } 05264 } else { 05265 printf("Invalid new address!\n\r"); 05266 return VL53L0X_ERROR_INVALID_PARAMS; 05267 } 05268 } 05269 return status; 05270 } 05271 05272 int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void)) 05273 { 05274 int status, clr_status; 05275 05276 status = VL53L0X_stop_measurement(_device); // it is safer to do this while sensor is stopped 05277 05278 // status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300); 05279 05280 status = VL53L0X_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 05281 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, 05282 VL53L0X_INTERRUPTPOLARITY_HIGH); 05283 05284 if (!status) { 05285 attach_interrupt_measure_detection_irq(fptr); 05286 enable_interrupt_measure_detection_irq(); 05287 } 05288 05289 clr_status = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS); 05290 if (clr_status) { 05291 VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n"); 05292 } 05293 05294 if (!status) { 05295 status = range_start_continuous_mode(); 05296 } 05297 return status; 05298 } 05299 05300 05301 int VL53L0X::start_measurement(OperatingMode operating_mode, void (*fptr)(void)) 05302 { 05303 int Status = VL53L0X_ERROR_NONE; 05304 int ClrStatus; 05305 05306 uint8_t VhvSettings; 05307 uint8_t PhaseCal; 05308 // *** from mass market cube expansion v1.1, ranging with satellites. 05309 // default settings, for normal range. 05310 FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536); 05311 FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536); 05312 uint32_t timingBudget = 33000; 05313 uint8_t preRangeVcselPeriod = 14; 05314 uint8_t finalRangeVcselPeriod = 10; 05315 05316 if (operating_mode == range_continuous_interrupt) { 05317 if (_gpio1Int == NULL) { 05318 printf("GPIO1 Error\r\n"); 05319 return 1; 05320 } 05321 05322 Status = VL53L0X_stop_measurement(_device); // it is safer to do this while sensor is stopped 05323 05324 // Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300); 05325 05326 Status = VL53L0X_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 05327 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, 05328 VL53L0X_INTERRUPTPOLARITY_HIGH); 05329 05330 if (Status == VL53L0X_ERROR_NONE) { 05331 attach_interrupt_measure_detection_irq(fptr); 05332 enable_interrupt_measure_detection_irq(); 05333 } 05334 05335 ClrStatus = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS); 05336 if (ClrStatus) { 05337 VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n"); 05338 } 05339 05340 if (Status == VL53L0X_ERROR_NONE) { 05341 Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode 05342 } 05343 05344 if (Status == VL53L0X_ERROR_NONE) { 05345 Status = VL53L0X_start_measurement(_device); 05346 } 05347 } 05348 05349 if (operating_mode == range_single_shot_polling) { 05350 // singelshot, polled ranging 05351 if (Status == VL53L0X_ERROR_NONE) { 05352 // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement 05353 Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode 05354 } 05355 05356 // Enable/Disable Sigma and Signal check 05357 if (Status == VL53L0X_ERROR_NONE) { 05358 Status = VL53L0X_set_limit_check_enable(_device, 05359 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); 05360 } 05361 if (Status == VL53L0X_ERROR_NONE) { 05362 Status = VL53L0X_set_limit_check_enable(_device, 05363 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); 05364 } 05365 05366 // *** from mass market cube expansion v1.1, ranging with satellites. 05367 /* Ranging configuration */ 05368 //* 05369 // switch(rangingConfig) { 05370 // case LONG_RANGE: 05371 signalLimit = (FixPoint1616_t)(0.1 * 65536); 05372 sigmaLimit = (FixPoint1616_t)(60 * 65536); 05373 timingBudget = 33000; 05374 preRangeVcselPeriod = 18; 05375 finalRangeVcselPeriod = 14; 05376 /* break; 05377 case HIGH_ACCURACY: 05378 signalLimit = (FixPoint1616_t)(0.25*65536); 05379 sigmaLimit = (FixPoint1616_t)(18*65536); 05380 timingBudget = 200000; 05381 preRangeVcselPeriod = 14; 05382 finalRangeVcselPeriod = 10; 05383 break; 05384 case HIGH_SPEED: 05385 signalLimit = (FixPoint1616_t)(0.25*65536); 05386 sigmaLimit = (FixPoint1616_t)(32*65536); 05387 timingBudget = 20000; 05388 preRangeVcselPeriod = 14; 05389 finalRangeVcselPeriod = 10; 05390 break; 05391 default: 05392 debug_printf("Not Supported"); 05393 } 05394 */ 05395 05396 if (Status == VL53L0X_ERROR_NONE) { 05397 Status = VL53L0X_set_limit_check_value(_device, 05398 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit); 05399 } 05400 05401 if (Status == VL53L0X_ERROR_NONE) { 05402 Status = VL53L0X_set_limit_check_value(_device, 05403 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit); 05404 } 05405 05406 if (Status == VL53L0X_ERROR_NONE) { 05407 Status = VL53L0X_set_measurement_timing_budget_micro_seconds(_device, timingBudget); 05408 } 05409 05410 if (Status == VL53L0X_ERROR_NONE) { 05411 Status = VL53L0X_set_vcsel_pulse_period(_device, 05412 VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod); 05413 } 05414 05415 if (Status == VL53L0X_ERROR_NONE) { 05416 Status = VL53L0X_set_vcsel_pulse_period(_device, 05417 VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod); 05418 } 05419 05420 if (Status == VL53L0X_ERROR_NONE) { 05421 Status = VL53L0X_perform_ref_calibration(_device, &VhvSettings, &PhaseCal); 05422 } 05423 05424 } 05425 05426 if (operating_mode == range_continuous_polling) { 05427 if (Status == VL53L0X_ERROR_NONE) { 05428 //printf("Call of VL53L0X_SetDeviceMode\n"); 05429 Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode 05430 } 05431 05432 if (Status == VL53L0X_ERROR_NONE) { 05433 //printf("Call of VL53L0X_StartMeasurement\n"); 05434 Status = VL53L0X_start_measurement(_device); 05435 } 05436 } 05437 05438 return Status; 05439 } 05440 05441 05442 int VL53L0X::get_measurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data) 05443 { 05444 int Status = VL53L0X_ERROR_NONE; 05445 05446 if (operating_mode == range_single_shot_polling) { 05447 Status = VL53L0X_perform_single_ranging_measurement(_device, p_data); 05448 } 05449 05450 if (operating_mode == range_continuous_polling) { 05451 if (Status == VL53L0X_ERROR_NONE) { 05452 Status = VL53L0X_measurement_poll_for_completion(_device); 05453 } 05454 05455 if (Status == VL53L0X_ERROR_NONE) { 05456 Status = VL53L0X_get_ranging_measurement_data(_device, p_data); 05457 05458 // Clear the interrupt 05459 VL53L0X_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); 05460 VL53L0X_polling_delay(_device); 05461 } 05462 } 05463 05464 if (operating_mode == range_continuous_interrupt) { 05465 Status = VL53L0X_get_ranging_measurement_data(_device, p_data); 05466 VL53L0X_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS); 05467 } 05468 05469 return Status; 05470 } 05471 05472 05473 int VL53L0X::stop_measurement(OperatingMode operating_mode) 05474 { 05475 int status = VL53L0X_ERROR_NONE; 05476 05477 05478 // don't need to stop for a singleshot range! 05479 if (operating_mode == range_single_shot_polling) { 05480 } 05481 05482 if (operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling) { 05483 // continuous mode 05484 if (status == VL53L0X_ERROR_NONE) { 05485 //printf("Call of VL53L0X_StopMeasurement\n"); 05486 status = VL53L0X_stop_measurement(_device); 05487 } 05488 05489 if (status == VL53L0X_ERROR_NONE) { 05490 //printf("Wait Stop to be competed\n"); 05491 status = wait_stop_completed(_device); 05492 } 05493 05494 if (status == VL53L0X_ERROR_NONE) 05495 status = VL53L0X_clear_interrupt_mask(_device, 05496 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); 05497 } 05498 05499 return status; 05500 } 05501 05502 05503 int VL53L0X::handle_irq(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data) 05504 { 05505 int status; 05506 status = get_measurement(operating_mode, data); 05507 enable_interrupt_measure_detection_irq(); 05508 return status; 05509 } 05510 05511 05512 /******************************************************************************/
Generated on Tue Jul 12 2022 22:34:15 by
1.7.2