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