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