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