Tarek Lule / VL53L0X_Condensed2

Greatly simplified Architecture, Identical Functions Removed: Platform Interfaces, STP6001 interface

VL53L0X.cpp@9:cb4c6d4e5030, 2019-06-25 (annotated)

Committer:
sepp_nepp
Date:
Tue Jun 25 11:03:34 2019 +0000
Revision:
9:cb4c6d4e5030
Parent:
8:2fd7cb217068
Child:
10:cd1758e186a4
Latest Release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sepp_nepp 6:fb11b746ceb5 1 /**
sepp_nepp 6:fb11b746ceb5 2 ******************************************************************************
sepp_nepp 6:fb11b746ceb5 3 * @file VL53L0X_class.cpp
sepp_nepp 6:fb11b746ceb5 4 * @author IMG
sepp_nepp 6:fb11b746ceb5 5 * @version V0.0.1
sepp_nepp 6:fb11b746ceb5 6 * @date 28-June-2016
sepp_nepp 6:fb11b746ceb5 7 * @brief Implementation file for the VL53L0X driver class
sepp_nepp 6:fb11b746ceb5 8 ******************************************************************************
sepp_nepp 6:fb11b746ceb5 9 * @attention
sepp_nepp 6:fb11b746ceb5 10 *
sepp_nepp 6:fb11b746ceb5 11 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
sepp_nepp 6:fb11b746ceb5 12 *
sepp_nepp 6:fb11b746ceb5 13 * Redistribution and use in source and binary forms, with or without modification,
sepp_nepp 6:fb11b746ceb5 14 * are permitted provided that the following conditions are met:
sepp_nepp 6:fb11b746ceb5 15 * 1. Redistributions of source code must retain the above copyright notice,
sepp_nepp 6:fb11b746ceb5 16 * this list of conditions and the following disclaimer.
sepp_nepp 6:fb11b746ceb5 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
sepp_nepp 6:fb11b746ceb5 18 * this list of conditions and the following disclaimer in the documentation
sepp_nepp 6:fb11b746ceb5 19 * and/or other materials provided with the distribution.
sepp_nepp 6:fb11b746ceb5 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
sepp_nepp 6:fb11b746ceb5 21 * may be used to endorse or promote products derived from this software
sepp_nepp 6:fb11b746ceb5 22 * without specific prior written permission.
sepp_nepp 6:fb11b746ceb5 23 *
sepp_nepp 6:fb11b746ceb5 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
sepp_nepp 6:fb11b746ceb5 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
sepp_nepp 6:fb11b746ceb5 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
sepp_nepp 6:fb11b746ceb5 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
sepp_nepp 6:fb11b746ceb5 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
sepp_nepp 6:fb11b746ceb5 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
sepp_nepp 6:fb11b746ceb5 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
sepp_nepp 6:fb11b746ceb5 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
sepp_nepp 6:fb11b746ceb5 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
sepp_nepp 6:fb11b746ceb5 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
sepp_nepp 6:fb11b746ceb5 34 *
sepp_nepp 6:fb11b746ceb5 35 ******************************************************************************
sepp_nepp 6:fb11b746ceb5 36 */
sepp_nepp 6:fb11b746ceb5 37
sepp_nepp 8:2fd7cb217068 38 /* Some example regular expressinos used to simplify the code:
sepp_nepp 8:2fd7cb217068 39 b) Search for: \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E
sepp_nepp 8:2fd7cb217068 40 Replace by: \3 = Read_Byte\(\1\);
sepp_nepp 8:2fd7cb217068 41 to replace: Read_Byte(0x90,&module_id);
sepp_nepp 8:2fd7cb217068 42 by this: module_id = Read_Byte(0x90);
sepp_nepp 8:2fd7cb217068 43
sepp_nepp 8:2fd7cb217068 44 c) Search for: ([A-Za-z_\d]+)\Q(\E\r\n(\s*)
sepp_nepp 8:2fd7cb217068 45 Replace by: \1\(
sepp_nepp 8:2fd7cb217068 46 To join lines where the first line has an open bracket, and the next line starts listing the parameters.
sepp_nepp 8:2fd7cb217068 47 for example: Status = VL53L0X_UpdateByte(V
sepp_nepp 8:2fd7cb217068 48 L53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, ....
sepp_nepp 8:2fd7cb217068 49 becomes: Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, ....
sepp_nepp 8:2fd7cb217068 50
sepp_nepp 8:2fd7cb217068 51 */
sepp_nepp 8:2fd7cb217068 52
sepp_nepp 6:fb11b746ceb5 53 /* Includes */
sepp_nepp 6:fb11b746ceb5 54 #include <stdlib.h>
sepp_nepp 6:fb11b746ceb5 55 #include "VL53L0X.h"
sepp_nepp 6:fb11b746ceb5 56
sepp_nepp 6:fb11b746ceb5 57 int VL53L0X::read_id(uint8_t *id)
sepp_nepp 8:2fd7cb217068 58 { int status = 0;
sepp_nepp 6:fb11b746ceb5 59 uint16_t rl_id = 0;
sepp_nepp 6:fb11b746ceb5 60
sepp_nepp 7:3a1115c2556b 61 status = VL53L0X_read_word(VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id);
sepp_nepp 6:fb11b746ceb5 62 if (rl_id == 0xEEAA) {
sepp_nepp 6:fb11b746ceb5 63 return status;
sepp_nepp 6:fb11b746ceb5 64 }
sepp_nepp 6:fb11b746ceb5 65 return -1;
sepp_nepp 6:fb11b746ceb5 66 }
sepp_nepp 6:fb11b746ceb5 67
sepp_nepp 6:fb11b746ceb5 68 int VL53L0X::init_sensor(uint8_t new_addr)
sepp_nepp 6:fb11b746ceb5 69 { int status;
sepp_nepp 6:fb11b746ceb5 70
sepp_nepp 6:fb11b746ceb5 71 VL53L0X_off();
sepp_nepp 6:fb11b746ceb5 72 VL53L0X_on();
sepp_nepp 6:fb11b746ceb5 73
sepp_nepp 6:fb11b746ceb5 74 // Verify if the device is actually present
sepp_nepp 6:fb11b746ceb5 75 uint8_t id = 0;
sepp_nepp 6:fb11b746ceb5 76 status = read_id(&id);
sepp_nepp 6:fb11b746ceb5 77 if (status != 0) {
sepp_nepp 6:fb11b746ceb5 78 printf("VL53L0X sensor is not present!\n\r");
sepp_nepp 6:fb11b746ceb5 79 return 99; } // device is not present
sepp_nepp 6:fb11b746ceb5 80
sepp_nepp 7:3a1115c2556b 81 status = VL53L0X_data_init();
sepp_nepp 6:fb11b746ceb5 82 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 83 printf("Failed to init VL53L0X sensor!\n\r");
sepp_nepp 6:fb11b746ceb5 84 return status;
sepp_nepp 6:fb11b746ceb5 85 }
sepp_nepp 6:fb11b746ceb5 86
sepp_nepp 6:fb11b746ceb5 87 // deduce silicon version
sepp_nepp 7:3a1115c2556b 88 status = VL53L0X_get_device_info(&_device_info);
sepp_nepp 6:fb11b746ceb5 89
sepp_nepp 6:fb11b746ceb5 90 status = prepare();
sepp_nepp 6:fb11b746ceb5 91 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 92 printf("Failed to prepare VL53L0X!\n\r");
sepp_nepp 6:fb11b746ceb5 93 return status;
sepp_nepp 6:fb11b746ceb5 94 }
sepp_nepp 6:fb11b746ceb5 95
sepp_nepp 6:fb11b746ceb5 96 if (new_addr != VL53L0X_DEFAULT_ADDRESS) {
sepp_nepp 6:fb11b746ceb5 97 status = set_device_address(new_addr);
sepp_nepp 6:fb11b746ceb5 98 if (status) {
sepp_nepp 6:fb11b746ceb5 99 printf("Failed to change I2C address!\n\r");
sepp_nepp 6:fb11b746ceb5 100 return status;
sepp_nepp 6:fb11b746ceb5 101 }
sepp_nepp 6:fb11b746ceb5 102 }
sepp_nepp 6:fb11b746ceb5 103 return status;
sepp_nepp 6:fb11b746ceb5 104 }
sepp_nepp 6:fb11b746ceb5 105
sepp_nepp 7:3a1115c2556b 106 VL53L0X_Error VL53L0X::VL53L0X_data_init(void)
sepp_nepp 8:2fd7cb217068 107 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 108 VL53L0X_DeviceParameters_t CurrentParameters;
sepp_nepp 6:fb11b746ceb5 109 int i;
sepp_nepp 6:fb11b746ceb5 110 uint8_t StopVariable;
sepp_nepp 6:fb11b746ceb5 111
sepp_nepp 6:fb11b746ceb5 112 /* by default the I2C is running at 1V8 if you want to change it you
sepp_nepp 6:fb11b746ceb5 113 * need to include this define at compilation level. */
sepp_nepp 6:fb11b746ceb5 114 #ifdef USE_I2C_2V8
sepp_nepp 8:2fd7cb217068 115 Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
sepp_nepp 6:fb11b746ceb5 116 0xFE,0x01);
sepp_nepp 6:fb11b746ceb5 117 #endif
sepp_nepp 6:fb11b746ceb5 118
sepp_nepp 6:fb11b746ceb5 119 /* Set I2C standard mode */
sepp_nepp 6:fb11b746ceb5 120 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 121 status = VL53L0X_write_byte( 0x88, 0x00);
sepp_nepp 6:fb11b746ceb5 122 }
sepp_nepp 6:fb11b746ceb5 123
sepp_nepp 8:2fd7cb217068 124 Data.ReadDataFromDeviceDone = 0;
sepp_nepp 8:2fd7cb217068 125 Data.ReadDataFromDeviceDone = 0;
sepp_nepp 6:fb11b746ceb5 126
sepp_nepp 6:fb11b746ceb5 127 #ifdef USE_IQC_STATION
sepp_nepp 6:fb11b746ceb5 128 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 129 Status = VL53L0X_apply_offset_adjustment();
sepp_nepp 6:fb11b746ceb5 130 }
sepp_nepp 6:fb11b746ceb5 131 #endif
sepp_nepp 6:fb11b746ceb5 132
sepp_nepp 6:fb11b746ceb5 133 /* Default value is 1000 for Linearity Corrective Gain */
sepp_nepp 8:2fd7cb217068 134 Data.LinearityCorrectiveGain = 1000;
sepp_nepp 6:fb11b746ceb5 135
sepp_nepp 6:fb11b746ceb5 136 /* Dmax default Parameter */
sepp_nepp 8:2fd7cb217068 137 Data.DmaxCalRangeMilliMeter = 400;
sepp_nepp 8:2fd7cb217068 138 Data.DmaxCalSignalRateRtnMegaCps = (FixPoint1616_t)((0x00016B85)); /* 1.42 No Cover Glass*/
sepp_nepp 6:fb11b746ceb5 139
sepp_nepp 6:fb11b746ceb5 140 /* Set Default static parameters
sepp_nepp 6:fb11b746ceb5 141 *set first temporary values 9.44MHz * 65536 = 618660 */
sepp_nepp 8:2fd7cb217068 142 Data.OscFrequencyMHz = 618660;
sepp_nepp 6:fb11b746ceb5 143
sepp_nepp 6:fb11b746ceb5 144 /* Set Default XTalkCompensationRateMegaCps to 0 */
sepp_nepp 8:2fd7cb217068 145 Data.CurrentParameters.XTalkCompensationRateMegaCps = 0;
sepp_nepp 6:fb11b746ceb5 146
sepp_nepp 6:fb11b746ceb5 147 /* Get default parameters */
sepp_nepp 7:3a1115c2556b 148 status = VL53L0X_get_device_parameters( &CurrentParameters);
sepp_nepp 6:fb11b746ceb5 149 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 150 /* initialize PAL values */
sepp_nepp 6:fb11b746ceb5 151 CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
sepp_nepp 6:fb11b746ceb5 152 CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
sepp_nepp 8:2fd7cb217068 153 Data.CurrentParameters = CurrentParameters;
sepp_nepp 6:fb11b746ceb5 154 }
sepp_nepp 6:fb11b746ceb5 155
sepp_nepp 6:fb11b746ceb5 156 /* Sigma estimator variable */
sepp_nepp 8:2fd7cb217068 157 Data.SigmaEstRefArray = 100;
sepp_nepp 8:2fd7cb217068 158 Data.SigmaEstEffPulseWidth = 900;
sepp_nepp 8:2fd7cb217068 159 Data.SigmaEstEffAmbWidth = 500;
sepp_nepp 8:2fd7cb217068 160 Data.targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
sepp_nepp 6:fb11b746ceb5 161
sepp_nepp 6:fb11b746ceb5 162 /* Use internal default settings */
sepp_nepp 8:2fd7cb217068 163 Data.UseInternalTuningSettings = 1;
sepp_nepp 7:3a1115c2556b 164
sepp_nepp 7:3a1115c2556b 165 status |= VL53L0X_write_byte( 0x80, 0x01);
sepp_nepp 7:3a1115c2556b 166 status |= VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 167 status |= VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 7:3a1115c2556b 168 status |= VL53L0X_read_byte( 0x91, &StopVariable);
sepp_nepp 8:2fd7cb217068 169 Data.StopVariable = StopVariable;
sepp_nepp 7:3a1115c2556b 170 status |= VL53L0X_write_byte( 0x00, 0x01);
sepp_nepp 7:3a1115c2556b 171 status |= VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 7:3a1115c2556b 172 status |= VL53L0X_write_byte( 0x80, 0x00);
sepp_nepp 6:fb11b746ceb5 173
sepp_nepp 6:fb11b746ceb5 174 /* Enable all check */
sepp_nepp 6:fb11b746ceb5 175 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
sepp_nepp 6:fb11b746ceb5 176 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 177 status |= VL53L0X_set_limit_check_enable( i, 1);
sepp_nepp 8:2fd7cb217068 178 } else { break; }
sepp_nepp 6:fb11b746ceb5 179 }
sepp_nepp 6:fb11b746ceb5 180
sepp_nepp 6:fb11b746ceb5 181 /* Disable the following checks */
sepp_nepp 6:fb11b746ceb5 182 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 183 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
sepp_nepp 6:fb11b746ceb5 184
sepp_nepp 6:fb11b746ceb5 185 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 186 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
sepp_nepp 6:fb11b746ceb5 187
sepp_nepp 6:fb11b746ceb5 188 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 189 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
sepp_nepp 6:fb11b746ceb5 190
sepp_nepp 6:fb11b746ceb5 191 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 192 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
sepp_nepp 6:fb11b746ceb5 193
sepp_nepp 6:fb11b746ceb5 194 /* Limit default values */
sepp_nepp 6:fb11b746ceb5 195 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 196 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 197 (FixPoint1616_t)(18 * 65536));
sepp_nepp 6:fb11b746ceb5 198 }
sepp_nepp 6:fb11b746ceb5 199 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 200 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 201 (FixPoint1616_t)(25 * 65536 / 100));
sepp_nepp 6:fb11b746ceb5 202 /* 0.25 * 65536 */
sepp_nepp 6:fb11b746ceb5 203 }
sepp_nepp 6:fb11b746ceb5 204
sepp_nepp 6:fb11b746ceb5 205 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 206 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
sepp_nepp 6:fb11b746ceb5 207 (FixPoint1616_t)(35 * 65536));
sepp_nepp 6:fb11b746ceb5 208 }
sepp_nepp 6:fb11b746ceb5 209
sepp_nepp 6:fb11b746ceb5 210 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 211 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
sepp_nepp 6:fb11b746ceb5 212 (FixPoint1616_t)(0 * 65536));
sepp_nepp 6:fb11b746ceb5 213 }
sepp_nepp 6:fb11b746ceb5 214
sepp_nepp 6:fb11b746ceb5 215 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 216
sepp_nepp 8:2fd7cb217068 217 Data.SequenceConfig = 0xFF;
sepp_nepp 7:3a1115c2556b 218 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0xFF);
sepp_nepp 6:fb11b746ceb5 219
sepp_nepp 6:fb11b746ceb5 220 /* Set PAL state to tell that we are waiting for call to VL53L0X_StaticInit */
sepp_nepp 8:2fd7cb217068 221 Data.PalState = VL53L0X_STATE_WAIT_STATICINIT;
sepp_nepp 6:fb11b746ceb5 222 }
sepp_nepp 6:fb11b746ceb5 223
sepp_nepp 6:fb11b746ceb5 224 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 225 Data.RefSpadsInitialised = 0;
sepp_nepp 6:fb11b746ceb5 226 }
sepp_nepp 6:fb11b746ceb5 227
sepp_nepp 6:fb11b746ceb5 228 return status;
sepp_nepp 6:fb11b746ceb5 229 }
sepp_nepp 6:fb11b746ceb5 230
sepp_nepp 6:fb11b746ceb5 231 int VL53L0X::prepare()
sepp_nepp 8:2fd7cb217068 232 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 233 uint32_t ref_spad_count;
sepp_nepp 6:fb11b746ceb5 234 uint8_t is_aperture_spads;
sepp_nepp 6:fb11b746ceb5 235 uint8_t vhv_settings;
sepp_nepp 6:fb11b746ceb5 236 uint8_t phase_cal;
sepp_nepp 6:fb11b746ceb5 237
sepp_nepp 6:fb11b746ceb5 238 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 239 //printf("Call of VL53L0X_StaticInit\r\n");
sepp_nepp 7:3a1115c2556b 240 status = VL53L0X_static_init(); // Device Initialization
sepp_nepp 6:fb11b746ceb5 241 }
sepp_nepp 6:fb11b746ceb5 242
sepp_nepp 6:fb11b746ceb5 243 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 244 //printf("Call of VL53L0X_PerformRefCalibration\r\n");
sepp_nepp 7:3a1115c2556b 245 status = VL53L0X_perform_ref_calibration(&vhv_settings, &phase_cal); // Device Initialization
sepp_nepp 6:fb11b746ceb5 246 }
sepp_nepp 6:fb11b746ceb5 247
sepp_nepp 6:fb11b746ceb5 248 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 249 //printf("Call of VL53L0X_PerformRefSpadManagement\r\n");
sepp_nepp 7:3a1115c2556b 250 status = VL53L0X_perform_ref_spad_management(&ref_spad_count, &is_aperture_spads); // Device Initialization
sepp_nepp 6:fb11b746ceb5 251 // printf ("refSpadCount = %d, isApertureSpads = %d\r\n", refSpadCount, isApertureSpads);
sepp_nepp 6:fb11b746ceb5 252 }
sepp_nepp 6:fb11b746ceb5 253
sepp_nepp 6:fb11b746ceb5 254 return status;
sepp_nepp 6:fb11b746ceb5 255 }
sepp_nepp 6:fb11b746ceb5 256
sepp_nepp 6:fb11b746ceb5 257 int VL53L0X::start_measurement(OperatingMode operating_mode, void (*fptr)(void),
sepp_nepp 6:fb11b746ceb5 258 VL53L0X_RangingConfig rangingConfig)
sepp_nepp 6:fb11b746ceb5 259 { int Status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 260 int ClrStatus;
sepp_nepp 6:fb11b746ceb5 261
sepp_nepp 6:fb11b746ceb5 262 uint8_t VhvSettings;
sepp_nepp 6:fb11b746ceb5 263 uint8_t PhaseCal;
sepp_nepp 6:fb11b746ceb5 264 // default settings, for normal range.
sepp_nepp 6:fb11b746ceb5 265 FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536);
sepp_nepp 6:fb11b746ceb5 266 FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536);
sepp_nepp 6:fb11b746ceb5 267 uint32_t timingBudget = 33000;
sepp_nepp 6:fb11b746ceb5 268 uint8_t preRangeVcselPeriod = 14;
sepp_nepp 6:fb11b746ceb5 269 uint8_t finalRangeVcselPeriod = 10;
sepp_nepp 6:fb11b746ceb5 270
sepp_nepp 6:fb11b746ceb5 271 if (operating_mode == range_continuous_interrupt) {
sepp_nepp 6:fb11b746ceb5 272 if (_gpio1Int == NULL) {
sepp_nepp 6:fb11b746ceb5 273 printf("GPIO1 Error\r\n");
sepp_nepp 6:fb11b746ceb5 274 return 1;
sepp_nepp 6:fb11b746ceb5 275 }
sepp_nepp 6:fb11b746ceb5 276
sepp_nepp 7:3a1115c2556b 277 Status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped
sepp_nepp 6:fb11b746ceb5 278
sepp_nepp 6:fb11b746ceb5 279 // Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
sepp_nepp 6:fb11b746ceb5 280
sepp_nepp 7:3a1115c2556b 281 Status = VL53L0X_set_gpio_config(0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp 6:fb11b746ceb5 282 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
sepp_nepp 6:fb11b746ceb5 283 VL53L0X_INTERRUPTPOLARITY_HIGH);
sepp_nepp 6:fb11b746ceb5 284
sepp_nepp 6:fb11b746ceb5 285 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 286 attach_interrupt_measure_detection_irq(fptr);
sepp_nepp 6:fb11b746ceb5 287 enable_interrupt_measure_detection_irq();
sepp_nepp 6:fb11b746ceb5 288 }
sepp_nepp 6:fb11b746ceb5 289
sepp_nepp 6:fb11b746ceb5 290 ClrStatus = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS);
sepp_nepp 6:fb11b746ceb5 291 if (ClrStatus) { Status = 97; } // VL53L0X_ClearErrorInterrupt fail
sepp_nepp 6:fb11b746ceb5 292
sepp_nepp 6:fb11b746ceb5 293 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 294 Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
sepp_nepp 6:fb11b746ceb5 295 }
sepp_nepp 6:fb11b746ceb5 296
sepp_nepp 6:fb11b746ceb5 297 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 298 Status = VL53L0X_start_measurement();
sepp_nepp 6:fb11b746ceb5 299 }
sepp_nepp 6:fb11b746ceb5 300 }
sepp_nepp 6:fb11b746ceb5 301
sepp_nepp 6:fb11b746ceb5 302 if (operating_mode == range_single_shot_polling) {
sepp_nepp 6:fb11b746ceb5 303 // singelshot, polled ranging
sepp_nepp 6:fb11b746ceb5 304 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 305 // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
sepp_nepp 7:3a1115c2556b 306 Status = VL53L0X_set_device_mode( VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
sepp_nepp 6:fb11b746ceb5 307 }
sepp_nepp 6:fb11b746ceb5 308
sepp_nepp 6:fb11b746ceb5 309 // Enable/Disable Sigma and Signal check
sepp_nepp 6:fb11b746ceb5 310 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 311 Status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
sepp_nepp 6:fb11b746ceb5 312 }
sepp_nepp 6:fb11b746ceb5 313 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 314 Status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
sepp_nepp 6:fb11b746ceb5 315 }
sepp_nepp 6:fb11b746ceb5 316
sepp_nepp 6:fb11b746ceb5 317 /* Preselected Ranging configurations */
sepp_nepp 6:fb11b746ceb5 318 switch(rangingConfig) {
sepp_nepp 6:fb11b746ceb5 319 case Range_Config_DEFAULT:
sepp_nepp 6:fb11b746ceb5 320 // default settings, for normal range.
sepp_nepp 6:fb11b746ceb5 321 signalLimit = (FixPoint1616_t)(0.25 * 65536);
sepp_nepp 6:fb11b746ceb5 322 sigmaLimit = (FixPoint1616_t)(18 * 65536);
sepp_nepp 6:fb11b746ceb5 323 timingBudget = 33000;
sepp_nepp 6:fb11b746ceb5 324 preRangeVcselPeriod = 14;
sepp_nepp 6:fb11b746ceb5 325 finalRangeVcselPeriod = 10;
sepp_nepp 6:fb11b746ceb5 326 break;
sepp_nepp 6:fb11b746ceb5 327 case Range_Config_LONG_RANGE: // *** from mass market cube expansion v1.1, ranging with satellites.
sepp_nepp 6:fb11b746ceb5 328 signalLimit = (FixPoint1616_t)(0.1 * 65536);
sepp_nepp 6:fb11b746ceb5 329 sigmaLimit = (FixPoint1616_t)(60 * 65536);
sepp_nepp 6:fb11b746ceb5 330 timingBudget = 33000;
sepp_nepp 6:fb11b746ceb5 331 preRangeVcselPeriod = 18;
sepp_nepp 6:fb11b746ceb5 332 finalRangeVcselPeriod = 14;
sepp_nepp 6:fb11b746ceb5 333 break;
sepp_nepp 6:fb11b746ceb5 334 case Range_Config_HIGH_ACCURACY:
sepp_nepp 6:fb11b746ceb5 335 signalLimit = (FixPoint1616_t)(0.25*65536);
sepp_nepp 6:fb11b746ceb5 336 sigmaLimit = (FixPoint1616_t)(18*65536);
sepp_nepp 6:fb11b746ceb5 337 timingBudget = 200000;
sepp_nepp 6:fb11b746ceb5 338 preRangeVcselPeriod = 14;
sepp_nepp 6:fb11b746ceb5 339 finalRangeVcselPeriod = 10;
sepp_nepp 6:fb11b746ceb5 340 break;
sepp_nepp 6:fb11b746ceb5 341 case Range_Config_HIGH_SPEED:
sepp_nepp 6:fb11b746ceb5 342 signalLimit = (FixPoint1616_t)(0.25*65536);
sepp_nepp 6:fb11b746ceb5 343 sigmaLimit = (FixPoint1616_t)(32*65536);
sepp_nepp 6:fb11b746ceb5 344 timingBudget = 20000;
sepp_nepp 6:fb11b746ceb5 345 preRangeVcselPeriod = 14;
sepp_nepp 6:fb11b746ceb5 346 finalRangeVcselPeriod = 10;
sepp_nepp 6:fb11b746ceb5 347 break;
sepp_nepp 6:fb11b746ceb5 348 default:
sepp_nepp 6:fb11b746ceb5 349 Status = 96; // Config Not Supported
sepp_nepp 6:fb11b746ceb5 350 }
sepp_nepp 6:fb11b746ceb5 351
sepp_nepp 6:fb11b746ceb5 352 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 353 Status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);}
sepp_nepp 6:fb11b746ceb5 354
sepp_nepp 6:fb11b746ceb5 355 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 356 Status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);}
sepp_nepp 6:fb11b746ceb5 357
sepp_nepp 6:fb11b746ceb5 358 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 359 Status = VL53L0X_set_measurement_timing_budget_micro_seconds( timingBudget);}
sepp_nepp 6:fb11b746ceb5 360
sepp_nepp 6:fb11b746ceb5 361 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 362 Status = VL53L0X_set_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);}
sepp_nepp 6:fb11b746ceb5 363
sepp_nepp 6:fb11b746ceb5 364 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 365 Status = VL53L0X_set_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);}
sepp_nepp 6:fb11b746ceb5 366
sepp_nepp 6:fb11b746ceb5 367 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 368 Status = VL53L0X_perform_ref_calibration( &VhvSettings, &PhaseCal);}
sepp_nepp 6:fb11b746ceb5 369
sepp_nepp 6:fb11b746ceb5 370 }
sepp_nepp 6:fb11b746ceb5 371
sepp_nepp 6:fb11b746ceb5 372 if (operating_mode == range_continuous_polling) {
sepp_nepp 6:fb11b746ceb5 373 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 374 //printf("Call of VL53L0X_SetDeviceMode\n");
sepp_nepp 7:3a1115c2556b 375 Status = VL53L0X_set_device_mode( VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
sepp_nepp 6:fb11b746ceb5 376 }
sepp_nepp 6:fb11b746ceb5 377
sepp_nepp 6:fb11b746ceb5 378 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 379 //printf("Call of VL53L0X_StartMeasurement\n");
sepp_nepp 7:3a1115c2556b 380 Status = VL53L0X_start_measurement();
sepp_nepp 6:fb11b746ceb5 381 }
sepp_nepp 6:fb11b746ceb5 382 }
sepp_nepp 6:fb11b746ceb5 383 return Status;
sepp_nepp 6:fb11b746ceb5 384 }
sepp_nepp 6:fb11b746ceb5 385
sepp_nepp 6:fb11b746ceb5 386 int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void))
sepp_nepp 8:2fd7cb217068 387 { int status, clr_status;
sepp_nepp 6:fb11b746ceb5 388
sepp_nepp 7:3a1115c2556b 389 status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped
sepp_nepp 6:fb11b746ceb5 390
sepp_nepp 6:fb11b746ceb5 391 // status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
sepp_nepp 6:fb11b746ceb5 392
sepp_nepp 7:3a1115c2556b 393 status = VL53L0X_set_gpio_config( 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp 6:fb11b746ceb5 394 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
sepp_nepp 6:fb11b746ceb5 395 VL53L0X_INTERRUPTPOLARITY_HIGH);
sepp_nepp 6:fb11b746ceb5 396
sepp_nepp 6:fb11b746ceb5 397 if (!status) {
sepp_nepp 6:fb11b746ceb5 398 attach_interrupt_measure_detection_irq(fptr);
sepp_nepp 6:fb11b746ceb5 399 enable_interrupt_measure_detection_irq();
sepp_nepp 6:fb11b746ceb5 400 }
sepp_nepp 6:fb11b746ceb5 401
sepp_nepp 6:fb11b746ceb5 402 clr_status = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS);
sepp_nepp 6:fb11b746ceb5 403 if (clr_status!=0) { status = 98; } // VL53L0X_ClearErrorInterrupt_fail;
sepp_nepp 6:fb11b746ceb5 404
sepp_nepp 6:fb11b746ceb5 405 if (!status) {
sepp_nepp 6:fb11b746ceb5 406 status = range_start_continuous_mode();
sepp_nepp 6:fb11b746ceb5 407 }
sepp_nepp 6:fb11b746ceb5 408 return status;
sepp_nepp 6:fb11b746ceb5 409 }
sepp_nepp 6:fb11b746ceb5 410
sepp_nepp 7:3a1115c2556b 411 VL53L0X_Error VL53L0X::wait_measurement_data_ready(void)
sepp_nepp 8:2fd7cb217068 412 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 413 uint8_t new_dat_ready = 0;
sepp_nepp 6:fb11b746ceb5 414 uint32_t loop_nb;
sepp_nepp 6:fb11b746ceb5 415
sepp_nepp 6:fb11b746ceb5 416 // Wait until it finished
sepp_nepp 6:fb11b746ceb5 417 // use timeout to avoid deadlock
sepp_nepp 6:fb11b746ceb5 418 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 419 loop_nb = 0;
sepp_nepp 6:fb11b746ceb5 420 do {
sepp_nepp 7:3a1115c2556b 421 status = VL53L0X_get_measurement_data_ready( &new_dat_ready);
sepp_nepp 6:fb11b746ceb5 422 if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 423 break;
sepp_nepp 6:fb11b746ceb5 424 }
sepp_nepp 6:fb11b746ceb5 425 loop_nb = loop_nb + 1;
sepp_nepp 7:3a1115c2556b 426 VL53L0X_polling_delay();
sepp_nepp 6:fb11b746ceb5 427 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
sepp_nepp 6:fb11b746ceb5 428
sepp_nepp 6:fb11b746ceb5 429 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp 6:fb11b746ceb5 430 status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp 6:fb11b746ceb5 431 }
sepp_nepp 6:fb11b746ceb5 432 }
sepp_nepp 6:fb11b746ceb5 433
sepp_nepp 6:fb11b746ceb5 434 return status;
sepp_nepp 6:fb11b746ceb5 435 }
sepp_nepp 6:fb11b746ceb5 436
sepp_nepp 8:2fd7cb217068 437 int VL53L0X::get_distance(uint32_t *p_data)
sepp_nepp 6:fb11b746ceb5 438 {
sepp_nepp 8:2fd7cb217068 439 int status = 0;
sepp_nepp 8:2fd7cb217068 440 VL53L0X_RangingMeasurementData_t p_ranging_measurement_data;
sepp_nepp 8:2fd7cb217068 441
sepp_nepp 8:2fd7cb217068 442 status = start_measurement(range_single_shot_polling, NULL);
sepp_nepp 8:2fd7cb217068 443 if (!status) {
sepp_nepp 8:2fd7cb217068 444 status = get_measurement(range_single_shot_polling, &p_ranging_measurement_data);
sepp_nepp 8:2fd7cb217068 445 }
sepp_nepp 8:2fd7cb217068 446 if (p_ranging_measurement_data.RangeStatus == 0) {
sepp_nepp 8:2fd7cb217068 447 // we have a valid range.
sepp_nepp 8:2fd7cb217068 448 *p_data = p_ranging_measurement_data.RangeMilliMeter;
sepp_nepp 8:2fd7cb217068 449 } else {
sepp_nepp 8:2fd7cb217068 450 *p_data = 0;
sepp_nepp 8:2fd7cb217068 451 status = VL53L0X_ERROR_RANGE_ERROR;
sepp_nepp 8:2fd7cb217068 452 }
sepp_nepp 8:2fd7cb217068 453 stop_measurement(range_single_shot_polling);
sepp_nepp 8:2fd7cb217068 454 return status;
sepp_nepp 8:2fd7cb217068 455 }
sepp_nepp 8:2fd7cb217068 456
sepp_nepp 8:2fd7cb217068 457 VL53L0X_Error VL53L0X::wait_stop_completed(void)
sepp_nepp 8:2fd7cb217068 458 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 459 uint32_t stop_completed = 0;
sepp_nepp 6:fb11b746ceb5 460 uint32_t loop_nb;
sepp_nepp 6:fb11b746ceb5 461
sepp_nepp 6:fb11b746ceb5 462 // Wait until it finished
sepp_nepp 6:fb11b746ceb5 463 // use timeout to avoid deadlock
sepp_nepp 6:fb11b746ceb5 464 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 465 loop_nb = 0;
sepp_nepp 6:fb11b746ceb5 466 do {
sepp_nepp 7:3a1115c2556b 467 status = VL53L0X_get_stop_completed_status( &stop_completed);
sepp_nepp 6:fb11b746ceb5 468 if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 469 break;
sepp_nepp 6:fb11b746ceb5 470 }
sepp_nepp 6:fb11b746ceb5 471 loop_nb = loop_nb + 1;
sepp_nepp 7:3a1115c2556b 472 VL53L0X_polling_delay();
sepp_nepp 6:fb11b746ceb5 473 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
sepp_nepp 6:fb11b746ceb5 474
sepp_nepp 6:fb11b746ceb5 475 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp 6:fb11b746ceb5 476 status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp 6:fb11b746ceb5 477 }
sepp_nepp 6:fb11b746ceb5 478
sepp_nepp 6:fb11b746ceb5 479 }
sepp_nepp 6:fb11b746ceb5 480
sepp_nepp 6:fb11b746ceb5 481 return status;
sepp_nepp 6:fb11b746ceb5 482 }
sepp_nepp 6:fb11b746ceb5 483
sepp_nepp 6:fb11b746ceb5 484 int VL53L0X::get_measurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data)
sepp_nepp 8:2fd7cb217068 485 { int Status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 486
sepp_nepp 6:fb11b746ceb5 487 if (operating_mode == range_single_shot_polling) {
sepp_nepp 7:3a1115c2556b 488 Status = VL53L0X_perform_single_ranging_measurement( p_data);
sepp_nepp 6:fb11b746ceb5 489 }
sepp_nepp 6:fb11b746ceb5 490
sepp_nepp 6:fb11b746ceb5 491 if (operating_mode == range_continuous_polling) {
sepp_nepp 6:fb11b746ceb5 492 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 493 Status = VL53L0X_measurement_poll_for_completion();
sepp_nepp 6:fb11b746ceb5 494 }
sepp_nepp 6:fb11b746ceb5 495
sepp_nepp 6:fb11b746ceb5 496 if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 497 Status = VL53L0X_get_ranging_measurement_data( p_data);
sepp_nepp 6:fb11b746ceb5 498
sepp_nepp 6:fb11b746ceb5 499 // Clear the interrupt
sepp_nepp 7:3a1115c2556b 500 VL53L0X_clear_interrupt_mask( VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
sepp_nepp 7:3a1115c2556b 501 VL53L0X_polling_delay();
sepp_nepp 6:fb11b746ceb5 502 }
sepp_nepp 6:fb11b746ceb5 503 }
sepp_nepp 6:fb11b746ceb5 504
sepp_nepp 6:fb11b746ceb5 505 if (operating_mode == range_continuous_interrupt) {
sepp_nepp 7:3a1115c2556b 506 Status = VL53L0X_get_ranging_measurement_data( p_data);
sepp_nepp 7:3a1115c2556b 507 VL53L0X_clear_interrupt_mask( VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS);
sepp_nepp 6:fb11b746ceb5 508 }
sepp_nepp 6:fb11b746ceb5 509
sepp_nepp 6:fb11b746ceb5 510 return Status;
sepp_nepp 6:fb11b746ceb5 511 }
sepp_nepp 6:fb11b746ceb5 512
sepp_nepp 6:fb11b746ceb5 513 int VL53L0X::stop_measurement(OperatingMode operating_mode)
sepp_nepp 8:2fd7cb217068 514 { int status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 515
sepp_nepp 6:fb11b746ceb5 516 // don't need to stop for a singleshot range!
sepp_nepp 6:fb11b746ceb5 517 if (operating_mode == range_single_shot_polling) {
sepp_nepp 6:fb11b746ceb5 518 }
sepp_nepp 6:fb11b746ceb5 519
sepp_nepp 6:fb11b746ceb5 520 if (operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling) {
sepp_nepp 6:fb11b746ceb5 521 // continuous mode
sepp_nepp 6:fb11b746ceb5 522 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 523 //printf("Call of VL53L0X_StopMeasurement\n");
sepp_nepp 7:3a1115c2556b 524 status = VL53L0X_stop_measurement();
sepp_nepp 6:fb11b746ceb5 525 }
sepp_nepp 6:fb11b746ceb5 526
sepp_nepp 6:fb11b746ceb5 527 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 528 //printf("Wait Stop to be competed\n");
sepp_nepp 7:3a1115c2556b 529 status = wait_stop_completed();
sepp_nepp 6:fb11b746ceb5 530 }
sepp_nepp 6:fb11b746ceb5 531
sepp_nepp 6:fb11b746ceb5 532 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 533 status = VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
sepp_nepp 6:fb11b746ceb5 534 }
sepp_nepp 6:fb11b746ceb5 535
sepp_nepp 6:fb11b746ceb5 536 return status;
sepp_nepp 6:fb11b746ceb5 537 }
sepp_nepp 6:fb11b746ceb5 538
sepp_nepp 6:fb11b746ceb5 539 int VL53L0X::handle_irq(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data)
sepp_nepp 8:2fd7cb217068 540 { int status;
sepp_nepp 6:fb11b746ceb5 541 status = get_measurement(operating_mode, data);
sepp_nepp 6:fb11b746ceb5 542 enable_interrupt_measure_detection_irq();
sepp_nepp 6:fb11b746ceb5 543 return status;
sepp_nepp 6:fb11b746ceb5 544 }
sepp_nepp 6:fb11b746ceb5 545
sepp_nepp 6:fb11b746ceb5 546 int VL53L0X::range_start_continuous_mode()
sepp_nepp 6:fb11b746ceb5 547 { int status;
sepp_nepp 7:3a1115c2556b 548 status = VL53L0X_set_device_mode( VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
sepp_nepp 6:fb11b746ceb5 549
sepp_nepp 6:fb11b746ceb5 550 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 7:3a1115c2556b 551 { status = VL53L0X_start_measurement(); }
sepp_nepp 6:fb11b746ceb5 552
sepp_nepp 6:fb11b746ceb5 553 return status;
sepp_nepp 6:fb11b746ceb5 554 }
sepp_nepp 6:fb11b746ceb5 555
sepp_nepp 7:3a1115c2556b 556 VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(void)
sepp_nepp 8:2fd7cb217068 557 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 558 uint8_t strobe;
sepp_nepp 6:fb11b746ceb5 559 uint32_t loop_nb;
sepp_nepp 6:fb11b746ceb5 560
sepp_nepp 6:fb11b746ceb5 561
sepp_nepp 7:3a1115c2556b 562 status |= VL53L0X_write_byte( 0x83, 0x00);
sepp_nepp 6:fb11b746ceb5 563
sepp_nepp 6:fb11b746ceb5 564 /* polling
sepp_nepp 6:fb11b746ceb5 565 * use timeout to avoid deadlock*/
sepp_nepp 6:fb11b746ceb5 566 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 567 loop_nb = 0;
sepp_nepp 6:fb11b746ceb5 568 do {
sepp_nepp 7:3a1115c2556b 569 status = VL53L0X_read_byte( 0x83, &strobe);
sepp_nepp 6:fb11b746ceb5 570 if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 571 break;
sepp_nepp 6:fb11b746ceb5 572 }
sepp_nepp 6:fb11b746ceb5 573
sepp_nepp 6:fb11b746ceb5 574 loop_nb = loop_nb + 1;
sepp_nepp 6:fb11b746ceb5 575 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
sepp_nepp 6:fb11b746ceb5 576
sepp_nepp 6:fb11b746ceb5 577 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp 6:fb11b746ceb5 578 status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp 6:fb11b746ceb5 579 }
sepp_nepp 6:fb11b746ceb5 580 }
sepp_nepp 6:fb11b746ceb5 581
sepp_nepp 7:3a1115c2556b 582 status |= VL53L0X_write_byte( 0x83, 0x01);
sepp_nepp 6:fb11b746ceb5 583
sepp_nepp 6:fb11b746ceb5 584 return status;
sepp_nepp 6:fb11b746ceb5 585 }
sepp_nepp 6:fb11b746ceb5 586
sepp_nepp 7:3a1115c2556b 587 VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device( uint8_t option)
sepp_nepp 8:2fd7cb217068 588 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 589 uint8_t byte;
sepp_nepp 6:fb11b746ceb5 590 uint32_t tmp_dword;
sepp_nepp 6:fb11b746ceb5 591 uint8_t module_id;
sepp_nepp 6:fb11b746ceb5 592 uint8_t revision;
sepp_nepp 6:fb11b746ceb5 593 uint8_t reference_spad_count = 0;
sepp_nepp 6:fb11b746ceb5 594 uint8_t reference_spad_type = 0;
sepp_nepp 6:fb11b746ceb5 595 uint32_t part_uid_upper = 0;
sepp_nepp 6:fb11b746ceb5 596 uint32_t part_uid_lower = 0;
sepp_nepp 6:fb11b746ceb5 597 uint32_t offset_fixed1104_mm = 0;
sepp_nepp 6:fb11b746ceb5 598 int16_t offset_micro_meters = 0;
sepp_nepp 6:fb11b746ceb5 599 uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
sepp_nepp 6:fb11b746ceb5 600 uint32_t dist_meas_fixed1104_400_mm = 0;
sepp_nepp 6:fb11b746ceb5 601 uint32_t signal_rate_meas_fixed1104_400_mm = 0;
sepp_nepp 6:fb11b746ceb5 602 char product_id[19];
sepp_nepp 6:fb11b746ceb5 603 char *product_id_tmp;
sepp_nepp 6:fb11b746ceb5 604 uint8_t read_data_from_device_done;
sepp_nepp 6:fb11b746ceb5 605 FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0;
sepp_nepp 6:fb11b746ceb5 606 uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE];
sepp_nepp 6:fb11b746ceb5 607 int i;
sepp_nepp 6:fb11b746ceb5 608
sepp_nepp 8:2fd7cb217068 609 read_data_from_device_done = Data.ReadDataFromDeviceDone;
sepp_nepp 8:2fd7cb217068 610 read_data_from_device_done = Data.ReadDataFromDeviceDone;
sepp_nepp 8:2fd7cb217068 611 read_data_from_device_done = Data.ReadDataFromDeviceDone;
sepp_nepp 6:fb11b746ceb5 612
sepp_nepp 6:fb11b746ceb5 613 /* This access is done only once after that a GetDeviceInfo or
sepp_nepp 6:fb11b746ceb5 614 * datainit is done*/
sepp_nepp 6:fb11b746ceb5 615 if (read_data_from_device_done != 7) {
sepp_nepp 6:fb11b746ceb5 616
sepp_nepp 7:3a1115c2556b 617 status |= VL53L0X_write_byte( 0x80, 0x01);
sepp_nepp 7:3a1115c2556b 618 status |= VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 619 status |= VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 7:3a1115c2556b 620
sepp_nepp 7:3a1115c2556b 621 status |= VL53L0X_write_byte( 0xFF, 0x06);
sepp_nepp 7:3a1115c2556b 622 status |= VL53L0X_read_byte( 0x83, &byte);
sepp_nepp 7:3a1115c2556b 623 status |= VL53L0X_write_byte( 0x83, byte | 4);
sepp_nepp 7:3a1115c2556b 624 status |= VL53L0X_write_byte( 0xFF, 0x07);
sepp_nepp 7:3a1115c2556b 625 status |= VL53L0X_write_byte( 0x81, 0x01);
sepp_nepp 7:3a1115c2556b 626
sepp_nepp 7:3a1115c2556b 627 status |= VL53L0X_polling_delay();
sepp_nepp 7:3a1115c2556b 628
sepp_nepp 7:3a1115c2556b 629 status |= VL53L0X_write_byte( 0x80, 0x01);
sepp_nepp 6:fb11b746ceb5 630
sepp_nepp 6:fb11b746ceb5 631 if (((option & 1) == 1) &&
sepp_nepp 6:fb11b746ceb5 632 ((read_data_from_device_done & 1) == 0)) {
sepp_nepp 7:3a1115c2556b 633 status |= VL53L0X_write_byte( 0x94, 0x6b);
sepp_nepp 7:3a1115c2556b 634 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 635 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 636
sepp_nepp 6:fb11b746ceb5 637 reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f);
sepp_nepp 6:fb11b746ceb5 638 reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01);
sepp_nepp 6:fb11b746ceb5 639
sepp_nepp 7:3a1115c2556b 640 status |= VL53L0X_write_byte( 0x94, 0x24);
sepp_nepp 7:3a1115c2556b 641 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 642 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 643
sepp_nepp 6:fb11b746ceb5 644 nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24)
sepp_nepp 6:fb11b746ceb5 645 & 0xff);
sepp_nepp 6:fb11b746ceb5 646 nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16)
sepp_nepp 6:fb11b746ceb5 647 & 0xff);
sepp_nepp 6:fb11b746ceb5 648 nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8)
sepp_nepp 6:fb11b746ceb5 649 & 0xff);
sepp_nepp 6:fb11b746ceb5 650 nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff);
sepp_nepp 6:fb11b746ceb5 651
sepp_nepp 7:3a1115c2556b 652 status |= VL53L0X_write_byte( 0x94, 0x25);
sepp_nepp 7:3a1115c2556b 653 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 654 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 655
sepp_nepp 6:fb11b746ceb5 656 nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24)
sepp_nepp 6:fb11b746ceb5 657 & 0xff);
sepp_nepp 6:fb11b746ceb5 658 nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16)
sepp_nepp 6:fb11b746ceb5 659 & 0xff);
sepp_nepp 6:fb11b746ceb5 660 }
sepp_nepp 6:fb11b746ceb5 661
sepp_nepp 6:fb11b746ceb5 662 if (((option & 2) == 2) &&
sepp_nepp 6:fb11b746ceb5 663 ((read_data_from_device_done & 2) == 0)) {
sepp_nepp 6:fb11b746ceb5 664
sepp_nepp 7:3a1115c2556b 665 status |= VL53L0X_write_byte( 0x94, 0x02);
sepp_nepp 7:3a1115c2556b 666 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 667 status |= VL53L0X_read_byte( 0x90, &module_id);
sepp_nepp 7:3a1115c2556b 668
sepp_nepp 7:3a1115c2556b 669 status |= VL53L0X_write_byte( 0x94, 0x7B);
sepp_nepp 7:3a1115c2556b 670 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 671 status |= VL53L0X_read_byte( 0x90, &revision);
sepp_nepp 7:3a1115c2556b 672
sepp_nepp 7:3a1115c2556b 673 status |= VL53L0X_write_byte( 0x94, 0x77);
sepp_nepp 7:3a1115c2556b 674 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 675 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 676
sepp_nepp 6:fb11b746ceb5 677 product_id[0] = (char)((tmp_dword >> 25) & 0x07f);
sepp_nepp 6:fb11b746ceb5 678 product_id[1] = (char)((tmp_dword >> 18) & 0x07f);
sepp_nepp 6:fb11b746ceb5 679 product_id[2] = (char)((tmp_dword >> 11) & 0x07f);
sepp_nepp 6:fb11b746ceb5 680 product_id[3] = (char)((tmp_dword >> 4) & 0x07f);
sepp_nepp 6:fb11b746ceb5 681
sepp_nepp 6:fb11b746ceb5 682 byte = (uint8_t)((tmp_dword & 0x00f) << 3);
sepp_nepp 6:fb11b746ceb5 683
sepp_nepp 7:3a1115c2556b 684 status |= VL53L0X_write_byte( 0x94, 0x78);
sepp_nepp 7:3a1115c2556b 685 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 686 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 687
sepp_nepp 6:fb11b746ceb5 688 product_id[4] = (char)(byte +
sepp_nepp 6:fb11b746ceb5 689 ((tmp_dword >> 29) & 0x07f));
sepp_nepp 6:fb11b746ceb5 690 product_id[5] = (char)((tmp_dword >> 22) & 0x07f);
sepp_nepp 6:fb11b746ceb5 691 product_id[6] = (char)((tmp_dword >> 15) & 0x07f);
sepp_nepp 6:fb11b746ceb5 692 product_id[7] = (char)((tmp_dword >> 8) & 0x07f);
sepp_nepp 6:fb11b746ceb5 693 product_id[8] = (char)((tmp_dword >> 1) & 0x07f);
sepp_nepp 6:fb11b746ceb5 694
sepp_nepp 6:fb11b746ceb5 695 byte = (uint8_t)((tmp_dword & 0x001) << 6);
sepp_nepp 6:fb11b746ceb5 696
sepp_nepp 7:3a1115c2556b 697 status |= VL53L0X_write_byte( 0x94, 0x79);
sepp_nepp 7:3a1115c2556b 698
sepp_nepp 7:3a1115c2556b 699 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 700
sepp_nepp 7:3a1115c2556b 701 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 702
sepp_nepp 6:fb11b746ceb5 703 product_id[9] = (char)(byte +
sepp_nepp 6:fb11b746ceb5 704 ((tmp_dword >> 26) & 0x07f));
sepp_nepp 6:fb11b746ceb5 705 product_id[10] = (char)((tmp_dword >> 19) & 0x07f);
sepp_nepp 6:fb11b746ceb5 706 product_id[11] = (char)((tmp_dword >> 12) & 0x07f);
sepp_nepp 6:fb11b746ceb5 707 product_id[12] = (char)((tmp_dword >> 5) & 0x07f);
sepp_nepp 6:fb11b746ceb5 708
sepp_nepp 6:fb11b746ceb5 709 byte = (uint8_t)((tmp_dword & 0x01f) << 2);
sepp_nepp 6:fb11b746ceb5 710
sepp_nepp 7:3a1115c2556b 711 status |= VL53L0X_write_byte( 0x94, 0x7A);
sepp_nepp 7:3a1115c2556b 712
sepp_nepp 7:3a1115c2556b 713 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 714
sepp_nepp 7:3a1115c2556b 715 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 716
sepp_nepp 6:fb11b746ceb5 717 product_id[13] = (char)(byte +
sepp_nepp 6:fb11b746ceb5 718 ((tmp_dword >> 30) & 0x07f));
sepp_nepp 6:fb11b746ceb5 719 product_id[14] = (char)((tmp_dword >> 23) & 0x07f);
sepp_nepp 6:fb11b746ceb5 720 product_id[15] = (char)((tmp_dword >> 16) & 0x07f);
sepp_nepp 6:fb11b746ceb5 721 product_id[16] = (char)((tmp_dword >> 9) & 0x07f);
sepp_nepp 6:fb11b746ceb5 722 product_id[17] = (char)((tmp_dword >> 2) & 0x07f);
sepp_nepp 6:fb11b746ceb5 723 product_id[18] = '\0';
sepp_nepp 6:fb11b746ceb5 724
sepp_nepp 6:fb11b746ceb5 725 }
sepp_nepp 6:fb11b746ceb5 726
sepp_nepp 6:fb11b746ceb5 727 if (((option & 4) == 4) &&
sepp_nepp 6:fb11b746ceb5 728 ((read_data_from_device_done & 4) == 0)) {
sepp_nepp 6:fb11b746ceb5 729
sepp_nepp 7:3a1115c2556b 730 status |= VL53L0X_write_byte( 0x94, 0x7B);
sepp_nepp 7:3a1115c2556b 731 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 732 status |= VL53L0X_read_dword( 0x90, &part_uid_upper);
sepp_nepp 7:3a1115c2556b 733
sepp_nepp 7:3a1115c2556b 734 status |= VL53L0X_write_byte( 0x94, 0x7C);
sepp_nepp 7:3a1115c2556b 735 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 736 status |= VL53L0X_read_dword( 0x90, &part_uid_lower);
sepp_nepp 7:3a1115c2556b 737
sepp_nepp 7:3a1115c2556b 738 status |= VL53L0X_write_byte( 0x94, 0x73);
sepp_nepp 7:3a1115c2556b 739 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 740 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 741
sepp_nepp 6:fb11b746ceb5 742 signal_rate_meas_fixed1104_400_mm = (tmp_dword &
sepp_nepp 6:fb11b746ceb5 743 0x0000000ff) << 8;
sepp_nepp 6:fb11b746ceb5 744
sepp_nepp 7:3a1115c2556b 745 status |= VL53L0X_write_byte( 0x94, 0x74);
sepp_nepp 7:3a1115c2556b 746 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 747 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 748
sepp_nepp 6:fb11b746ceb5 749 signal_rate_meas_fixed1104_400_mm |= ((tmp_dword &
sepp_nepp 6:fb11b746ceb5 750 0xff000000) >> 24);
sepp_nepp 6:fb11b746ceb5 751
sepp_nepp 7:3a1115c2556b 752 status |= VL53L0X_write_byte( 0x94, 0x75);
sepp_nepp 7:3a1115c2556b 753 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 754 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 755
sepp_nepp 6:fb11b746ceb5 756 dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)
sepp_nepp 6:fb11b746ceb5 757 << 8;
sepp_nepp 6:fb11b746ceb5 758
sepp_nepp 7:3a1115c2556b 759 status |= VL53L0X_write_byte( 0x94, 0x76);
sepp_nepp 7:3a1115c2556b 760 status |= VL53L0X_device_read_strobe();
sepp_nepp 7:3a1115c2556b 761 status |= VL53L0X_read_dword( 0x90, &tmp_dword);
sepp_nepp 6:fb11b746ceb5 762
sepp_nepp 6:fb11b746ceb5 763 dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000)
sepp_nepp 6:fb11b746ceb5 764 >> 24);
sepp_nepp 6:fb11b746ceb5 765 }
sepp_nepp 6:fb11b746ceb5 766
sepp_nepp 7:3a1115c2556b 767 status |= VL53L0X_write_byte( 0x81, 0x00);
sepp_nepp 7:3a1115c2556b 768 status |= VL53L0X_write_byte( 0xFF, 0x06);
sepp_nepp 7:3a1115c2556b 769 status |= VL53L0X_read_byte( 0x83, &byte);
sepp_nepp 7:3a1115c2556b 770 status |= VL53L0X_write_byte( 0x83, byte & 0xfb);
sepp_nepp 7:3a1115c2556b 771 status |= VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 772 status |= VL53L0X_write_byte( 0x00, 0x01);
sepp_nepp 7:3a1115c2556b 773
sepp_nepp 7:3a1115c2556b 774 status |= VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 7:3a1115c2556b 775 status |= VL53L0X_write_byte( 0x80, 0x00);
sepp_nepp 6:fb11b746ceb5 776 }
sepp_nepp 6:fb11b746ceb5 777
sepp_nepp 6:fb11b746ceb5 778 if ((status == VL53L0X_ERROR_NONE) &&
sepp_nepp 6:fb11b746ceb5 779 (read_data_from_device_done != 7)) {
sepp_nepp 6:fb11b746ceb5 780 /* Assign to variable if status is ok */
sepp_nepp 6:fb11b746ceb5 781 if (((option & 1) == 1) &&
sepp_nepp 6:fb11b746ceb5 782 ((read_data_from_device_done & 1) == 0)) {
sepp_nepp 8:2fd7cb217068 783 Data.ReferenceSpadCount = reference_spad_count;
sepp_nepp 8:2fd7cb217068 784
sepp_nepp 8:2fd7cb217068 785 Data.ReferenceSpadType = reference_spad_type;
sepp_nepp 6:fb11b746ceb5 786
sepp_nepp 6:fb11b746ceb5 787 for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
sepp_nepp 7:3a1115c2556b 788 Data.SpadData.RefGoodSpadMap[i] =
sepp_nepp 6:fb11b746ceb5 789 nvm_ref_good_spad_map[i];
sepp_nepp 6:fb11b746ceb5 790 }
sepp_nepp 6:fb11b746ceb5 791 }
sepp_nepp 6:fb11b746ceb5 792
sepp_nepp 6:fb11b746ceb5 793 if (((option & 2) == 2) &&
sepp_nepp 6:fb11b746ceb5 794 ((read_data_from_device_done & 2) == 0)) {
sepp_nepp 8:2fd7cb217068 795 Data.ModuleId = module_id;
sepp_nepp 8:2fd7cb217068 796
sepp_nepp 8:2fd7cb217068 797 Data.Revision = revision;
sepp_nepp 8:2fd7cb217068 798
sepp_nepp 8:2fd7cb217068 799 product_id_tmp = Data.ProductId;
sepp_nepp 6:fb11b746ceb5 800 VL53L0X_COPYSTRING(product_id_tmp, product_id);
sepp_nepp 6:fb11b746ceb5 801
sepp_nepp 6:fb11b746ceb5 802 }
sepp_nepp 6:fb11b746ceb5 803
sepp_nepp 6:fb11b746ceb5 804 if (((option & 4) == 4) &&
sepp_nepp 6:fb11b746ceb5 805 ((read_data_from_device_done & 4) == 0)) {
sepp_nepp 8:2fd7cb217068 806 Data.PartUIDUpper = part_uid_upper;
sepp_nepp 8:2fd7cb217068 807
sepp_nepp 8:2fd7cb217068 808 Data.PartUIDLower = part_uid_lower;
sepp_nepp 6:fb11b746ceb5 809
sepp_nepp 6:fb11b746ceb5 810 signal_rate_meas_fixed400_mm_fix =
sepp_nepp 8:2fd7cb217068 811 VL53L0X_FIXPOINT97TOFIXPOINT1616(signal_rate_meas_fixed1104_400_mm);
sepp_nepp 8:2fd7cb217068 812
sepp_nepp 8:2fd7cb217068 813 Data.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
sepp_nepp 6:fb11b746ceb5 814
sepp_nepp 6:fb11b746ceb5 815 offset_micro_meters = 0;
sepp_nepp 6:fb11b746ceb5 816 if (dist_meas_fixed1104_400_mm != 0) {
sepp_nepp 6:fb11b746ceb5 817 offset_fixed1104_mm =
sepp_nepp 6:fb11b746ceb5 818 dist_meas_fixed1104_400_mm -
sepp_nepp 6:fb11b746ceb5 819 dist_meas_tgt_fixed1104_mm;
sepp_nepp 6:fb11b746ceb5 820 offset_micro_meters = (offset_fixed1104_mm
sepp_nepp 6:fb11b746ceb5 821 * 1000) >> 4;
sepp_nepp 6:fb11b746ceb5 822 offset_micro_meters *= -1;
sepp_nepp 6:fb11b746ceb5 823 }
sepp_nepp 6:fb11b746ceb5 824
sepp_nepp 8:2fd7cb217068 825 Data.Part2PartOffsetAdjustmentNVMMicroMeter = offset_micro_meters;
sepp_nepp 6:fb11b746ceb5 826 }
sepp_nepp 6:fb11b746ceb5 827 byte = (uint8_t)(read_data_from_device_done | option);
sepp_nepp 8:2fd7cb217068 828 Data.ReadDataFromDeviceDone = byte;
sepp_nepp 6:fb11b746ceb5 829 }
sepp_nepp 6:fb11b746ceb5 830 return status;
sepp_nepp 6:fb11b746ceb5 831 }
sepp_nepp 6:fb11b746ceb5 832
sepp_nepp 8:2fd7cb217068 833 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter)
sepp_nepp 8:2fd7cb217068 834 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 835 uint16_t range_offset_register;
sepp_nepp 6:fb11b746ceb5 836 int16_t c_max_offset = 2047;
sepp_nepp 6:fb11b746ceb5 837 int16_t c_offset_range = 4096;
sepp_nepp 6:fb11b746ceb5 838
sepp_nepp 6:fb11b746ceb5 839 /* Note that offset has 10.2 format */
sepp_nepp 6:fb11b746ceb5 840
sepp_nepp 8:2fd7cb217068 841 status = VL53L0X_read_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
sepp_nepp 6:fb11b746ceb5 842 &range_offset_register);
sepp_nepp 6:fb11b746ceb5 843
sepp_nepp 6:fb11b746ceb5 844 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 845 range_offset_register = (range_offset_register & 0x0fff);
sepp_nepp 6:fb11b746ceb5 846
sepp_nepp 6:fb11b746ceb5 847 /* Apply 12 bit 2's compliment conversion */
sepp_nepp 6:fb11b746ceb5 848 if (range_offset_register > c_max_offset) {
sepp_nepp 6:fb11b746ceb5 849 *p_offset_calibration_data_micro_meter =
sepp_nepp 6:fb11b746ceb5 850 (int16_t)(range_offset_register - c_offset_range)
sepp_nepp 6:fb11b746ceb5 851 * 250;
sepp_nepp 6:fb11b746ceb5 852 } else {
sepp_nepp 6:fb11b746ceb5 853 *p_offset_calibration_data_micro_meter =
sepp_nepp 6:fb11b746ceb5 854 (int16_t)range_offset_register * 250;
sepp_nepp 6:fb11b746ceb5 855 }
sepp_nepp 6:fb11b746ceb5 856
sepp_nepp 6:fb11b746ceb5 857 }
sepp_nepp 6:fb11b746ceb5 858
sepp_nepp 6:fb11b746ceb5 859 return status;
sepp_nepp 6:fb11b746ceb5 860 }
sepp_nepp 6:fb11b746ceb5 861
sepp_nepp 8:2fd7cb217068 862 VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter)
sepp_nepp 8:2fd7cb217068 863 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 864
sepp_nepp 6:fb11b746ceb5 865
sepp_nepp 8:2fd7cb217068 866 status = wrapped_VL53L0X_get_offset_calibration_data_micro_meter(p_offset_calibration_data_micro_meter);
sepp_nepp 6:fb11b746ceb5 867
sepp_nepp 6:fb11b746ceb5 868
sepp_nepp 6:fb11b746ceb5 869 return status;
sepp_nepp 6:fb11b746ceb5 870 }
sepp_nepp 6:fb11b746ceb5 871
sepp_nepp 8:2fd7cb217068 872 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter)
sepp_nepp 8:2fd7cb217068 873 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 874 int32_t c_max_offset_micro_meter = 511000;
sepp_nepp 6:fb11b746ceb5 875 int32_t c_min_offset_micro_meter = -512000;
sepp_nepp 6:fb11b746ceb5 876 int16_t c_offset_range = 4096;
sepp_nepp 6:fb11b746ceb5 877 uint32_t encoded_offset_val;
sepp_nepp 6:fb11b746ceb5 878
sepp_nepp 6:fb11b746ceb5 879
sepp_nepp 6:fb11b746ceb5 880
sepp_nepp 6:fb11b746ceb5 881 if (offset_calibration_data_micro_meter > c_max_offset_micro_meter) {
sepp_nepp 6:fb11b746ceb5 882 offset_calibration_data_micro_meter = c_max_offset_micro_meter;
sepp_nepp 6:fb11b746ceb5 883 } else {
sepp_nepp 6:fb11b746ceb5 884 if (offset_calibration_data_micro_meter < c_min_offset_micro_meter) {
sepp_nepp 6:fb11b746ceb5 885 offset_calibration_data_micro_meter = c_min_offset_micro_meter;
sepp_nepp 6:fb11b746ceb5 886 }
sepp_nepp 6:fb11b746ceb5 887 }
sepp_nepp 6:fb11b746ceb5 888
sepp_nepp 6:fb11b746ceb5 889 /* The offset register is 10.2 format and units are mm
sepp_nepp 6:fb11b746ceb5 890 * therefore conversion is applied by a division of
sepp_nepp 6:fb11b746ceb5 891 * 250.
sepp_nepp 6:fb11b746ceb5 892 */
sepp_nepp 6:fb11b746ceb5 893 if (offset_calibration_data_micro_meter >= 0) {
sepp_nepp 6:fb11b746ceb5 894 encoded_offset_val =
sepp_nepp 6:fb11b746ceb5 895 offset_calibration_data_micro_meter / 250;
sepp_nepp 6:fb11b746ceb5 896 } else {
sepp_nepp 6:fb11b746ceb5 897 encoded_offset_val =
sepp_nepp 6:fb11b746ceb5 898 c_offset_range +
sepp_nepp 6:fb11b746ceb5 899 offset_calibration_data_micro_meter / 250;
sepp_nepp 6:fb11b746ceb5 900 }
sepp_nepp 6:fb11b746ceb5 901
sepp_nepp 8:2fd7cb217068 902 status = VL53L0X_write_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
sepp_nepp 6:fb11b746ceb5 903 encoded_offset_val);
sepp_nepp 6:fb11b746ceb5 904
sepp_nepp 6:fb11b746ceb5 905
sepp_nepp 6:fb11b746ceb5 906 return status;
sepp_nepp 6:fb11b746ceb5 907 }
sepp_nepp 6:fb11b746ceb5 908
sepp_nepp 8:2fd7cb217068 909 VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter)
sepp_nepp 8:2fd7cb217068 910 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 911
sepp_nepp 6:fb11b746ceb5 912
sepp_nepp 8:2fd7cb217068 913 status = wrapped_VL53L0X_set_offset_calibration_data_micro_meter(offset_calibration_data_micro_meter);
sepp_nepp 6:fb11b746ceb5 914
sepp_nepp 6:fb11b746ceb5 915
sepp_nepp 6:fb11b746ceb5 916 return status;
sepp_nepp 6:fb11b746ceb5 917 }
sepp_nepp 6:fb11b746ceb5 918
sepp_nepp 7:3a1115c2556b 919 VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(void)
sepp_nepp 8:2fd7cb217068 920 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 921 int32_t corrected_offset_micro_meters;
sepp_nepp 6:fb11b746ceb5 922 int32_t current_offset_micro_meters;
sepp_nepp 6:fb11b746ceb5 923
sepp_nepp 8:2fd7cb217068 924 /* if we run on this function we can read all the NVM info used by the API */
sepp_nepp 7:3a1115c2556b 925 status = VL53L0X_get_info_from_device( 7);
sepp_nepp 6:fb11b746ceb5 926
sepp_nepp 6:fb11b746ceb5 927 /* Read back current device offset */
sepp_nepp 6:fb11b746ceb5 928 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 929 status = VL53L0X_get_offset_calibration_data_micro_meter(&current_offset_micro_meters);
sepp_nepp 6:fb11b746ceb5 930 }
sepp_nepp 6:fb11b746ceb5 931
sepp_nepp 6:fb11b746ceb5 932 /* Apply Offset Adjustment derived from 400mm measurements */
sepp_nepp 6:fb11b746ceb5 933 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 934
sepp_nepp 6:fb11b746ceb5 935 /* Store initial device offset */
sepp_nepp 8:2fd7cb217068 936 Data.Part2PartOffsetNVMMicroMeter = current_offset_micro_meters;
sepp_nepp 6:fb11b746ceb5 937
sepp_nepp 6:fb11b746ceb5 938 corrected_offset_micro_meters = current_offset_micro_meters +
sepp_nepp 8:2fd7cb217068 939 (int32_t)Data.Part2PartOffsetAdjustmentNVMMicroMeter;
sepp_nepp 8:2fd7cb217068 940
sepp_nepp 8:2fd7cb217068 941 status = VL53L0X_set_offset_calibration_data_micro_meter(corrected_offset_micro_meters);
sepp_nepp 6:fb11b746ceb5 942
sepp_nepp 6:fb11b746ceb5 943 /* store current, adjusted offset */
sepp_nepp 6:fb11b746ceb5 944 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 945 Data.CurrentParameters.RangeOffsetMicroMeters = corrected_offset_micro_meters;
sepp_nepp 6:fb11b746ceb5 946 }
sepp_nepp 6:fb11b746ceb5 947 }
sepp_nepp 6:fb11b746ceb5 948
sepp_nepp 6:fb11b746ceb5 949 return status;
sepp_nepp 6:fb11b746ceb5 950 }
sepp_nepp 6:fb11b746ceb5 951
sepp_nepp 8:2fd7cb217068 952 VL53L0X_Error VL53L0X::VL53L0X_get_device_mode(VL53L0X_DeviceModes *p_device_mode)
sepp_nepp 8:2fd7cb217068 953 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 954
sepp_nepp 6:fb11b746ceb5 955
sepp_nepp 8:2fd7cb217068 956 *p_device_mode = Data.CurrentParameters.DeviceMode ;
sepp_nepp 6:fb11b746ceb5 957
sepp_nepp 6:fb11b746ceb5 958
sepp_nepp 6:fb11b746ceb5 959 return status;
sepp_nepp 6:fb11b746ceb5 960 }
sepp_nepp 6:fb11b746ceb5 961
sepp_nepp 8:2fd7cb217068 962 VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_milli_seconds(uint32_t *p_inter_measurement_period_milli_seconds)
sepp_nepp 8:2fd7cb217068 963 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 964 uint16_t osc_calibrate_val;
sepp_nepp 6:fb11b746ceb5 965 uint32_t im_period_milli_seconds;
sepp_nepp 6:fb11b746ceb5 966
sepp_nepp 6:fb11b746ceb5 967
sepp_nepp 6:fb11b746ceb5 968
sepp_nepp 7:3a1115c2556b 969 status = VL53L0X_read_word( VL53L0X_REG_OSC_CALIBRATE_VAL,
sepp_nepp 6:fb11b746ceb5 970 &osc_calibrate_val);
sepp_nepp 6:fb11b746ceb5 971
sepp_nepp 6:fb11b746ceb5 972 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 973 status = VL53L0X_read_dword(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
sepp_nepp 6:fb11b746ceb5 974 &im_period_milli_seconds);
sepp_nepp 6:fb11b746ceb5 975 }
sepp_nepp 6:fb11b746ceb5 976
sepp_nepp 6:fb11b746ceb5 977 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 978 if (osc_calibrate_val != 0) {
sepp_nepp 6:fb11b746ceb5 979 *p_inter_measurement_period_milli_seconds =
sepp_nepp 6:fb11b746ceb5 980 im_period_milli_seconds / osc_calibrate_val;
sepp_nepp 6:fb11b746ceb5 981 }
sepp_nepp 8:2fd7cb217068 982 Data.CurrentParameters.InterMeasurementPeriodMilliSeconds = *p_inter_measurement_period_milli_seconds;
sepp_nepp 6:fb11b746ceb5 983 }
sepp_nepp 6:fb11b746ceb5 984
sepp_nepp 6:fb11b746ceb5 985
sepp_nepp 6:fb11b746ceb5 986 return status;
sepp_nepp 6:fb11b746ceb5 987 }
sepp_nepp 6:fb11b746ceb5 988
sepp_nepp 8:2fd7cb217068 989 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_mega_cps(FixPoint1616_t *p_xtalk_compensation_rate_mega_cps)
sepp_nepp 8:2fd7cb217068 990 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 991 uint16_t value;
sepp_nepp 6:fb11b746ceb5 992 FixPoint1616_t temp_fix1616;
sepp_nepp 6:fb11b746ceb5 993
sepp_nepp 6:fb11b746ceb5 994
sepp_nepp 6:fb11b746ceb5 995
sepp_nepp 8:2fd7cb217068 996 status = VL53L0X_read_word(VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&value);
sepp_nepp 6:fb11b746ceb5 997 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 998 if (value == 0) {
sepp_nepp 6:fb11b746ceb5 999 /* the Xtalk is disabled return value from memory */
sepp_nepp 8:2fd7cb217068 1000 temp_fix1616 = Data.CurrentParameters.XTalkCompensationRateMegaCps ;
sepp_nepp 6:fb11b746ceb5 1001 *p_xtalk_compensation_rate_mega_cps = temp_fix1616;
sepp_nepp 8:2fd7cb217068 1002 Data.CurrentParameters.XTalkCompensationEnable = 0;
sepp_nepp 6:fb11b746ceb5 1003 } else {
sepp_nepp 6:fb11b746ceb5 1004 temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value);
sepp_nepp 6:fb11b746ceb5 1005 *p_xtalk_compensation_rate_mega_cps = temp_fix1616;
sepp_nepp 8:2fd7cb217068 1006 Data.CurrentParameters.XTalkCompensationRateMegaCps = temp_fix1616;
sepp_nepp 8:2fd7cb217068 1007 Data.CurrentParameters.XTalkCompensationEnable = 1;
sepp_nepp 6:fb11b746ceb5 1008 }
sepp_nepp 6:fb11b746ceb5 1009 }
sepp_nepp 6:fb11b746ceb5 1010
sepp_nepp 6:fb11b746ceb5 1011
sepp_nepp 6:fb11b746ceb5 1012 return status;
sepp_nepp 6:fb11b746ceb5 1013 }
sepp_nepp 6:fb11b746ceb5 1014
sepp_nepp 7:3a1115c2556b 1015 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value( uint16_t limit_check_id,
sepp_nepp 6:fb11b746ceb5 1016 FixPoint1616_t *p_limit_check_value)
sepp_nepp 8:2fd7cb217068 1017 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1018 uint8_t enable_zero_value = 0;
sepp_nepp 6:fb11b746ceb5 1019 uint16_t temp16;
sepp_nepp 6:fb11b746ceb5 1020 FixPoint1616_t temp_fix1616;
sepp_nepp 6:fb11b746ceb5 1021
sepp_nepp 6:fb11b746ceb5 1022
sepp_nepp 6:fb11b746ceb5 1023
sepp_nepp 6:fb11b746ceb5 1024 switch (limit_check_id) {
sepp_nepp 6:fb11b746ceb5 1025
sepp_nepp 6:fb11b746ceb5 1026 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 1027 /* internal computation: */
sepp_nepp 8:2fd7cb217068 1028 temp_fix1616 = Data.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE];
sepp_nepp 6:fb11b746ceb5 1029 enable_zero_value = 0;
sepp_nepp 6:fb11b746ceb5 1030 break;
sepp_nepp 6:fb11b746ceb5 1031
sepp_nepp 6:fb11b746ceb5 1032 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
sepp_nepp 8:2fd7cb217068 1033 status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 6:fb11b746ceb5 1034 &temp16);
sepp_nepp 6:fb11b746ceb5 1035 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1036 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
sepp_nepp 6:fb11b746ceb5 1037 }
sepp_nepp 6:fb11b746ceb5 1038
sepp_nepp 6:fb11b746ceb5 1039 enable_zero_value = 1;
sepp_nepp 6:fb11b746ceb5 1040 break;
sepp_nepp 6:fb11b746ceb5 1041
sepp_nepp 6:fb11b746ceb5 1042 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
sepp_nepp 6:fb11b746ceb5 1043 /* internal computation: */
sepp_nepp 8:2fd7cb217068 1044 temp_fix1616 = Data.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP];
sepp_nepp 6:fb11b746ceb5 1045 enable_zero_value = 0;
sepp_nepp 6:fb11b746ceb5 1046 break;
sepp_nepp 6:fb11b746ceb5 1047
sepp_nepp 6:fb11b746ceb5 1048 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
sepp_nepp 6:fb11b746ceb5 1049 /* internal computation: */
sepp_nepp 8:2fd7cb217068 1050 temp_fix1616 = Data.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD];
sepp_nepp 6:fb11b746ceb5 1051 enable_zero_value = 0;
sepp_nepp 6:fb11b746ceb5 1052 break;
sepp_nepp 6:fb11b746ceb5 1053
sepp_nepp 6:fb11b746ceb5 1054 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
sepp_nepp 6:fb11b746ceb5 1055 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
sepp_nepp 8:2fd7cb217068 1056 status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 6:fb11b746ceb5 1057 &temp16);
sepp_nepp 6:fb11b746ceb5 1058 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1059 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
sepp_nepp 6:fb11b746ceb5 1060 }
sepp_nepp 6:fb11b746ceb5 1061
sepp_nepp 6:fb11b746ceb5 1062 enable_zero_value = 0;
sepp_nepp 6:fb11b746ceb5 1063 break;
sepp_nepp 6:fb11b746ceb5 1064
sepp_nepp 6:fb11b746ceb5 1065 default:
sepp_nepp 6:fb11b746ceb5 1066 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1067
sepp_nepp 6:fb11b746ceb5 1068 }
sepp_nepp 6:fb11b746ceb5 1069
sepp_nepp 6:fb11b746ceb5 1070 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1071
sepp_nepp 6:fb11b746ceb5 1072 if (enable_zero_value == 1) {
sepp_nepp 6:fb11b746ceb5 1073
sepp_nepp 6:fb11b746ceb5 1074 if (temp_fix1616 == 0) {
sepp_nepp 6:fb11b746ceb5 1075 /* disabled: return value from memory */
sepp_nepp 8:2fd7cb217068 1076 temp_fix1616 = Data.CurrentParameters.LimitChecksValue[limit_check_id];
sepp_nepp 6:fb11b746ceb5 1077 *p_limit_check_value = temp_fix1616;
sepp_nepp 8:2fd7cb217068 1078 Data.CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
sepp_nepp 6:fb11b746ceb5 1079 } else {
sepp_nepp 6:fb11b746ceb5 1080 *p_limit_check_value = temp_fix1616;
sepp_nepp 8:2fd7cb217068 1081 Data.CurrentParameters.LimitChecksValue[limit_check_id] = temp_fix1616;
sepp_nepp 8:2fd7cb217068 1082 Data.CurrentParameters.LimitChecksEnable[limit_check_id] = 1;
sepp_nepp 6:fb11b746ceb5 1083 }
sepp_nepp 6:fb11b746ceb5 1084 } else {
sepp_nepp 6:fb11b746ceb5 1085 *p_limit_check_value = temp_fix1616;
sepp_nepp 6:fb11b746ceb5 1086 }
sepp_nepp 6:fb11b746ceb5 1087 }
sepp_nepp 6:fb11b746ceb5 1088
sepp_nepp 6:fb11b746ceb5 1089
sepp_nepp 6:fb11b746ceb5 1090 return status;
sepp_nepp 6:fb11b746ceb5 1091
sepp_nepp 6:fb11b746ceb5 1092 }
sepp_nepp 6:fb11b746ceb5 1093
sepp_nepp 7:3a1115c2556b 1094 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable( uint16_t limit_check_id,
sepp_nepp 6:fb11b746ceb5 1095 uint8_t *p_limit_check_enable)
sepp_nepp 8:2fd7cb217068 1096 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1097 uint8_t temp8;
sepp_nepp 6:fb11b746ceb5 1098
sepp_nepp 6:fb11b746ceb5 1099
sepp_nepp 6:fb11b746ceb5 1100
sepp_nepp 6:fb11b746ceb5 1101 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
sepp_nepp 6:fb11b746ceb5 1102 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1103 *p_limit_check_enable = 0;
sepp_nepp 6:fb11b746ceb5 1104 } else {
sepp_nepp 8:2fd7cb217068 1105 temp8 = Data.CurrentParameters.LimitChecksEnable[limit_check_id];
sepp_nepp 6:fb11b746ceb5 1106 *p_limit_check_enable = temp8;
sepp_nepp 6:fb11b746ceb5 1107 }
sepp_nepp 6:fb11b746ceb5 1108
sepp_nepp 6:fb11b746ceb5 1109
sepp_nepp 6:fb11b746ceb5 1110 return status;
sepp_nepp 6:fb11b746ceb5 1111 }
sepp_nepp 6:fb11b746ceb5 1112
sepp_nepp 8:2fd7cb217068 1113 VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(uint8_t *p_wrap_around_check_enable)
sepp_nepp 8:2fd7cb217068 1114 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1115 uint8_t data;
sepp_nepp 6:fb11b746ceb5 1116
sepp_nepp 6:fb11b746ceb5 1117
sepp_nepp 6:fb11b746ceb5 1118
sepp_nepp 7:3a1115c2556b 1119 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data);
sepp_nepp 6:fb11b746ceb5 1120 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1121 Data.SequenceConfig = data;
sepp_nepp 6:fb11b746ceb5 1122 if (data & (0x01 << 7)) {
sepp_nepp 6:fb11b746ceb5 1123 *p_wrap_around_check_enable = 0x01;
sepp_nepp 6:fb11b746ceb5 1124 } else {
sepp_nepp 6:fb11b746ceb5 1125 *p_wrap_around_check_enable = 0x00;
sepp_nepp 6:fb11b746ceb5 1126 }
sepp_nepp 6:fb11b746ceb5 1127 }
sepp_nepp 6:fb11b746ceb5 1128 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1129 Data.CurrentParameters.WrapAroundCheckEnable = *p_wrap_around_check_enable;
sepp_nepp 6:fb11b746ceb5 1130 }
sepp_nepp 6:fb11b746ceb5 1131
sepp_nepp 6:fb11b746ceb5 1132
sepp_nepp 6:fb11b746ceb5 1133 return status;
sepp_nepp 6:fb11b746ceb5 1134 }
sepp_nepp 6:fb11b746ceb5 1135
sepp_nepp 8:2fd7cb217068 1136 VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config,
sepp_nepp 6:fb11b746ceb5 1137 uint8_t *p_sequence_step_enabled)
sepp_nepp 8:2fd7cb217068 1138 { VL53L0X_Error Status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1139 *p_sequence_step_enabled = 0;
sepp_nepp 6:fb11b746ceb5 1140
sepp_nepp 6:fb11b746ceb5 1141
sepp_nepp 6:fb11b746ceb5 1142 switch (sequence_step_id) {
sepp_nepp 6:fb11b746ceb5 1143 case VL53L0X_SEQUENCESTEP_TCC:
sepp_nepp 6:fb11b746ceb5 1144 *p_sequence_step_enabled = (sequence_config & 0x10) >> 4;
sepp_nepp 6:fb11b746ceb5 1145 break;
sepp_nepp 6:fb11b746ceb5 1146 case VL53L0X_SEQUENCESTEP_DSS:
sepp_nepp 6:fb11b746ceb5 1147 *p_sequence_step_enabled = (sequence_config & 0x08) >> 3;
sepp_nepp 6:fb11b746ceb5 1148 break;
sepp_nepp 6:fb11b746ceb5 1149 case VL53L0X_SEQUENCESTEP_MSRC:
sepp_nepp 6:fb11b746ceb5 1150 *p_sequence_step_enabled = (sequence_config & 0x04) >> 2;
sepp_nepp 6:fb11b746ceb5 1151 break;
sepp_nepp 6:fb11b746ceb5 1152 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sepp_nepp 6:fb11b746ceb5 1153 *p_sequence_step_enabled = (sequence_config & 0x40) >> 6;
sepp_nepp 6:fb11b746ceb5 1154 break;
sepp_nepp 6:fb11b746ceb5 1155 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 1156 *p_sequence_step_enabled = (sequence_config & 0x80) >> 7;
sepp_nepp 6:fb11b746ceb5 1157 break;
sepp_nepp 6:fb11b746ceb5 1158 default:
sepp_nepp 6:fb11b746ceb5 1159 Status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1160 }
sepp_nepp 6:fb11b746ceb5 1161
sepp_nepp 6:fb11b746ceb5 1162
sepp_nepp 6:fb11b746ceb5 1163 return Status;
sepp_nepp 6:fb11b746ceb5 1164 }
sepp_nepp 6:fb11b746ceb5 1165
sepp_nepp 8:2fd7cb217068 1166 VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps)
sepp_nepp 8:2fd7cb217068 1167 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1168 uint8_t sequence_config = 0;
sepp_nepp 6:fb11b746ceb5 1169
sepp_nepp 6:fb11b746ceb5 1170
sepp_nepp 7:3a1115c2556b 1171 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sepp_nepp 6:fb11b746ceb5 1172 &sequence_config);
sepp_nepp 6:fb11b746ceb5 1173
sepp_nepp 6:fb11b746ceb5 1174 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1175 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_TCC, sequence_config,
sepp_nepp 6:fb11b746ceb5 1176 &p_scheduler_sequence_steps->TccOn);
sepp_nepp 6:fb11b746ceb5 1177 }
sepp_nepp 6:fb11b746ceb5 1178 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1179 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_DSS, sequence_config,
sepp_nepp 6:fb11b746ceb5 1180 &p_scheduler_sequence_steps->DssOn);
sepp_nepp 6:fb11b746ceb5 1181 }
sepp_nepp 6:fb11b746ceb5 1182 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1183 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_MSRC, sequence_config,
sepp_nepp 6:fb11b746ceb5 1184 &p_scheduler_sequence_steps->MsrcOn);
sepp_nepp 6:fb11b746ceb5 1185 }
sepp_nepp 6:fb11b746ceb5 1186 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1187 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config,
sepp_nepp 6:fb11b746ceb5 1188 &p_scheduler_sequence_steps->PreRangeOn);
sepp_nepp 6:fb11b746ceb5 1189 }
sepp_nepp 6:fb11b746ceb5 1190 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1191 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config,
sepp_nepp 6:fb11b746ceb5 1192 &p_scheduler_sequence_steps->FinalRangeOn);
sepp_nepp 6:fb11b746ceb5 1193 }
sepp_nepp 6:fb11b746ceb5 1194
sepp_nepp 6:fb11b746ceb5 1195
sepp_nepp 6:fb11b746ceb5 1196 return status;
sepp_nepp 6:fb11b746ceb5 1197 }
sepp_nepp 6:fb11b746ceb5 1198
sepp_nepp 6:fb11b746ceb5 1199 uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
sepp_nepp 8:2fd7cb217068 1200 { /*!
sepp_nepp 6:fb11b746ceb5 1201 * Converts the encoded VCSEL period register value into the real
sepp_nepp 6:fb11b746ceb5 1202 * period in PLL clocks
sepp_nepp 6:fb11b746ceb5 1203 */
sepp_nepp 6:fb11b746ceb5 1204
sepp_nepp 6:fb11b746ceb5 1205 uint8_t vcsel_period_pclks = 0;
sepp_nepp 6:fb11b746ceb5 1206
sepp_nepp 6:fb11b746ceb5 1207 vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
sepp_nepp 6:fb11b746ceb5 1208
sepp_nepp 6:fb11b746ceb5 1209 return vcsel_period_pclks;
sepp_nepp 6:fb11b746ceb5 1210 }
sepp_nepp 6:fb11b746ceb5 1211
sepp_nepp 6:fb11b746ceb5 1212 uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks)
sepp_nepp 8:2fd7cb217068 1213 { /*!
sepp_nepp 6:fb11b746ceb5 1214 * Converts the encoded VCSEL period register value into the real period
sepp_nepp 6:fb11b746ceb5 1215 * in PLL clocks
sepp_nepp 6:fb11b746ceb5 1216 */
sepp_nepp 6:fb11b746ceb5 1217
sepp_nepp 6:fb11b746ceb5 1218 uint8_t vcsel_period_reg = 0;
sepp_nepp 6:fb11b746ceb5 1219
sepp_nepp 6:fb11b746ceb5 1220 vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
sepp_nepp 6:fb11b746ceb5 1221
sepp_nepp 6:fb11b746ceb5 1222 return vcsel_period_reg;
sepp_nepp 6:fb11b746ceb5 1223 }
sepp_nepp 6:fb11b746ceb5 1224
sepp_nepp 8:2fd7cb217068 1225 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk)
sepp_nepp 8:2fd7cb217068 1226 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1227 uint8_t vcsel_period_reg;
sepp_nepp 6:fb11b746ceb5 1228 uint8_t min_pre_vcsel_period_pclk = 12;
sepp_nepp 6:fb11b746ceb5 1229 uint8_t max_pre_vcsel_period_pclk = 18;
sepp_nepp 6:fb11b746ceb5 1230 uint8_t min_final_vcsel_period_pclk = 8;
sepp_nepp 6:fb11b746ceb5 1231 uint8_t max_final_vcsel_period_pclk = 14;
sepp_nepp 6:fb11b746ceb5 1232 uint32_t measurement_timing_budget_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1233 uint32_t final_range_timeout_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1234 uint32_t pre_range_timeout_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1235 uint32_t msrc_timeout_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1236 uint8_t phase_cal_int = 0;
sepp_nepp 6:fb11b746ceb5 1237
sepp_nepp 6:fb11b746ceb5 1238 /* Check if valid clock period requested */
sepp_nepp 6:fb11b746ceb5 1239
sepp_nepp 6:fb11b746ceb5 1240 if ((vcsel_pulse_period_pclk % 2) != 0) {
sepp_nepp 6:fb11b746ceb5 1241 /* Value must be an even number */
sepp_nepp 6:fb11b746ceb5 1242 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1243 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
sepp_nepp 6:fb11b746ceb5 1244 (vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk ||
sepp_nepp 6:fb11b746ceb5 1245 vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) {
sepp_nepp 6:fb11b746ceb5 1246 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1247 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
sepp_nepp 6:fb11b746ceb5 1248 (vcsel_pulse_period_pclk < min_final_vcsel_period_pclk ||
sepp_nepp 6:fb11b746ceb5 1249 vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) {
sepp_nepp 6:fb11b746ceb5 1250
sepp_nepp 6:fb11b746ceb5 1251 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1252 }
sepp_nepp 6:fb11b746ceb5 1253
sepp_nepp 6:fb11b746ceb5 1254 /* Apply specific settings for the requested clock period */
sepp_nepp 6:fb11b746ceb5 1255
sepp_nepp 6:fb11b746ceb5 1256 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1257 return status;
sepp_nepp 6:fb11b746ceb5 1258 }
sepp_nepp 6:fb11b746ceb5 1259
sepp_nepp 6:fb11b746ceb5 1260 if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
sepp_nepp 6:fb11b746ceb5 1261
sepp_nepp 6:fb11b746ceb5 1262 /* Set phase check limits */
sepp_nepp 6:fb11b746ceb5 1263 if (vcsel_pulse_period_pclk == 12) {
sepp_nepp 6:fb11b746ceb5 1264
sepp_nepp 8:2fd7cb217068 1265 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1266 0x18);
sepp_nepp 8:2fd7cb217068 1267 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1268 0x08);
sepp_nepp 6:fb11b746ceb5 1269 } else if (vcsel_pulse_period_pclk == 14) {
sepp_nepp 6:fb11b746ceb5 1270
sepp_nepp 8:2fd7cb217068 1271 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1272 0x30);
sepp_nepp 8:2fd7cb217068 1273 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1274 0x08);
sepp_nepp 6:fb11b746ceb5 1275 } else if (vcsel_pulse_period_pclk == 16) {
sepp_nepp 6:fb11b746ceb5 1276
sepp_nepp 8:2fd7cb217068 1277 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1278 0x40);
sepp_nepp 8:2fd7cb217068 1279 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1280 0x08);
sepp_nepp 6:fb11b746ceb5 1281 } else if (vcsel_pulse_period_pclk == 18) {
sepp_nepp 6:fb11b746ceb5 1282
sepp_nepp 8:2fd7cb217068 1283 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1284 0x50);
sepp_nepp 8:2fd7cb217068 1285 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1286 0x08);
sepp_nepp 6:fb11b746ceb5 1287 }
sepp_nepp 6:fb11b746ceb5 1288 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
sepp_nepp 6:fb11b746ceb5 1289
sepp_nepp 6:fb11b746ceb5 1290 if (vcsel_pulse_period_pclk == 8) {
sepp_nepp 6:fb11b746ceb5 1291
sepp_nepp 8:2fd7cb217068 1292 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1293 0x10);
sepp_nepp 8:2fd7cb217068 1294 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1295 0x08);
sepp_nepp 6:fb11b746ceb5 1296
sepp_nepp 8:2fd7cb217068 1297 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
sepp_nepp 8:2fd7cb217068 1298 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
sepp_nepp 6:fb11b746ceb5 1299
sepp_nepp 7:3a1115c2556b 1300 status |= VL53L0X_write_byte( 0xff, 0x01);
sepp_nepp 8:2fd7cb217068 1301 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
sepp_nepp 6:fb11b746ceb5 1302 0x30);
sepp_nepp 7:3a1115c2556b 1303 status |= VL53L0X_write_byte( 0xff, 0x00);
sepp_nepp 6:fb11b746ceb5 1304 } else if (vcsel_pulse_period_pclk == 10) {
sepp_nepp 6:fb11b746ceb5 1305
sepp_nepp 8:2fd7cb217068 1306 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1307 0x28);
sepp_nepp 8:2fd7cb217068 1308 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1309 0x08);
sepp_nepp 6:fb11b746ceb5 1310
sepp_nepp 8:2fd7cb217068 1311 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
sepp_nepp 8:2fd7cb217068 1312 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
sepp_nepp 6:fb11b746ceb5 1313
sepp_nepp 7:3a1115c2556b 1314 status |= VL53L0X_write_byte( 0xff, 0x01);
sepp_nepp 8:2fd7cb217068 1315 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
sepp_nepp 6:fb11b746ceb5 1316 0x20);
sepp_nepp 7:3a1115c2556b 1317 status |= VL53L0X_write_byte( 0xff, 0x00);
sepp_nepp 6:fb11b746ceb5 1318 } else if (vcsel_pulse_period_pclk == 12) {
sepp_nepp 6:fb11b746ceb5 1319
sepp_nepp 8:2fd7cb217068 1320 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1321 0x38);
sepp_nepp 8:2fd7cb217068 1322 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1323 0x08);
sepp_nepp 6:fb11b746ceb5 1324
sepp_nepp 8:2fd7cb217068 1325 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
sepp_nepp 8:2fd7cb217068 1326 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
sepp_nepp 6:fb11b746ceb5 1327
sepp_nepp 7:3a1115c2556b 1328 status |= VL53L0X_write_byte( 0xff, 0x01);
sepp_nepp 8:2fd7cb217068 1329 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
sepp_nepp 6:fb11b746ceb5 1330 0x20);
sepp_nepp 7:3a1115c2556b 1331 status |= VL53L0X_write_byte( 0xff, 0x00);
sepp_nepp 6:fb11b746ceb5 1332 } else if (vcsel_pulse_period_pclk == 14) {
sepp_nepp 6:fb11b746ceb5 1333
sepp_nepp 8:2fd7cb217068 1334 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
sepp_nepp 6:fb11b746ceb5 1335 0x048);
sepp_nepp 8:2fd7cb217068 1336 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
sepp_nepp 6:fb11b746ceb5 1337 0x08);
sepp_nepp 6:fb11b746ceb5 1338
sepp_nepp 8:2fd7cb217068 1339 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
sepp_nepp 8:2fd7cb217068 1340 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
sepp_nepp 6:fb11b746ceb5 1341
sepp_nepp 7:3a1115c2556b 1342 status |= VL53L0X_write_byte( 0xff, 0x01);
sepp_nepp 8:2fd7cb217068 1343 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
sepp_nepp 6:fb11b746ceb5 1344 0x20);
sepp_nepp 7:3a1115c2556b 1345 status |= VL53L0X_write_byte( 0xff, 0x00);
sepp_nepp 6:fb11b746ceb5 1346 }
sepp_nepp 6:fb11b746ceb5 1347 }
sepp_nepp 6:fb11b746ceb5 1348
sepp_nepp 6:fb11b746ceb5 1349 /* Re-calculate and apply timeouts, in macro periods */
sepp_nepp 6:fb11b746ceb5 1350
sepp_nepp 6:fb11b746ceb5 1351 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1352 vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t)
sepp_nepp 6:fb11b746ceb5 1353 vcsel_pulse_period_pclk);
sepp_nepp 6:fb11b746ceb5 1354
sepp_nepp 6:fb11b746ceb5 1355 /* When the VCSEL period for the pre or final range is changed,
sepp_nepp 6:fb11b746ceb5 1356 * the corresponding timeout must be read from the device using
sepp_nepp 6:fb11b746ceb5 1357 * the current VCSEL period, then the new VCSEL period can be
sepp_nepp 6:fb11b746ceb5 1358 * applied. The timeout then must be written back to the device
sepp_nepp 6:fb11b746ceb5 1359 * using the new VCSEL period.
sepp_nepp 6:fb11b746ceb5 1360 *
sepp_nepp 6:fb11b746ceb5 1361 * For the MSRC timeout, the same applies - this timeout being
sepp_nepp 6:fb11b746ceb5 1362 * dependant on the pre-range vcsel period.
sepp_nepp 6:fb11b746ceb5 1363 */
sepp_nepp 6:fb11b746ceb5 1364 switch (vcsel_period_type) {
sepp_nepp 6:fb11b746ceb5 1365 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
sepp_nepp 8:2fd7cb217068 1366 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1367 &pre_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1368
sepp_nepp 6:fb11b746ceb5 1369 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1370 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
sepp_nepp 6:fb11b746ceb5 1371 &msrc_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1372
sepp_nepp 6:fb11b746ceb5 1373 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1374 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
sepp_nepp 6:fb11b746ceb5 1375 vcsel_period_reg);
sepp_nepp 6:fb11b746ceb5 1376
sepp_nepp 6:fb11b746ceb5 1377 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 7:3a1115c2556b 1378 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1379 pre_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1380
sepp_nepp 6:fb11b746ceb5 1381 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 7:3a1115c2556b 1382 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
sepp_nepp 6:fb11b746ceb5 1383 msrc_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1384
sepp_nepp 8:2fd7cb217068 1385 Data.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
sepp_nepp 6:fb11b746ceb5 1386 break;
sepp_nepp 6:fb11b746ceb5 1387 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
sepp_nepp 7:3a1115c2556b 1388 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 1389 &final_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1390
sepp_nepp 6:fb11b746ceb5 1391 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1392 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
sepp_nepp 6:fb11b746ceb5 1393 vcsel_period_reg);
sepp_nepp 6:fb11b746ceb5 1394
sepp_nepp 6:fb11b746ceb5 1395 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1396 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 1397 final_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1398
sepp_nepp 8:2fd7cb217068 1399 Data.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
sepp_nepp 6:fb11b746ceb5 1400 break;
sepp_nepp 6:fb11b746ceb5 1401 default:
sepp_nepp 6:fb11b746ceb5 1402 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1403 }
sepp_nepp 6:fb11b746ceb5 1404 }
sepp_nepp 6:fb11b746ceb5 1405
sepp_nepp 6:fb11b746ceb5 1406 /* Finally, the timing budget must be re-applied */
sepp_nepp 6:fb11b746ceb5 1407 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1408 measurement_timing_budget_micro_seconds = Data.CurrentParameters.MeasurementTimingBudgetMicroSeconds ;
sepp_nepp 8:2fd7cb217068 1409
sepp_nepp 8:2fd7cb217068 1410 status = VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1411 }
sepp_nepp 6:fb11b746ceb5 1412
sepp_nepp 6:fb11b746ceb5 1413 /* Perform the phase calibration. This is needed after changing on
sepp_nepp 6:fb11b746ceb5 1414 * vcsel period.
sepp_nepp 6:fb11b746ceb5 1415 * get_data_enable = 0, restore_config = 1 */
sepp_nepp 6:fb11b746ceb5 1416 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 7:3a1115c2556b 1417 status = VL53L0X_perform_phase_calibration(&phase_cal_int, 0, 1);
sepp_nepp 6:fb11b746ceb5 1418
sepp_nepp 6:fb11b746ceb5 1419 return status;
sepp_nepp 6:fb11b746ceb5 1420 }
sepp_nepp 6:fb11b746ceb5 1421
sepp_nepp 8:2fd7cb217068 1422 VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period)
sepp_nepp 8:2fd7cb217068 1423 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1424
sepp_nepp 6:fb11b746ceb5 1425
sepp_nepp 7:3a1115c2556b 1426 status = wrapped_VL53L0X_set_vcsel_pulse_period( vcsel_period_type,
sepp_nepp 6:fb11b746ceb5 1427 vcsel_pulse_period);
sepp_nepp 6:fb11b746ceb5 1428
sepp_nepp 6:fb11b746ceb5 1429
sepp_nepp 6:fb11b746ceb5 1430 return status;
sepp_nepp 6:fb11b746ceb5 1431 }
sepp_nepp 6:fb11b746ceb5 1432
sepp_nepp 8:2fd7cb217068 1433 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk)
sepp_nepp 8:2fd7cb217068 1434 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1435 uint8_t vcsel_period_reg;
sepp_nepp 6:fb11b746ceb5 1436
sepp_nepp 6:fb11b746ceb5 1437 switch (vcsel_period_type) {
sepp_nepp 6:fb11b746ceb5 1438 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
sepp_nepp 8:2fd7cb217068 1439 status = VL53L0X_read_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
sepp_nepp 6:fb11b746ceb5 1440 &vcsel_period_reg);
sepp_nepp 6:fb11b746ceb5 1441 break;
sepp_nepp 6:fb11b746ceb5 1442 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
sepp_nepp 8:2fd7cb217068 1443 status = VL53L0X_read_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
sepp_nepp 6:fb11b746ceb5 1444 &vcsel_period_reg);
sepp_nepp 6:fb11b746ceb5 1445 break;
sepp_nepp 6:fb11b746ceb5 1446 default:
sepp_nepp 6:fb11b746ceb5 1447 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1448 }
sepp_nepp 6:fb11b746ceb5 1449
sepp_nepp 6:fb11b746ceb5 1450 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 6:fb11b746ceb5 1451 *p_vcsel_pulse_period_pclk =
sepp_nepp 6:fb11b746ceb5 1452 VL53L0X_decode_vcsel_period(vcsel_period_reg);
sepp_nepp 6:fb11b746ceb5 1453
sepp_nepp 6:fb11b746ceb5 1454 return status;
sepp_nepp 6:fb11b746ceb5 1455 }
sepp_nepp 6:fb11b746ceb5 1456
sepp_nepp 8:2fd7cb217068 1457 VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk)
sepp_nepp 8:2fd7cb217068 1458 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1459
sepp_nepp 6:fb11b746ceb5 1460
sepp_nepp 7:3a1115c2556b 1461 status = wrapped_VL53L0X_get_vcsel_pulse_period( vcsel_period_type,
sepp_nepp 6:fb11b746ceb5 1462 p_vcsel_pulse_period_pclk);
sepp_nepp 6:fb11b746ceb5 1463
sepp_nepp 6:fb11b746ceb5 1464
sepp_nepp 6:fb11b746ceb5 1465 return status;
sepp_nepp 6:fb11b746ceb5 1466 }
sepp_nepp 6:fb11b746ceb5 1467
sepp_nepp 6:fb11b746ceb5 1468 uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout)
sepp_nepp 8:2fd7cb217068 1469 { /*!
sepp_nepp 6:fb11b746ceb5 1470 * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
sepp_nepp 6:fb11b746ceb5 1471 */
sepp_nepp 6:fb11b746ceb5 1472
sepp_nepp 6:fb11b746ceb5 1473 uint32_t timeout_macro_clks = 0;
sepp_nepp 6:fb11b746ceb5 1474
sepp_nepp 6:fb11b746ceb5 1475 timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF)
sepp_nepp 6:fb11b746ceb5 1476 << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
sepp_nepp 6:fb11b746ceb5 1477
sepp_nepp 6:fb11b746ceb5 1478 return timeout_macro_clks;
sepp_nepp 6:fb11b746ceb5 1479 }
sepp_nepp 6:fb11b746ceb5 1480
sepp_nepp 7:3a1115c2556b 1481 uint32_t VL53L0X::VL53L0X_calc_macro_period_ps( uint8_t vcsel_period_pclks)
sepp_nepp 8:2fd7cb217068 1482 { uint64_t pll_period_ps;
sepp_nepp 6:fb11b746ceb5 1483 uint32_t macro_period_vclks;
sepp_nepp 6:fb11b746ceb5 1484 uint32_t macro_period_ps;
sepp_nepp 6:fb11b746ceb5 1485
sepp_nepp 6:fb11b746ceb5 1486
sepp_nepp 6:fb11b746ceb5 1487
sepp_nepp 6:fb11b746ceb5 1488 /* The above calculation will produce rounding errors,
sepp_nepp 6:fb11b746ceb5 1489 therefore set fixed value
sepp_nepp 6:fb11b746ceb5 1490 */
sepp_nepp 6:fb11b746ceb5 1491 pll_period_ps = 1655;
sepp_nepp 6:fb11b746ceb5 1492
sepp_nepp 6:fb11b746ceb5 1493 macro_period_vclks = 2304;
sepp_nepp 6:fb11b746ceb5 1494 macro_period_ps = (uint32_t)(macro_period_vclks
sepp_nepp 6:fb11b746ceb5 1495 * vcsel_period_pclks * pll_period_ps);
sepp_nepp 6:fb11b746ceb5 1496
sepp_nepp 6:fb11b746ceb5 1497 return macro_period_ps;
sepp_nepp 6:fb11b746ceb5 1498 }
sepp_nepp 6:fb11b746ceb5 1499
sepp_nepp 6:fb11b746ceb5 1500 /* To convert register value into us */
sepp_nepp 8:2fd7cb217068 1501 uint32_t VL53L0X::VL53L0X_calc_timeout_us(uint16_t timeout_period_mclks,
sepp_nepp 6:fb11b746ceb5 1502 uint8_t vcsel_period_pclks)
sepp_nepp 8:2fd7cb217068 1503 { uint32_t macro_period_ps;
sepp_nepp 6:fb11b746ceb5 1504 uint32_t macro_period_ns;
sepp_nepp 6:fb11b746ceb5 1505 uint32_t actual_timeout_period_us = 0;
sepp_nepp 6:fb11b746ceb5 1506
sepp_nepp 7:3a1115c2556b 1507 macro_period_ps = VL53L0X_calc_macro_period_ps( vcsel_period_pclks);
sepp_nepp 6:fb11b746ceb5 1508 macro_period_ns = (macro_period_ps + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 1509
sepp_nepp 6:fb11b746ceb5 1510 actual_timeout_period_us =
sepp_nepp 6:fb11b746ceb5 1511 ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 1512
sepp_nepp 6:fb11b746ceb5 1513 return actual_timeout_period_us;
sepp_nepp 6:fb11b746ceb5 1514 }
sepp_nepp 6:fb11b746ceb5 1515
sepp_nepp 8:2fd7cb217068 1516 VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id,
sepp_nepp 6:fb11b746ceb5 1517 uint32_t *p_time_out_micro_secs)
sepp_nepp 8:2fd7cb217068 1518 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1519 uint8_t current_vcsel_pulse_period_p_clk;
sepp_nepp 6:fb11b746ceb5 1520 uint8_t encoded_time_out_byte = 0;
sepp_nepp 6:fb11b746ceb5 1521 uint32_t timeout_micro_seconds = 0;
sepp_nepp 6:fb11b746ceb5 1522 uint16_t pre_range_encoded_time_out = 0;
sepp_nepp 6:fb11b746ceb5 1523 uint16_t msrc_time_out_m_clks;
sepp_nepp 6:fb11b746ceb5 1524 uint16_t pre_range_time_out_m_clks;
sepp_nepp 6:fb11b746ceb5 1525 uint16_t final_range_time_out_m_clks = 0;
sepp_nepp 6:fb11b746ceb5 1526 uint16_t final_range_encoded_time_out;
sepp_nepp 6:fb11b746ceb5 1527 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
sepp_nepp 6:fb11b746ceb5 1528
sepp_nepp 6:fb11b746ceb5 1529 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
sepp_nepp 6:fb11b746ceb5 1530 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
sepp_nepp 6:fb11b746ceb5 1531 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
sepp_nepp 6:fb11b746ceb5 1532
sepp_nepp 8:2fd7cb217068 1533 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1534 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1535 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1536 status = VL53L0X_read_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
sepp_nepp 6:fb11b746ceb5 1537 &encoded_time_out_byte);
sepp_nepp 6:fb11b746ceb5 1538 }
sepp_nepp 6:fb11b746ceb5 1539 msrc_time_out_m_clks = VL53L0X_decode_timeout(encoded_time_out_byte);
sepp_nepp 6:fb11b746ceb5 1540
sepp_nepp 8:2fd7cb217068 1541 timeout_micro_seconds = VL53L0X_calc_timeout_us(msrc_time_out_m_clks,
sepp_nepp 6:fb11b746ceb5 1542 current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1543 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
sepp_nepp 6:fb11b746ceb5 1544 /* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp 8:2fd7cb217068 1545 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1546 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1547
sepp_nepp 6:fb11b746ceb5 1548 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp 6:fb11b746ceb5 1549 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1550
sepp_nepp 6:fb11b746ceb5 1551 /* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp 8:2fd7cb217068 1552 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1553 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1554
sepp_nepp 6:fb11b746ceb5 1555 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1556 status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
sepp_nepp 6:fb11b746ceb5 1557 &pre_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 1558 }
sepp_nepp 6:fb11b746ceb5 1559
sepp_nepp 8:2fd7cb217068 1560 pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out);
sepp_nepp 8:2fd7cb217068 1561
sepp_nepp 8:2fd7cb217068 1562 timeout_micro_seconds = VL53L0X_calc_timeout_us(pre_range_time_out_m_clks,
sepp_nepp 6:fb11b746ceb5 1563 current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1564 }
sepp_nepp 6:fb11b746ceb5 1565 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
sepp_nepp 6:fb11b746ceb5 1566
sepp_nepp 7:3a1115c2556b 1567 VL53L0X_get_sequence_step_enables( &scheduler_sequence_steps);
sepp_nepp 6:fb11b746ceb5 1568 pre_range_time_out_m_clks = 0;
sepp_nepp 6:fb11b746ceb5 1569
sepp_nepp 6:fb11b746ceb5 1570 if (scheduler_sequence_steps.PreRangeOn) {
sepp_nepp 6:fb11b746ceb5 1571 /* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp 8:2fd7cb217068 1572 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1573 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1574
sepp_nepp 6:fb11b746ceb5 1575 /* Retrieve PRE-RANGE Timeout in Macro periods
sepp_nepp 6:fb11b746ceb5 1576 * (MCLKS) */
sepp_nepp 6:fb11b746ceb5 1577 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1578 status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
sepp_nepp 6:fb11b746ceb5 1579 &pre_range_encoded_time_out);
sepp_nepp 8:2fd7cb217068 1580 pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 1581 }
sepp_nepp 6:fb11b746ceb5 1582 }
sepp_nepp 6:fb11b746ceb5 1583
sepp_nepp 6:fb11b746ceb5 1584 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1585 /* Retrieve FINAL-RANGE VCSEL Period */
sepp_nepp 8:2fd7cb217068 1586 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 1587 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1588 }
sepp_nepp 6:fb11b746ceb5 1589
sepp_nepp 6:fb11b746ceb5 1590 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp 6:fb11b746ceb5 1591 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1592 status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
sepp_nepp 6:fb11b746ceb5 1593 &final_range_encoded_time_out);
sepp_nepp 8:2fd7cb217068 1594 final_range_time_out_m_clks = VL53L0X_decode_timeout(final_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 1595 }
sepp_nepp 6:fb11b746ceb5 1596
sepp_nepp 6:fb11b746ceb5 1597 final_range_time_out_m_clks -= pre_range_time_out_m_clks;
sepp_nepp 8:2fd7cb217068 1598 timeout_micro_seconds = VL53L0X_calc_timeout_us(final_range_time_out_m_clks,
sepp_nepp 6:fb11b746ceb5 1599 current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 1600 }
sepp_nepp 6:fb11b746ceb5 1601
sepp_nepp 6:fb11b746ceb5 1602 *p_time_out_micro_secs = timeout_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1603
sepp_nepp 6:fb11b746ceb5 1604 return status;
sepp_nepp 6:fb11b746ceb5 1605 }
sepp_nepp 6:fb11b746ceb5 1606
sepp_nepp 8:2fd7cb217068 1607 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(uint32_t *p_measurement_timing_budget_micro_seconds)
sepp_nepp 8:2fd7cb217068 1608 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1609 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
sepp_nepp 6:fb11b746ceb5 1610 uint32_t final_range_timeout_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1611 uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
sepp_nepp 6:fb11b746ceb5 1612 uint32_t start_overhead_micro_seconds = 1910;
sepp_nepp 6:fb11b746ceb5 1613 uint32_t end_overhead_micro_seconds = 960;
sepp_nepp 6:fb11b746ceb5 1614 uint32_t msrc_overhead_micro_seconds = 660;
sepp_nepp 6:fb11b746ceb5 1615 uint32_t tcc_overhead_micro_seconds = 590;
sepp_nepp 6:fb11b746ceb5 1616 uint32_t dss_overhead_micro_seconds = 690;
sepp_nepp 6:fb11b746ceb5 1617 uint32_t pre_range_overhead_micro_seconds = 660;
sepp_nepp 6:fb11b746ceb5 1618 uint32_t final_range_overhead_micro_seconds = 550;
sepp_nepp 6:fb11b746ceb5 1619 uint32_t pre_range_timeout_micro_seconds = 0;
sepp_nepp 6:fb11b746ceb5 1620
sepp_nepp 6:fb11b746ceb5 1621
sepp_nepp 6:fb11b746ceb5 1622
sepp_nepp 6:fb11b746ceb5 1623 /* Start and end overhead times always present */
sepp_nepp 6:fb11b746ceb5 1624 *p_measurement_timing_budget_micro_seconds
sepp_nepp 6:fb11b746ceb5 1625 = start_overhead_micro_seconds + end_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1626
sepp_nepp 7:3a1115c2556b 1627 status = VL53L0X_get_sequence_step_enables( &scheduler_sequence_steps);
sepp_nepp 6:fb11b746ceb5 1628
sepp_nepp 6:fb11b746ceb5 1629 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1630
sepp_nepp 6:fb11b746ceb5 1631 return status;
sepp_nepp 6:fb11b746ceb5 1632 }
sepp_nepp 6:fb11b746ceb5 1633
sepp_nepp 6:fb11b746ceb5 1634 if (scheduler_sequence_steps.TccOn ||
sepp_nepp 6:fb11b746ceb5 1635 scheduler_sequence_steps.MsrcOn ||
sepp_nepp 6:fb11b746ceb5 1636 scheduler_sequence_steps.DssOn) {
sepp_nepp 6:fb11b746ceb5 1637
sepp_nepp 8:2fd7cb217068 1638 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
sepp_nepp 6:fb11b746ceb5 1639 &msrc_dcc_tcc_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1640
sepp_nepp 6:fb11b746ceb5 1641 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1642 if (scheduler_sequence_steps.TccOn) {
sepp_nepp 6:fb11b746ceb5 1643 *p_measurement_timing_budget_micro_seconds +=
sepp_nepp 6:fb11b746ceb5 1644 msrc_dcc_tcc_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 1645 tcc_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1646 }
sepp_nepp 6:fb11b746ceb5 1647
sepp_nepp 6:fb11b746ceb5 1648 if (scheduler_sequence_steps.DssOn) {
sepp_nepp 6:fb11b746ceb5 1649 *p_measurement_timing_budget_micro_seconds +=
sepp_nepp 6:fb11b746ceb5 1650 2 * (msrc_dcc_tcc_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 1651 dss_overhead_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1652 } else if (scheduler_sequence_steps.MsrcOn) {
sepp_nepp 6:fb11b746ceb5 1653 *p_measurement_timing_budget_micro_seconds +=
sepp_nepp 6:fb11b746ceb5 1654 msrc_dcc_tcc_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 1655 msrc_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1656 }
sepp_nepp 6:fb11b746ceb5 1657 }
sepp_nepp 6:fb11b746ceb5 1658 }
sepp_nepp 6:fb11b746ceb5 1659
sepp_nepp 6:fb11b746ceb5 1660 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1661 if (scheduler_sequence_steps.PreRangeOn) {
sepp_nepp 8:2fd7cb217068 1662 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 1663 &pre_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1664 *p_measurement_timing_budget_micro_seconds +=
sepp_nepp 6:fb11b746ceb5 1665 pre_range_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 1666 pre_range_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1667 }
sepp_nepp 6:fb11b746ceb5 1668 }
sepp_nepp 6:fb11b746ceb5 1669
sepp_nepp 6:fb11b746ceb5 1670 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1671 if (scheduler_sequence_steps.FinalRangeOn) {
sepp_nepp 8:2fd7cb217068 1672 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 1673 &final_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1674 *p_measurement_timing_budget_micro_seconds +=
sepp_nepp 6:fb11b746ceb5 1675 (final_range_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 1676 final_range_overhead_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1677 }
sepp_nepp 6:fb11b746ceb5 1678 }
sepp_nepp 6:fb11b746ceb5 1679
sepp_nepp 6:fb11b746ceb5 1680 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1681 Data.CurrentParameters.MeasurementTimingBudgetMicroSeconds = *p_measurement_timing_budget_micro_seconds;
sepp_nepp 6:fb11b746ceb5 1682 }
sepp_nepp 6:fb11b746ceb5 1683
sepp_nepp 6:fb11b746ceb5 1684
sepp_nepp 6:fb11b746ceb5 1685 return status;
sepp_nepp 6:fb11b746ceb5 1686 }
sepp_nepp 6:fb11b746ceb5 1687
sepp_nepp 8:2fd7cb217068 1688 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(uint32_t *p_measurement_timing_budget_micro_seconds)
sepp_nepp 8:2fd7cb217068 1689 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1690
sepp_nepp 6:fb11b746ceb5 1691
sepp_nepp 8:2fd7cb217068 1692 status = wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(p_measurement_timing_budget_micro_seconds);
sepp_nepp 6:fb11b746ceb5 1693
sepp_nepp 6:fb11b746ceb5 1694
sepp_nepp 6:fb11b746ceb5 1695 return status;
sepp_nepp 6:fb11b746ceb5 1696 }
sepp_nepp 6:fb11b746ceb5 1697
sepp_nepp 8:2fd7cb217068 1698 VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters(VL53L0X_DeviceParameters_t *p_device_parameters)
sepp_nepp 8:2fd7cb217068 1699 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1700 int i;
sepp_nepp 6:fb11b746ceb5 1701
sepp_nepp 6:fb11b746ceb5 1702
sepp_nepp 6:fb11b746ceb5 1703
sepp_nepp 7:3a1115c2556b 1704 status = VL53L0X_get_device_mode( &(p_device_parameters->DeviceMode));
sepp_nepp 6:fb11b746ceb5 1705
sepp_nepp 6:fb11b746ceb5 1706 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1707 status = VL53L0X_get_inter_measurement_period_milli_seconds(&(p_device_parameters->InterMeasurementPeriodMilliSeconds));
sepp_nepp 6:fb11b746ceb5 1708
sepp_nepp 6:fb11b746ceb5 1709 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1710 p_device_parameters->XTalkCompensationEnable = 0;
sepp_nepp 6:fb11b746ceb5 1711 }
sepp_nepp 6:fb11b746ceb5 1712
sepp_nepp 6:fb11b746ceb5 1713 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1714 status = VL53L0X_get_x_talk_compensation_rate_mega_cps(&(p_device_parameters->XTalkCompensationRateMegaCps));
sepp_nepp 6:fb11b746ceb5 1715
sepp_nepp 6:fb11b746ceb5 1716 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 8:2fd7cb217068 1717 status = VL53L0X_get_offset_calibration_data_micro_meter(&(p_device_parameters->RangeOffsetMicroMeters));
sepp_nepp 6:fb11b746ceb5 1718
sepp_nepp 6:fb11b746ceb5 1719 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1720 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
sepp_nepp 6:fb11b746ceb5 1721 /* get first the values, then the enables.
sepp_nepp 6:fb11b746ceb5 1722 * VL53L0X_GetLimitCheckValue will modify the enable
sepp_nepp 6:fb11b746ceb5 1723 * flags
sepp_nepp 6:fb11b746ceb5 1724 */
sepp_nepp 6:fb11b746ceb5 1725 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 1726 status |= VL53L0X_get_limit_check_value( i,
sepp_nepp 6:fb11b746ceb5 1727 &(p_device_parameters->LimitChecksValue[i]));
sepp_nepp 6:fb11b746ceb5 1728 } else {
sepp_nepp 6:fb11b746ceb5 1729 break;
sepp_nepp 6:fb11b746ceb5 1730 }
sepp_nepp 6:fb11b746ceb5 1731 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 1732 status |= VL53L0X_get_limit_check_enable( i,
sepp_nepp 6:fb11b746ceb5 1733 &(p_device_parameters->LimitChecksEnable[i]));
sepp_nepp 6:fb11b746ceb5 1734 } else {
sepp_nepp 6:fb11b746ceb5 1735 break;
sepp_nepp 6:fb11b746ceb5 1736 }
sepp_nepp 6:fb11b746ceb5 1737 }
sepp_nepp 6:fb11b746ceb5 1738 }
sepp_nepp 6:fb11b746ceb5 1739
sepp_nepp 6:fb11b746ceb5 1740 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1741 status = VL53L0X_get_wrap_around_check_enable(&(p_device_parameters->WrapAroundCheckEnable));
sepp_nepp 6:fb11b746ceb5 1742 }
sepp_nepp 6:fb11b746ceb5 1743
sepp_nepp 6:fb11b746ceb5 1744 /* Need to be done at the end as it uses VCSELPulsePeriod */
sepp_nepp 6:fb11b746ceb5 1745 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1746 status = VL53L0X_get_measurement_timing_budget_micro_seconds(&(p_device_parameters->MeasurementTimingBudgetMicroSeconds));
sepp_nepp 6:fb11b746ceb5 1747 }
sepp_nepp 6:fb11b746ceb5 1748
sepp_nepp 6:fb11b746ceb5 1749
sepp_nepp 6:fb11b746ceb5 1750 return status;
sepp_nepp 6:fb11b746ceb5 1751 }
sepp_nepp 6:fb11b746ceb5 1752
sepp_nepp 7:3a1115c2556b 1753 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value( uint16_t limit_check_id,
sepp_nepp 6:fb11b746ceb5 1754 FixPoint1616_t limit_check_value)
sepp_nepp 8:2fd7cb217068 1755 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1756 uint8_t temp8;
sepp_nepp 6:fb11b746ceb5 1757
sepp_nepp 6:fb11b746ceb5 1758
sepp_nepp 6:fb11b746ceb5 1759
sepp_nepp 8:2fd7cb217068 1760 temp8 = Data.CurrentParameters.LimitChecksEnable[limit_check_id];
sepp_nepp 6:fb11b746ceb5 1761
sepp_nepp 6:fb11b746ceb5 1762 if (temp8 == 0) { /* disabled write only internal value */
sepp_nepp 8:2fd7cb217068 1763 Data.CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value;
sepp_nepp 6:fb11b746ceb5 1764 } else {
sepp_nepp 6:fb11b746ceb5 1765
sepp_nepp 6:fb11b746ceb5 1766 switch (limit_check_id) {
sepp_nepp 6:fb11b746ceb5 1767
sepp_nepp 6:fb11b746ceb5 1768 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 1769 /* internal computation: */
sepp_nepp 8:2fd7cb217068 1770 Data.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_value;
sepp_nepp 6:fb11b746ceb5 1771 break;
sepp_nepp 6:fb11b746ceb5 1772
sepp_nepp 6:fb11b746ceb5 1773 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 1774
sepp_nepp 8:2fd7cb217068 1775 status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 8:2fd7cb217068 1776 VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value));
sepp_nepp 6:fb11b746ceb5 1777
sepp_nepp 6:fb11b746ceb5 1778 break;
sepp_nepp 6:fb11b746ceb5 1779
sepp_nepp 6:fb11b746ceb5 1780 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
sepp_nepp 6:fb11b746ceb5 1781
sepp_nepp 6:fb11b746ceb5 1782 /* internal computation: */
sepp_nepp 8:2fd7cb217068 1783 Data.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_value;
sepp_nepp 6:fb11b746ceb5 1784
sepp_nepp 6:fb11b746ceb5 1785 break;
sepp_nepp 6:fb11b746ceb5 1786
sepp_nepp 6:fb11b746ceb5 1787 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
sepp_nepp 6:fb11b746ceb5 1788
sepp_nepp 6:fb11b746ceb5 1789 /* internal computation: */
sepp_nepp 8:2fd7cb217068 1790 Data.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_value;
sepp_nepp 6:fb11b746ceb5 1791
sepp_nepp 6:fb11b746ceb5 1792 break;
sepp_nepp 6:fb11b746ceb5 1793
sepp_nepp 6:fb11b746ceb5 1794 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
sepp_nepp 6:fb11b746ceb5 1795 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
sepp_nepp 6:fb11b746ceb5 1796
sepp_nepp 8:2fd7cb217068 1797 status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 8:2fd7cb217068 1798 VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value));
sepp_nepp 6:fb11b746ceb5 1799
sepp_nepp 6:fb11b746ceb5 1800 break;
sepp_nepp 6:fb11b746ceb5 1801
sepp_nepp 6:fb11b746ceb5 1802 default:
sepp_nepp 6:fb11b746ceb5 1803 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 1804
sepp_nepp 6:fb11b746ceb5 1805 }
sepp_nepp 6:fb11b746ceb5 1806
sepp_nepp 6:fb11b746ceb5 1807 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1808 Data.CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value;
sepp_nepp 6:fb11b746ceb5 1809 }
sepp_nepp 6:fb11b746ceb5 1810 }
sepp_nepp 6:fb11b746ceb5 1811
sepp_nepp 6:fb11b746ceb5 1812
sepp_nepp 6:fb11b746ceb5 1813 return status;
sepp_nepp 6:fb11b746ceb5 1814 }
sepp_nepp 6:fb11b746ceb5 1815
sepp_nepp 7:3a1115c2556b 1816 VL53L0X_Error VL53L0X::VL53L0X_check_part_used( uint8_t *revision,
sepp_nepp 6:fb11b746ceb5 1817 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
sepp_nepp 8:2fd7cb217068 1818 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1819 uint8_t module_id_int;
sepp_nepp 6:fb11b746ceb5 1820 char *product_id_tmp;
sepp_nepp 6:fb11b746ceb5 1821
sepp_nepp 7:3a1115c2556b 1822 status = VL53L0X_get_info_from_device( 2);
sepp_nepp 6:fb11b746ceb5 1823
sepp_nepp 6:fb11b746ceb5 1824 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 1825 module_id_int = Data.ModuleId;
sepp_nepp 6:fb11b746ceb5 1826
sepp_nepp 6:fb11b746ceb5 1827 if (module_id_int == 0) {
sepp_nepp 6:fb11b746ceb5 1828 *revision = 0;
sepp_nepp 6:fb11b746ceb5 1829 VL53L0X_COPYSTRING(p_VL53L0X_device_info->ProductId, "");
sepp_nepp 6:fb11b746ceb5 1830 } else {
sepp_nepp 8:2fd7cb217068 1831 *revision = Data.Revision;
sepp_nepp 8:2fd7cb217068 1832 product_id_tmp = Data.ProductId;
sepp_nepp 6:fb11b746ceb5 1833 VL53L0X_COPYSTRING(p_VL53L0X_device_info->ProductId, product_id_tmp);
sepp_nepp 6:fb11b746ceb5 1834 }
sepp_nepp 6:fb11b746ceb5 1835 }
sepp_nepp 6:fb11b746ceb5 1836 return status;
sepp_nepp 6:fb11b746ceb5 1837 }
sepp_nepp 6:fb11b746ceb5 1838
sepp_nepp 8:2fd7cb217068 1839 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_device_info(VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
sepp_nepp 7:3a1115c2556b 1840 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1841 uint8_t revision_id;
sepp_nepp 6:fb11b746ceb5 1842 uint8_t revision;
sepp_nepp 6:fb11b746ceb5 1843
sepp_nepp 7:3a1115c2556b 1844 status = VL53L0X_check_part_used( &revision, p_VL53L0X_device_info);
sepp_nepp 6:fb11b746ceb5 1845
sepp_nepp 6:fb11b746ceb5 1846 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1847 if (revision == 0) {
sepp_nepp 6:fb11b746ceb5 1848 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
sepp_nepp 6:fb11b746ceb5 1849 VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
sepp_nepp 6:fb11b746ceb5 1850 } else if ((revision <= 34) && (revision != 32)) {
sepp_nepp 6:fb11b746ceb5 1851 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
sepp_nepp 6:fb11b746ceb5 1852 VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
sepp_nepp 6:fb11b746ceb5 1853 } else if (revision < 39) {
sepp_nepp 6:fb11b746ceb5 1854 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
sepp_nepp 6:fb11b746ceb5 1855 VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
sepp_nepp 7:3a1115c2556b 1856 } else {VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
sepp_nepp 6:fb11b746ceb5 1857 VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
sepp_nepp 6:fb11b746ceb5 1858 }
sepp_nepp 6:fb11b746ceb5 1859
sepp_nepp 6:fb11b746ceb5 1860 VL53L0X_COPYSTRING(p_VL53L0X_device_info->Type,
sepp_nepp 6:fb11b746ceb5 1861 VL53L0X_STRING_DEVICE_INFO_TYPE);
sepp_nepp 6:fb11b746ceb5 1862 }
sepp_nepp 6:fb11b746ceb5 1863
sepp_nepp 6:fb11b746ceb5 1864 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 1865 status = VL53L0X_read_byte( VL53L0X_REG_IDENTIFICATION_MODEL_ID,
sepp_nepp 7:3a1115c2556b 1866 &p_VL53L0X_device_info->ProductType);}
sepp_nepp 6:fb11b746ceb5 1867
sepp_nepp 6:fb11b746ceb5 1868 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 1869 status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_REVISION_ID,
sepp_nepp 6:fb11b746ceb5 1870 &revision_id);
sepp_nepp 6:fb11b746ceb5 1871 p_VL53L0X_device_info->ProductRevisionMajor = 1;
sepp_nepp 6:fb11b746ceb5 1872 p_VL53L0X_device_info->ProductRevisionMinor =
sepp_nepp 6:fb11b746ceb5 1873 (revision_id & 0xF0) >> 4;
sepp_nepp 6:fb11b746ceb5 1874 }
sepp_nepp 6:fb11b746ceb5 1875
sepp_nepp 6:fb11b746ceb5 1876 return status;
sepp_nepp 6:fb11b746ceb5 1877 }
sepp_nepp 6:fb11b746ceb5 1878
sepp_nepp 8:2fd7cb217068 1879 VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
sepp_nepp 8:2fd7cb217068 1880 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 7:3a1115c2556b 1881
sepp_nepp 7:3a1115c2556b 1882 status = wrapped_VL53L0X_get_device_info( p_VL53L0X_device_info);
sepp_nepp 7:3a1115c2556b 1883
sepp_nepp 6:fb11b746ceb5 1884 return status;
sepp_nepp 6:fb11b746ceb5 1885 }
sepp_nepp 6:fb11b746ceb5 1886
sepp_nepp 8:2fd7cb217068 1887 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
sepp_nepp 7:3a1115c2556b 1888 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1889 uint8_t byte;
sepp_nepp 6:fb11b746ceb5 1890
sepp_nepp 7:3a1115c2556b 1891 status = VL53L0X_read_byte( VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
sepp_nepp 6:fb11b746ceb5 1892 *p_interrupt_mask_status = byte & 0x07;
sepp_nepp 6:fb11b746ceb5 1893
sepp_nepp 7:3a1115c2556b 1894 if (byte & 0x18) { status = VL53L0X_ERROR_RANGE_ERROR;}
sepp_nepp 7:3a1115c2556b 1895
sepp_nepp 6:fb11b746ceb5 1896 return status;
sepp_nepp 6:fb11b746ceb5 1897 }
sepp_nepp 6:fb11b746ceb5 1898
sepp_nepp 8:2fd7cb217068 1899 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(uint8_t *p_measurement_data_ready)
sepp_nepp 7:3a1115c2556b 1900 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1901 uint8_t sys_range_status_register;
sepp_nepp 6:fb11b746ceb5 1902 uint8_t interrupt_config;
sepp_nepp 6:fb11b746ceb5 1903 uint32_t interrupt_mask;
sepp_nepp 6:fb11b746ceb5 1904
sepp_nepp 8:2fd7cb217068 1905 interrupt_config = Data.Pin0GpioFunctionality;
sepp_nepp 6:fb11b746ceb5 1906
sepp_nepp 6:fb11b746ceb5 1907 if (interrupt_config ==
sepp_nepp 6:fb11b746ceb5 1908 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
sepp_nepp 7:3a1115c2556b 1909 status = VL53L0X_get_interrupt_mask_status( &interrupt_mask);
sepp_nepp 6:fb11b746ceb5 1910 if (interrupt_mask ==
sepp_nepp 6:fb11b746ceb5 1911 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
sepp_nepp 6:fb11b746ceb5 1912 *p_measurement_data_ready = 1;
sepp_nepp 6:fb11b746ceb5 1913 } else {
sepp_nepp 6:fb11b746ceb5 1914 *p_measurement_data_ready = 0;
sepp_nepp 6:fb11b746ceb5 1915 }
sepp_nepp 6:fb11b746ceb5 1916 } else {
sepp_nepp 7:3a1115c2556b 1917 status = VL53L0X_read_byte( VL53L0X_REG_RESULT_RANGE_STATUS,
sepp_nepp 6:fb11b746ceb5 1918 &sys_range_status_register);
sepp_nepp 6:fb11b746ceb5 1919 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 1920 if (sys_range_status_register & 0x01) {
sepp_nepp 6:fb11b746ceb5 1921 *p_measurement_data_ready = 1;
sepp_nepp 7:3a1115c2556b 1922 } else { *p_measurement_data_ready = 0; }
sepp_nepp 6:fb11b746ceb5 1923 }
sepp_nepp 6:fb11b746ceb5 1924 }
sepp_nepp 6:fb11b746ceb5 1925
sepp_nepp 6:fb11b746ceb5 1926 return status;
sepp_nepp 6:fb11b746ceb5 1927 }
sepp_nepp 6:fb11b746ceb5 1928
sepp_nepp 7:3a1115c2556b 1929 VL53L0X_Error VL53L0X::VL53L0X_polling_delay(void)
sepp_nepp 7:3a1115c2556b 1930 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1931
sepp_nepp 6:fb11b746ceb5 1932 // do nothing
sepp_nepp 6:fb11b746ceb5 1933 VL53L0X_OsDelay();
sepp_nepp 6:fb11b746ceb5 1934 return status;
sepp_nepp 6:fb11b746ceb5 1935 }
sepp_nepp 6:fb11b746ceb5 1936
sepp_nepp 7:3a1115c2556b 1937 VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(void)
sepp_nepp 7:3a1115c2556b 1938 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1939 uint8_t new_data_ready = 0;
sepp_nepp 6:fb11b746ceb5 1940 uint32_t loop_nb;
sepp_nepp 6:fb11b746ceb5 1941
sepp_nepp 6:fb11b746ceb5 1942 loop_nb = 0;
sepp_nepp 6:fb11b746ceb5 1943
sepp_nepp 7:3a1115c2556b 1944 do {status = VL53L0X_get_measurement_data_ready( &new_data_ready);
sepp_nepp 6:fb11b746ceb5 1945 if (status != 0) {
sepp_nepp 6:fb11b746ceb5 1946 break; /* the error is set */
sepp_nepp 6:fb11b746ceb5 1947 }
sepp_nepp 6:fb11b746ceb5 1948
sepp_nepp 6:fb11b746ceb5 1949 if (new_data_ready == 1) {
sepp_nepp 6:fb11b746ceb5 1950 break; /* done note that status == 0 */
sepp_nepp 6:fb11b746ceb5 1951 }
sepp_nepp 6:fb11b746ceb5 1952
sepp_nepp 6:fb11b746ceb5 1953 loop_nb++;
sepp_nepp 6:fb11b746ceb5 1954 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp 6:fb11b746ceb5 1955 status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp 6:fb11b746ceb5 1956 break;
sepp_nepp 6:fb11b746ceb5 1957 }
sepp_nepp 6:fb11b746ceb5 1958
sepp_nepp 7:3a1115c2556b 1959 VL53L0X_polling_delay();
sepp_nepp 6:fb11b746ceb5 1960 } while (1);
sepp_nepp 6:fb11b746ceb5 1961
sepp_nepp 6:fb11b746ceb5 1962 return status;
sepp_nepp 6:fb11b746ceb5 1963 }
sepp_nepp 6:fb11b746ceb5 1964
sepp_nepp 6:fb11b746ceb5 1965 /* Group PAL Interrupt Functions */
sepp_nepp 7:3a1115c2556b 1966 VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask( uint32_t interrupt_mask)
sepp_nepp 7:3a1115c2556b 1967 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1968 uint8_t loop_count;
sepp_nepp 6:fb11b746ceb5 1969 uint8_t byte;
sepp_nepp 6:fb11b746ceb5 1970
sepp_nepp 6:fb11b746ceb5 1971 /* clear bit 0 range interrupt, bit 1 error interrupt */
sepp_nepp 6:fb11b746ceb5 1972 loop_count = 0;
sepp_nepp 6:fb11b746ceb5 1973 do {
sepp_nepp 7:3a1115c2556b 1974 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
sepp_nepp 7:3a1115c2556b 1975 status |= VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
sepp_nepp 7:3a1115c2556b 1976 status |= VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
sepp_nepp 6:fb11b746ceb5 1977 loop_count++;
sepp_nepp 6:fb11b746ceb5 1978 } while (((byte & 0x07) != 0x00)
sepp_nepp 6:fb11b746ceb5 1979 && (loop_count < 3)
sepp_nepp 6:fb11b746ceb5 1980 && (status == VL53L0X_ERROR_NONE));
sepp_nepp 6:fb11b746ceb5 1981
sepp_nepp 6:fb11b746ceb5 1982 if (loop_count >= 3) {
sepp_nepp 6:fb11b746ceb5 1983 status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
sepp_nepp 6:fb11b746ceb5 1984 }
sepp_nepp 6:fb11b746ceb5 1985
sepp_nepp 6:fb11b746ceb5 1986
sepp_nepp 6:fb11b746ceb5 1987 return status;
sepp_nepp 6:fb11b746ceb5 1988 }
sepp_nepp 6:fb11b746ceb5 1989
sepp_nepp 8:2fd7cb217068 1990 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(uint8_t vhv_init_byte)
sepp_nepp 8:2fd7cb217068 1991 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 1992
sepp_nepp 6:fb11b746ceb5 1993 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 1994 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START,
sepp_nepp 6:fb11b746ceb5 1995 VL53L0X_REG_SYSRANGE_MODE_START_STOP |
sepp_nepp 6:fb11b746ceb5 1996 vhv_init_byte);
sepp_nepp 6:fb11b746ceb5 1997 }
sepp_nepp 6:fb11b746ceb5 1998
sepp_nepp 6:fb11b746ceb5 1999 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2000 status = VL53L0X_measurement_poll_for_completion();}
sepp_nepp 6:fb11b746ceb5 2001
sepp_nepp 6:fb11b746ceb5 2002 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2003 status = VL53L0X_clear_interrupt_mask( 0);}
sepp_nepp 6:fb11b746ceb5 2004
sepp_nepp 6:fb11b746ceb5 2005 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2006 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START, 0x00);
sepp_nepp 6:fb11b746ceb5 2007 }
sepp_nepp 6:fb11b746ceb5 2008
sepp_nepp 6:fb11b746ceb5 2009 return status;
sepp_nepp 6:fb11b746ceb5 2010 }
sepp_nepp 6:fb11b746ceb5 2011
sepp_nepp 7:3a1115c2556b 2012 VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io( uint8_t read_not_write,
sepp_nepp 6:fb11b746ceb5 2013 uint8_t vhv_settings, uint8_t phase_cal,
sepp_nepp 6:fb11b746ceb5 2014 uint8_t *p_vhv_settings, uint8_t *p_phase_cal,
sepp_nepp 6:fb11b746ceb5 2015 const uint8_t vhv_enable, const uint8_t phase_enable)
sepp_nepp 8:2fd7cb217068 2016 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2017 uint8_t phase_calint = 0;
sepp_nepp 6:fb11b746ceb5 2018
sepp_nepp 6:fb11b746ceb5 2019 /* Read VHV from device */
sepp_nepp 7:3a1115c2556b 2020 status |= VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 2021 status |= VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 7:3a1115c2556b 2022 status |= VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 2023
sepp_nepp 6:fb11b746ceb5 2024 if (read_not_write) {
sepp_nepp 6:fb11b746ceb5 2025 if (vhv_enable) {
sepp_nepp 7:3a1115c2556b 2026 status |= VL53L0X_read_byte( 0xCB, p_vhv_settings);}
sepp_nepp 6:fb11b746ceb5 2027 if (phase_enable) {
sepp_nepp 7:3a1115c2556b 2028 status |= VL53L0X_read_byte( 0xEE, &phase_calint);}
sepp_nepp 6:fb11b746ceb5 2029 } else {
sepp_nepp 6:fb11b746ceb5 2030 if (vhv_enable) {
sepp_nepp 7:3a1115c2556b 2031 status |= VL53L0X_write_byte( 0xCB, vhv_settings);}
sepp_nepp 6:fb11b746ceb5 2032 if (phase_enable) {
sepp_nepp 7:3a1115c2556b 2033 status |= VL53L0X_update_byte( 0xEE, 0x80, phase_cal);}
sepp_nepp 6:fb11b746ceb5 2034 }
sepp_nepp 6:fb11b746ceb5 2035
sepp_nepp 7:3a1115c2556b 2036 status |= VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 2037 status |= VL53L0X_write_byte( 0x00, 0x01);
sepp_nepp 7:3a1115c2556b 2038 status |= VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 2039
sepp_nepp 6:fb11b746ceb5 2040 *p_phase_cal = (uint8_t)(phase_calint & 0xEF);
sepp_nepp 6:fb11b746ceb5 2041
sepp_nepp 6:fb11b746ceb5 2042 return status;
sepp_nepp 6:fb11b746ceb5 2043 }
sepp_nepp 6:fb11b746ceb5 2044
sepp_nepp 8:2fd7cb217068 2045 VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(uint8_t *p_vhv_settings, const uint8_t get_data_enable,
sepp_nepp 6:fb11b746ceb5 2046 const uint8_t restore_config)
sepp_nepp 8:2fd7cb217068 2047 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2048 uint8_t sequence_config = 0;
sepp_nepp 6:fb11b746ceb5 2049 uint8_t vhv_settings = 0;
sepp_nepp 6:fb11b746ceb5 2050 uint8_t phase_cal = 0;
sepp_nepp 6:fb11b746ceb5 2051 uint8_t phase_cal_int = 0;
sepp_nepp 6:fb11b746ceb5 2052
sepp_nepp 6:fb11b746ceb5 2053 /* store the value of the sequence config,
sepp_nepp 6:fb11b746ceb5 2054 * this will be reset before the end of the function
sepp_nepp 6:fb11b746ceb5 2055 */
sepp_nepp 6:fb11b746ceb5 2056
sepp_nepp 6:fb11b746ceb5 2057 if (restore_config) {
sepp_nepp 8:2fd7cb217068 2058 sequence_config = Data.SequenceConfig;
sepp_nepp 6:fb11b746ceb5 2059 }
sepp_nepp 6:fb11b746ceb5 2060
sepp_nepp 6:fb11b746ceb5 2061 /* Run VHV */
sepp_nepp 7:3a1115c2556b 2062 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
sepp_nepp 6:fb11b746ceb5 2063
sepp_nepp 6:fb11b746ceb5 2064 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2065 status = VL53L0X_perform_single_ref_calibration( 0x40);}
sepp_nepp 6:fb11b746ceb5 2066
sepp_nepp 6:fb11b746ceb5 2067 /* Read VHV from device */
sepp_nepp 6:fb11b746ceb5 2068 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
sepp_nepp 7:3a1115c2556b 2069 status = VL53L0X_ref_calibration_io( 1, vhv_settings, phase_cal, /* Not used here */
sepp_nepp 7:3a1115c2556b 2070 p_vhv_settings, &phase_cal_int, 1, 0);
sepp_nepp 7:3a1115c2556b 2071 } else {*p_vhv_settings = 0; }
sepp_nepp 6:fb11b746ceb5 2072
sepp_nepp 6:fb11b746ceb5 2073 if ((status == VL53L0X_ERROR_NONE) && restore_config) {
sepp_nepp 6:fb11b746ceb5 2074 /* restore the previous Sequence Config */
sepp_nepp 7:3a1115c2556b 2075 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sepp_nepp 6:fb11b746ceb5 2076 sequence_config);
sepp_nepp 6:fb11b746ceb5 2077 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 2078 Data.SequenceConfig = sequence_config; }
sepp_nepp 6:fb11b746ceb5 2079 }
sepp_nepp 6:fb11b746ceb5 2080
sepp_nepp 6:fb11b746ceb5 2081 return status;
sepp_nepp 6:fb11b746ceb5 2082 }
sepp_nepp 6:fb11b746ceb5 2083
sepp_nepp 8:2fd7cb217068 2084 VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(uint8_t *p_phase_cal, const uint8_t get_data_enable,
sepp_nepp 6:fb11b746ceb5 2085 const uint8_t restore_config)
sepp_nepp 7:3a1115c2556b 2086 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2087 uint8_t sequence_config = 0;
sepp_nepp 6:fb11b746ceb5 2088 uint8_t vhv_settings = 0;
sepp_nepp 6:fb11b746ceb5 2089 uint8_t phase_cal = 0;
sepp_nepp 6:fb11b746ceb5 2090 uint8_t vhv_settingsint;
sepp_nepp 6:fb11b746ceb5 2091
sepp_nepp 6:fb11b746ceb5 2092 /* store the value of the sequence config,
sepp_nepp 7:3a1115c2556b 2093 * this will be reset before the end of the function */
sepp_nepp 6:fb11b746ceb5 2094
sepp_nepp 6:fb11b746ceb5 2095 if (restore_config) {
sepp_nepp 8:2fd7cb217068 2096 sequence_config = Data.SequenceConfig;
sepp_nepp 6:fb11b746ceb5 2097 }
sepp_nepp 6:fb11b746ceb5 2098
sepp_nepp 6:fb11b746ceb5 2099 /* Run PhaseCal */
sepp_nepp 7:3a1115c2556b 2100 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
sepp_nepp 6:fb11b746ceb5 2101
sepp_nepp 6:fb11b746ceb5 2102 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2103 status = VL53L0X_perform_single_ref_calibration( 0x0);
sepp_nepp 6:fb11b746ceb5 2104 }
sepp_nepp 6:fb11b746ceb5 2105
sepp_nepp 6:fb11b746ceb5 2106 /* Read PhaseCal from device */
sepp_nepp 6:fb11b746ceb5 2107 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
sepp_nepp 7:3a1115c2556b 2108 status = VL53L0X_ref_calibration_io( 1,
sepp_nepp 6:fb11b746ceb5 2109 vhv_settings, phase_cal, /* Not used here */
sepp_nepp 6:fb11b746ceb5 2110 &vhv_settingsint, p_phase_cal,
sepp_nepp 6:fb11b746ceb5 2111 0, 1);
sepp_nepp 6:fb11b746ceb5 2112 } else {
sepp_nepp 6:fb11b746ceb5 2113 *p_phase_cal = 0;
sepp_nepp 6:fb11b746ceb5 2114 }
sepp_nepp 6:fb11b746ceb5 2115
sepp_nepp 6:fb11b746ceb5 2116 if ((status == VL53L0X_ERROR_NONE) && restore_config) {
sepp_nepp 6:fb11b746ceb5 2117 /* restore the previous Sequence Config */
sepp_nepp 7:3a1115c2556b 2118 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sepp_nepp 6:fb11b746ceb5 2119 sequence_config);
sepp_nepp 6:fb11b746ceb5 2120 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 2121 Data.SequenceConfig = sequence_config;
sepp_nepp 6:fb11b746ceb5 2122 }
sepp_nepp 6:fb11b746ceb5 2123
sepp_nepp 6:fb11b746ceb5 2124 }
sepp_nepp 6:fb11b746ceb5 2125
sepp_nepp 6:fb11b746ceb5 2126 return status;
sepp_nepp 6:fb11b746ceb5 2127 }
sepp_nepp 6:fb11b746ceb5 2128
sepp_nepp 8:2fd7cb217068 2129 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable)
sepp_nepp 8:2fd7cb217068 2130 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2131 uint8_t sequence_config = 0;
sepp_nepp 6:fb11b746ceb5 2132
sepp_nepp 6:fb11b746ceb5 2133 /* store the value of the sequence config,
sepp_nepp 7:3a1115c2556b 2134 * this will be reset before the end of the function */
sepp_nepp 7:3a1115c2556b 2135
sepp_nepp 8:2fd7cb217068 2136 sequence_config = Data.SequenceConfig;
sepp_nepp 6:fb11b746ceb5 2137
sepp_nepp 6:fb11b746ceb5 2138 /* In the following function we don't save the config to optimize
sepp_nepp 6:fb11b746ceb5 2139 * writes on device. Config is saved and restored only once. */
sepp_nepp 7:3a1115c2556b 2140 status = VL53L0X_perform_vhv_calibration(p_vhv_settings, get_data_enable, 0);
sepp_nepp 6:fb11b746ceb5 2141
sepp_nepp 6:fb11b746ceb5 2142 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2143 status = VL53L0X_perform_phase_calibration(p_phase_cal, get_data_enable, 0); }
sepp_nepp 6:fb11b746ceb5 2144
sepp_nepp 6:fb11b746ceb5 2145 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2146 /* restore the previous Sequence Config */
sepp_nepp 7:3a1115c2556b 2147 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sepp_nepp 6:fb11b746ceb5 2148 sequence_config);
sepp_nepp 6:fb11b746ceb5 2149 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 2150 Data.SequenceConfig = sequence_config; }
sepp_nepp 6:fb11b746ceb5 2151 }
sepp_nepp 6:fb11b746ceb5 2152
sepp_nepp 6:fb11b746ceb5 2153 return status;
sepp_nepp 6:fb11b746ceb5 2154 }
sepp_nepp 6:fb11b746ceb5 2155
sepp_nepp 6:fb11b746ceb5 2156 void VL53L0X::get_next_good_spad(uint8_t good_spad_array[], uint32_t size,
sepp_nepp 6:fb11b746ceb5 2157 uint32_t curr, int32_t *p_next)
sepp_nepp 8:2fd7cb217068 2158 { uint32_t start_index;
sepp_nepp 6:fb11b746ceb5 2159 uint32_t fine_offset;
sepp_nepp 6:fb11b746ceb5 2160 uint32_t c_spads_per_byte = 8;
sepp_nepp 6:fb11b746ceb5 2161 uint32_t coarse_index;
sepp_nepp 6:fb11b746ceb5 2162 uint32_t fine_index;
sepp_nepp 6:fb11b746ceb5 2163 uint8_t data_byte;
sepp_nepp 6:fb11b746ceb5 2164 uint8_t success = 0;
sepp_nepp 6:fb11b746ceb5 2165
sepp_nepp 7:3a1115c2556b 2166 /* Starting with the current good spad, loop through the array to find
sepp_nepp 6:fb11b746ceb5 2167 * the next. i.e. the next bit set in the sequence.
sepp_nepp 6:fb11b746ceb5 2168 *
sepp_nepp 6:fb11b746ceb5 2169 * The coarse index is the byte index of the array and the fine index is
sepp_nepp 7:3a1115c2556b 2170 * the index of the bit within each byte. */
sepp_nepp 6:fb11b746ceb5 2171
sepp_nepp 6:fb11b746ceb5 2172 *p_next = -1;
sepp_nepp 6:fb11b746ceb5 2173
sepp_nepp 6:fb11b746ceb5 2174 start_index = curr / c_spads_per_byte;
sepp_nepp 6:fb11b746ceb5 2175 fine_offset = curr % c_spads_per_byte;
sepp_nepp 6:fb11b746ceb5 2176
sepp_nepp 6:fb11b746ceb5 2177 for (coarse_index = start_index; ((coarse_index < size) && !success);
sepp_nepp 6:fb11b746ceb5 2178 coarse_index++) {
sepp_nepp 6:fb11b746ceb5 2179 fine_index = 0;
sepp_nepp 6:fb11b746ceb5 2180 data_byte = good_spad_array[coarse_index];
sepp_nepp 6:fb11b746ceb5 2181
sepp_nepp 6:fb11b746ceb5 2182 if (coarse_index == start_index) {
sepp_nepp 6:fb11b746ceb5 2183 /* locate the bit position of the provided current
sepp_nepp 6:fb11b746ceb5 2184 * spad bit before iterating */
sepp_nepp 6:fb11b746ceb5 2185 data_byte >>= fine_offset;
sepp_nepp 6:fb11b746ceb5 2186 fine_index = fine_offset;
sepp_nepp 6:fb11b746ceb5 2187 }
sepp_nepp 6:fb11b746ceb5 2188
sepp_nepp 6:fb11b746ceb5 2189 while (fine_index < c_spads_per_byte) {
sepp_nepp 6:fb11b746ceb5 2190 if ((data_byte & 0x1) == 1) {
sepp_nepp 6:fb11b746ceb5 2191 success = 1;
sepp_nepp 6:fb11b746ceb5 2192 *p_next = coarse_index * c_spads_per_byte + fine_index;
sepp_nepp 6:fb11b746ceb5 2193 break;
sepp_nepp 6:fb11b746ceb5 2194 }
sepp_nepp 6:fb11b746ceb5 2195 data_byte >>= 1;
sepp_nepp 6:fb11b746ceb5 2196 fine_index++;
sepp_nepp 6:fb11b746ceb5 2197 }
sepp_nepp 6:fb11b746ceb5 2198 }
sepp_nepp 6:fb11b746ceb5 2199 }
sepp_nepp 6:fb11b746ceb5 2200
sepp_nepp 6:fb11b746ceb5 2201 uint8_t VL53L0X::is_aperture(uint32_t spad_index)
sepp_nepp 8:2fd7cb217068 2202 { /* This function reports if a given spad index is an aperture SPAD by
sepp_nepp 7:3a1115c2556b 2203 * deriving the quadrant.*/
sepp_nepp 6:fb11b746ceb5 2204 uint32_t quadrant;
sepp_nepp 6:fb11b746ceb5 2205 uint8_t is_aperture = 1;
sepp_nepp 6:fb11b746ceb5 2206 quadrant = spad_index >> 6;
sepp_nepp 6:fb11b746ceb5 2207 if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) {
sepp_nepp 6:fb11b746ceb5 2208 is_aperture = 0;
sepp_nepp 6:fb11b746ceb5 2209 }
sepp_nepp 6:fb11b746ceb5 2210
sepp_nepp 6:fb11b746ceb5 2211 return is_aperture;
sepp_nepp 6:fb11b746ceb5 2212 }
sepp_nepp 6:fb11b746ceb5 2213
sepp_nepp 6:fb11b746ceb5 2214 VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[], uint32_t size,
sepp_nepp 6:fb11b746ceb5 2215 uint32_t spad_index)
sepp_nepp 7:3a1115c2556b 2216 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2217 uint32_t c_spads_per_byte = 8;
sepp_nepp 6:fb11b746ceb5 2218 uint32_t coarse_index;
sepp_nepp 6:fb11b746ceb5 2219 uint32_t fine_index;
sepp_nepp 6:fb11b746ceb5 2220
sepp_nepp 6:fb11b746ceb5 2221 coarse_index = spad_index / c_spads_per_byte;
sepp_nepp 6:fb11b746ceb5 2222 fine_index = spad_index % c_spads_per_byte;
sepp_nepp 6:fb11b746ceb5 2223 if (coarse_index >= size) {
sepp_nepp 6:fb11b746ceb5 2224 status = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 6:fb11b746ceb5 2225 } else {
sepp_nepp 6:fb11b746ceb5 2226 spad_array[coarse_index] |= (1 << fine_index);
sepp_nepp 6:fb11b746ceb5 2227 }
sepp_nepp 6:fb11b746ceb5 2228
sepp_nepp 6:fb11b746ceb5 2229 return status;
sepp_nepp 6:fb11b746ceb5 2230 }
sepp_nepp 6:fb11b746ceb5 2231
sepp_nepp 7:3a1115c2556b 2232 VL53L0X_Error VL53L0X::set_ref_spad_map( uint8_t *p_ref_spad_array)
sepp_nepp 9:cb4c6d4e5030 2233 { VL53L0X_Error status = VL53L0X_i2c_write(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
sepp_nepp 6:fb11b746ceb5 2234 p_ref_spad_array, 6);
sepp_nepp 6:fb11b746ceb5 2235
sepp_nepp 6:fb11b746ceb5 2236 return status;
sepp_nepp 6:fb11b746ceb5 2237 }
sepp_nepp 6:fb11b746ceb5 2238
sepp_nepp 7:3a1115c2556b 2239 VL53L0X_Error VL53L0X::get_ref_spad_map( uint8_t *p_ref_spad_array)
sepp_nepp 8:2fd7cb217068 2240 { VL53L0X_Error status = VL53L0X_read_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
sepp_nepp 6:fb11b746ceb5 2241 p_ref_spad_array,
sepp_nepp 6:fb11b746ceb5 2242 6);
sepp_nepp 6:fb11b746ceb5 2243 // VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2244 // uint8_t count=0;
sepp_nepp 6:fb11b746ceb5 2245
sepp_nepp 6:fb11b746ceb5 2246 // for (count = 0; count < 6; count++)
sepp_nepp 7:3a1115c2556b 2247 // status = VL53L0X_RdByte( (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]);
sepp_nepp 6:fb11b746ceb5 2248 return status;
sepp_nepp 6:fb11b746ceb5 2249 }
sepp_nepp 6:fb11b746ceb5 2250
sepp_nepp 8:2fd7cb217068 2251 VL53L0X_Error VL53L0X::enable_ref_spads(uint8_t aperture_spads,
sepp_nepp 6:fb11b746ceb5 2252 uint8_t good_spad_array[],
sepp_nepp 6:fb11b746ceb5 2253 uint8_t spad_array[],
sepp_nepp 6:fb11b746ceb5 2254 uint32_t size,
sepp_nepp 6:fb11b746ceb5 2255 uint32_t start,
sepp_nepp 6:fb11b746ceb5 2256 uint32_t offset,
sepp_nepp 6:fb11b746ceb5 2257 uint32_t spad_count,
sepp_nepp 6:fb11b746ceb5 2258 uint32_t *p_last_spad)
sepp_nepp 8:2fd7cb217068 2259 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2260 uint32_t index;
sepp_nepp 6:fb11b746ceb5 2261 uint32_t i;
sepp_nepp 6:fb11b746ceb5 2262 int32_t next_good_spad = offset;
sepp_nepp 6:fb11b746ceb5 2263 uint32_t current_spad;
sepp_nepp 6:fb11b746ceb5 2264 uint8_t check_spad_array[6];
sepp_nepp 6:fb11b746ceb5 2265
sepp_nepp 6:fb11b746ceb5 2266 /*
sepp_nepp 6:fb11b746ceb5 2267 * This function takes in a spad array which may or may not have SPADS
sepp_nepp 6:fb11b746ceb5 2268 * already enabled and appends from a given offset a requested number
sepp_nepp 6:fb11b746ceb5 2269 * of new SPAD enables. The 'good spad map' is applied to
sepp_nepp 6:fb11b746ceb5 2270 * determine the next SPADs to enable.
sepp_nepp 6:fb11b746ceb5 2271 *
sepp_nepp 6:fb11b746ceb5 2272 * This function applies to only aperture or only non-aperture spads.
sepp_nepp 6:fb11b746ceb5 2273 * Checks are performed to ensure this.
sepp_nepp 6:fb11b746ceb5 2274 */
sepp_nepp 6:fb11b746ceb5 2275
sepp_nepp 6:fb11b746ceb5 2276 current_spad = offset;
sepp_nepp 6:fb11b746ceb5 2277 for (index = 0; index < spad_count; index++) {
sepp_nepp 6:fb11b746ceb5 2278 get_next_good_spad(good_spad_array, size, current_spad,
sepp_nepp 6:fb11b746ceb5 2279 &next_good_spad);
sepp_nepp 6:fb11b746ceb5 2280
sepp_nepp 6:fb11b746ceb5 2281 if (next_good_spad == -1) {
sepp_nepp 6:fb11b746ceb5 2282 status = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 6:fb11b746ceb5 2283 break;
sepp_nepp 6:fb11b746ceb5 2284 }
sepp_nepp 6:fb11b746ceb5 2285
sepp_nepp 6:fb11b746ceb5 2286 /* Confirm that the next good SPAD is non-aperture */
sepp_nepp 6:fb11b746ceb5 2287 if (is_aperture(start + next_good_spad) != aperture_spads) {
sepp_nepp 6:fb11b746ceb5 2288 /* if we can't get the required number of good aperture
sepp_nepp 6:fb11b746ceb5 2289 * spads from the current quadrant then this is an error
sepp_nepp 6:fb11b746ceb5 2290 */
sepp_nepp 6:fb11b746ceb5 2291 status = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 6:fb11b746ceb5 2292 break;
sepp_nepp 6:fb11b746ceb5 2293 }
sepp_nepp 6:fb11b746ceb5 2294 current_spad = (uint32_t)next_good_spad;
sepp_nepp 6:fb11b746ceb5 2295 enable_spad_bit(spad_array, size, current_spad);
sepp_nepp 6:fb11b746ceb5 2296 current_spad++;
sepp_nepp 6:fb11b746ceb5 2297 }
sepp_nepp 6:fb11b746ceb5 2298 *p_last_spad = current_spad;
sepp_nepp 6:fb11b746ceb5 2299
sepp_nepp 6:fb11b746ceb5 2300 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2301 status = set_ref_spad_map( spad_array);
sepp_nepp 6:fb11b746ceb5 2302 }
sepp_nepp 6:fb11b746ceb5 2303
sepp_nepp 6:fb11b746ceb5 2304 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2305 status = get_ref_spad_map( check_spad_array);
sepp_nepp 6:fb11b746ceb5 2306
sepp_nepp 6:fb11b746ceb5 2307 i = 0;
sepp_nepp 6:fb11b746ceb5 2308
sepp_nepp 6:fb11b746ceb5 2309 /* Compare spad maps. If not equal report error. */
sepp_nepp 6:fb11b746ceb5 2310 while (i < size) {
sepp_nepp 6:fb11b746ceb5 2311 if (spad_array[i] != check_spad_array[i]) {
sepp_nepp 6:fb11b746ceb5 2312 status = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 6:fb11b746ceb5 2313 break;
sepp_nepp 6:fb11b746ceb5 2314 }
sepp_nepp 6:fb11b746ceb5 2315 i++;
sepp_nepp 6:fb11b746ceb5 2316 }
sepp_nepp 6:fb11b746ceb5 2317 }
sepp_nepp 6:fb11b746ceb5 2318 return status;
sepp_nepp 6:fb11b746ceb5 2319 }
sepp_nepp 6:fb11b746ceb5 2320
sepp_nepp 7:3a1115c2556b 2321 VL53L0X_Error VL53L0X::VL53L0X_set_device_mode( VL53L0X_DeviceModes device_mode)
sepp_nepp 8:2fd7cb217068 2322 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2323
sepp_nepp 6:fb11b746ceb5 2324 switch (device_mode) {
sepp_nepp 6:fb11b746ceb5 2325 case VL53L0X_DEVICEMODE_SINGLE_RANGING:
sepp_nepp 6:fb11b746ceb5 2326 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
sepp_nepp 6:fb11b746ceb5 2327 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
sepp_nepp 6:fb11b746ceb5 2328 case VL53L0X_DEVICEMODE_GPIO_DRIVE:
sepp_nepp 6:fb11b746ceb5 2329 case VL53L0X_DEVICEMODE_GPIO_OSC:
sepp_nepp 6:fb11b746ceb5 2330 /* Supported modes */
sepp_nepp 8:2fd7cb217068 2331 Data.CurrentParameters.DeviceMode = device_mode;
sepp_nepp 6:fb11b746ceb5 2332 break;
sepp_nepp 6:fb11b746ceb5 2333 default:
sepp_nepp 6:fb11b746ceb5 2334 /* Unsupported mode */
sepp_nepp 6:fb11b746ceb5 2335 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
sepp_nepp 6:fb11b746ceb5 2336 }
sepp_nepp 6:fb11b746ceb5 2337
sepp_nepp 6:fb11b746ceb5 2338
sepp_nepp 6:fb11b746ceb5 2339 return status;
sepp_nepp 6:fb11b746ceb5 2340 }
sepp_nepp 6:fb11b746ceb5 2341
sepp_nepp 8:2fd7cb217068 2342 VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low,
sepp_nepp 6:fb11b746ceb5 2343 FixPoint1616_t threshold_high)
sepp_nepp 8:2fd7cb217068 2344 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2345 uint16_t threshold16;
sepp_nepp 6:fb11b746ceb5 2346
sepp_nepp 6:fb11b746ceb5 2347
sepp_nepp 6:fb11b746ceb5 2348 /* no dependency on DeviceMode for Ewok */
sepp_nepp 6:fb11b746ceb5 2349 /* Need to divide by 2 because the FW will apply a x2 */
sepp_nepp 6:fb11b746ceb5 2350 threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
sepp_nepp 7:3a1115c2556b 2351 status = VL53L0X_write_word( VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16);
sepp_nepp 6:fb11b746ceb5 2352
sepp_nepp 6:fb11b746ceb5 2353 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2354 /* Need to divide by 2 because the FW will apply a x2 */
sepp_nepp 6:fb11b746ceb5 2355 threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
sepp_nepp 7:3a1115c2556b 2356 status = VL53L0X_write_word( VL53L0X_REG_SYSTEM_THRESH_HIGH,
sepp_nepp 6:fb11b746ceb5 2357 threshold16);
sepp_nepp 6:fb11b746ceb5 2358 }
sepp_nepp 6:fb11b746ceb5 2359
sepp_nepp 6:fb11b746ceb5 2360
sepp_nepp 6:fb11b746ceb5 2361 return status;
sepp_nepp 6:fb11b746ceb5 2362 }
sepp_nepp 6:fb11b746ceb5 2363
sepp_nepp 8:2fd7cb217068 2364 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low,
sepp_nepp 6:fb11b746ceb5 2365 FixPoint1616_t *p_threshold_high)
sepp_nepp 8:2fd7cb217068 2366 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2367 uint16_t threshold16;
sepp_nepp 6:fb11b746ceb5 2368
sepp_nepp 6:fb11b746ceb5 2369
sepp_nepp 6:fb11b746ceb5 2370 /* no dependency on DeviceMode for Ewok */
sepp_nepp 6:fb11b746ceb5 2371
sepp_nepp 7:3a1115c2556b 2372 status = VL53L0X_read_word( VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16);
sepp_nepp 6:fb11b746ceb5 2373 /* Need to multiply by 2 because the FW will apply a x2 */
sepp_nepp 6:fb11b746ceb5 2374 *p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17);
sepp_nepp 6:fb11b746ceb5 2375
sepp_nepp 6:fb11b746ceb5 2376 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2377 status = VL53L0X_read_word( VL53L0X_REG_SYSTEM_THRESH_HIGH,
sepp_nepp 6:fb11b746ceb5 2378 &threshold16);
sepp_nepp 6:fb11b746ceb5 2379 /* Need to multiply by 2 because the FW will apply a x2 */
sepp_nepp 6:fb11b746ceb5 2380 *p_threshold_high =
sepp_nepp 6:fb11b746ceb5 2381 (FixPoint1616_t)((0x00fff & threshold16) << 17);
sepp_nepp 6:fb11b746ceb5 2382 }
sepp_nepp 6:fb11b746ceb5 2383
sepp_nepp 6:fb11b746ceb5 2384
sepp_nepp 6:fb11b746ceb5 2385 return status;
sepp_nepp 6:fb11b746ceb5 2386 }
sepp_nepp 6:fb11b746ceb5 2387
sepp_nepp 8:2fd7cb217068 2388 VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(uint8_t *p_tuning_setting_buffer)
sepp_nepp 8:2fd7cb217068 2389 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2390 int i;
sepp_nepp 6:fb11b746ceb5 2391 int index;
sepp_nepp 6:fb11b746ceb5 2392 uint8_t msb;
sepp_nepp 6:fb11b746ceb5 2393 uint8_t lsb;
sepp_nepp 6:fb11b746ceb5 2394 uint8_t select_param;
sepp_nepp 9:cb4c6d4e5030 2395 uint16_t number_of_writes;
sepp_nepp 6:fb11b746ceb5 2396 uint8_t address;
sepp_nepp 6:fb11b746ceb5 2397 uint8_t local_buffer[4]; /* max */
sepp_nepp 6:fb11b746ceb5 2398 uint16_t temp16;
sepp_nepp 8:2fd7cb217068 2399
sepp_nepp 6:fb11b746ceb5 2400 index = 0;
sepp_nepp 6:fb11b746ceb5 2401
sepp_nepp 6:fb11b746ceb5 2402 while ((*(p_tuning_setting_buffer + index) != 0) &&
sepp_nepp 6:fb11b746ceb5 2403 (status == VL53L0X_ERROR_NONE)) {
sepp_nepp 6:fb11b746ceb5 2404 number_of_writes = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2405 index++;
sepp_nepp 6:fb11b746ceb5 2406 if (number_of_writes == 0xFF) {
sepp_nepp 6:fb11b746ceb5 2407 /* internal parameters */
sepp_nepp 6:fb11b746ceb5 2408 select_param = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2409 index++;
sepp_nepp 6:fb11b746ceb5 2410 switch (select_param) {
sepp_nepp 6:fb11b746ceb5 2411 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
sepp_nepp 6:fb11b746ceb5 2412 msb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2413 index++;
sepp_nepp 6:fb11b746ceb5 2414 lsb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2415 index++;
sepp_nepp 6:fb11b746ceb5 2416 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
sepp_nepp 8:2fd7cb217068 2417 Data.SigmaEstRefArray = temp16;
sepp_nepp 6:fb11b746ceb5 2418 break;
sepp_nepp 6:fb11b746ceb5 2419 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
sepp_nepp 6:fb11b746ceb5 2420 msb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2421 index++;
sepp_nepp 6:fb11b746ceb5 2422 lsb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2423 index++;
sepp_nepp 6:fb11b746ceb5 2424 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
sepp_nepp 8:2fd7cb217068 2425 Data.SigmaEstEffPulseWidth = temp16;
sepp_nepp 6:fb11b746ceb5 2426 break;
sepp_nepp 6:fb11b746ceb5 2427 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
sepp_nepp 6:fb11b746ceb5 2428 msb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2429 index++;
sepp_nepp 6:fb11b746ceb5 2430 lsb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2431 index++;
sepp_nepp 6:fb11b746ceb5 2432 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
sepp_nepp 8:2fd7cb217068 2433 Data.SigmaEstEffAmbWidth = temp16;
sepp_nepp 6:fb11b746ceb5 2434 break;
sepp_nepp 6:fb11b746ceb5 2435 case 3: /* uint16_t targetRefRate -> 2 bytes */
sepp_nepp 6:fb11b746ceb5 2436 msb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2437 index++;
sepp_nepp 6:fb11b746ceb5 2438 lsb = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2439 index++;
sepp_nepp 6:fb11b746ceb5 2440 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
sepp_nepp 8:2fd7cb217068 2441 Data.targetRefRate = temp16;
sepp_nepp 6:fb11b746ceb5 2442 break;
sepp_nepp 6:fb11b746ceb5 2443 default: /* invalid parameter */
sepp_nepp 6:fb11b746ceb5 2444 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 2445 }
sepp_nepp 6:fb11b746ceb5 2446
sepp_nepp 6:fb11b746ceb5 2447 } else if (number_of_writes <= 4) {
sepp_nepp 6:fb11b746ceb5 2448 address = *(p_tuning_setting_buffer + index);
sepp_nepp 6:fb11b746ceb5 2449 index++;
sepp_nepp 6:fb11b746ceb5 2450
sepp_nepp 6:fb11b746ceb5 2451 for (i = 0; i < number_of_writes; i++) {
sepp_nepp 6:fb11b746ceb5 2452 local_buffer[i] = *(p_tuning_setting_buffer +
sepp_nepp 6:fb11b746ceb5 2453 index);
sepp_nepp 6:fb11b746ceb5 2454 index++;
sepp_nepp 6:fb11b746ceb5 2455 }
sepp_nepp 6:fb11b746ceb5 2456
sepp_nepp 9:cb4c6d4e5030 2457 status = VL53L0X_i2c_write( address, local_buffer, number_of_writes);
sepp_nepp 8:2fd7cb217068 2458
sepp_nepp 8:2fd7cb217068 2459 } else { status = VL53L0X_ERROR_INVALID_PARAMS; }
sepp_nepp 6:fb11b746ceb5 2460 }
sepp_nepp 6:fb11b746ceb5 2461 return status;
sepp_nepp 6:fb11b746ceb5 2462 }
sepp_nepp 6:fb11b746ceb5 2463
sepp_nepp 8:2fd7cb217068 2464 VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(uint8_t start_not_stopflag)
sepp_nepp 8:2fd7cb217068 2465 { uint8_t interrupt_config;
sepp_nepp 6:fb11b746ceb5 2466 FixPoint1616_t threshold_low;
sepp_nepp 6:fb11b746ceb5 2467 FixPoint1616_t threshold_high;
sepp_nepp 6:fb11b746ceb5 2468 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2469
sepp_nepp 8:2fd7cb217068 2470 interrupt_config = Data.Pin0GpioFunctionality;
sepp_nepp 6:fb11b746ceb5 2471
sepp_nepp 6:fb11b746ceb5 2472 if ((interrupt_config ==
sepp_nepp 6:fb11b746ceb5 2473 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) ||
sepp_nepp 6:fb11b746ceb5 2474 (interrupt_config ==
sepp_nepp 6:fb11b746ceb5 2475 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) ||
sepp_nepp 6:fb11b746ceb5 2476 (interrupt_config ==
sepp_nepp 6:fb11b746ceb5 2477 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
sepp_nepp 6:fb11b746ceb5 2478
sepp_nepp 8:2fd7cb217068 2479 status = VL53L0X_get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp 6:fb11b746ceb5 2480 &threshold_low, &threshold_high);
sepp_nepp 6:fb11b746ceb5 2481
sepp_nepp 6:fb11b746ceb5 2482 if (((threshold_low > 255 * 65536) ||
sepp_nepp 6:fb11b746ceb5 2483 (threshold_high > 255 * 65536)) &&
sepp_nepp 6:fb11b746ceb5 2484 (status == VL53L0X_ERROR_NONE)) {
sepp_nepp 6:fb11b746ceb5 2485
sepp_nepp 6:fb11b746ceb5 2486 if (start_not_stopflag != 0) {
sepp_nepp 8:2fd7cb217068 2487 status = VL53L0X_load_tuning_settings(InterruptThresholdSettings);
sepp_nepp 6:fb11b746ceb5 2488 } else {
sepp_nepp 7:3a1115c2556b 2489 status |= VL53L0X_write_byte( 0xFF, 0x04);
sepp_nepp 7:3a1115c2556b 2490 status |= VL53L0X_write_byte( 0x70, 0x00);
sepp_nepp 7:3a1115c2556b 2491 status |= VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 7:3a1115c2556b 2492 status |= VL53L0X_write_byte( 0x80, 0x00);
sepp_nepp 6:fb11b746ceb5 2493 }
sepp_nepp 6:fb11b746ceb5 2494 }
sepp_nepp 6:fb11b746ceb5 2495 }
sepp_nepp 6:fb11b746ceb5 2496 return status;
sepp_nepp 6:fb11b746ceb5 2497 }
sepp_nepp 6:fb11b746ceb5 2498
sepp_nepp 7:3a1115c2556b 2499 VL53L0X_Error VL53L0X::VL53L0X_start_measurement(void)
sepp_nepp 8:2fd7cb217068 2500 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2501 VL53L0X_DeviceModes device_mode;
sepp_nepp 6:fb11b746ceb5 2502 uint8_t byte;
sepp_nepp 6:fb11b746ceb5 2503 uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
sepp_nepp 6:fb11b746ceb5 2504 uint32_t loop_nb;
sepp_nepp 6:fb11b746ceb5 2505
sepp_nepp 6:fb11b746ceb5 2506
sepp_nepp 6:fb11b746ceb5 2507 /* Get Current DeviceMode */
sepp_nepp 7:3a1115c2556b 2508 VL53L0X_get_device_mode( &device_mode);
sepp_nepp 7:3a1115c2556b 2509
sepp_nepp 7:3a1115c2556b 2510 status = VL53L0X_write_byte( 0x80, 0x01);
sepp_nepp 7:3a1115c2556b 2511 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 2512 status = VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 8:2fd7cb217068 2513 status = VL53L0X_write_byte( 0x91, Data.StopVariable);
sepp_nepp 7:3a1115c2556b 2514 status = VL53L0X_write_byte( 0x00, 0x01);
sepp_nepp 7:3a1115c2556b 2515 status = VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 7:3a1115c2556b 2516 status = VL53L0X_write_byte( 0x80, 0x00);
sepp_nepp 6:fb11b746ceb5 2517
sepp_nepp 6:fb11b746ceb5 2518 switch (device_mode) {
sepp_nepp 6:fb11b746ceb5 2519 case VL53L0X_DEVICEMODE_SINGLE_RANGING:
sepp_nepp 7:3a1115c2556b 2520 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START, 0x01);
sepp_nepp 6:fb11b746ceb5 2521
sepp_nepp 6:fb11b746ceb5 2522 byte = start_stop_byte;
sepp_nepp 6:fb11b746ceb5 2523 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2524 /* Wait until start bit has been cleared */
sepp_nepp 6:fb11b746ceb5 2525 loop_nb = 0;
sepp_nepp 6:fb11b746ceb5 2526 do {
sepp_nepp 6:fb11b746ceb5 2527 if (loop_nb > 0)
sepp_nepp 8:2fd7cb217068 2528 status = VL53L0X_read_byte(VL53L0X_REG_SYSRANGE_START, &byte);
sepp_nepp 6:fb11b746ceb5 2529 loop_nb = loop_nb + 1;
sepp_nepp 6:fb11b746ceb5 2530 } while (((byte & start_stop_byte) == start_stop_byte)
sepp_nepp 6:fb11b746ceb5 2531 && (status == VL53L0X_ERROR_NONE)
sepp_nepp 6:fb11b746ceb5 2532 && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
sepp_nepp 6:fb11b746ceb5 2533
sepp_nepp 6:fb11b746ceb5 2534 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp 6:fb11b746ceb5 2535 status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp 6:fb11b746ceb5 2536 }
sepp_nepp 6:fb11b746ceb5 2537 }
sepp_nepp 6:fb11b746ceb5 2538
sepp_nepp 6:fb11b746ceb5 2539 break;
sepp_nepp 9:cb4c6d4e5030 2540 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: /* Back-to-back mode */
sepp_nepp 6:fb11b746ceb5 2541
sepp_nepp 6:fb11b746ceb5 2542 /* Check if need to apply interrupt settings */
sepp_nepp 9:cb4c6d4e5030 2543 if (status == VL53L0X_ERROR_NONE)
sepp_nepp 9:cb4c6d4e5030 2544 { status = VL53L0X_check_and_load_interrupt_settings( 1); }
sepp_nepp 6:fb11b746ceb5 2545
sepp_nepp 8:2fd7cb217068 2546 status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
sepp_nepp 6:fb11b746ceb5 2547 VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
sepp_nepp 6:fb11b746ceb5 2548 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2549 /* Set PAL State to Running */
sepp_nepp 8:2fd7cb217068 2550 Data.PalState = VL53L0X_STATE_RUNNING;
sepp_nepp 6:fb11b746ceb5 2551 }
sepp_nepp 6:fb11b746ceb5 2552 break;
sepp_nepp 6:fb11b746ceb5 2553 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
sepp_nepp 6:fb11b746ceb5 2554 /* Continuous mode */
sepp_nepp 6:fb11b746ceb5 2555 /* Check if need to apply interrupt settings */
sepp_nepp 6:fb11b746ceb5 2556 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 2557 status = VL53L0X_check_and_load_interrupt_settings( 1);
sepp_nepp 6:fb11b746ceb5 2558 }
sepp_nepp 6:fb11b746ceb5 2559
sepp_nepp 8:2fd7cb217068 2560 status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
sepp_nepp 6:fb11b746ceb5 2561 VL53L0X_REG_SYSRANGE_MODE_TIMED);
sepp_nepp 6:fb11b746ceb5 2562
sepp_nepp 9:cb4c6d4e5030 2563 if (status == VL53L0X_ERROR_NONE)/* Set PAL State to Running */
sepp_nepp 9:cb4c6d4e5030 2564 { Data.PalState = VL53L0X_STATE_RUNNING; }
sepp_nepp 6:fb11b746ceb5 2565 break;
sepp_nepp 6:fb11b746ceb5 2566 default:
sepp_nepp 6:fb11b746ceb5 2567 /* Selected mode not supported */
sepp_nepp 6:fb11b746ceb5 2568 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
sepp_nepp 6:fb11b746ceb5 2569 }
sepp_nepp 9:cb4c6d4e5030 2570
sepp_nepp 6:fb11b746ceb5 2571 return status;
sepp_nepp 6:fb11b746ceb5 2572 }
sepp_nepp 6:fb11b746ceb5 2573
sepp_nepp 6:fb11b746ceb5 2574 /* Group PAL Measurement Functions */
sepp_nepp 7:3a1115c2556b 2575 VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement(void)
sepp_nepp 8:2fd7cb217068 2576 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 9:cb4c6d4e5030 2577 VL53L0X_DeviceModes device_mode;
sepp_nepp 6:fb11b746ceb5 2578
sepp_nepp 6:fb11b746ceb5 2579 /* Get Current DeviceMode */
sepp_nepp 7:3a1115c2556b 2580 status = VL53L0X_get_device_mode( &device_mode);
sepp_nepp 6:fb11b746ceb5 2581
sepp_nepp 6:fb11b746ceb5 2582 /* Start immediately to run a single ranging measurement in case of
sepp_nepp 6:fb11b746ceb5 2583 * single ranging or single histogram */
sepp_nepp 9:cb4c6d4e5030 2584 if (status == VL53L0X_ERROR_NONE && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
sepp_nepp 9:cb4c6d4e5030 2585 status = VL53L0X_start_measurement(); }
sepp_nepp 6:fb11b746ceb5 2586
sepp_nepp 6:fb11b746ceb5 2587 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 2588 status = VL53L0X_measurement_poll_for_completion(); }
sepp_nepp 6:fb11b746ceb5 2589
sepp_nepp 6:fb11b746ceb5 2590 /* Change PAL State in case of single ranging or single histogram */
sepp_nepp 7:3a1115c2556b 2591 if (status == VL53L0X_ERROR_NONE && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
sepp_nepp 8:2fd7cb217068 2592 Data.PalState = VL53L0X_STATE_IDLE; }
sepp_nepp 7:3a1115c2556b 2593
sepp_nepp 6:fb11b746ceb5 2594 return status;
sepp_nepp 6:fb11b746ceb5 2595 }
sepp_nepp 6:fb11b746ceb5 2596
sepp_nepp 8:2fd7cb217068 2597 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(uint8_t *p_x_talk_compensation_enable)
sepp_nepp 7:3a1115c2556b 2598 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2599 uint8_t temp8;
sepp_nepp 7:3a1115c2556b 2600
sepp_nepp 8:2fd7cb217068 2601 temp8 = Data.CurrentParameters.XTalkCompensationEnable ;
sepp_nepp 6:fb11b746ceb5 2602 *p_x_talk_compensation_enable = temp8;
sepp_nepp 6:fb11b746ceb5 2603
sepp_nepp 6:fb11b746ceb5 2604 return status;
sepp_nepp 6:fb11b746ceb5 2605 }
sepp_nepp 6:fb11b746ceb5 2606
sepp_nepp 8:2fd7cb217068 2607 VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
sepp_nepp 6:fb11b746ceb5 2608 FixPoint1616_t *p_total_xtalk_rate_mcps)
sepp_nepp 8:2fd7cb217068 2609 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2610
sepp_nepp 6:fb11b746ceb5 2611 uint8_t xtalk_comp_enable;
sepp_nepp 6:fb11b746ceb5 2612 FixPoint1616_t total_xtalk_mega_cps;
sepp_nepp 6:fb11b746ceb5 2613 FixPoint1616_t xtalk_per_spad_mega_cps;
sepp_nepp 6:fb11b746ceb5 2614
sepp_nepp 6:fb11b746ceb5 2615 *p_total_xtalk_rate_mcps = 0;
sepp_nepp 6:fb11b746ceb5 2616
sepp_nepp 7:3a1115c2556b 2617 status = VL53L0X_get_x_talk_compensation_enable( &xtalk_comp_enable);
sepp_nepp 6:fb11b746ceb5 2618 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2619
sepp_nepp 6:fb11b746ceb5 2620 if (xtalk_comp_enable) {
sepp_nepp 6:fb11b746ceb5 2621
sepp_nepp 8:2fd7cb217068 2622 xtalk_per_spad_mega_cps = Data.CurrentParameters.XTalkCompensationRateMegaCps ;
sepp_nepp 6:fb11b746ceb5 2623
sepp_nepp 6:fb11b746ceb5 2624 /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
sepp_nepp 6:fb11b746ceb5 2625 total_xtalk_mega_cps =
sepp_nepp 6:fb11b746ceb5 2626 p_ranging_measurement_data->EffectiveSpadRtnCount *
sepp_nepp 6:fb11b746ceb5 2627 xtalk_per_spad_mega_cps;
sepp_nepp 6:fb11b746ceb5 2628
sepp_nepp 6:fb11b746ceb5 2629 /* FixPoint0824 >> 8 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 2630 *p_total_xtalk_rate_mcps =
sepp_nepp 6:fb11b746ceb5 2631 (total_xtalk_mega_cps + 0x80) >> 8;
sepp_nepp 6:fb11b746ceb5 2632 }
sepp_nepp 6:fb11b746ceb5 2633 }
sepp_nepp 6:fb11b746ceb5 2634 return status;
sepp_nepp 6:fb11b746ceb5 2635 }
sepp_nepp 6:fb11b746ceb5 2636
sepp_nepp 8:2fd7cb217068 2637 VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
sepp_nepp 6:fb11b746ceb5 2638 FixPoint1616_t *p_total_signal_rate_mcps)
sepp_nepp 8:2fd7cb217068 2639 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2640 FixPoint1616_t total_xtalk_mega_cps;
sepp_nepp 6:fb11b746ceb5 2641
sepp_nepp 6:fb11b746ceb5 2642 *p_total_signal_rate_mcps =
sepp_nepp 6:fb11b746ceb5 2643 p_ranging_measurement_data->SignalRateRtnMegaCps;
sepp_nepp 6:fb11b746ceb5 2644
sepp_nepp 7:3a1115c2556b 2645 status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data, &total_xtalk_mega_cps);
sepp_nepp 6:fb11b746ceb5 2646
sepp_nepp 6:fb11b746ceb5 2647 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 2648 *p_total_signal_rate_mcps += total_xtalk_mega_cps; }
sepp_nepp 6:fb11b746ceb5 2649
sepp_nepp 6:fb11b746ceb5 2650 return status;
sepp_nepp 6:fb11b746ceb5 2651 }
sepp_nepp 6:fb11b746ceb5 2652
sepp_nepp 6:fb11b746ceb5 2653 /* To convert ms into register value */
sepp_nepp 8:2fd7cb217068 2654 uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(uint32_t timeout_period_us,
sepp_nepp 6:fb11b746ceb5 2655 uint8_t vcsel_period_pclks)
sepp_nepp 8:2fd7cb217068 2656 { uint32_t macro_period_ps;
sepp_nepp 6:fb11b746ceb5 2657 uint32_t macro_period_ns;
sepp_nepp 6:fb11b746ceb5 2658 uint32_t timeout_period_mclks = 0;
sepp_nepp 6:fb11b746ceb5 2659
sepp_nepp 7:3a1115c2556b 2660 macro_period_ps = VL53L0X_calc_macro_period_ps( vcsel_period_pclks);
sepp_nepp 6:fb11b746ceb5 2661 macro_period_ns = (macro_period_ps + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 2662
sepp_nepp 9:cb4c6d4e5030 2663 timeout_period_mclks = (uint32_t)(((timeout_period_us * 1000)
sepp_nepp 6:fb11b746ceb5 2664 + (macro_period_ns / 2)) / macro_period_ns);
sepp_nepp 6:fb11b746ceb5 2665
sepp_nepp 6:fb11b746ceb5 2666 return timeout_period_mclks;
sepp_nepp 6:fb11b746ceb5 2667 }
sepp_nepp 6:fb11b746ceb5 2668
sepp_nepp 6:fb11b746ceb5 2669 uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
sepp_nepp 9:cb4c6d4e5030 2670 { /* Implements an integer square root
sepp_nepp 9:cb4c6d4e5030 2671 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */
sepp_nepp 6:fb11b746ceb5 2672
sepp_nepp 6:fb11b746ceb5 2673 uint32_t res = 0;
sepp_nepp 6:fb11b746ceb5 2674 uint32_t bit = 1 << 30;
sepp_nepp 9:cb4c6d4e5030 2675 /* The second-to-top bit is set: 1 << 14 for 16-bits, 1 << 30 for 32 bits */
sepp_nepp 6:fb11b746ceb5 2676
sepp_nepp 6:fb11b746ceb5 2677 /* "bit" starts at the highest power of four <= the argument. */
sepp_nepp 9:cb4c6d4e5030 2678 while (bit > num) { bit >>= 2; }
sepp_nepp 6:fb11b746ceb5 2679
sepp_nepp 6:fb11b746ceb5 2680 while (bit != 0) {
sepp_nepp 6:fb11b746ceb5 2681 if (num >= res + bit) {
sepp_nepp 6:fb11b746ceb5 2682 num -= res + bit;
sepp_nepp 6:fb11b746ceb5 2683 res = (res >> 1) + bit;
sepp_nepp 9:cb4c6d4e5030 2684 }
sepp_nepp 9:cb4c6d4e5030 2685 else { res >>= 1; }
sepp_nepp 6:fb11b746ceb5 2686 bit >>= 2;
sepp_nepp 6:fb11b746ceb5 2687 }
sepp_nepp 6:fb11b746ceb5 2688 return res;
sepp_nepp 6:fb11b746ceb5 2689 }
sepp_nepp 6:fb11b746ceb5 2690
sepp_nepp 8:2fd7cb217068 2691 VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(FixPoint1616_t total_signal_rate_mcps,
sepp_nepp 6:fb11b746ceb5 2692 FixPoint1616_t total_corr_signal_rate_mcps,
sepp_nepp 6:fb11b746ceb5 2693 FixPoint1616_t pw_mult,
sepp_nepp 6:fb11b746ceb5 2694 uint32_t sigma_estimate_p1,
sepp_nepp 6:fb11b746ceb5 2695 FixPoint1616_t sigma_estimate_p2,
sepp_nepp 6:fb11b746ceb5 2696 uint32_t peak_vcsel_duration_us,
sepp_nepp 6:fb11b746ceb5 2697 uint32_t *pd_max_mm)
sepp_nepp 8:2fd7cb217068 2698 { const uint32_t c_sigma_limit = 18;
sepp_nepp 6:fb11b746ceb5 2699 const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
sepp_nepp 6:fb11b746ceb5 2700 const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */
sepp_nepp 6:fb11b746ceb5 2701 const uint32_t c_amb_eff_width_sigma_est_ns = 6;
sepp_nepp 6:fb11b746ceb5 2702 const uint32_t c_amb_eff_width_d_max_ns = 7;
sepp_nepp 6:fb11b746ceb5 2703 uint32_t dmax_cal_range_mm;
sepp_nepp 6:fb11b746ceb5 2704 FixPoint1616_t dmax_cal_signal_rate_rtn_mcps;
sepp_nepp 6:fb11b746ceb5 2705 FixPoint1616_t min_signal_needed;
sepp_nepp 6:fb11b746ceb5 2706 FixPoint1616_t min_signal_needed_p1;
sepp_nepp 6:fb11b746ceb5 2707 FixPoint1616_t min_signal_needed_p2;
sepp_nepp 6:fb11b746ceb5 2708 FixPoint1616_t min_signal_needed_p3;
sepp_nepp 6:fb11b746ceb5 2709 FixPoint1616_t min_signal_needed_p4;
sepp_nepp 6:fb11b746ceb5 2710 FixPoint1616_t sigma_limit_tmp;
sepp_nepp 6:fb11b746ceb5 2711 FixPoint1616_t sigma_est_sq_tmp;
sepp_nepp 6:fb11b746ceb5 2712 FixPoint1616_t signal_limit_tmp;
sepp_nepp 6:fb11b746ceb5 2713 FixPoint1616_t signal_at0_mm;
sepp_nepp 6:fb11b746ceb5 2714 FixPoint1616_t dmax_dark;
sepp_nepp 6:fb11b746ceb5 2715 FixPoint1616_t dmax_ambient;
sepp_nepp 6:fb11b746ceb5 2716 FixPoint1616_t dmax_dark_tmp;
sepp_nepp 6:fb11b746ceb5 2717 FixPoint1616_t sigma_est_p2_tmp;
sepp_nepp 6:fb11b746ceb5 2718 uint32_t signal_rate_temp_mcps;
sepp_nepp 6:fb11b746ceb5 2719
sepp_nepp 6:fb11b746ceb5 2720 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2721
sepp_nepp 9:cb4c6d4e5030 2722 dmax_cal_range_mm = Data.DmaxCalRangeMilliMeter;
sepp_nepp 9:cb4c6d4e5030 2723
sepp_nepp 9:cb4c6d4e5030 2724 dmax_cal_signal_rate_rtn_mcps = Data.DmaxCalSignalRateRtnMegaCps;
sepp_nepp 6:fb11b746ceb5 2725
sepp_nepp 6:fb11b746ceb5 2726 /* uint32 * FixPoint1616 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 2727 signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
sepp_nepp 6:fb11b746ceb5 2728
sepp_nepp 6:fb11b746ceb5 2729 /* FixPoint1616 >> 8 = FixPoint2408 */
sepp_nepp 6:fb11b746ceb5 2730 signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
sepp_nepp 6:fb11b746ceb5 2731 signal_at0_mm *= dmax_cal_range_mm;
sepp_nepp 6:fb11b746ceb5 2732
sepp_nepp 6:fb11b746ceb5 2733 min_signal_needed_p1 = 0;
sepp_nepp 6:fb11b746ceb5 2734 if (total_corr_signal_rate_mcps > 0) {
sepp_nepp 6:fb11b746ceb5 2735
sepp_nepp 9:cb4c6d4e5030 2736 /* Shift by 10 bits to increase resolution prior to the division */
sepp_nepp 6:fb11b746ceb5 2737 signal_rate_temp_mcps = total_signal_rate_mcps << 10;
sepp_nepp 6:fb11b746ceb5 2738
sepp_nepp 6:fb11b746ceb5 2739 /* Add rounding value prior to division */
sepp_nepp 6:fb11b746ceb5 2740 min_signal_needed_p1 = signal_rate_temp_mcps +
sepp_nepp 6:fb11b746ceb5 2741 (total_corr_signal_rate_mcps / 2);
sepp_nepp 6:fb11b746ceb5 2742
sepp_nepp 6:fb11b746ceb5 2743 /* FixPoint0626/FixPoint1616 = FixPoint2210 */
sepp_nepp 6:fb11b746ceb5 2744 min_signal_needed_p1 /= total_corr_signal_rate_mcps;
sepp_nepp 6:fb11b746ceb5 2745
sepp_nepp 6:fb11b746ceb5 2746 /* Apply a factored version of the speed of light.
sepp_nepp 6:fb11b746ceb5 2747 Correction to be applied at the end */
sepp_nepp 6:fb11b746ceb5 2748 min_signal_needed_p1 *= 3;
sepp_nepp 6:fb11b746ceb5 2749
sepp_nepp 6:fb11b746ceb5 2750 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
sepp_nepp 6:fb11b746ceb5 2751 min_signal_needed_p1 *= min_signal_needed_p1;
sepp_nepp 6:fb11b746ceb5 2752
sepp_nepp 6:fb11b746ceb5 2753 /* FixPoint1220 >> 16 = FixPoint2804 */
sepp_nepp 6:fb11b746ceb5 2754 min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 2755 }
sepp_nepp 6:fb11b746ceb5 2756 min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
sepp_nepp 6:fb11b746ceb5 2757
sepp_nepp 6:fb11b746ceb5 2758 /* FixPoint1616 >> 16 = uint32 */
sepp_nepp 6:fb11b746ceb5 2759 min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 2760
sepp_nepp 6:fb11b746ceb5 2761 /* uint32 * uint32 = uint32 */
sepp_nepp 6:fb11b746ceb5 2762 min_signal_needed_p2 *= min_signal_needed_p2;
sepp_nepp 6:fb11b746ceb5 2763
sepp_nepp 6:fb11b746ceb5 2764 /* Check sigmaEstimateP2
sepp_nepp 6:fb11b746ceb5 2765 * If this value is too high there is not enough signal rate
sepp_nepp 6:fb11b746ceb5 2766 * to calculate dmax value so set a suitable value to ensure
sepp_nepp 9:cb4c6d4e5030 2767 * a very small dmax. */
sepp_nepp 6:fb11b746ceb5 2768 sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 2769 sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
sepp_nepp 6:fb11b746ceb5 2770 c_amb_eff_width_sigma_est_ns;
sepp_nepp 6:fb11b746ceb5 2771 sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
sepp_nepp 6:fb11b746ceb5 2772
sepp_nepp 6:fb11b746ceb5 2773 if (sigma_est_p2_tmp > 0xffff) {
sepp_nepp 6:fb11b746ceb5 2774 min_signal_needed_p3 = 0xfff00000;
sepp_nepp 6:fb11b746ceb5 2775 } else {
sepp_nepp 9:cb4c6d4e5030 2776 /* DMAX uses a different ambient width from sigma, so apply correction.
sepp_nepp 9:cb4c6d4e5030 2777 * Perform division before multiplication to prevent overflow. */
sepp_nepp 6:fb11b746ceb5 2778 sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
sepp_nepp 6:fb11b746ceb5 2779 c_amb_eff_width_sigma_est_ns;
sepp_nepp 6:fb11b746ceb5 2780 sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
sepp_nepp 6:fb11b746ceb5 2781
sepp_nepp 6:fb11b746ceb5 2782 /* FixPoint1616 >> 16 = uint32 */
sepp_nepp 6:fb11b746ceb5 2783 min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 2784 min_signal_needed_p3 *= min_signal_needed_p3;
sepp_nepp 6:fb11b746ceb5 2785 }
sepp_nepp 6:fb11b746ceb5 2786
sepp_nepp 6:fb11b746ceb5 2787 /* FixPoint1814 / uint32 = FixPoint1814 */
sepp_nepp 6:fb11b746ceb5 2788 sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 2789
sepp_nepp 6:fb11b746ceb5 2790 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
sepp_nepp 6:fb11b746ceb5 2791 sigma_limit_tmp *= sigma_limit_tmp;
sepp_nepp 6:fb11b746ceb5 2792
sepp_nepp 6:fb11b746ceb5 2793 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp 6:fb11b746ceb5 2794 sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
sepp_nepp 6:fb11b746ceb5 2795
sepp_nepp 6:fb11b746ceb5 2796 /* FixPoint3232 >> 4 = FixPoint0428 */
sepp_nepp 6:fb11b746ceb5 2797 sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
sepp_nepp 6:fb11b746ceb5 2798
sepp_nepp 6:fb11b746ceb5 2799 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */
sepp_nepp 6:fb11b746ceb5 2800 sigma_limit_tmp -= sigma_est_sq_tmp;
sepp_nepp 6:fb11b746ceb5 2801
sepp_nepp 6:fb11b746ceb5 2802 /* uint32_t * FixPoint0428 = FixPoint0428 */
sepp_nepp 6:fb11b746ceb5 2803 min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
sepp_nepp 6:fb11b746ceb5 2804
sepp_nepp 6:fb11b746ceb5 2805 /* FixPoint0428 >> 14 = FixPoint1814 */
sepp_nepp 6:fb11b746ceb5 2806 min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14;
sepp_nepp 6:fb11b746ceb5 2807
sepp_nepp 6:fb11b746ceb5 2808 /* uint32 + uint32 = uint32 */
sepp_nepp 6:fb11b746ceb5 2809 min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3);
sepp_nepp 6:fb11b746ceb5 2810
sepp_nepp 6:fb11b746ceb5 2811 /* uint32 / uint32 = uint32 */
sepp_nepp 6:fb11b746ceb5 2812 min_signal_needed += (peak_vcsel_duration_us / 2);
sepp_nepp 6:fb11b746ceb5 2813 min_signal_needed /= peak_vcsel_duration_us;
sepp_nepp 6:fb11b746ceb5 2814
sepp_nepp 6:fb11b746ceb5 2815 /* uint32 << 14 = FixPoint1814 */
sepp_nepp 6:fb11b746ceb5 2816 min_signal_needed <<= 14;
sepp_nepp 6:fb11b746ceb5 2817
sepp_nepp 6:fb11b746ceb5 2818 /* FixPoint1814 / FixPoint1814 = uint32 */
sepp_nepp 6:fb11b746ceb5 2819 min_signal_needed += (min_signal_needed_p4 / 2);
sepp_nepp 6:fb11b746ceb5 2820 min_signal_needed /= min_signal_needed_p4;
sepp_nepp 6:fb11b746ceb5 2821
sepp_nepp 6:fb11b746ceb5 2822 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
sepp_nepp 6:fb11b746ceb5 2823 min_signal_needed *= min_signal_needed_p1;
sepp_nepp 6:fb11b746ceb5 2824
sepp_nepp 6:fb11b746ceb5 2825 /* Apply correction by dividing by 1000000.
sepp_nepp 6:fb11b746ceb5 2826 * This assumes 10E16 on the numerator of the equation
sepp_nepp 6:fb11b746ceb5 2827 * and 10E-22 on the denominator.
sepp_nepp 6:fb11b746ceb5 2828 * We do this because 32bit fix point calculation can't
sepp_nepp 6:fb11b746ceb5 2829 * handle the larger and smaller elements of this equation,
sepp_nepp 6:fb11b746ceb5 2830 * i.e. speed of light and pulse widths.
sepp_nepp 6:fb11b746ceb5 2831 */
sepp_nepp 6:fb11b746ceb5 2832 min_signal_needed = (min_signal_needed + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 2833 min_signal_needed <<= 4;
sepp_nepp 6:fb11b746ceb5 2834
sepp_nepp 6:fb11b746ceb5 2835 min_signal_needed = (min_signal_needed + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 2836
sepp_nepp 6:fb11b746ceb5 2837 /* FixPoint1616 >> 8 = FixPoint2408 */
sepp_nepp 6:fb11b746ceb5 2838 signal_limit_tmp = (c_signal_limit + 0x80) >> 8;
sepp_nepp 6:fb11b746ceb5 2839
sepp_nepp 6:fb11b746ceb5 2840 /* FixPoint2408/FixPoint2408 = uint32 */
sepp_nepp 6:fb11b746ceb5 2841 if (signal_limit_tmp != 0) {
sepp_nepp 6:fb11b746ceb5 2842 dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
sepp_nepp 6:fb11b746ceb5 2843 / signal_limit_tmp;
sepp_nepp 9:cb4c6d4e5030 2844 } else { dmax_dark_tmp = 0; }
sepp_nepp 6:fb11b746ceb5 2845
sepp_nepp 6:fb11b746ceb5 2846 dmax_dark = VL53L0X_isqrt(dmax_dark_tmp);
sepp_nepp 6:fb11b746ceb5 2847
sepp_nepp 6:fb11b746ceb5 2848 /* FixPoint2408/FixPoint2408 = uint32 */
sepp_nepp 6:fb11b746ceb5 2849 if (min_signal_needed != 0) {
sepp_nepp 6:fb11b746ceb5 2850 dmax_ambient = (signal_at0_mm + min_signal_needed / 2)
sepp_nepp 6:fb11b746ceb5 2851 / min_signal_needed;
sepp_nepp 9:cb4c6d4e5030 2852 } else { dmax_ambient = 0; }
sepp_nepp 6:fb11b746ceb5 2853
sepp_nepp 6:fb11b746ceb5 2854 dmax_ambient = VL53L0X_isqrt(dmax_ambient);
sepp_nepp 6:fb11b746ceb5 2855
sepp_nepp 6:fb11b746ceb5 2856 *pd_max_mm = dmax_dark;
sepp_nepp 9:cb4c6d4e5030 2857 if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; }
sepp_nepp 6:fb11b746ceb5 2858
sepp_nepp 6:fb11b746ceb5 2859 return status;
sepp_nepp 6:fb11b746ceb5 2860 }
sepp_nepp 6:fb11b746ceb5 2861
sepp_nepp 8:2fd7cb217068 2862 VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
sepp_nepp 6:fb11b746ceb5 2863 FixPoint1616_t *p_sigma_estimate,
sepp_nepp 6:fb11b746ceb5 2864 uint32_t *p_dmax_mm)
sepp_nepp 8:2fd7cb217068 2865 { /* Expressed in 100ths of a ns, i.e. centi-ns */
sepp_nepp 6:fb11b746ceb5 2866 const uint32_t c_pulse_effective_width_centi_ns = 800;
sepp_nepp 6:fb11b746ceb5 2867 /* Expressed in 100ths of a ns, i.e. centi-ns */
sepp_nepp 6:fb11b746ceb5 2868 const uint32_t c_ambient_effective_width_centi_ns = 600;
sepp_nepp 6:fb11b746ceb5 2869 const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
sepp_nepp 6:fb11b746ceb5 2870 const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
sepp_nepp 6:fb11b746ceb5 2871 const FixPoint1616_t c_sigma_est_max = 0x028F87AE;
sepp_nepp 6:fb11b746ceb5 2872 const FixPoint1616_t c_sigma_est_rtn_max = 0xF000;
sepp_nepp 6:fb11b746ceb5 2873 const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
sepp_nepp 6:fb11b746ceb5 2874 c_ambient_effective_width_centi_ns;
sepp_nepp 6:fb11b746ceb5 2875 /* Time Of Flight per mm (6.6 pico secs) */
sepp_nepp 6:fb11b746ceb5 2876 const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A;
sepp_nepp 6:fb11b746ceb5 2877 const uint32_t c_16bit_rounding_param = 0x00008000;
sepp_nepp 6:fb11b746ceb5 2878 const FixPoint1616_t c_max_x_talk_kcps = 0x00320000;
sepp_nepp 6:fb11b746ceb5 2879 const uint32_t c_pll_period_ps = 1655;
sepp_nepp 6:fb11b746ceb5 2880
sepp_nepp 6:fb11b746ceb5 2881 uint32_t vcsel_total_events_rtn;
sepp_nepp 6:fb11b746ceb5 2882 uint32_t final_range_timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 2883 uint32_t pre_range_timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 2884 uint32_t final_range_integration_time_milli_secs;
sepp_nepp 6:fb11b746ceb5 2885 FixPoint1616_t sigma_estimate_p1;
sepp_nepp 6:fb11b746ceb5 2886 FixPoint1616_t sigma_estimate_p2;
sepp_nepp 6:fb11b746ceb5 2887 FixPoint1616_t sigma_estimate_p3;
sepp_nepp 6:fb11b746ceb5 2888 FixPoint1616_t delta_t_ps;
sepp_nepp 6:fb11b746ceb5 2889 FixPoint1616_t pw_mult;
sepp_nepp 6:fb11b746ceb5 2890 FixPoint1616_t sigma_est_rtn;
sepp_nepp 6:fb11b746ceb5 2891 FixPoint1616_t sigma_estimate;
sepp_nepp 6:fb11b746ceb5 2892 FixPoint1616_t x_talk_correction;
sepp_nepp 6:fb11b746ceb5 2893 FixPoint1616_t ambient_rate_kcps;
sepp_nepp 6:fb11b746ceb5 2894 FixPoint1616_t peak_signal_rate_kcps;
sepp_nepp 6:fb11b746ceb5 2895 FixPoint1616_t x_talk_comp_rate_mcps;
sepp_nepp 6:fb11b746ceb5 2896 uint32_t x_talk_comp_rate_kcps;
sepp_nepp 6:fb11b746ceb5 2897 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 2898 FixPoint1616_t diff1_mcps;
sepp_nepp 6:fb11b746ceb5 2899 FixPoint1616_t diff2_mcps;
sepp_nepp 6:fb11b746ceb5 2900 FixPoint1616_t sqr1;
sepp_nepp 6:fb11b746ceb5 2901 FixPoint1616_t sqr2;
sepp_nepp 6:fb11b746ceb5 2902 FixPoint1616_t sqr_sum;
sepp_nepp 6:fb11b746ceb5 2903 FixPoint1616_t sqrt_result_centi_ns;
sepp_nepp 6:fb11b746ceb5 2904 FixPoint1616_t sqrt_result;
sepp_nepp 6:fb11b746ceb5 2905 FixPoint1616_t total_signal_rate_mcps;
sepp_nepp 6:fb11b746ceb5 2906 FixPoint1616_t corrected_signal_rate_mcps;
sepp_nepp 6:fb11b746ceb5 2907 FixPoint1616_t sigma_est_ref;
sepp_nepp 6:fb11b746ceb5 2908 uint32_t vcsel_width;
sepp_nepp 6:fb11b746ceb5 2909 uint32_t final_range_macro_pclks;
sepp_nepp 6:fb11b746ceb5 2910 uint32_t pre_range_macro_pclks;
sepp_nepp 6:fb11b746ceb5 2911 uint32_t peak_vcsel_duration_us;
sepp_nepp 6:fb11b746ceb5 2912 uint8_t final_range_vcsel_pclks;
sepp_nepp 6:fb11b746ceb5 2913 uint8_t pre_range_vcsel_pclks;
sepp_nepp 6:fb11b746ceb5 2914 /*! \addtogroup calc_sigma_estimate
sepp_nepp 6:fb11b746ceb5 2915 * @{
sepp_nepp 6:fb11b746ceb5 2916 *
sepp_nepp 6:fb11b746ceb5 2917 * Estimates the range sigma
sepp_nepp 6:fb11b746ceb5 2918 */
sepp_nepp 6:fb11b746ceb5 2919
sepp_nepp 8:2fd7cb217068 2920 x_talk_comp_rate_mcps = Data.CurrentParameters.XTalkCompensationRateMegaCps ;
sepp_nepp 6:fb11b746ceb5 2921
sepp_nepp 6:fb11b746ceb5 2922 /*
sepp_nepp 6:fb11b746ceb5 2923 * We work in kcps rather than mcps as this helps keep within the
sepp_nepp 6:fb11b746ceb5 2924 * confines of the 32 Fix1616 type.
sepp_nepp 6:fb11b746ceb5 2925 */
sepp_nepp 6:fb11b746ceb5 2926
sepp_nepp 6:fb11b746ceb5 2927 ambient_rate_kcps =
sepp_nepp 6:fb11b746ceb5 2928 (p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16;
sepp_nepp 6:fb11b746ceb5 2929
sepp_nepp 6:fb11b746ceb5 2930 corrected_signal_rate_mcps =
sepp_nepp 6:fb11b746ceb5 2931 p_ranging_measurement_data->SignalRateRtnMegaCps;
sepp_nepp 6:fb11b746ceb5 2932
sepp_nepp 7:3a1115c2556b 2933 status = VL53L0X_get_total_signal_rate(p_ranging_measurement_data, &total_signal_rate_mcps);
sepp_nepp 7:3a1115c2556b 2934 status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data, &x_talk_comp_rate_mcps);
sepp_nepp 6:fb11b746ceb5 2935
sepp_nepp 6:fb11b746ceb5 2936 /* Signal rate measurement provided by device is the
sepp_nepp 6:fb11b746ceb5 2937 * peak signal rate, not average.
sepp_nepp 6:fb11b746ceb5 2938 */
sepp_nepp 6:fb11b746ceb5 2939 peak_signal_rate_kcps = (total_signal_rate_mcps * 1000);
sepp_nepp 6:fb11b746ceb5 2940 peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 2941
sepp_nepp 6:fb11b746ceb5 2942 x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
sepp_nepp 6:fb11b746ceb5 2943
sepp_nepp 6:fb11b746ceb5 2944 if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) {
sepp_nepp 6:fb11b746ceb5 2945 x_talk_comp_rate_kcps = c_max_x_talk_kcps;
sepp_nepp 6:fb11b746ceb5 2946 }
sepp_nepp 6:fb11b746ceb5 2947
sepp_nepp 6:fb11b746ceb5 2948 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2949
sepp_nepp 6:fb11b746ceb5 2950 /* Calculate final range macro periods */
sepp_nepp 8:2fd7cb217068 2951 final_range_timeout_micro_secs = Data.FinalRangeTimeoutMicroSecs;
sepp_nepp 8:2fd7cb217068 2952 final_range_vcsel_pclks = Data.FinalRangeVcselPulsePeriod;
sepp_nepp 7:3a1115c2556b 2953 final_range_macro_pclks = VL53L0X_calc_timeout_mclks( final_range_timeout_micro_secs, final_range_vcsel_pclks);
sepp_nepp 6:fb11b746ceb5 2954
sepp_nepp 6:fb11b746ceb5 2955 /* Calculate pre-range macro periods */
sepp_nepp 8:2fd7cb217068 2956 pre_range_timeout_micro_secs = Data.PreRangeTimeoutMicroSecs;
sepp_nepp 8:2fd7cb217068 2957 pre_range_vcsel_pclks = Data.PreRangeVcselPulsePeriod;
sepp_nepp 7:3a1115c2556b 2958
sepp_nepp 7:3a1115c2556b 2959 pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(pre_range_timeout_micro_secs, pre_range_vcsel_pclks);
sepp_nepp 6:fb11b746ceb5 2960
sepp_nepp 6:fb11b746ceb5 2961 vcsel_width = 3;
sepp_nepp 6:fb11b746ceb5 2962 if (final_range_vcsel_pclks == 8) {
sepp_nepp 6:fb11b746ceb5 2963 vcsel_width = 2;
sepp_nepp 6:fb11b746ceb5 2964 }
sepp_nepp 6:fb11b746ceb5 2965
sepp_nepp 6:fb11b746ceb5 2966 peak_vcsel_duration_us = vcsel_width * 2048 *
sepp_nepp 6:fb11b746ceb5 2967 (pre_range_macro_pclks + final_range_macro_pclks);
sepp_nepp 6:fb11b746ceb5 2968 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 2969 peak_vcsel_duration_us *= c_pll_period_ps;
sepp_nepp 6:fb11b746ceb5 2970 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 2971
sepp_nepp 6:fb11b746ceb5 2972 /* Fix1616 >> 8 = Fix2408 */
sepp_nepp 6:fb11b746ceb5 2973 total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8;
sepp_nepp 6:fb11b746ceb5 2974
sepp_nepp 6:fb11b746ceb5 2975 /* Fix2408 * uint32 = Fix2408 */
sepp_nepp 6:fb11b746ceb5 2976 vcsel_total_events_rtn = total_signal_rate_mcps *
sepp_nepp 6:fb11b746ceb5 2977 peak_vcsel_duration_us;
sepp_nepp 6:fb11b746ceb5 2978
sepp_nepp 6:fb11b746ceb5 2979 /* Fix2408 >> 8 = uint32 */
sepp_nepp 6:fb11b746ceb5 2980 vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
sepp_nepp 6:fb11b746ceb5 2981
sepp_nepp 6:fb11b746ceb5 2982 /* Fix2408 << 8 = Fix1616 = */
sepp_nepp 6:fb11b746ceb5 2983 total_signal_rate_mcps <<= 8;
sepp_nepp 6:fb11b746ceb5 2984 }
sepp_nepp 6:fb11b746ceb5 2985
sepp_nepp 6:fb11b746ceb5 2986 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 2987
sepp_nepp 6:fb11b746ceb5 2988 return status;
sepp_nepp 6:fb11b746ceb5 2989 }
sepp_nepp 6:fb11b746ceb5 2990
sepp_nepp 6:fb11b746ceb5 2991 if (peak_signal_rate_kcps == 0) {
sepp_nepp 6:fb11b746ceb5 2992 *p_sigma_estimate = c_sigma_est_max;
sepp_nepp 8:2fd7cb217068 2993 Data.SigmaEstimate = c_sigma_est_max;
sepp_nepp 6:fb11b746ceb5 2994 *p_dmax_mm = 0;
sepp_nepp 6:fb11b746ceb5 2995 } else {
sepp_nepp 6:fb11b746ceb5 2996 if (vcsel_total_events_rtn < 1) {
sepp_nepp 6:fb11b746ceb5 2997 vcsel_total_events_rtn = 1;
sepp_nepp 6:fb11b746ceb5 2998 }
sepp_nepp 6:fb11b746ceb5 2999
sepp_nepp 6:fb11b746ceb5 3000 sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
sepp_nepp 6:fb11b746ceb5 3001
sepp_nepp 6:fb11b746ceb5 3002 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3003 sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps;
sepp_nepp 6:fb11b746ceb5 3004 if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) {
sepp_nepp 6:fb11b746ceb5 3005 /* Clip to prevent overflow. Will ensure safe
sepp_nepp 6:fb11b746ceb5 3006 * max result. */
sepp_nepp 6:fb11b746ceb5 3007 sigma_estimate_p2 = c_amb_to_signal_ratio_max;
sepp_nepp 6:fb11b746ceb5 3008 }
sepp_nepp 6:fb11b746ceb5 3009 sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
sepp_nepp 6:fb11b746ceb5 3010
sepp_nepp 6:fb11b746ceb5 3011 sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12);
sepp_nepp 6:fb11b746ceb5 3012
sepp_nepp 6:fb11b746ceb5 3013 /* uint32 * FixPoint1616 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3014 delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
sepp_nepp 6:fb11b746ceb5 3015 c_tof_per_mm_ps;
sepp_nepp 6:fb11b746ceb5 3016
sepp_nepp 6:fb11b746ceb5 3017 /*
sepp_nepp 6:fb11b746ceb5 3018 * vcselRate - xtalkCompRate
sepp_nepp 6:fb11b746ceb5 3019 * (uint32 << 16) - FixPoint1616 = FixPoint1616.
sepp_nepp 6:fb11b746ceb5 3020 * Divide result by 1000 to convert to mcps.
sepp_nepp 6:fb11b746ceb5 3021 * 500 is added to ensure rounding when integer division
sepp_nepp 6:fb11b746ceb5 3022 * truncates.
sepp_nepp 6:fb11b746ceb5 3023 */
sepp_nepp 6:fb11b746ceb5 3024 diff1_mcps = (((peak_signal_rate_kcps << 16) -
sepp_nepp 6:fb11b746ceb5 3025 2 * x_talk_comp_rate_kcps) + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 3026
sepp_nepp 6:fb11b746ceb5 3027 /* vcselRate + xtalkCompRate */
sepp_nepp 6:fb11b746ceb5 3028 diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 3029
sepp_nepp 6:fb11b746ceb5 3030 /* Shift by 8 bits to increase resolution prior to the
sepp_nepp 6:fb11b746ceb5 3031 * division */
sepp_nepp 6:fb11b746ceb5 3032 diff1_mcps <<= 8;
sepp_nepp 6:fb11b746ceb5 3033
sepp_nepp 6:fb11b746ceb5 3034 /* FixPoint0824/FixPoint1616 = FixPoint2408 */
sepp_nepp 6:fb11b746ceb5 3035 // xTalkCorrection = abs(diff1_mcps/diff2_mcps);
sepp_nepp 6:fb11b746ceb5 3036 // abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
sepp_nepp 6:fb11b746ceb5 3037 x_talk_correction = diff1_mcps / diff2_mcps;
sepp_nepp 6:fb11b746ceb5 3038
sepp_nepp 6:fb11b746ceb5 3039 /* FixPoint2408 << 8 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3040 x_talk_correction <<= 8;
sepp_nepp 6:fb11b746ceb5 3041
sepp_nepp 6:fb11b746ceb5 3042 if (p_ranging_measurement_data->RangeStatus != 0) {
sepp_nepp 6:fb11b746ceb5 3043 pw_mult = 1 << 16;
sepp_nepp 6:fb11b746ceb5 3044 } else {
sepp_nepp 6:fb11b746ceb5 3045 /* FixPoint1616/uint32 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3046 pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
sepp_nepp 6:fb11b746ceb5 3047
sepp_nepp 6:fb11b746ceb5 3048 /*
sepp_nepp 6:fb11b746ceb5 3049 * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
sepp_nepp 6:fb11b746ceb5 3050 * values are small enough such that32 bits will not be
sepp_nepp 6:fb11b746ceb5 3051 * exceeded.
sepp_nepp 6:fb11b746ceb5 3052 */
sepp_nepp 6:fb11b746ceb5 3053 pw_mult *= ((1 << 16) - x_talk_correction);
sepp_nepp 6:fb11b746ceb5 3054
sepp_nepp 6:fb11b746ceb5 3055 /* (FixPoint3232 >> 16) = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3056 pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
sepp_nepp 6:fb11b746ceb5 3057
sepp_nepp 6:fb11b746ceb5 3058 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3059 pw_mult += (1 << 16);
sepp_nepp 6:fb11b746ceb5 3060
sepp_nepp 6:fb11b746ceb5 3061 /*
sepp_nepp 6:fb11b746ceb5 3062 * At this point the value will be 1.xx, therefore if we square
sepp_nepp 6:fb11b746ceb5 3063 * the value this will exceed 32 bits. To address this perform
sepp_nepp 6:fb11b746ceb5 3064 * a single shift to the right before the multiplication.
sepp_nepp 6:fb11b746ceb5 3065 */
sepp_nepp 6:fb11b746ceb5 3066 pw_mult >>= 1;
sepp_nepp 6:fb11b746ceb5 3067 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
sepp_nepp 6:fb11b746ceb5 3068 pw_mult = pw_mult * pw_mult;
sepp_nepp 6:fb11b746ceb5 3069
sepp_nepp 6:fb11b746ceb5 3070 /* (FixPoint3430 >> 14) = Fix1616 */
sepp_nepp 6:fb11b746ceb5 3071 pw_mult >>= 14;
sepp_nepp 6:fb11b746ceb5 3072 }
sepp_nepp 6:fb11b746ceb5 3073
sepp_nepp 6:fb11b746ceb5 3074 /* FixPoint1616 * uint32 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3075 sqr1 = pw_mult * sigma_estimate_p1;
sepp_nepp 6:fb11b746ceb5 3076
sepp_nepp 6:fb11b746ceb5 3077 /* (FixPoint1616 >> 16) = FixPoint3200 */
sepp_nepp 6:fb11b746ceb5 3078 sqr1 = (sqr1 + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 3079
sepp_nepp 6:fb11b746ceb5 3080 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sepp_nepp 6:fb11b746ceb5 3081 sqr1 *= sqr1;
sepp_nepp 6:fb11b746ceb5 3082
sepp_nepp 6:fb11b746ceb5 3083 sqr2 = sigma_estimate_p2;
sepp_nepp 6:fb11b746ceb5 3084
sepp_nepp 6:fb11b746ceb5 3085 /* (FixPoint1616 >> 16) = FixPoint3200 */
sepp_nepp 6:fb11b746ceb5 3086 sqr2 = (sqr2 + 0x8000) >> 16;
sepp_nepp 6:fb11b746ceb5 3087
sepp_nepp 6:fb11b746ceb5 3088 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sepp_nepp 6:fb11b746ceb5 3089 sqr2 *= sqr2;
sepp_nepp 6:fb11b746ceb5 3090
sepp_nepp 6:fb11b746ceb5 3091 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
sepp_nepp 6:fb11b746ceb5 3092 sqr_sum = sqr1 + sqr2;
sepp_nepp 6:fb11b746ceb5 3093
sepp_nepp 6:fb11b746ceb5 3094 /* SQRT(FixPoin6400) = FixPoint3200 */
sepp_nepp 6:fb11b746ceb5 3095 sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum);
sepp_nepp 6:fb11b746ceb5 3096
sepp_nepp 6:fb11b746ceb5 3097 /* (FixPoint3200 << 16) = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3098 sqrt_result_centi_ns <<= 16;
sepp_nepp 6:fb11b746ceb5 3099
sepp_nepp 6:fb11b746ceb5 3100 /*
sepp_nepp 6:fb11b746ceb5 3101 * Note that the Speed Of Light is expressed in um per 1E-10
sepp_nepp 6:fb11b746ceb5 3102 * seconds (2997) Therefore to get mm/ns we have to divide by
sepp_nepp 6:fb11b746ceb5 3103 * 10000
sepp_nepp 6:fb11b746ceb5 3104 */
sepp_nepp 6:fb11b746ceb5 3105 sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
sepp_nepp 6:fb11b746ceb5 3106 sigma_estimate_p3);
sepp_nepp 6:fb11b746ceb5 3107 sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
sepp_nepp 6:fb11b746ceb5 3108
sepp_nepp 6:fb11b746ceb5 3109 /* Add 5000 before dividing by 10000 to ensure rounding. */
sepp_nepp 6:fb11b746ceb5 3110 sigma_est_rtn += 5000;
sepp_nepp 6:fb11b746ceb5 3111 sigma_est_rtn /= 10000;
sepp_nepp 6:fb11b746ceb5 3112
sepp_nepp 6:fb11b746ceb5 3113 if (sigma_est_rtn > c_sigma_est_rtn_max) {
sepp_nepp 6:fb11b746ceb5 3114 /* Clip to prevent overflow. Will ensure safe
sepp_nepp 6:fb11b746ceb5 3115 * max result. */
sepp_nepp 6:fb11b746ceb5 3116 sigma_est_rtn = c_sigma_est_rtn_max;
sepp_nepp 6:fb11b746ceb5 3117 }
sepp_nepp 6:fb11b746ceb5 3118 final_range_integration_time_milli_secs =
sepp_nepp 6:fb11b746ceb5 3119 (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 3120
sepp_nepp 6:fb11b746ceb5 3121 /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
sepp_nepp 6:fb11b746ceb5 3122 * sqrt(FixPoint1616/int) = FixPoint2408)
sepp_nepp 6:fb11b746ceb5 3123 */
sepp_nepp 6:fb11b746ceb5 3124 sigma_est_ref =
sepp_nepp 6:fb11b746ceb5 3125 VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs +
sepp_nepp 6:fb11b746ceb5 3126 final_range_integration_time_milli_secs / 2) /
sepp_nepp 6:fb11b746ceb5 3127 final_range_integration_time_milli_secs);
sepp_nepp 6:fb11b746ceb5 3128
sepp_nepp 6:fb11b746ceb5 3129 /* FixPoint2408 << 8 = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3130 sigma_est_ref <<= 8;
sepp_nepp 6:fb11b746ceb5 3131 sigma_est_ref = (sigma_est_ref + 500) / 1000;
sepp_nepp 6:fb11b746ceb5 3132
sepp_nepp 6:fb11b746ceb5 3133 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp 6:fb11b746ceb5 3134 sqr1 = sigma_est_rtn * sigma_est_rtn;
sepp_nepp 6:fb11b746ceb5 3135 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp 6:fb11b746ceb5 3136 sqr2 = sigma_est_ref * sigma_est_ref;
sepp_nepp 6:fb11b746ceb5 3137
sepp_nepp 6:fb11b746ceb5 3138 /* sqrt(FixPoint3232) = FixPoint1616 */
sepp_nepp 6:fb11b746ceb5 3139 sqrt_result = VL53L0X_isqrt((sqr1 + sqr2));
sepp_nepp 6:fb11b746ceb5 3140 /*
sepp_nepp 6:fb11b746ceb5 3141 * Note that the Shift by 4 bits increases resolution prior to
sepp_nepp 6:fb11b746ceb5 3142 * the sqrt, therefore the result must be shifted by 2 bits to
sepp_nepp 6:fb11b746ceb5 3143 * the right to revert back to the FixPoint1616 format.
sepp_nepp 6:fb11b746ceb5 3144 */
sepp_nepp 6:fb11b746ceb5 3145
sepp_nepp 6:fb11b746ceb5 3146 sigma_estimate = 1000 * sqrt_result;
sepp_nepp 6:fb11b746ceb5 3147
sepp_nepp 6:fb11b746ceb5 3148 if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
sepp_nepp 6:fb11b746ceb5 3149 (sigma_estimate > c_sigma_est_max)) {
sepp_nepp 6:fb11b746ceb5 3150 sigma_estimate = c_sigma_est_max;
sepp_nepp 6:fb11b746ceb5 3151 }
sepp_nepp 6:fb11b746ceb5 3152
sepp_nepp 6:fb11b746ceb5 3153 *p_sigma_estimate = (uint32_t)(sigma_estimate);
sepp_nepp 8:2fd7cb217068 3154 Data.SigmaEstimate = *p_sigma_estimate;
sepp_nepp 7:3a1115c2556b 3155 status = VL53L0X_calc_dmax(total_signal_rate_mcps,
sepp_nepp 6:fb11b746ceb5 3156 corrected_signal_rate_mcps,
sepp_nepp 6:fb11b746ceb5 3157 pw_mult,
sepp_nepp 6:fb11b746ceb5 3158 sigma_estimate_p1,
sepp_nepp 6:fb11b746ceb5 3159 sigma_estimate_p2,
sepp_nepp 6:fb11b746ceb5 3160 peak_vcsel_duration_us,
sepp_nepp 6:fb11b746ceb5 3161 p_dmax_mm);
sepp_nepp 6:fb11b746ceb5 3162 }
sepp_nepp 6:fb11b746ceb5 3163
sepp_nepp 6:fb11b746ceb5 3164 return status;
sepp_nepp 6:fb11b746ceb5 3165 }
sepp_nepp 6:fb11b746ceb5 3166
sepp_nepp 8:2fd7cb217068 3167 VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(uint8_t device_range_status,
sepp_nepp 6:fb11b746ceb5 3168 FixPoint1616_t signal_rate,
sepp_nepp 6:fb11b746ceb5 3169 uint16_t effective_spad_rtn_count,
sepp_nepp 6:fb11b746ceb5 3170 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
sepp_nepp 6:fb11b746ceb5 3171 uint8_t *p_pal_range_status)
sepp_nepp 8:2fd7cb217068 3172 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3173 uint8_t none_flag;
sepp_nepp 6:fb11b746ceb5 3174 uint8_t sigma_limitflag = 0;
sepp_nepp 6:fb11b746ceb5 3175 uint8_t signal_ref_clipflag = 0;
sepp_nepp 6:fb11b746ceb5 3176 uint8_t range_ignore_thresholdflag = 0;
sepp_nepp 6:fb11b746ceb5 3177 uint8_t sigma_limit_check_enable = 0;
sepp_nepp 6:fb11b746ceb5 3178 uint8_t signal_rate_final_range_limit_check_enable = 0;
sepp_nepp 6:fb11b746ceb5 3179 uint8_t signal_ref_clip_limit_check_enable = 0;
sepp_nepp 6:fb11b746ceb5 3180 uint8_t range_ignore_threshold_limit_check_enable = 0;
sepp_nepp 6:fb11b746ceb5 3181 FixPoint1616_t sigma_estimate;
sepp_nepp 6:fb11b746ceb5 3182 FixPoint1616_t sigma_limit_value;
sepp_nepp 6:fb11b746ceb5 3183 FixPoint1616_t signal_ref_clip_value;
sepp_nepp 6:fb11b746ceb5 3184 FixPoint1616_t range_ignore_threshold_value;
sepp_nepp 6:fb11b746ceb5 3185 FixPoint1616_t signal_rate_per_spad;
sepp_nepp 6:fb11b746ceb5 3186 uint8_t device_range_status_internal = 0;
sepp_nepp 6:fb11b746ceb5 3187 uint16_t tmp_word = 0;
sepp_nepp 6:fb11b746ceb5 3188 uint8_t temp8;
sepp_nepp 6:fb11b746ceb5 3189 uint32_t dmax_mm = 0;
sepp_nepp 6:fb11b746ceb5 3190 FixPoint1616_t last_signal_ref_mcps;
sepp_nepp 9:cb4c6d4e5030 3191
sepp_nepp 9:cb4c6d4e5030 3192 /* VL53L0X has a good ranging when the value of the
sepp_nepp 6:fb11b746ceb5 3193 * DeviceRangeStatus = 11. This function will replace the value 0 with
sepp_nepp 6:fb11b746ceb5 3194 * the value 11 in the DeviceRangeStatus.
sepp_nepp 6:fb11b746ceb5 3195 * In addition, the SigmaEstimator is not included in the VL53L0X
sepp_nepp 9:cb4c6d4e5030 3196 * DeviceRangeStatus, this will be added in the PalRangeStatus. */
sepp_nepp 6:fb11b746ceb5 3197
sepp_nepp 6:fb11b746ceb5 3198 device_range_status_internal = ((device_range_status & 0x78) >> 3);
sepp_nepp 6:fb11b746ceb5 3199
sepp_nepp 6:fb11b746ceb5 3200 if (device_range_status_internal == 0 ||
sepp_nepp 6:fb11b746ceb5 3201 device_range_status_internal == 5 ||
sepp_nepp 6:fb11b746ceb5 3202 device_range_status_internal == 7 ||
sepp_nepp 6:fb11b746ceb5 3203 device_range_status_internal == 12 ||
sepp_nepp 6:fb11b746ceb5 3204 device_range_status_internal == 13 ||
sepp_nepp 6:fb11b746ceb5 3205 device_range_status_internal == 14 ||
sepp_nepp 6:fb11b746ceb5 3206 device_range_status_internal == 15
sepp_nepp 6:fb11b746ceb5 3207 ) {
sepp_nepp 6:fb11b746ceb5 3208 none_flag = 1;
sepp_nepp 6:fb11b746ceb5 3209 } else {
sepp_nepp 6:fb11b746ceb5 3210 none_flag = 0;
sepp_nepp 6:fb11b746ceb5 3211 }
sepp_nepp 6:fb11b746ceb5 3212
sepp_nepp 6:fb11b746ceb5 3213 /*
sepp_nepp 6:fb11b746ceb5 3214 * Check if Sigma limit is enabled, if yes then do comparison with limit
sepp_nepp 6:fb11b746ceb5 3215 * value and put the result back into pPalRangeStatus.
sepp_nepp 6:fb11b746ceb5 3216 */
sepp_nepp 6:fb11b746ceb5 3217 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3218 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 3219 &sigma_limit_check_enable);
sepp_nepp 6:fb11b746ceb5 3220 }
sepp_nepp 6:fb11b746ceb5 3221
sepp_nepp 6:fb11b746ceb5 3222 if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
sepp_nepp 6:fb11b746ceb5 3223 /*
sepp_nepp 6:fb11b746ceb5 3224 * compute the Sigma and check with limit
sepp_nepp 6:fb11b746ceb5 3225 */
sepp_nepp 7:3a1115c2556b 3226 status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data,
sepp_nepp 6:fb11b746ceb5 3227 &sigma_estimate,
sepp_nepp 6:fb11b746ceb5 3228 &dmax_mm);
sepp_nepp 6:fb11b746ceb5 3229 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3230 p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
sepp_nepp 6:fb11b746ceb5 3231 }
sepp_nepp 6:fb11b746ceb5 3232
sepp_nepp 6:fb11b746ceb5 3233 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3234 status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 3235 &sigma_limit_value);
sepp_nepp 6:fb11b746ceb5 3236
sepp_nepp 6:fb11b746ceb5 3237 if ((sigma_limit_value > 0) &&
sepp_nepp 6:fb11b746ceb5 3238 (sigma_estimate > sigma_limit_value)) {
sepp_nepp 6:fb11b746ceb5 3239 /* Limit Fail */
sepp_nepp 6:fb11b746ceb5 3240 sigma_limitflag = 1;
sepp_nepp 6:fb11b746ceb5 3241 }
sepp_nepp 6:fb11b746ceb5 3242 }
sepp_nepp 6:fb11b746ceb5 3243 }
sepp_nepp 6:fb11b746ceb5 3244
sepp_nepp 6:fb11b746ceb5 3245 /*
sepp_nepp 6:fb11b746ceb5 3246 * Check if Signal ref clip limit is enabled, if yes then do comparison
sepp_nepp 6:fb11b746ceb5 3247 * with limit value and put the result back into pPalRangeStatus.
sepp_nepp 6:fb11b746ceb5 3248 */
sepp_nepp 6:fb11b746ceb5 3249 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3250 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
sepp_nepp 6:fb11b746ceb5 3251 &signal_ref_clip_limit_check_enable);
sepp_nepp 6:fb11b746ceb5 3252 }
sepp_nepp 6:fb11b746ceb5 3253
sepp_nepp 6:fb11b746ceb5 3254 if ((signal_ref_clip_limit_check_enable != 0) &&
sepp_nepp 6:fb11b746ceb5 3255 (status == VL53L0X_ERROR_NONE)) {
sepp_nepp 6:fb11b746ceb5 3256
sepp_nepp 8:2fd7cb217068 3257 status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
sepp_nepp 6:fb11b746ceb5 3258 &signal_ref_clip_value);
sepp_nepp 6:fb11b746ceb5 3259
sepp_nepp 6:fb11b746ceb5 3260 /* Read LastSignalRefMcps from device */
sepp_nepp 6:fb11b746ceb5 3261 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3262 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 6:fb11b746ceb5 3263 }
sepp_nepp 6:fb11b746ceb5 3264
sepp_nepp 6:fb11b746ceb5 3265 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3266 status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
sepp_nepp 6:fb11b746ceb5 3267 &tmp_word);
sepp_nepp 6:fb11b746ceb5 3268 }
sepp_nepp 6:fb11b746ceb5 3269
sepp_nepp 6:fb11b746ceb5 3270 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3271 status = VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 3272 }
sepp_nepp 6:fb11b746ceb5 3273
sepp_nepp 6:fb11b746ceb5 3274 last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word);
sepp_nepp 8:2fd7cb217068 3275 Data.LastSignalRefMcps = last_signal_ref_mcps;
sepp_nepp 6:fb11b746ceb5 3276
sepp_nepp 6:fb11b746ceb5 3277 if ((signal_ref_clip_value > 0) &&
sepp_nepp 6:fb11b746ceb5 3278 (last_signal_ref_mcps > signal_ref_clip_value)) {
sepp_nepp 6:fb11b746ceb5 3279 /* Limit Fail */
sepp_nepp 6:fb11b746ceb5 3280 signal_ref_clipflag = 1;
sepp_nepp 6:fb11b746ceb5 3281 }
sepp_nepp 6:fb11b746ceb5 3282 }
sepp_nepp 6:fb11b746ceb5 3283
sepp_nepp 6:fb11b746ceb5 3284 /*
sepp_nepp 6:fb11b746ceb5 3285 * Check if Signal ref clip limit is enabled, if yes then do comparison
sepp_nepp 6:fb11b746ceb5 3286 * with limit value and put the result back into pPalRangeStatus.
sepp_nepp 6:fb11b746ceb5 3287 * EffectiveSpadRtnCount has a format 8.8
sepp_nepp 6:fb11b746ceb5 3288 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
sepp_nepp 6:fb11b746ceb5 3289 */
sepp_nepp 6:fb11b746ceb5 3290 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3291 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
sepp_nepp 6:fb11b746ceb5 3292 &range_ignore_threshold_limit_check_enable);
sepp_nepp 6:fb11b746ceb5 3293 }
sepp_nepp 6:fb11b746ceb5 3294
sepp_nepp 6:fb11b746ceb5 3295 if ((range_ignore_threshold_limit_check_enable != 0) &&
sepp_nepp 6:fb11b746ceb5 3296 (status == VL53L0X_ERROR_NONE)) {
sepp_nepp 6:fb11b746ceb5 3297
sepp_nepp 6:fb11b746ceb5 3298 /* Compute the signal rate per spad */
sepp_nepp 6:fb11b746ceb5 3299 if (effective_spad_rtn_count == 0) {
sepp_nepp 6:fb11b746ceb5 3300 signal_rate_per_spad = 0;
sepp_nepp 6:fb11b746ceb5 3301 } else {
sepp_nepp 6:fb11b746ceb5 3302 signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate)
sepp_nepp 6:fb11b746ceb5 3303 / effective_spad_rtn_count);
sepp_nepp 6:fb11b746ceb5 3304 }
sepp_nepp 6:fb11b746ceb5 3305
sepp_nepp 8:2fd7cb217068 3306 status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
sepp_nepp 6:fb11b746ceb5 3307 &range_ignore_threshold_value);
sepp_nepp 6:fb11b746ceb5 3308
sepp_nepp 6:fb11b746ceb5 3309 if ((range_ignore_threshold_value > 0) &&
sepp_nepp 6:fb11b746ceb5 3310 (signal_rate_per_spad < range_ignore_threshold_value)) {
sepp_nepp 6:fb11b746ceb5 3311 /* Limit Fail add 2^6 to range status */
sepp_nepp 6:fb11b746ceb5 3312 range_ignore_thresholdflag = 1;
sepp_nepp 6:fb11b746ceb5 3313 }
sepp_nepp 6:fb11b746ceb5 3314 }
sepp_nepp 6:fb11b746ceb5 3315
sepp_nepp 6:fb11b746ceb5 3316 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3317 if (none_flag == 1) {
sepp_nepp 6:fb11b746ceb5 3318 *p_pal_range_status = 255; /* NONE */
sepp_nepp 6:fb11b746ceb5 3319 } else if (device_range_status_internal == 1 ||
sepp_nepp 6:fb11b746ceb5 3320 device_range_status_internal == 2 ||
sepp_nepp 6:fb11b746ceb5 3321 device_range_status_internal == 3) {
sepp_nepp 6:fb11b746ceb5 3322 *p_pal_range_status = 5; /* HW fail */
sepp_nepp 6:fb11b746ceb5 3323 } else if (device_range_status_internal == 6 ||
sepp_nepp 6:fb11b746ceb5 3324 device_range_status_internal == 9) {
sepp_nepp 6:fb11b746ceb5 3325 *p_pal_range_status = 4; /* Phase fail */
sepp_nepp 6:fb11b746ceb5 3326 } else if (device_range_status_internal == 8 ||
sepp_nepp 6:fb11b746ceb5 3327 device_range_status_internal == 10 ||
sepp_nepp 6:fb11b746ceb5 3328 signal_ref_clipflag == 1) {
sepp_nepp 6:fb11b746ceb5 3329 *p_pal_range_status = 3; /* Min range */
sepp_nepp 6:fb11b746ceb5 3330 } else if (device_range_status_internal == 4 ||
sepp_nepp 6:fb11b746ceb5 3331 range_ignore_thresholdflag == 1) {
sepp_nepp 6:fb11b746ceb5 3332 *p_pal_range_status = 2; /* Signal Fail */
sepp_nepp 6:fb11b746ceb5 3333 } else if (sigma_limitflag == 1) {
sepp_nepp 6:fb11b746ceb5 3334 *p_pal_range_status = 1; /* Sigma Fail */
sepp_nepp 6:fb11b746ceb5 3335 } else {
sepp_nepp 6:fb11b746ceb5 3336 *p_pal_range_status = 0; /* Range Valid */
sepp_nepp 6:fb11b746ceb5 3337 }
sepp_nepp 6:fb11b746ceb5 3338 }
sepp_nepp 6:fb11b746ceb5 3339
sepp_nepp 6:fb11b746ceb5 3340 /* DMAX only relevant during range error */
sepp_nepp 6:fb11b746ceb5 3341 if (*p_pal_range_status == 0) {
sepp_nepp 6:fb11b746ceb5 3342 p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
sepp_nepp 6:fb11b746ceb5 3343 }
sepp_nepp 6:fb11b746ceb5 3344
sepp_nepp 6:fb11b746ceb5 3345 /* fill the Limit Check Status */
sepp_nepp 6:fb11b746ceb5 3346
sepp_nepp 8:2fd7cb217068 3347 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 3348 &signal_rate_final_range_limit_check_enable);
sepp_nepp 6:fb11b746ceb5 3349
sepp_nepp 6:fb11b746ceb5 3350 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3351 if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1)) {
sepp_nepp 6:fb11b746ceb5 3352 temp8 = 1;
sepp_nepp 6:fb11b746ceb5 3353 } else {
sepp_nepp 6:fb11b746ceb5 3354 temp8 = 0;
sepp_nepp 6:fb11b746ceb5 3355 }
sepp_nepp 8:2fd7cb217068 3356 Data.CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = temp8;
sepp_nepp 6:fb11b746ceb5 3357
sepp_nepp 6:fb11b746ceb5 3358 if ((device_range_status_internal == 4) ||
sepp_nepp 6:fb11b746ceb5 3359 (signal_rate_final_range_limit_check_enable == 0)) {
sepp_nepp 6:fb11b746ceb5 3360 temp8 = 1;
sepp_nepp 6:fb11b746ceb5 3361 } else {
sepp_nepp 6:fb11b746ceb5 3362 temp8 = 0;
sepp_nepp 6:fb11b746ceb5 3363 }
sepp_nepp 8:2fd7cb217068 3364 Data.CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = temp8;
sepp_nepp 6:fb11b746ceb5 3365
sepp_nepp 6:fb11b746ceb5 3366 if ((signal_ref_clip_limit_check_enable == 0) ||
sepp_nepp 6:fb11b746ceb5 3367 (signal_ref_clipflag == 1)) {
sepp_nepp 6:fb11b746ceb5 3368 temp8 = 1;
sepp_nepp 6:fb11b746ceb5 3369 } else {
sepp_nepp 6:fb11b746ceb5 3370 temp8 = 0;
sepp_nepp 6:fb11b746ceb5 3371 }
sepp_nepp 6:fb11b746ceb5 3372
sepp_nepp 8:2fd7cb217068 3373 Data.CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = temp8;
sepp_nepp 6:fb11b746ceb5 3374
sepp_nepp 6:fb11b746ceb5 3375 if ((range_ignore_threshold_limit_check_enable == 0) ||
sepp_nepp 6:fb11b746ceb5 3376 (range_ignore_thresholdflag == 1)) {
sepp_nepp 6:fb11b746ceb5 3377 temp8 = 1;
sepp_nepp 6:fb11b746ceb5 3378 } else {
sepp_nepp 6:fb11b746ceb5 3379 temp8 = 0;
sepp_nepp 6:fb11b746ceb5 3380 }
sepp_nepp 6:fb11b746ceb5 3381
sepp_nepp 8:2fd7cb217068 3382 Data.CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = temp8;
sepp_nepp 6:fb11b746ceb5 3383 }
sepp_nepp 6:fb11b746ceb5 3384
sepp_nepp 6:fb11b746ceb5 3385 return status;
sepp_nepp 6:fb11b746ceb5 3386 }
sepp_nepp 6:fb11b746ceb5 3387
sepp_nepp 8:2fd7cb217068 3388 VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
sepp_nepp 8:2fd7cb217068 3389 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3390 uint8_t device_range_status;
sepp_nepp 6:fb11b746ceb5 3391 uint8_t range_fractional_enable;
sepp_nepp 6:fb11b746ceb5 3392 uint8_t pal_range_status;
sepp_nepp 6:fb11b746ceb5 3393 uint8_t x_talk_compensation_enable;
sepp_nepp 6:fb11b746ceb5 3394 uint16_t ambient_rate;
sepp_nepp 6:fb11b746ceb5 3395 FixPoint1616_t signal_rate;
sepp_nepp 6:fb11b746ceb5 3396 uint16_t x_talk_compensation_rate_mega_cps;
sepp_nepp 6:fb11b746ceb5 3397 uint16_t effective_spad_rtn_count;
sepp_nepp 6:fb11b746ceb5 3398 uint16_t tmpuint16;
sepp_nepp 6:fb11b746ceb5 3399 uint16_t xtalk_range_milli_meter;
sepp_nepp 6:fb11b746ceb5 3400 uint16_t linearity_corrective_gain;
sepp_nepp 6:fb11b746ceb5 3401 uint8_t localBuffer[12];
sepp_nepp 6:fb11b746ceb5 3402 VL53L0X_RangingMeasurementData_t last_range_data_buffer;
sepp_nepp 6:fb11b746ceb5 3403
sepp_nepp 6:fb11b746ceb5 3404 /* use multi read even if some registers are not useful, result will
sepp_nepp 9:cb4c6d4e5030 3405 * be more efficient start reading at 0x14 dec20
sepp_nepp 9:cb4c6d4e5030 3406 * end reading at 0x21 dec33 total 14 bytes to read */
sepp_nepp 7:3a1115c2556b 3407 status = VL53L0X_read_multi( 0x14, localBuffer, 12);
sepp_nepp 6:fb11b746ceb5 3408
sepp_nepp 6:fb11b746ceb5 3409 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3410
sepp_nepp 6:fb11b746ceb5 3411 tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]);
sepp_nepp 6:fb11b746ceb5 3412 /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
sepp_nepp 6:fb11b746ceb5 3413 *(format 11.2) else no fractional */
sepp_nepp 6:fb11b746ceb5 3414
sepp_nepp 8:2fd7cb217068 3415 signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
sepp_nepp 6:fb11b746ceb5 3416 /* peak_signal_count_rate_rtn_mcps */
sepp_nepp 6:fb11b746ceb5 3417 p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
sepp_nepp 6:fb11b746ceb5 3418
sepp_nepp 6:fb11b746ceb5 3419 ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
sepp_nepp 6:fb11b746ceb5 3420 p_ranging_measurement_data->AmbientRateRtnMegaCps =
sepp_nepp 6:fb11b746ceb5 3421 VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate);
sepp_nepp 6:fb11b746ceb5 3422
sepp_nepp 6:fb11b746ceb5 3423 effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3],
sepp_nepp 6:fb11b746ceb5 3424 localBuffer[2]);
sepp_nepp 6:fb11b746ceb5 3425 /* EffectiveSpadRtnCount is 8.8 format */
sepp_nepp 6:fb11b746ceb5 3426 p_ranging_measurement_data->EffectiveSpadRtnCount =
sepp_nepp 6:fb11b746ceb5 3427 effective_spad_rtn_count;
sepp_nepp 6:fb11b746ceb5 3428
sepp_nepp 6:fb11b746ceb5 3429 device_range_status = localBuffer[0];
sepp_nepp 6:fb11b746ceb5 3430
sepp_nepp 6:fb11b746ceb5 3431 /* Get Linearity Corrective Gain */
sepp_nepp 8:2fd7cb217068 3432 linearity_corrective_gain = Data.LinearityCorrectiveGain;
sepp_nepp 6:fb11b746ceb5 3433
sepp_nepp 6:fb11b746ceb5 3434 /* Get ranging configuration */
sepp_nepp 8:2fd7cb217068 3435 range_fractional_enable = Data.RangeFractionalEnable;
sepp_nepp 6:fb11b746ceb5 3436
sepp_nepp 6:fb11b746ceb5 3437 if (linearity_corrective_gain != 1000) {
sepp_nepp 6:fb11b746ceb5 3438
sepp_nepp 9:cb4c6d4e5030 3439 tmpuint16 = (uint16_t)((linearity_corrective_gain * tmpuint16 + 500) / 1000);
sepp_nepp 6:fb11b746ceb5 3440
sepp_nepp 6:fb11b746ceb5 3441 /* Implement Xtalk */
sepp_nepp 8:2fd7cb217068 3442 x_talk_compensation_rate_mega_cps = Data.CurrentParameters.XTalkCompensationRateMegaCps ;
sepp_nepp 8:2fd7cb217068 3443 x_talk_compensation_enable = Data.CurrentParameters.XTalkCompensationEnable ;
sepp_nepp 6:fb11b746ceb5 3444
sepp_nepp 6:fb11b746ceb5 3445 if (x_talk_compensation_enable) {
sepp_nepp 6:fb11b746ceb5 3446
sepp_nepp 6:fb11b746ceb5 3447 if ((signal_rate
sepp_nepp 6:fb11b746ceb5 3448 - ((x_talk_compensation_rate_mega_cps
sepp_nepp 6:fb11b746ceb5 3449 * effective_spad_rtn_count) >> 8))
sepp_nepp 6:fb11b746ceb5 3450 <= 0) {
sepp_nepp 6:fb11b746ceb5 3451 if (range_fractional_enable) {
sepp_nepp 6:fb11b746ceb5 3452 xtalk_range_milli_meter = 8888;
sepp_nepp 6:fb11b746ceb5 3453 } else {
sepp_nepp 6:fb11b746ceb5 3454 xtalk_range_milli_meter = 8888 << 2;
sepp_nepp 6:fb11b746ceb5 3455 }
sepp_nepp 6:fb11b746ceb5 3456 } else {
sepp_nepp 6:fb11b746ceb5 3457 xtalk_range_milli_meter =
sepp_nepp 6:fb11b746ceb5 3458 (tmpuint16 * signal_rate)
sepp_nepp 6:fb11b746ceb5 3459 / (signal_rate
sepp_nepp 6:fb11b746ceb5 3460 - ((x_talk_compensation_rate_mega_cps
sepp_nepp 6:fb11b746ceb5 3461 * effective_spad_rtn_count)
sepp_nepp 6:fb11b746ceb5 3462 >> 8));
sepp_nepp 6:fb11b746ceb5 3463 }
sepp_nepp 6:fb11b746ceb5 3464 tmpuint16 = xtalk_range_milli_meter;
sepp_nepp 6:fb11b746ceb5 3465 }
sepp_nepp 6:fb11b746ceb5 3466 }
sepp_nepp 6:fb11b746ceb5 3467
sepp_nepp 6:fb11b746ceb5 3468 if (range_fractional_enable) {
sepp_nepp 6:fb11b746ceb5 3469 p_ranging_measurement_data->RangeMilliMeter =
sepp_nepp 6:fb11b746ceb5 3470 (uint16_t)((tmpuint16) >> 2);
sepp_nepp 6:fb11b746ceb5 3471 p_ranging_measurement_data->RangeFractionalPart =
sepp_nepp 6:fb11b746ceb5 3472 (uint8_t)((tmpuint16 & 0x03) << 6);
sepp_nepp 6:fb11b746ceb5 3473 } else {
sepp_nepp 6:fb11b746ceb5 3474 p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
sepp_nepp 6:fb11b746ceb5 3475 p_ranging_measurement_data->RangeFractionalPart = 0;
sepp_nepp 6:fb11b746ceb5 3476 }
sepp_nepp 6:fb11b746ceb5 3477
sepp_nepp 9:cb4c6d4e5030 3478 /* For a standard definition of RangeStatus, this should
sepp_nepp 6:fb11b746ceb5 3479 * return 0 in case of good result after a ranging
sepp_nepp 6:fb11b746ceb5 3480 * The range status depends on the device so call a device
sepp_nepp 6:fb11b746ceb5 3481 * specific function to obtain the right Status.
sepp_nepp 6:fb11b746ceb5 3482 */
sepp_nepp 7:3a1115c2556b 3483 status |= VL53L0X_get_pal_range_status( device_range_status,
sepp_nepp 6:fb11b746ceb5 3484 signal_rate, effective_spad_rtn_count,
sepp_nepp 6:fb11b746ceb5 3485 p_ranging_measurement_data, &pal_range_status);
sepp_nepp 6:fb11b746ceb5 3486
sepp_nepp 6:fb11b746ceb5 3487 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3488 p_ranging_measurement_data->RangeStatus = pal_range_status;}
sepp_nepp 6:fb11b746ceb5 3489
sepp_nepp 6:fb11b746ceb5 3490 }
sepp_nepp 6:fb11b746ceb5 3491
sepp_nepp 6:fb11b746ceb5 3492 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3493 /* Copy last read data into Device buffer */
sepp_nepp 8:2fd7cb217068 3494 last_range_data_buffer = Data.LastRangeMeasure;
sepp_nepp 6:fb11b746ceb5 3495
sepp_nepp 6:fb11b746ceb5 3496 last_range_data_buffer.RangeMilliMeter =
sepp_nepp 6:fb11b746ceb5 3497 p_ranging_measurement_data->RangeMilliMeter;
sepp_nepp 6:fb11b746ceb5 3498 last_range_data_buffer.RangeFractionalPart =
sepp_nepp 6:fb11b746ceb5 3499 p_ranging_measurement_data->RangeFractionalPart;
sepp_nepp 6:fb11b746ceb5 3500 last_range_data_buffer.RangeDMaxMilliMeter =
sepp_nepp 6:fb11b746ceb5 3501 p_ranging_measurement_data->RangeDMaxMilliMeter;
sepp_nepp 6:fb11b746ceb5 3502 last_range_data_buffer.SignalRateRtnMegaCps =
sepp_nepp 6:fb11b746ceb5 3503 p_ranging_measurement_data->SignalRateRtnMegaCps;
sepp_nepp 6:fb11b746ceb5 3504 last_range_data_buffer.AmbientRateRtnMegaCps =
sepp_nepp 6:fb11b746ceb5 3505 p_ranging_measurement_data->AmbientRateRtnMegaCps;
sepp_nepp 6:fb11b746ceb5 3506 last_range_data_buffer.EffectiveSpadRtnCount =
sepp_nepp 6:fb11b746ceb5 3507 p_ranging_measurement_data->EffectiveSpadRtnCount;
sepp_nepp 6:fb11b746ceb5 3508 last_range_data_buffer.RangeStatus =
sepp_nepp 6:fb11b746ceb5 3509 p_ranging_measurement_data->RangeStatus;
sepp_nepp 6:fb11b746ceb5 3510
sepp_nepp 8:2fd7cb217068 3511 Data.LastRangeMeasure = last_range_data_buffer;
sepp_nepp 6:fb11b746ceb5 3512 }
sepp_nepp 9:cb4c6d4e5030 3513
sepp_nepp 6:fb11b746ceb5 3514 return status;
sepp_nepp 6:fb11b746ceb5 3515 }
sepp_nepp 6:fb11b746ceb5 3516
sepp_nepp 8:2fd7cb217068 3517 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
sepp_nepp 8:2fd7cb217068 3518 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3519
sepp_nepp 6:fb11b746ceb5 3520 /* This function will do a complete single ranging
sepp_nepp 6:fb11b746ceb5 3521 * Here we fix the mode! */
sepp_nepp 7:3a1115c2556b 3522 status = VL53L0X_set_device_mode( VL53L0X_DEVICEMODE_SINGLE_RANGING);
sepp_nepp 6:fb11b746ceb5 3523
sepp_nepp 6:fb11b746ceb5 3524 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3525 status = VL53L0X_perform_single_measurement(); }
sepp_nepp 6:fb11b746ceb5 3526
sepp_nepp 6:fb11b746ceb5 3527 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3528 status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data); }
sepp_nepp 6:fb11b746ceb5 3529
sepp_nepp 6:fb11b746ceb5 3530 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3531 status = VL53L0X_clear_interrupt_mask( 0);}
sepp_nepp 9:cb4c6d4e5030 3532
sepp_nepp 6:fb11b746ceb5 3533 return status;
sepp_nepp 6:fb11b746ceb5 3534 }
sepp_nepp 6:fb11b746ceb5 3535
sepp_nepp 8:2fd7cb217068 3536 VL53L0X_Error VL53L0X::perform_ref_signal_measurement(uint16_t *p_ref_signal_rate)
sepp_nepp 8:2fd7cb217068 3537 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3538 VL53L0X_RangingMeasurementData_t ranging_measurement_data;
sepp_nepp 6:fb11b746ceb5 3539
sepp_nepp 6:fb11b746ceb5 3540 uint8_t sequence_config = 0;
sepp_nepp 6:fb11b746ceb5 3541
sepp_nepp 6:fb11b746ceb5 3542 /* store the value of the sequence config,
sepp_nepp 9:cb4c6d4e5030 3543 * this will be reset before the end of the function*/
sepp_nepp 8:2fd7cb217068 3544 sequence_config = Data.SequenceConfig;
sepp_nepp 6:fb11b746ceb5 3545
sepp_nepp 6:fb11b746ceb5 3546 /*
sepp_nepp 6:fb11b746ceb5 3547 * This function performs a reference signal rate measurement.
sepp_nepp 6:fb11b746ceb5 3548 */
sepp_nepp 6:fb11b746ceb5 3549 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3550 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);}
sepp_nepp 6:fb11b746ceb5 3551
sepp_nepp 6:fb11b746ceb5 3552 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3553 status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data); }
sepp_nepp 6:fb11b746ceb5 3554
sepp_nepp 6:fb11b746ceb5 3555 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3556 status = VL53L0X_write_byte( 0xFF, 0x01); }
sepp_nepp 6:fb11b746ceb5 3557
sepp_nepp 6:fb11b746ceb5 3558 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3559 status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
sepp_nepp 9:cb4c6d4e5030 3560 p_ref_signal_rate);}
sepp_nepp 6:fb11b746ceb5 3561
sepp_nepp 6:fb11b746ceb5 3562 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 9:cb4c6d4e5030 3563 status = VL53L0X_write_byte( 0xFF, 0x00);}
sepp_nepp 6:fb11b746ceb5 3564
sepp_nepp 6:fb11b746ceb5 3565 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3566 /* restore the previous Sequence Config */
sepp_nepp 7:3a1115c2556b 3567 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sepp_nepp 6:fb11b746ceb5 3568 sequence_config);
sepp_nepp 6:fb11b746ceb5 3569 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3570 Data.SequenceConfig = sequence_config;
sepp_nepp 6:fb11b746ceb5 3571 }
sepp_nepp 6:fb11b746ceb5 3572 }
sepp_nepp 6:fb11b746ceb5 3573 return status;
sepp_nepp 6:fb11b746ceb5 3574 }
sepp_nepp 6:fb11b746ceb5 3575
sepp_nepp 8:2fd7cb217068 3576 VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count,
sepp_nepp 6:fb11b746ceb5 3577 uint8_t *is_aperture_spads)
sepp_nepp 8:2fd7cb217068 3578 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3579 uint8_t last_spad_array[6];
sepp_nepp 6:fb11b746ceb5 3580 uint8_t start_select = 0xB4;
sepp_nepp 6:fb11b746ceb5 3581 uint32_t minimum_spad_count = 3;
sepp_nepp 6:fb11b746ceb5 3582 uint32_t max_spad_count = 44;
sepp_nepp 6:fb11b746ceb5 3583 uint32_t current_spad_index = 0;
sepp_nepp 6:fb11b746ceb5 3584 uint32_t last_spad_index = 0;
sepp_nepp 6:fb11b746ceb5 3585 int32_t next_good_spad = 0;
sepp_nepp 6:fb11b746ceb5 3586 uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */
sepp_nepp 6:fb11b746ceb5 3587 uint16_t peak_signal_rate_ref;
sepp_nepp 6:fb11b746ceb5 3588 uint32_t need_apt_spads = 0;
sepp_nepp 6:fb11b746ceb5 3589 uint32_t index = 0;
sepp_nepp 6:fb11b746ceb5 3590 uint32_t spad_array_size = 6;
sepp_nepp 6:fb11b746ceb5 3591 uint32_t signal_rate_diff = 0;
sepp_nepp 6:fb11b746ceb5 3592 uint32_t last_signal_rate_diff = 0;
sepp_nepp 6:fb11b746ceb5 3593 uint8_t complete = 0;
sepp_nepp 6:fb11b746ceb5 3594 uint8_t vhv_settings = 0;
sepp_nepp 6:fb11b746ceb5 3595 uint8_t phase_cal = 0;
sepp_nepp 6:fb11b746ceb5 3596 uint32_t ref_spad_count_int = 0;
sepp_nepp 6:fb11b746ceb5 3597 uint8_t is_aperture_spads_int = 0;
sepp_nepp 6:fb11b746ceb5 3598
sepp_nepp 6:fb11b746ceb5 3599 /*
sepp_nepp 6:fb11b746ceb5 3600 * The reference SPAD initialization procedure determines the minimum
sepp_nepp 6:fb11b746ceb5 3601 * amount of reference spads to be enables to achieve a target reference
sepp_nepp 6:fb11b746ceb5 3602 * signal rate and should be performed once during initialization.
sepp_nepp 6:fb11b746ceb5 3603 *
sepp_nepp 6:fb11b746ceb5 3604 * Either aperture or non-aperture spads are applied but never both.
sepp_nepp 6:fb11b746ceb5 3605 * Firstly non-aperture spads are set, begining with 5 spads, and
sepp_nepp 6:fb11b746ceb5 3606 * increased one spad at a time until the closest measurement to the
sepp_nepp 6:fb11b746ceb5 3607 * target rate is achieved.
sepp_nepp 6:fb11b746ceb5 3608 *
sepp_nepp 6:fb11b746ceb5 3609 * If the target rate is exceeded when 5 non-aperture spads are enabled,
sepp_nepp 6:fb11b746ceb5 3610 * initialization is performed instead with aperture spads.
sepp_nepp 6:fb11b746ceb5 3611 *
sepp_nepp 6:fb11b746ceb5 3612 * When setting spads, a 'Good Spad Map' is applied.
sepp_nepp 6:fb11b746ceb5 3613 *
sepp_nepp 6:fb11b746ceb5 3614 * This procedure operates within a SPAD window of interest of a maximum
sepp_nepp 6:fb11b746ceb5 3615 * 44 spads.
sepp_nepp 6:fb11b746ceb5 3616 * The start point is currently fixed to 180, which lies towards the end
sepp_nepp 6:fb11b746ceb5 3617 * of the non-aperture quadrant and runs in to the adjacent aperture
sepp_nepp 6:fb11b746ceb5 3618 * quadrant.
sepp_nepp 6:fb11b746ceb5 3619 */
sepp_nepp 8:2fd7cb217068 3620 target_ref_rate = Data.targetRefRate;
sepp_nepp 6:fb11b746ceb5 3621
sepp_nepp 6:fb11b746ceb5 3622 /*
sepp_nepp 6:fb11b746ceb5 3623 * Initialize Spad arrays.
sepp_nepp 6:fb11b746ceb5 3624 * Currently the good spad map is initialised to 'All good'.
sepp_nepp 6:fb11b746ceb5 3625 * This is a short term implementation. The good spad map will be
sepp_nepp 6:fb11b746ceb5 3626 * provided as an input.
sepp_nepp 6:fb11b746ceb5 3627 * Note that there are 6 bytes. Only the first 44 bits will be used to
sepp_nepp 6:fb11b746ceb5 3628 * represent spads.
sepp_nepp 6:fb11b746ceb5 3629 */
sepp_nepp 6:fb11b746ceb5 3630 for (index = 0; index < spad_array_size; index++) {
sepp_nepp 7:3a1115c2556b 3631 Data.SpadData.RefSpadEnables[index] = 0;
sepp_nepp 6:fb11b746ceb5 3632 }
sepp_nepp 6:fb11b746ceb5 3633
sepp_nepp 7:3a1115c2556b 3634 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 6:fb11b746ceb5 3635
sepp_nepp 6:fb11b746ceb5 3636 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3637 status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
sepp_nepp 6:fb11b746ceb5 3638 }
sepp_nepp 6:fb11b746ceb5 3639
sepp_nepp 6:fb11b746ceb5 3640 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3641 status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
sepp_nepp 6:fb11b746ceb5 3642 }
sepp_nepp 6:fb11b746ceb5 3643
sepp_nepp 6:fb11b746ceb5 3644 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3645 status = VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 3646 }
sepp_nepp 6:fb11b746ceb5 3647
sepp_nepp 6:fb11b746ceb5 3648 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3649 status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
sepp_nepp 6:fb11b746ceb5 3650 start_select);
sepp_nepp 6:fb11b746ceb5 3651 }
sepp_nepp 6:fb11b746ceb5 3652
sepp_nepp 6:fb11b746ceb5 3653 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3654 status = VL53L0X_write_byte(VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
sepp_nepp 6:fb11b746ceb5 3655 }
sepp_nepp 6:fb11b746ceb5 3656
sepp_nepp 6:fb11b746ceb5 3657 /* Perform ref calibration */
sepp_nepp 6:fb11b746ceb5 3658 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3659 status = VL53L0X_perform_ref_calibration( &vhv_settings,
sepp_nepp 6:fb11b746ceb5 3660 &phase_cal, 0);
sepp_nepp 6:fb11b746ceb5 3661 }
sepp_nepp 6:fb11b746ceb5 3662
sepp_nepp 6:fb11b746ceb5 3663 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3664 /* Enable Minimum NON-APERTURE Spads */
sepp_nepp 6:fb11b746ceb5 3665 current_spad_index = 0;
sepp_nepp 6:fb11b746ceb5 3666 last_spad_index = current_spad_index;
sepp_nepp 6:fb11b746ceb5 3667 need_apt_spads = 0;
sepp_nepp 8:2fd7cb217068 3668 status = enable_ref_spads(need_apt_spads,
sepp_nepp 7:3a1115c2556b 3669 Data.SpadData.RefGoodSpadMap,
sepp_nepp 7:3a1115c2556b 3670 Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3671 spad_array_size,
sepp_nepp 6:fb11b746ceb5 3672 start_select,
sepp_nepp 6:fb11b746ceb5 3673 current_spad_index,
sepp_nepp 6:fb11b746ceb5 3674 minimum_spad_count,
sepp_nepp 6:fb11b746ceb5 3675 &last_spad_index);
sepp_nepp 6:fb11b746ceb5 3676 }
sepp_nepp 6:fb11b746ceb5 3677
sepp_nepp 6:fb11b746ceb5 3678 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3679 current_spad_index = last_spad_index;
sepp_nepp 6:fb11b746ceb5 3680
sepp_nepp 8:2fd7cb217068 3681 status = perform_ref_signal_measurement(&peak_signal_rate_ref);
sepp_nepp 6:fb11b746ceb5 3682 if ((status == VL53L0X_ERROR_NONE) &&
sepp_nepp 6:fb11b746ceb5 3683 (peak_signal_rate_ref > target_ref_rate)) {
sepp_nepp 6:fb11b746ceb5 3684 /* Signal rate measurement too high,
sepp_nepp 6:fb11b746ceb5 3685 * switch to APERTURE SPADs */
sepp_nepp 6:fb11b746ceb5 3686
sepp_nepp 6:fb11b746ceb5 3687 for (index = 0; index < spad_array_size; index++) {
sepp_nepp 7:3a1115c2556b 3688 Data.SpadData.RefSpadEnables[index] = 0;
sepp_nepp 6:fb11b746ceb5 3689 }
sepp_nepp 6:fb11b746ceb5 3690
sepp_nepp 6:fb11b746ceb5 3691 /* Increment to the first APERTURE spad */
sepp_nepp 6:fb11b746ceb5 3692 while ((is_aperture(start_select + current_spad_index)
sepp_nepp 6:fb11b746ceb5 3693 == 0) && (current_spad_index < max_spad_count)) {
sepp_nepp 6:fb11b746ceb5 3694 current_spad_index++;
sepp_nepp 6:fb11b746ceb5 3695 }
sepp_nepp 6:fb11b746ceb5 3696
sepp_nepp 6:fb11b746ceb5 3697 need_apt_spads = 1;
sepp_nepp 6:fb11b746ceb5 3698
sepp_nepp 7:3a1115c2556b 3699 status = enable_ref_spads(need_apt_spads,
sepp_nepp 7:3a1115c2556b 3700 Data.SpadData.RefGoodSpadMap,
sepp_nepp 7:3a1115c2556b 3701 Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3702 spad_array_size,
sepp_nepp 6:fb11b746ceb5 3703 start_select,
sepp_nepp 6:fb11b746ceb5 3704 current_spad_index,
sepp_nepp 6:fb11b746ceb5 3705 minimum_spad_count,
sepp_nepp 6:fb11b746ceb5 3706 &last_spad_index);
sepp_nepp 6:fb11b746ceb5 3707
sepp_nepp 6:fb11b746ceb5 3708 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3709 current_spad_index = last_spad_index;
sepp_nepp 8:2fd7cb217068 3710 status = perform_ref_signal_measurement(&peak_signal_rate_ref);
sepp_nepp 6:fb11b746ceb5 3711
sepp_nepp 6:fb11b746ceb5 3712 if ((status == VL53L0X_ERROR_NONE) &&
sepp_nepp 6:fb11b746ceb5 3713 (peak_signal_rate_ref > target_ref_rate)) {
sepp_nepp 6:fb11b746ceb5 3714 /* Signal rate still too high after
sepp_nepp 6:fb11b746ceb5 3715 * setting the minimum number of
sepp_nepp 6:fb11b746ceb5 3716 * APERTURE spads. Can do no more
sepp_nepp 6:fb11b746ceb5 3717 * therefore set the min number of
sepp_nepp 6:fb11b746ceb5 3718 * aperture spads as the result.
sepp_nepp 6:fb11b746ceb5 3719 */
sepp_nepp 6:fb11b746ceb5 3720 is_aperture_spads_int = 1;
sepp_nepp 6:fb11b746ceb5 3721 ref_spad_count_int = minimum_spad_count;
sepp_nepp 6:fb11b746ceb5 3722 }
sepp_nepp 6:fb11b746ceb5 3723 }
sepp_nepp 6:fb11b746ceb5 3724 } else {
sepp_nepp 6:fb11b746ceb5 3725 need_apt_spads = 0;
sepp_nepp 6:fb11b746ceb5 3726 }
sepp_nepp 6:fb11b746ceb5 3727 }
sepp_nepp 6:fb11b746ceb5 3728
sepp_nepp 6:fb11b746ceb5 3729 if ((status == VL53L0X_ERROR_NONE) &&
sepp_nepp 6:fb11b746ceb5 3730 (peak_signal_rate_ref < target_ref_rate)) {
sepp_nepp 6:fb11b746ceb5 3731 /* At this point, the minimum number of either aperture
sepp_nepp 6:fb11b746ceb5 3732 * or non-aperture spads have been set. Proceed to add
sepp_nepp 6:fb11b746ceb5 3733 * spads and perform measurements until the target
sepp_nepp 6:fb11b746ceb5 3734 * reference is reached.
sepp_nepp 6:fb11b746ceb5 3735 */
sepp_nepp 6:fb11b746ceb5 3736 is_aperture_spads_int = need_apt_spads;
sepp_nepp 6:fb11b746ceb5 3737 ref_spad_count_int = minimum_spad_count;
sepp_nepp 6:fb11b746ceb5 3738
sepp_nepp 7:3a1115c2556b 3739 memcpy(last_spad_array, Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3740 spad_array_size);
sepp_nepp 6:fb11b746ceb5 3741 last_signal_rate_diff = abs(peak_signal_rate_ref -
sepp_nepp 6:fb11b746ceb5 3742 target_ref_rate);
sepp_nepp 6:fb11b746ceb5 3743 complete = 0;
sepp_nepp 6:fb11b746ceb5 3744
sepp_nepp 6:fb11b746ceb5 3745 while (!complete) {
sepp_nepp 8:2fd7cb217068 3746 get_next_good_spad(Data.SpadData.RefGoodSpadMap,
sepp_nepp 6:fb11b746ceb5 3747 spad_array_size, current_spad_index,
sepp_nepp 6:fb11b746ceb5 3748 &next_good_spad);
sepp_nepp 6:fb11b746ceb5 3749
sepp_nepp 6:fb11b746ceb5 3750 if (next_good_spad == -1) {
sepp_nepp 6:fb11b746ceb5 3751 status = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 6:fb11b746ceb5 3752 break;
sepp_nepp 6:fb11b746ceb5 3753 }
sepp_nepp 6:fb11b746ceb5 3754
sepp_nepp 6:fb11b746ceb5 3755 /* Cannot combine Aperture and Non-Aperture spads, so
sepp_nepp 6:fb11b746ceb5 3756 * ensure the current spad is of the correct type.
sepp_nepp 6:fb11b746ceb5 3757 */
sepp_nepp 6:fb11b746ceb5 3758 if (is_aperture((uint32_t)start_select + next_good_spad) !=
sepp_nepp 6:fb11b746ceb5 3759 need_apt_spads) {
sepp_nepp 6:fb11b746ceb5 3760 /* At this point we have enabled the maximum
sepp_nepp 6:fb11b746ceb5 3761 * number of Aperture spads.
sepp_nepp 6:fb11b746ceb5 3762 */
sepp_nepp 6:fb11b746ceb5 3763 complete = 1;
sepp_nepp 6:fb11b746ceb5 3764 break;
sepp_nepp 6:fb11b746ceb5 3765 }
sepp_nepp 6:fb11b746ceb5 3766
sepp_nepp 6:fb11b746ceb5 3767 (ref_spad_count_int)++;
sepp_nepp 6:fb11b746ceb5 3768
sepp_nepp 6:fb11b746ceb5 3769 current_spad_index = next_good_spad;
sepp_nepp 7:3a1115c2556b 3770 status = enable_spad_bit(Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3771 spad_array_size, current_spad_index);
sepp_nepp 6:fb11b746ceb5 3772
sepp_nepp 6:fb11b746ceb5 3773 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3774 current_spad_index++;
sepp_nepp 6:fb11b746ceb5 3775 /* Proceed to apply the additional spad and
sepp_nepp 6:fb11b746ceb5 3776 * perform measurement. */
sepp_nepp 7:3a1115c2556b 3777 status = set_ref_spad_map(Data.SpadData.RefSpadEnables);
sepp_nepp 6:fb11b746ceb5 3778 }
sepp_nepp 6:fb11b746ceb5 3779
sepp_nepp 6:fb11b746ceb5 3780 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3781 break;
sepp_nepp 6:fb11b746ceb5 3782 }
sepp_nepp 6:fb11b746ceb5 3783
sepp_nepp 7:3a1115c2556b 3784 status = perform_ref_signal_measurement(&peak_signal_rate_ref);
sepp_nepp 6:fb11b746ceb5 3785
sepp_nepp 6:fb11b746ceb5 3786 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3787 break;
sepp_nepp 6:fb11b746ceb5 3788 }
sepp_nepp 6:fb11b746ceb5 3789
sepp_nepp 6:fb11b746ceb5 3790 signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate);
sepp_nepp 6:fb11b746ceb5 3791
sepp_nepp 6:fb11b746ceb5 3792 if (peak_signal_rate_ref > target_ref_rate) {
sepp_nepp 6:fb11b746ceb5 3793 /* Select the spad map that provides the
sepp_nepp 6:fb11b746ceb5 3794 * measurement closest to the target rate,
sepp_nepp 6:fb11b746ceb5 3795 * either above or below it.
sepp_nepp 6:fb11b746ceb5 3796 */
sepp_nepp 6:fb11b746ceb5 3797 if (signal_rate_diff > last_signal_rate_diff) {
sepp_nepp 6:fb11b746ceb5 3798 /* Previous spad map produced a closer
sepp_nepp 6:fb11b746ceb5 3799 * measurement, so choose this. */
sepp_nepp 7:3a1115c2556b 3800 status = set_ref_spad_map(last_spad_array);
sepp_nepp 7:3a1115c2556b 3801 memcpy(Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3802 last_spad_array, spad_array_size);
sepp_nepp 6:fb11b746ceb5 3803 (ref_spad_count_int)--;
sepp_nepp 6:fb11b746ceb5 3804 }
sepp_nepp 6:fb11b746ceb5 3805 complete = 1;
sepp_nepp 6:fb11b746ceb5 3806 } else {
sepp_nepp 6:fb11b746ceb5 3807 /* Continue to add spads */
sepp_nepp 6:fb11b746ceb5 3808 last_signal_rate_diff = signal_rate_diff;
sepp_nepp 6:fb11b746ceb5 3809 memcpy(last_spad_array,
sepp_nepp 7:3a1115c2556b 3810 Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3811 spad_array_size);
sepp_nepp 6:fb11b746ceb5 3812 }
sepp_nepp 6:fb11b746ceb5 3813
sepp_nepp 6:fb11b746ceb5 3814 } /* while */
sepp_nepp 6:fb11b746ceb5 3815 }
sepp_nepp 6:fb11b746ceb5 3816
sepp_nepp 6:fb11b746ceb5 3817 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3818 *ref_spad_count = ref_spad_count_int;
sepp_nepp 6:fb11b746ceb5 3819 *is_aperture_spads = is_aperture_spads_int;
sepp_nepp 8:2fd7cb217068 3820 Data.RefSpadsInitialised = 1;
sepp_nepp 8:2fd7cb217068 3821 Data.ReferenceSpadCount = (uint8_t)(*ref_spad_count);
sepp_nepp 9:cb4c6d4e5030 3822 Data.ReferenceSpadType = *is_aperture_spads;
sepp_nepp 6:fb11b746ceb5 3823 }
sepp_nepp 6:fb11b746ceb5 3824
sepp_nepp 6:fb11b746ceb5 3825 return status;
sepp_nepp 6:fb11b746ceb5 3826 }
sepp_nepp 6:fb11b746ceb5 3827
sepp_nepp 8:2fd7cb217068 3828 VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(uint32_t count, uint8_t is_aperture_spads)
sepp_nepp 8:2fd7cb217068 3829 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3830 uint32_t current_spad_index = 0;
sepp_nepp 6:fb11b746ceb5 3831 uint8_t start_select = 0xB4;
sepp_nepp 6:fb11b746ceb5 3832 uint32_t spad_array_size = 6;
sepp_nepp 6:fb11b746ceb5 3833 uint32_t max_spad_count = 44;
sepp_nepp 6:fb11b746ceb5 3834 uint32_t last_spad_index;
sepp_nepp 6:fb11b746ceb5 3835 uint32_t index;
sepp_nepp 6:fb11b746ceb5 3836
sepp_nepp 6:fb11b746ceb5 3837 /*
sepp_nepp 6:fb11b746ceb5 3838 * This function applies a requested number of reference spads, either
sepp_nepp 6:fb11b746ceb5 3839 * aperture or
sepp_nepp 6:fb11b746ceb5 3840 * non-aperture, as requested.
sepp_nepp 6:fb11b746ceb5 3841 * The good spad map will be applied.
sepp_nepp 6:fb11b746ceb5 3842 */
sepp_nepp 6:fb11b746ceb5 3843
sepp_nepp 7:3a1115c2556b 3844 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 6:fb11b746ceb5 3845
sepp_nepp 6:fb11b746ceb5 3846 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3847 status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
sepp_nepp 6:fb11b746ceb5 3848 }
sepp_nepp 6:fb11b746ceb5 3849
sepp_nepp 6:fb11b746ceb5 3850 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3851 status = VL53L0X_write_byte( VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
sepp_nepp 6:fb11b746ceb5 3852 }
sepp_nepp 6:fb11b746ceb5 3853
sepp_nepp 6:fb11b746ceb5 3854 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3855 status = VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 3856 }
sepp_nepp 6:fb11b746ceb5 3857
sepp_nepp 6:fb11b746ceb5 3858 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3859 status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
sepp_nepp 6:fb11b746ceb5 3860 start_select);
sepp_nepp 6:fb11b746ceb5 3861 }
sepp_nepp 6:fb11b746ceb5 3862
sepp_nepp 6:fb11b746ceb5 3863 for (index = 0; index < spad_array_size; index++) {
sepp_nepp 7:3a1115c2556b 3864 Data.SpadData.RefSpadEnables[index] = 0;
sepp_nepp 6:fb11b746ceb5 3865 }
sepp_nepp 6:fb11b746ceb5 3866
sepp_nepp 6:fb11b746ceb5 3867 if (is_aperture_spads) {
sepp_nepp 6:fb11b746ceb5 3868 /* Increment to the first APERTURE spad */
sepp_nepp 6:fb11b746ceb5 3869 while ((is_aperture(start_select + current_spad_index) == 0) &&
sepp_nepp 6:fb11b746ceb5 3870 (current_spad_index < max_spad_count)) {
sepp_nepp 6:fb11b746ceb5 3871 current_spad_index++;
sepp_nepp 6:fb11b746ceb5 3872 }
sepp_nepp 6:fb11b746ceb5 3873 }
sepp_nepp 7:3a1115c2556b 3874 status = enable_ref_spads(is_aperture_spads,
sepp_nepp 7:3a1115c2556b 3875 Data.SpadData.RefGoodSpadMap,
sepp_nepp 7:3a1115c2556b 3876 Data.SpadData.RefSpadEnables,
sepp_nepp 6:fb11b746ceb5 3877 spad_array_size,
sepp_nepp 6:fb11b746ceb5 3878 start_select,
sepp_nepp 6:fb11b746ceb5 3879 current_spad_index,
sepp_nepp 6:fb11b746ceb5 3880 count,
sepp_nepp 6:fb11b746ceb5 3881 &last_spad_index);
sepp_nepp 6:fb11b746ceb5 3882
sepp_nepp 6:fb11b746ceb5 3883 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3884 Data.RefSpadsInitialised = 1;
sepp_nepp 8:2fd7cb217068 3885 Data.ReferenceSpadCount = (uint8_t)(count);
sepp_nepp 8:2fd7cb217068 3886 Data.ReferenceSpadType = is_aperture_spads;
sepp_nepp 6:fb11b746ceb5 3887 }
sepp_nepp 6:fb11b746ceb5 3888
sepp_nepp 6:fb11b746ceb5 3889 return status;
sepp_nepp 6:fb11b746ceb5 3890 }
sepp_nepp 6:fb11b746ceb5 3891
sepp_nepp 7:3a1115c2556b 3892 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration( uint8_t *p_vhv_settings,
sepp_nepp 6:fb11b746ceb5 3893 uint8_t *p_phase_cal)
sepp_nepp 8:2fd7cb217068 3894 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3895
sepp_nepp 7:3a1115c2556b 3896 status = VL53L0X_perform_ref_calibration( p_vhv_settings, p_phase_cal, 1);
sepp_nepp 6:fb11b746ceb5 3897
sepp_nepp 6:fb11b746ceb5 3898 return status;
sepp_nepp 6:fb11b746ceb5 3899 }
sepp_nepp 6:fb11b746ceb5 3900
sepp_nepp 8:2fd7cb217068 3901 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count, uint8_t *is_aperture_spads)
sepp_nepp 8:2fd7cb217068 3902 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3903
sepp_nepp 7:3a1115c2556b 3904 status = wrapped_VL53L0X_perform_ref_spad_management( ref_spad_count,
sepp_nepp 6:fb11b746ceb5 3905 is_aperture_spads);
sepp_nepp 6:fb11b746ceb5 3906
sepp_nepp 6:fb11b746ceb5 3907 return status;
sepp_nepp 6:fb11b746ceb5 3908 }
sepp_nepp 6:fb11b746ceb5 3909
sepp_nepp 6:fb11b746ceb5 3910 /* Group PAL Init Functions */
sepp_nepp 7:3a1115c2556b 3911 VL53L0X_Error VL53L0X::VL53L0X_set_device_address( uint8_t device_address)
sepp_nepp 7:3a1115c2556b 3912 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3913
sepp_nepp 7:3a1115c2556b 3914 status = VL53L0X_write_byte( VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
sepp_nepp 6:fb11b746ceb5 3915 device_address / 2);
sepp_nepp 6:fb11b746ceb5 3916 return status;
sepp_nepp 6:fb11b746ceb5 3917 }
sepp_nepp 6:fb11b746ceb5 3918
sepp_nepp 7:3a1115c2556b 3919 VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config( uint8_t pin,
sepp_nepp 6:fb11b746ceb5 3920 VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
sepp_nepp 6:fb11b746ceb5 3921 VL53L0X_InterruptPolarity polarity)
sepp_nepp 7:3a1115c2556b 3922 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 3923 uint8_t data;
sepp_nepp 6:fb11b746ceb5 3924
sepp_nepp 6:fb11b746ceb5 3925 if (pin != 0) {
sepp_nepp 6:fb11b746ceb5 3926 status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
sepp_nepp 6:fb11b746ceb5 3927 } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
sepp_nepp 6:fb11b746ceb5 3928 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
sepp_nepp 6:fb11b746ceb5 3929 data = 0x10;
sepp_nepp 7:3a1115c2556b 3930 } else {data = 1;}
sepp_nepp 7:3a1115c2556b 3931
sepp_nepp 7:3a1115c2556b 3932 status = VL53L0X_write_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
sepp_nepp 6:fb11b746ceb5 3933
sepp_nepp 6:fb11b746ceb5 3934 } else {
sepp_nepp 6:fb11b746ceb5 3935 if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) {
sepp_nepp 6:fb11b746ceb5 3936
sepp_nepp 7:3a1115c2556b 3937 status |= VL53L0X_write_byte( 0xff, 0x01);
sepp_nepp 7:3a1115c2556b 3938 status |= VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 7:3a1115c2556b 3939 status |= VL53L0X_write_byte( 0xff, 0x00);
sepp_nepp 7:3a1115c2556b 3940 status |= VL53L0X_write_byte( 0x80, 0x01);
sepp_nepp 7:3a1115c2556b 3941 status |= VL53L0X_write_byte( 0x85, 0x02);
sepp_nepp 7:3a1115c2556b 3942 status |= VL53L0X_write_byte( 0xff, 0x04);
sepp_nepp 7:3a1115c2556b 3943 status |= VL53L0X_write_byte( 0xcd, 0x00);
sepp_nepp 7:3a1115c2556b 3944 status |= VL53L0X_write_byte( 0xcc, 0x11);
sepp_nepp 7:3a1115c2556b 3945 status |= VL53L0X_write_byte( 0xff, 0x07);
sepp_nepp 7:3a1115c2556b 3946 status |= VL53L0X_write_byte( 0xbe, 0x00);
sepp_nepp 7:3a1115c2556b 3947 status |= VL53L0X_write_byte( 0xff, 0x06);
sepp_nepp 7:3a1115c2556b 3948 status |= VL53L0X_write_byte( 0xcc, 0x09);
sepp_nepp 7:3a1115c2556b 3949 status |= VL53L0X_write_byte( 0xff, 0x00);
sepp_nepp 7:3a1115c2556b 3950 status |= VL53L0X_write_byte( 0xff, 0x01);
sepp_nepp 7:3a1115c2556b 3951 status |= VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 6:fb11b746ceb5 3952
sepp_nepp 6:fb11b746ceb5 3953 } else {
sepp_nepp 6:fb11b746ceb5 3954
sepp_nepp 6:fb11b746ceb5 3955 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3956 switch (functionality) {
sepp_nepp 6:fb11b746ceb5 3957 case VL53L0X_GPIOFUNCTIONALITY_OFF:
sepp_nepp 6:fb11b746ceb5 3958 data = 0x00;
sepp_nepp 6:fb11b746ceb5 3959 break;
sepp_nepp 6:fb11b746ceb5 3960 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
sepp_nepp 6:fb11b746ceb5 3961 data = 0x01;
sepp_nepp 6:fb11b746ceb5 3962 break;
sepp_nepp 6:fb11b746ceb5 3963 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
sepp_nepp 6:fb11b746ceb5 3964 data = 0x02;
sepp_nepp 6:fb11b746ceb5 3965 break;
sepp_nepp 6:fb11b746ceb5 3966 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
sepp_nepp 6:fb11b746ceb5 3967 data = 0x03;
sepp_nepp 6:fb11b746ceb5 3968 break;
sepp_nepp 6:fb11b746ceb5 3969 case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
sepp_nepp 6:fb11b746ceb5 3970 data = 0x04;
sepp_nepp 6:fb11b746ceb5 3971 break;
sepp_nepp 6:fb11b746ceb5 3972 default:
sepp_nepp 6:fb11b746ceb5 3973 status =
sepp_nepp 6:fb11b746ceb5 3974 VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
sepp_nepp 6:fb11b746ceb5 3975 }
sepp_nepp 6:fb11b746ceb5 3976 }
sepp_nepp 6:fb11b746ceb5 3977
sepp_nepp 6:fb11b746ceb5 3978 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 3979 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
sepp_nepp 6:fb11b746ceb5 3980 }
sepp_nepp 6:fb11b746ceb5 3981
sepp_nepp 6:fb11b746ceb5 3982 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 3983 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
sepp_nepp 6:fb11b746ceb5 3984 data = 0;
sepp_nepp 7:3a1115c2556b 3985 } else { data = (uint8_t)(1 << 4); }
sepp_nepp 7:3a1115c2556b 3986 status = VL53L0X_update_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
sepp_nepp 6:fb11b746ceb5 3987 }
sepp_nepp 6:fb11b746ceb5 3988
sepp_nepp 6:fb11b746ceb5 3989 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 3990 Data.Pin0GpioFunctionality = functionality; }
sepp_nepp 7:3a1115c2556b 3991
sepp_nepp 7:3a1115c2556b 3992 if (status == VL53L0X_ERROR_NONE) { status = VL53L0X_clear_interrupt_mask( 0); }
sepp_nepp 6:fb11b746ceb5 3993 }
sepp_nepp 6:fb11b746ceb5 3994 }
sepp_nepp 6:fb11b746ceb5 3995 return status;
sepp_nepp 6:fb11b746ceb5 3996 }
sepp_nepp 6:fb11b746ceb5 3997
sepp_nepp 7:3a1115c2556b 3998 VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable( uint8_t *p_enabled)
sepp_nepp 8:2fd7cb217068 3999 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 7:3a1115c2556b 4000 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled);
sepp_nepp 7:3a1115c2556b 4001 if (status == VL53L0X_ERROR_NONE) { *p_enabled = (*p_enabled & 1); }
sepp_nepp 6:fb11b746ceb5 4002 return status;
sepp_nepp 6:fb11b746ceb5 4003 }
sepp_nepp 6:fb11b746ceb5 4004
sepp_nepp 6:fb11b746ceb5 4005 uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
sepp_nepp 7:3a1115c2556b 4006 { /*!Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format*/
sepp_nepp 6:fb11b746ceb5 4007
sepp_nepp 6:fb11b746ceb5 4008 uint16_t encoded_timeout = 0;
sepp_nepp 6:fb11b746ceb5 4009 uint32_t ls_byte = 0;
sepp_nepp 6:fb11b746ceb5 4010 uint16_t ms_byte = 0;
sepp_nepp 6:fb11b746ceb5 4011
sepp_nepp 6:fb11b746ceb5 4012 if (timeout_macro_clks > 0) {
sepp_nepp 6:fb11b746ceb5 4013 ls_byte = timeout_macro_clks - 1;
sepp_nepp 6:fb11b746ceb5 4014
sepp_nepp 6:fb11b746ceb5 4015 while ((ls_byte & 0xFFFFFF00) > 0) {
sepp_nepp 6:fb11b746ceb5 4016 ls_byte = ls_byte >> 1;
sepp_nepp 6:fb11b746ceb5 4017 ms_byte++;
sepp_nepp 6:fb11b746ceb5 4018 }
sepp_nepp 7:3a1115c2556b 4019 encoded_timeout = (ms_byte << 8) + (uint16_t)(ls_byte & 0x000000FF);
sepp_nepp 6:fb11b746ceb5 4020 }
sepp_nepp 6:fb11b746ceb5 4021 return encoded_timeout;
sepp_nepp 6:fb11b746ceb5 4022 }
sepp_nepp 6:fb11b746ceb5 4023
sepp_nepp 8:2fd7cb217068 4024 VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id,
sepp_nepp 6:fb11b746ceb5 4025 uint32_t timeout_micro_secs)
sepp_nepp 8:2fd7cb217068 4026 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4027 uint8_t current_vcsel_pulse_period_p_clk;
sepp_nepp 6:fb11b746ceb5 4028 uint8_t msrc_encoded_time_out;
sepp_nepp 6:fb11b746ceb5 4029 uint16_t pre_range_encoded_time_out;
sepp_nepp 6:fb11b746ceb5 4030 uint16_t pre_range_time_out_m_clks;
sepp_nepp 6:fb11b746ceb5 4031 uint16_t msrc_range_time_out_m_clks;
sepp_nepp 6:fb11b746ceb5 4032 uint32_t final_range_time_out_m_clks;
sepp_nepp 6:fb11b746ceb5 4033 uint16_t final_range_encoded_time_out;
sepp_nepp 6:fb11b746ceb5 4034 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
sepp_nepp 6:fb11b746ceb5 4035
sepp_nepp 6:fb11b746ceb5 4036 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
sepp_nepp 6:fb11b746ceb5 4037 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
sepp_nepp 6:fb11b746ceb5 4038 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
sepp_nepp 6:fb11b746ceb5 4039
sepp_nepp 8:2fd7cb217068 4040 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 4041 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 4042
sepp_nepp 6:fb11b746ceb5 4043 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4044 msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(timeout_micro_secs,
sepp_nepp 6:fb11b746ceb5 4045 (uint8_t)current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 4046
sepp_nepp 6:fb11b746ceb5 4047 if (msrc_range_time_out_m_clks > 256) {
sepp_nepp 6:fb11b746ceb5 4048 msrc_encoded_time_out = 255;
sepp_nepp 6:fb11b746ceb5 4049 } else {
sepp_nepp 6:fb11b746ceb5 4050 msrc_encoded_time_out =
sepp_nepp 6:fb11b746ceb5 4051 (uint8_t)msrc_range_time_out_m_clks - 1;
sepp_nepp 6:fb11b746ceb5 4052 }
sepp_nepp 8:2fd7cb217068 4053 Data.LastEncodedTimeout = msrc_encoded_time_out;
sepp_nepp 6:fb11b746ceb5 4054 }
sepp_nepp 6:fb11b746ceb5 4055
sepp_nepp 6:fb11b746ceb5 4056 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4057 status = VL53L0X_write_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
sepp_nepp 6:fb11b746ceb5 4058 msrc_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 4059 }
sepp_nepp 6:fb11b746ceb5 4060 } else {
sepp_nepp 6:fb11b746ceb5 4061
sepp_nepp 6:fb11b746ceb5 4062 if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
sepp_nepp 6:fb11b746ceb5 4063
sepp_nepp 6:fb11b746ceb5 4064 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4065 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 4066 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 4067 pre_range_time_out_m_clks =
sepp_nepp 7:3a1115c2556b 4068 VL53L0X_calc_timeout_mclks(timeout_micro_secs,
sepp_nepp 6:fb11b746ceb5 4069 (uint8_t)current_vcsel_pulse_period_p_clk);
sepp_nepp 8:2fd7cb217068 4070 pre_range_encoded_time_out = VL53L0X_encode_timeout(pre_range_time_out_m_clks);
sepp_nepp 8:2fd7cb217068 4071 Data.LastEncodedTimeout = pre_range_encoded_time_out;
sepp_nepp 6:fb11b746ceb5 4072 }
sepp_nepp 6:fb11b746ceb5 4073
sepp_nepp 6:fb11b746ceb5 4074 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4075 status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
sepp_nepp 6:fb11b746ceb5 4076 pre_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 4077 }
sepp_nepp 6:fb11b746ceb5 4078
sepp_nepp 6:fb11b746ceb5 4079 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4080 Data.PreRangeTimeoutMicroSecs = timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 4081 }
sepp_nepp 6:fb11b746ceb5 4082 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
sepp_nepp 6:fb11b746ceb5 4083
sepp_nepp 6:fb11b746ceb5 4084 /* For the final range timeout, the pre-range timeout
sepp_nepp 6:fb11b746ceb5 4085 * must be added. To do this both final and pre-range
sepp_nepp 6:fb11b746ceb5 4086 * timeouts must be expressed in macro periods MClks
sepp_nepp 9:cb4c6d4e5030 4087 * because they have different vcsel periods. */
sepp_nepp 6:fb11b746ceb5 4088
sepp_nepp 7:3a1115c2556b 4089 VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
sepp_nepp 6:fb11b746ceb5 4090 pre_range_time_out_m_clks = 0;
sepp_nepp 6:fb11b746ceb5 4091 if (scheduler_sequence_steps.PreRangeOn) {
sepp_nepp 6:fb11b746ceb5 4092
sepp_nepp 6:fb11b746ceb5 4093 /* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp 7:3a1115c2556b 4094 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 4095 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 4096
sepp_nepp 6:fb11b746ceb5 4097 /* Retrieve PRE-RANGE Timeout in Macro periods
sepp_nepp 6:fb11b746ceb5 4098 * (MCLKS) */
sepp_nepp 6:fb11b746ceb5 4099 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4100 status = VL53L0X_read_word( 0x51, &pre_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 4101 pre_range_time_out_m_clks =
sepp_nepp 7:3a1115c2556b 4102 VL53L0X_decode_timeout( pre_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 4103 }
sepp_nepp 6:fb11b746ceb5 4104 }
sepp_nepp 6:fb11b746ceb5 4105
sepp_nepp 6:fb11b746ceb5 4106 /* Calculate FINAL RANGE Timeout in Macro Periods
sepp_nepp 6:fb11b746ceb5 4107 * (MCLKS) and add PRE-RANGE value
sepp_nepp 6:fb11b746ceb5 4108 */
sepp_nepp 6:fb11b746ceb5 4109 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4110 status = VL53L0X_get_vcsel_pulse_period( VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 4111 &current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 4112 }
sepp_nepp 6:fb11b746ceb5 4113 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 4114 final_range_time_out_m_clks =
sepp_nepp 7:3a1115c2556b 4115 VL53L0X_calc_timeout_mclks( timeout_micro_secs,
sepp_nepp 6:fb11b746ceb5 4116 (uint8_t) current_vcsel_pulse_period_p_clk);
sepp_nepp 6:fb11b746ceb5 4117
sepp_nepp 6:fb11b746ceb5 4118 final_range_time_out_m_clks += pre_range_time_out_m_clks;
sepp_nepp 6:fb11b746ceb5 4119 final_range_encoded_time_out =
sepp_nepp 6:fb11b746ceb5 4120 VL53L0X_encode_timeout(final_range_time_out_m_clks);
sepp_nepp 6:fb11b746ceb5 4121
sepp_nepp 6:fb11b746ceb5 4122 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4123 status = VL53L0X_write_word( 0x71, final_range_encoded_time_out);
sepp_nepp 6:fb11b746ceb5 4124 }
sepp_nepp 6:fb11b746ceb5 4125
sepp_nepp 6:fb11b746ceb5 4126 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4127 Data.FinalRangeTimeoutMicroSecs = timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 4128 }
sepp_nepp 6:fb11b746ceb5 4129 }
sepp_nepp 6:fb11b746ceb5 4130 } else {
sepp_nepp 6:fb11b746ceb5 4131 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4132 }
sepp_nepp 6:fb11b746ceb5 4133 }
sepp_nepp 6:fb11b746ceb5 4134 return status;
sepp_nepp 6:fb11b746ceb5 4135 }
sepp_nepp 6:fb11b746ceb5 4136
sepp_nepp 8:2fd7cb217068 4137 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
sepp_nepp 8:2fd7cb217068 4138 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4139 uint32_t final_range_timing_budget_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4140 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
sepp_nepp 6:fb11b746ceb5 4141 uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
sepp_nepp 6:fb11b746ceb5 4142 uint32_t start_overhead_micro_seconds = 1910;
sepp_nepp 6:fb11b746ceb5 4143 uint32_t end_overhead_micro_seconds = 960;
sepp_nepp 6:fb11b746ceb5 4144 uint32_t msrc_overhead_micro_seconds = 660;
sepp_nepp 6:fb11b746ceb5 4145 uint32_t tcc_overhead_micro_seconds = 590;
sepp_nepp 6:fb11b746ceb5 4146 uint32_t dss_overhead_micro_seconds = 690;
sepp_nepp 6:fb11b746ceb5 4147 uint32_t pre_range_overhead_micro_seconds = 660;
sepp_nepp 6:fb11b746ceb5 4148 uint32_t final_range_overhead_micro_seconds = 550;
sepp_nepp 6:fb11b746ceb5 4149 uint32_t pre_range_timeout_micro_seconds = 0;
sepp_nepp 6:fb11b746ceb5 4150 uint32_t c_min_timing_budget_micro_seconds = 20000;
sepp_nepp 6:fb11b746ceb5 4151 uint32_t sub_timeout = 0;
sepp_nepp 6:fb11b746ceb5 4152
sepp_nepp 7:3a1115c2556b 4153 if (measurement_timing_budget_micro_seconds < c_min_timing_budget_micro_seconds)
sepp_nepp 7:3a1115c2556b 4154 { status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 7:3a1115c2556b 4155 return status;
sepp_nepp 7:3a1115c2556b 4156 }
sepp_nepp 6:fb11b746ceb5 4157
sepp_nepp 6:fb11b746ceb5 4158 final_range_timing_budget_micro_seconds =
sepp_nepp 6:fb11b746ceb5 4159 measurement_timing_budget_micro_seconds -
sepp_nepp 6:fb11b746ceb5 4160 (start_overhead_micro_seconds + end_overhead_micro_seconds);
sepp_nepp 6:fb11b746ceb5 4161
sepp_nepp 7:3a1115c2556b 4162 status = VL53L0X_get_sequence_step_enables( &scheduler_sequence_steps);
sepp_nepp 6:fb11b746ceb5 4163
sepp_nepp 6:fb11b746ceb5 4164 if (status == VL53L0X_ERROR_NONE &&
sepp_nepp 6:fb11b746ceb5 4165 (scheduler_sequence_steps.TccOn ||
sepp_nepp 6:fb11b746ceb5 4166 scheduler_sequence_steps.MsrcOn ||
sepp_nepp 6:fb11b746ceb5 4167 scheduler_sequence_steps.DssOn)) {
sepp_nepp 6:fb11b746ceb5 4168
sepp_nepp 6:fb11b746ceb5 4169 /* TCC, MSRC and DSS all share the same timeout */
sepp_nepp 7:3a1115c2556b 4170 status = get_sequence_step_timeout( VL53L0X_SEQUENCESTEP_MSRC,
sepp_nepp 6:fb11b746ceb5 4171 &msrc_dcc_tcc_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 4172
sepp_nepp 6:fb11b746ceb5 4173 /* Subtract the TCC, MSRC and DSS timeouts if they are
sepp_nepp 6:fb11b746ceb5 4174 * enabled. */
sepp_nepp 6:fb11b746ceb5 4175
sepp_nepp 6:fb11b746ceb5 4176 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 4177 return status;
sepp_nepp 6:fb11b746ceb5 4178 }
sepp_nepp 6:fb11b746ceb5 4179
sepp_nepp 6:fb11b746ceb5 4180 /* TCC */
sepp_nepp 6:fb11b746ceb5 4181 if (scheduler_sequence_steps.TccOn) {
sepp_nepp 6:fb11b746ceb5 4182
sepp_nepp 6:fb11b746ceb5 4183 sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
sepp_nepp 6:fb11b746ceb5 4184 + tcc_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4185
sepp_nepp 6:fb11b746ceb5 4186 if (sub_timeout <
sepp_nepp 6:fb11b746ceb5 4187 final_range_timing_budget_micro_seconds) {
sepp_nepp 6:fb11b746ceb5 4188 final_range_timing_budget_micro_seconds -=
sepp_nepp 6:fb11b746ceb5 4189 sub_timeout;
sepp_nepp 6:fb11b746ceb5 4190 } else {
sepp_nepp 6:fb11b746ceb5 4191 /* Requested timeout too big. */
sepp_nepp 6:fb11b746ceb5 4192 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4193 }
sepp_nepp 6:fb11b746ceb5 4194 }
sepp_nepp 6:fb11b746ceb5 4195
sepp_nepp 7:3a1115c2556b 4196 if (status != VL53L0X_ERROR_NONE) {return status;}
sepp_nepp 6:fb11b746ceb5 4197
sepp_nepp 6:fb11b746ceb5 4198 /* DSS */
sepp_nepp 6:fb11b746ceb5 4199 if (scheduler_sequence_steps.DssOn) {
sepp_nepp 6:fb11b746ceb5 4200
sepp_nepp 6:fb11b746ceb5 4201 sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 4202 dss_overhead_micro_seconds);
sepp_nepp 6:fb11b746ceb5 4203
sepp_nepp 6:fb11b746ceb5 4204 if (sub_timeout < final_range_timing_budget_micro_seconds) {
sepp_nepp 6:fb11b746ceb5 4205 final_range_timing_budget_micro_seconds
sepp_nepp 6:fb11b746ceb5 4206 -= sub_timeout;
sepp_nepp 6:fb11b746ceb5 4207 } else {
sepp_nepp 6:fb11b746ceb5 4208 /* Requested timeout too big. */
sepp_nepp 6:fb11b746ceb5 4209 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4210 }
sepp_nepp 6:fb11b746ceb5 4211 } else if (scheduler_sequence_steps.MsrcOn) {
sepp_nepp 6:fb11b746ceb5 4212 /* MSRC */
sepp_nepp 6:fb11b746ceb5 4213 sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 4214 msrc_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4215
sepp_nepp 6:fb11b746ceb5 4216 if (sub_timeout < final_range_timing_budget_micro_seconds) {
sepp_nepp 6:fb11b746ceb5 4217 final_range_timing_budget_micro_seconds
sepp_nepp 6:fb11b746ceb5 4218 -= sub_timeout;
sepp_nepp 6:fb11b746ceb5 4219 } else {
sepp_nepp 6:fb11b746ceb5 4220 /* Requested timeout too big. */
sepp_nepp 6:fb11b746ceb5 4221 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4222 }
sepp_nepp 6:fb11b746ceb5 4223 }
sepp_nepp 6:fb11b746ceb5 4224 }
sepp_nepp 6:fb11b746ceb5 4225
sepp_nepp 9:cb4c6d4e5030 4226 if (status != VL53L0X_ERROR_NONE) { return status; }
sepp_nepp 6:fb11b746ceb5 4227
sepp_nepp 6:fb11b746ceb5 4228 if (scheduler_sequence_steps.PreRangeOn) {
sepp_nepp 6:fb11b746ceb5 4229
sepp_nepp 6:fb11b746ceb5 4230 /* Subtract the Pre-range timeout if enabled. */
sepp_nepp 6:fb11b746ceb5 4231
sepp_nepp 8:2fd7cb217068 4232 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp 6:fb11b746ceb5 4233 &pre_range_timeout_micro_seconds);
sepp_nepp 6:fb11b746ceb5 4234
sepp_nepp 6:fb11b746ceb5 4235 sub_timeout = pre_range_timeout_micro_seconds +
sepp_nepp 6:fb11b746ceb5 4236 pre_range_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4237
sepp_nepp 6:fb11b746ceb5 4238 if (sub_timeout < final_range_timing_budget_micro_seconds) {
sepp_nepp 6:fb11b746ceb5 4239 final_range_timing_budget_micro_seconds -= sub_timeout;
sepp_nepp 6:fb11b746ceb5 4240 } else {
sepp_nepp 6:fb11b746ceb5 4241 /* Requested timeout too big. */
sepp_nepp 6:fb11b746ceb5 4242 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4243 }
sepp_nepp 6:fb11b746ceb5 4244 }
sepp_nepp 6:fb11b746ceb5 4245
sepp_nepp 6:fb11b746ceb5 4246 if (status == VL53L0X_ERROR_NONE &&
sepp_nepp 6:fb11b746ceb5 4247 scheduler_sequence_steps.FinalRangeOn) {
sepp_nepp 6:fb11b746ceb5 4248
sepp_nepp 6:fb11b746ceb5 4249 final_range_timing_budget_micro_seconds -=
sepp_nepp 6:fb11b746ceb5 4250 final_range_overhead_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4251
sepp_nepp 6:fb11b746ceb5 4252 /* Final Range Timeout
sepp_nepp 6:fb11b746ceb5 4253 * Note that the final range timeout is determined by the timing
sepp_nepp 6:fb11b746ceb5 4254 * budget and the sum of all other timeouts within the sequence.
sepp_nepp 6:fb11b746ceb5 4255 * If there is no room for the final range timeout, then an error
sepp_nepp 6:fb11b746ceb5 4256 * will be set. Otherwise the remaining time will be applied to
sepp_nepp 6:fb11b746ceb5 4257 * the final range.
sepp_nepp 6:fb11b746ceb5 4258 */
sepp_nepp 7:3a1115c2556b 4259 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 4260 final_range_timing_budget_micro_seconds);
sepp_nepp 8:2fd7cb217068 4261 Data.CurrentParameters.MeasurementTimingBudgetMicroSeconds = measurement_timing_budget_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4262 }
sepp_nepp 6:fb11b746ceb5 4263
sepp_nepp 6:fb11b746ceb5 4264 return status;
sepp_nepp 6:fb11b746ceb5 4265 }
sepp_nepp 6:fb11b746ceb5 4266
sepp_nepp 8:2fd7cb217068 4267 VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
sepp_nepp 7:3a1115c2556b 4268 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 8:2fd7cb217068 4269 status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
sepp_nepp 6:fb11b746ceb5 4270 return status;
sepp_nepp 6:fb11b746ceb5 4271 }
sepp_nepp 6:fb11b746ceb5 4272
sepp_nepp 8:2fd7cb217068 4273 VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled)
sepp_nepp 8:2fd7cb217068 4274 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4275 uint8_t sequence_config = 0;
sepp_nepp 6:fb11b746ceb5 4276 uint8_t sequence_config_new = 0;
sepp_nepp 6:fb11b746ceb5 4277 uint32_t measurement_timing_budget_micro_seconds;
sepp_nepp 6:fb11b746ceb5 4278
sepp_nepp 7:3a1115c2556b 4279 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
sepp_nepp 6:fb11b746ceb5 4280
sepp_nepp 6:fb11b746ceb5 4281 sequence_config_new = sequence_config;
sepp_nepp 6:fb11b746ceb5 4282
sepp_nepp 6:fb11b746ceb5 4283 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 4284 if (sequence_step_enabled == 1) {
sepp_nepp 6:fb11b746ceb5 4285
sepp_nepp 6:fb11b746ceb5 4286 /* Enable requested sequence step
sepp_nepp 6:fb11b746ceb5 4287 */
sepp_nepp 6:fb11b746ceb5 4288 switch (sequence_step_id) {
sepp_nepp 6:fb11b746ceb5 4289 case VL53L0X_SEQUENCESTEP_TCC:
sepp_nepp 6:fb11b746ceb5 4290 sequence_config_new |= 0x10;
sepp_nepp 6:fb11b746ceb5 4291 break;
sepp_nepp 6:fb11b746ceb5 4292 case VL53L0X_SEQUENCESTEP_DSS:
sepp_nepp 6:fb11b746ceb5 4293 sequence_config_new |= 0x28;
sepp_nepp 6:fb11b746ceb5 4294 break;
sepp_nepp 6:fb11b746ceb5 4295 case VL53L0X_SEQUENCESTEP_MSRC:
sepp_nepp 6:fb11b746ceb5 4296 sequence_config_new |= 0x04;
sepp_nepp 6:fb11b746ceb5 4297 break;
sepp_nepp 6:fb11b746ceb5 4298 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sepp_nepp 6:fb11b746ceb5 4299 sequence_config_new |= 0x40;
sepp_nepp 6:fb11b746ceb5 4300 break;
sepp_nepp 6:fb11b746ceb5 4301 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 4302 sequence_config_new |= 0x80;
sepp_nepp 6:fb11b746ceb5 4303 break;
sepp_nepp 6:fb11b746ceb5 4304 default:
sepp_nepp 6:fb11b746ceb5 4305 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4306 }
sepp_nepp 6:fb11b746ceb5 4307 } else {
sepp_nepp 7:3a1115c2556b 4308 /* Disable requested sequence step */
sepp_nepp 6:fb11b746ceb5 4309 switch (sequence_step_id) {
sepp_nepp 6:fb11b746ceb5 4310 case VL53L0X_SEQUENCESTEP_TCC:
sepp_nepp 6:fb11b746ceb5 4311 sequence_config_new &= 0xef;
sepp_nepp 6:fb11b746ceb5 4312 break;
sepp_nepp 6:fb11b746ceb5 4313 case VL53L0X_SEQUENCESTEP_DSS:
sepp_nepp 6:fb11b746ceb5 4314 sequence_config_new &= 0xd7;
sepp_nepp 6:fb11b746ceb5 4315 break;
sepp_nepp 6:fb11b746ceb5 4316 case VL53L0X_SEQUENCESTEP_MSRC:
sepp_nepp 6:fb11b746ceb5 4317 sequence_config_new &= 0xfb;
sepp_nepp 6:fb11b746ceb5 4318 break;
sepp_nepp 6:fb11b746ceb5 4319 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sepp_nepp 6:fb11b746ceb5 4320 sequence_config_new &= 0xbf;
sepp_nepp 6:fb11b746ceb5 4321 break;
sepp_nepp 6:fb11b746ceb5 4322 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 4323 sequence_config_new &= 0x7f;
sepp_nepp 6:fb11b746ceb5 4324 break;
sepp_nepp 6:fb11b746ceb5 4325 default:
sepp_nepp 6:fb11b746ceb5 4326 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4327 }
sepp_nepp 6:fb11b746ceb5 4328 }
sepp_nepp 6:fb11b746ceb5 4329 }
sepp_nepp 6:fb11b746ceb5 4330
sepp_nepp 6:fb11b746ceb5 4331 if (sequence_config_new != sequence_config) {
sepp_nepp 6:fb11b746ceb5 4332 /* Apply New Setting */
sepp_nepp 6:fb11b746ceb5 4333 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4334 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new);
sepp_nepp 6:fb11b746ceb5 4335 }
sepp_nepp 6:fb11b746ceb5 4336 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4337 Data.SequenceConfig = sequence_config_new;}
sepp_nepp 6:fb11b746ceb5 4338
sepp_nepp 6:fb11b746ceb5 4339 /* Recalculate timing budget */
sepp_nepp 6:fb11b746ceb5 4340 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4341 measurement_timing_budget_micro_seconds = Data.CurrentParameters.MeasurementTimingBudgetMicroSeconds ;
sepp_nepp 8:2fd7cb217068 4342 VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
sepp_nepp 6:fb11b746ceb5 4343 }
sepp_nepp 6:fb11b746ceb5 4344 }
sepp_nepp 6:fb11b746ceb5 4345
sepp_nepp 6:fb11b746ceb5 4346 return status;
sepp_nepp 6:fb11b746ceb5 4347 }
sepp_nepp 6:fb11b746ceb5 4348
sepp_nepp 7:3a1115c2556b 4349 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable( uint16_t limit_check_id,
sepp_nepp 6:fb11b746ceb5 4350 uint8_t limit_check_enable)
sepp_nepp 8:2fd7cb217068 4351 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4352 FixPoint1616_t temp_fix1616 = 0;
sepp_nepp 6:fb11b746ceb5 4353 uint8_t limit_check_enable_int = 0;
sepp_nepp 6:fb11b746ceb5 4354 uint8_t limit_check_disable = 0;
sepp_nepp 6:fb11b746ceb5 4355 uint8_t temp8;
sepp_nepp 6:fb11b746ceb5 4356
sepp_nepp 6:fb11b746ceb5 4357 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
sepp_nepp 6:fb11b746ceb5 4358 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4359 } else {
sepp_nepp 6:fb11b746ceb5 4360 if (limit_check_enable == 0) {
sepp_nepp 6:fb11b746ceb5 4361 temp_fix1616 = 0;
sepp_nepp 6:fb11b746ceb5 4362 limit_check_enable_int = 0;
sepp_nepp 6:fb11b746ceb5 4363 limit_check_disable = 1;
sepp_nepp 6:fb11b746ceb5 4364 } else {
sepp_nepp 8:2fd7cb217068 4365 temp_fix1616 = Data.CurrentParameters.LimitChecksValue[limit_check_id];
sepp_nepp 6:fb11b746ceb5 4366 limit_check_disable = 0;
sepp_nepp 6:fb11b746ceb5 4367 /* this to be sure to have either 0 or 1 */
sepp_nepp 6:fb11b746ceb5 4368 limit_check_enable_int = 1;
sepp_nepp 6:fb11b746ceb5 4369 }
sepp_nepp 6:fb11b746ceb5 4370
sepp_nepp 6:fb11b746ceb5 4371 switch (limit_check_id) {
sepp_nepp 6:fb11b746ceb5 4372
sepp_nepp 6:fb11b746ceb5 4373 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
sepp_nepp 6:fb11b746ceb5 4374 /* internal computation: */
sepp_nepp 8:2fd7cb217068 4375 Data.CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_enable_int;
sepp_nepp 6:fb11b746ceb5 4376 break;
sepp_nepp 6:fb11b746ceb5 4377
sepp_nepp 6:fb11b746ceb5 4378 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
sepp_nepp 7:3a1115c2556b 4379 status = VL53L0X_write_word( VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 6:fb11b746ceb5 4380 VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616));
sepp_nepp 6:fb11b746ceb5 4381 break;
sepp_nepp 6:fb11b746ceb5 4382
sepp_nepp 7:3a1115c2556b 4383 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:/* internal computation: */
sepp_nepp 8:2fd7cb217068 4384 Data.CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_enable_int;
sepp_nepp 6:fb11b746ceb5 4385 break;
sepp_nepp 6:fb11b746ceb5 4386
sepp_nepp 7:3a1115c2556b 4387 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:/* internal computation: */
sepp_nepp 8:2fd7cb217068 4388 Data.CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_enable_int;
sepp_nepp 6:fb11b746ceb5 4389 break;
sepp_nepp 6:fb11b746ceb5 4390
sepp_nepp 6:fb11b746ceb5 4391 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
sepp_nepp 6:fb11b746ceb5 4392 temp8 = (uint8_t)(limit_check_disable << 1);
sepp_nepp 7:3a1115c2556b 4393 status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL,
sepp_nepp 6:fb11b746ceb5 4394 0xFE, temp8);
sepp_nepp 6:fb11b746ceb5 4395 break;
sepp_nepp 6:fb11b746ceb5 4396
sepp_nepp 6:fb11b746ceb5 4397 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
sepp_nepp 6:fb11b746ceb5 4398
sepp_nepp 6:fb11b746ceb5 4399 temp8 = (uint8_t)(limit_check_disable << 4);
sepp_nepp 7:3a1115c2556b 4400 status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL,
sepp_nepp 6:fb11b746ceb5 4401 0xEF, temp8);
sepp_nepp 6:fb11b746ceb5 4402 break;
sepp_nepp 6:fb11b746ceb5 4403
sepp_nepp 6:fb11b746ceb5 4404 default:
sepp_nepp 6:fb11b746ceb5 4405 status = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 6:fb11b746ceb5 4406 }
sepp_nepp 6:fb11b746ceb5 4407 }
sepp_nepp 6:fb11b746ceb5 4408
sepp_nepp 6:fb11b746ceb5 4409 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 4410 if (limit_check_enable == 0) {
sepp_nepp 8:2fd7cb217068 4411 Data.CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
sepp_nepp 6:fb11b746ceb5 4412 } else {
sepp_nepp 8:2fd7cb217068 4413 Data.CurrentParameters.LimitChecksEnable[limit_check_id] = 1;
sepp_nepp 6:fb11b746ceb5 4414 }
sepp_nepp 6:fb11b746ceb5 4415 }
sepp_nepp 6:fb11b746ceb5 4416 return status;
sepp_nepp 6:fb11b746ceb5 4417 }
sepp_nepp 6:fb11b746ceb5 4418
sepp_nepp 7:3a1115c2556b 4419 VL53L0X_Error VL53L0X::VL53L0X_static_init(void)
sepp_nepp 8:2fd7cb217068 4420 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4421 VL53L0X_DeviceParameters_t current_parameters = {0};
sepp_nepp 6:fb11b746ceb5 4422 uint8_t *p_tuning_setting_buffer;
sepp_nepp 6:fb11b746ceb5 4423 uint16_t tempword = 0;
sepp_nepp 6:fb11b746ceb5 4424 uint8_t tempbyte = 0;
sepp_nepp 6:fb11b746ceb5 4425 uint8_t use_internal_tuning_settings = 0;
sepp_nepp 6:fb11b746ceb5 4426 uint32_t count = 0;
sepp_nepp 6:fb11b746ceb5 4427 uint8_t is_aperture_spads = 0;
sepp_nepp 6:fb11b746ceb5 4428 uint32_t ref_spad_count = 0;
sepp_nepp 6:fb11b746ceb5 4429 uint8_t aperture_spads = 0;
sepp_nepp 6:fb11b746ceb5 4430 uint8_t vcsel_pulse_period_pclk;
sepp_nepp 6:fb11b746ceb5 4431 uint32_t seq_timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 4432
sepp_nepp 7:3a1115c2556b 4433 status = VL53L0X_get_info_from_device( 1);
sepp_nepp 6:fb11b746ceb5 4434
sepp_nepp 6:fb11b746ceb5 4435 /* set the ref spad from NVM */
sepp_nepp 8:2fd7cb217068 4436 count = (uint32_t)Data.ReferenceSpadCount;
sepp_nepp 8:2fd7cb217068 4437 aperture_spads = Data.ReferenceSpadType;
sepp_nepp 6:fb11b746ceb5 4438
sepp_nepp 6:fb11b746ceb5 4439 /* NVM value invalid */
sepp_nepp 6:fb11b746ceb5 4440 if ((aperture_spads > 1) ||
sepp_nepp 6:fb11b746ceb5 4441 ((aperture_spads == 1) && (count > 32)) ||
sepp_nepp 6:fb11b746ceb5 4442 ((aperture_spads == 0) && (count > 12))) {
sepp_nepp 7:3a1115c2556b 4443 status = wrapped_VL53L0X_perform_ref_spad_management( &ref_spad_count,
sepp_nepp 6:fb11b746ceb5 4444 &is_aperture_spads);
sepp_nepp 6:fb11b746ceb5 4445 } else {
sepp_nepp 7:3a1115c2556b 4446 status = VL53L0X_set_reference_spads( count, aperture_spads);
sepp_nepp 6:fb11b746ceb5 4447 }
sepp_nepp 6:fb11b746ceb5 4448
sepp_nepp 6:fb11b746ceb5 4449 /* Initialize tuning settings buffer to prevent compiler warning. */
sepp_nepp 6:fb11b746ceb5 4450 p_tuning_setting_buffer = DefaultTuningSettings;
sepp_nepp 6:fb11b746ceb5 4451
sepp_nepp 6:fb11b746ceb5 4452 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4453 use_internal_tuning_settings = Data.UseInternalTuningSettings;
sepp_nepp 6:fb11b746ceb5 4454
sepp_nepp 6:fb11b746ceb5 4455 if (use_internal_tuning_settings == 0) {
sepp_nepp 8:2fd7cb217068 4456 p_tuning_setting_buffer = Data.pTuningSettingsPointer; }
sepp_nepp 7:3a1115c2556b 4457 else { p_tuning_setting_buffer = DefaultTuningSettings; }
sepp_nepp 6:fb11b746ceb5 4458
sepp_nepp 6:fb11b746ceb5 4459 }
sepp_nepp 6:fb11b746ceb5 4460
sepp_nepp 6:fb11b746ceb5 4461 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4462 status = VL53L0X_load_tuning_settings( p_tuning_setting_buffer); }
sepp_nepp 6:fb11b746ceb5 4463
sepp_nepp 6:fb11b746ceb5 4464 /* Set interrupt config to new sample ready */
sepp_nepp 6:fb11b746ceb5 4465 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4466 status = VL53L0X_set_gpio_config( 0, 0,
sepp_nepp 6:fb11b746ceb5 4467 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
sepp_nepp 6:fb11b746ceb5 4468 VL53L0X_INTERRUPTPOLARITY_LOW);
sepp_nepp 6:fb11b746ceb5 4469 }
sepp_nepp 6:fb11b746ceb5 4470
sepp_nepp 6:fb11b746ceb5 4471 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4472 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 4473 status |= VL53L0X_read_word ( 0x84, &tempword);
sepp_nepp 7:3a1115c2556b 4474 status |= VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 4475 }
sepp_nepp 6:fb11b746ceb5 4476
sepp_nepp 6:fb11b746ceb5 4477 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4478 Data.OscFrequencyMHz = VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword) ;
sepp_nepp 6:fb11b746ceb5 4479 }
sepp_nepp 6:fb11b746ceb5 4480
sepp_nepp 6:fb11b746ceb5 4481 /* After static init, some device parameters may be changed,
sepp_nepp 6:fb11b746ceb5 4482 * so update them */
sepp_nepp 6:fb11b746ceb5 4483 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4484 status = VL53L0X_get_device_parameters( &current_parameters);
sepp_nepp 6:fb11b746ceb5 4485 }
sepp_nepp 6:fb11b746ceb5 4486
sepp_nepp 6:fb11b746ceb5 4487 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4488 status = VL53L0X_get_fraction_enable( &tempbyte);
sepp_nepp 6:fb11b746ceb5 4489 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4490 Data.RangeFractionalEnable = tempbyte;
sepp_nepp 6:fb11b746ceb5 4491 }
sepp_nepp 6:fb11b746ceb5 4492 }
sepp_nepp 6:fb11b746ceb5 4493
sepp_nepp 6:fb11b746ceb5 4494 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4495 Data.CurrentParameters = current_parameters;
sepp_nepp 6:fb11b746ceb5 4496 }
sepp_nepp 6:fb11b746ceb5 4497
sepp_nepp 6:fb11b746ceb5 4498 /* read the sequence config and save it */
sepp_nepp 6:fb11b746ceb5 4499 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4500 status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
sepp_nepp 6:fb11b746ceb5 4501 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4502 Data.SequenceConfig = tempbyte;
sepp_nepp 6:fb11b746ceb5 4503 }
sepp_nepp 6:fb11b746ceb5 4504 }
sepp_nepp 6:fb11b746ceb5 4505
sepp_nepp 6:fb11b746ceb5 4506 /* Disable MSRC and TCC by default */
sepp_nepp 6:fb11b746ceb5 4507 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4508 status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC, 0);
sepp_nepp 6:fb11b746ceb5 4509 }
sepp_nepp 6:fb11b746ceb5 4510
sepp_nepp 6:fb11b746ceb5 4511 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4512 status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC, 0);
sepp_nepp 6:fb11b746ceb5 4513 }
sepp_nepp 6:fb11b746ceb5 4514
sepp_nepp 6:fb11b746ceb5 4515 /* Set PAL State to standby */
sepp_nepp 6:fb11b746ceb5 4516 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4517 Data.PalState = VL53L0X_STATE_IDLE;
sepp_nepp 6:fb11b746ceb5 4518 }
sepp_nepp 6:fb11b746ceb5 4519
sepp_nepp 6:fb11b746ceb5 4520 /* Store pre-range vcsel period */
sepp_nepp 6:fb11b746ceb5 4521 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4522 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,&vcsel_pulse_period_pclk);
sepp_nepp 6:fb11b746ceb5 4523 }
sepp_nepp 6:fb11b746ceb5 4524
sepp_nepp 6:fb11b746ceb5 4525 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4526 Data.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
sepp_nepp 6:fb11b746ceb5 4527 }
sepp_nepp 6:fb11b746ceb5 4528
sepp_nepp 6:fb11b746ceb5 4529 /* Store final-range vcsel period */
sepp_nepp 6:fb11b746ceb5 4530 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4531 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
sepp_nepp 6:fb11b746ceb5 4532 &vcsel_pulse_period_pclk);
sepp_nepp 6:fb11b746ceb5 4533 }
sepp_nepp 6:fb11b746ceb5 4534
sepp_nepp 6:fb11b746ceb5 4535 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4536 Data.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
sepp_nepp 6:fb11b746ceb5 4537 }
sepp_nepp 6:fb11b746ceb5 4538
sepp_nepp 6:fb11b746ceb5 4539 /* Store pre-range timeout */
sepp_nepp 6:fb11b746ceb5 4540 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4541 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&seq_timeout_micro_secs);
sepp_nepp 6:fb11b746ceb5 4542 }
sepp_nepp 6:fb11b746ceb5 4543
sepp_nepp 6:fb11b746ceb5 4544 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4545 Data.PreRangeTimeoutMicroSecs = seq_timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 4546 }
sepp_nepp 6:fb11b746ceb5 4547
sepp_nepp 6:fb11b746ceb5 4548 /* Store final-range timeout */
sepp_nepp 6:fb11b746ceb5 4549 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4550 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&seq_timeout_micro_secs);
sepp_nepp 6:fb11b746ceb5 4551 }
sepp_nepp 6:fb11b746ceb5 4552
sepp_nepp 6:fb11b746ceb5 4553 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 8:2fd7cb217068 4554 Data.FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs;
sepp_nepp 6:fb11b746ceb5 4555 }
sepp_nepp 6:fb11b746ceb5 4556 return status;
sepp_nepp 6:fb11b746ceb5 4557 }
sepp_nepp 6:fb11b746ceb5 4558
sepp_nepp 7:3a1115c2556b 4559 VL53L0X_Error VL53L0X::VL53L0X_stop_measurement(void)
sepp_nepp 8:2fd7cb217068 4560 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4561
sepp_nepp 7:3a1115c2556b 4562 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START,
sepp_nepp 6:fb11b746ceb5 4563 VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
sepp_nepp 6:fb11b746ceb5 4564
sepp_nepp 7:3a1115c2556b 4565 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 4566 status = VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 7:3a1115c2556b 4567 status = VL53L0X_write_byte( 0x91, 0x00);
sepp_nepp 7:3a1115c2556b 4568 status = VL53L0X_write_byte( 0x00, 0x01);
sepp_nepp 7:3a1115c2556b 4569 status = VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 6:fb11b746ceb5 4570
sepp_nepp 6:fb11b746ceb5 4571 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 6:fb11b746ceb5 4572 /* Set PAL State to Idle */
sepp_nepp 8:2fd7cb217068 4573 Data.PalState = VL53L0X_STATE_IDLE;
sepp_nepp 6:fb11b746ceb5 4574 }
sepp_nepp 6:fb11b746ceb5 4575
sepp_nepp 6:fb11b746ceb5 4576 /* Check if need to apply interrupt settings */
sepp_nepp 6:fb11b746ceb5 4577 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4578 status = VL53L0X_check_and_load_interrupt_settings( 0);
sepp_nepp 6:fb11b746ceb5 4579 }
sepp_nepp 6:fb11b746ceb5 4580 return status;
sepp_nepp 6:fb11b746ceb5 4581 }
sepp_nepp 6:fb11b746ceb5 4582
sepp_nepp 8:2fd7cb217068 4583 VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(uint32_t *p_stop_status)
sepp_nepp 8:2fd7cb217068 4584 { VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 6:fb11b746ceb5 4585 uint8_t byte = 0;
sepp_nepp 6:fb11b746ceb5 4586
sepp_nepp 6:fb11b746ceb5 4587
sepp_nepp 7:3a1115c2556b 4588 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 6:fb11b746ceb5 4589
sepp_nepp 6:fb11b746ceb5 4590 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4591 status = VL53L0X_read_byte( 0x04, &byte);
sepp_nepp 6:fb11b746ceb5 4592 }
sepp_nepp 6:fb11b746ceb5 4593
sepp_nepp 6:fb11b746ceb5 4594 if (status == VL53L0X_ERROR_NONE) {
sepp_nepp 7:3a1115c2556b 4595 status = VL53L0X_write_byte( 0xFF, 0x0);
sepp_nepp 6:fb11b746ceb5 4596 }
sepp_nepp 6:fb11b746ceb5 4597
sepp_nepp 6:fb11b746ceb5 4598 *p_stop_status = byte;
sepp_nepp 6:fb11b746ceb5 4599
sepp_nepp 6:fb11b746ceb5 4600 if (byte == 0) {
sepp_nepp 7:3a1115c2556b 4601 status = VL53L0X_write_byte( 0x80, 0x01);
sepp_nepp 7:3a1115c2556b 4602 status = VL53L0X_write_byte( 0xFF, 0x01);
sepp_nepp 7:3a1115c2556b 4603 status = VL53L0X_write_byte( 0x00, 0x00);
sepp_nepp 8:2fd7cb217068 4604 status = VL53L0X_write_byte( 0x91,Data.StopVariable);
sepp_nepp 7:3a1115c2556b 4605 status = VL53L0X_write_byte( 0x00, 0x01);
sepp_nepp 7:3a1115c2556b 4606 status = VL53L0X_write_byte( 0xFF, 0x00);
sepp_nepp 7:3a1115c2556b 4607 status = VL53L0X_write_byte( 0x80, 0x00);
sepp_nepp 6:fb11b746ceb5 4608 }
sepp_nepp 6:fb11b746ceb5 4609 return status;
sepp_nepp 6:fb11b746ceb5 4610 }
sepp_nepp 6:fb11b746ceb5 4611
sepp_nepp 6:fb11b746ceb5 4612 /******************************************************************************/
sepp_nepp 6:fb11b746ceb5 4613
sepp_nepp 6:fb11b746ceb5 4614 /****************** Write and read functions from I2C *************************/
sepp_nepp 6:fb11b746ceb5 4615
sepp_nepp 7:3a1115c2556b 4616 VL53L0X_Error VL53L0X::VL53L0X_read_multi( uint8_t index, uint8_t *p_data, uint32_t count)
sepp_nepp 9:cb4c6d4e5030 4617 { if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
sepp_nepp 9:cb4c6d4e5030 4618 return VL53L0X_ERROR_INVALID_PARAMS;}
sepp_nepp 9:cb4c6d4e5030 4619 else { return VL53L0X_i2c_read(index, p_data, (uint16_t)count); }
sepp_nepp 6:fb11b746ceb5 4620 }
sepp_nepp 6:fb11b746ceb5 4621
sepp_nepp 7:3a1115c2556b 4622 VL53L0X_Error VL53L0X::VL53L0X_write_byte( uint8_t index, uint8_t data)
sepp_nepp 9:cb4c6d4e5030 4623 { return VL53L0X_i2c_write(index, &data, 1);
sepp_nepp 6:fb11b746ceb5 4624 }
sepp_nepp 6:fb11b746ceb5 4625
sepp_nepp 7:3a1115c2556b 4626 VL53L0X_Error VL53L0X::VL53L0X_write_word( uint8_t index, uint16_t data)
sepp_nepp 8:2fd7cb217068 4627 { int status;
sepp_nepp 6:fb11b746ceb5 4628 uint8_t buffer[2];
sepp_nepp 6:fb11b746ceb5 4629
sepp_nepp 6:fb11b746ceb5 4630 buffer[0] = data >> 8;
sepp_nepp 6:fb11b746ceb5 4631 buffer[1] = data & 0x00FF;
sepp_nepp 7:3a1115c2556b 4632 status = VL53L0X_i2c_write(index, (uint8_t *)buffer, 2);
sepp_nepp 6:fb11b746ceb5 4633 return status;
sepp_nepp 6:fb11b746ceb5 4634 }
sepp_nepp 6:fb11b746ceb5 4635
sepp_nepp 7:3a1115c2556b 4636 VL53L0X_Error VL53L0X::VL53L0X_write_dword( uint8_t index, uint32_t data)
sepp_nepp 8:2fd7cb217068 4637 { int status;
sepp_nepp 6:fb11b746ceb5 4638 uint8_t buffer[4];
sepp_nepp 6:fb11b746ceb5 4639
sepp_nepp 6:fb11b746ceb5 4640 buffer[0] = (data >> 24) & 0xFF;
sepp_nepp 6:fb11b746ceb5 4641 buffer[1] = (data >> 16) & 0xFF;
sepp_nepp 6:fb11b746ceb5 4642 buffer[2] = (data >> 8) & 0xFF;
sepp_nepp 6:fb11b746ceb5 4643 buffer[3] = (data >> 0) & 0xFF;
sepp_nepp 7:3a1115c2556b 4644 status = VL53L0X_i2c_write(index, (uint8_t *)buffer, 4);
sepp_nepp 6:fb11b746ceb5 4645 return status;
sepp_nepp 6:fb11b746ceb5 4646 }
sepp_nepp 6:fb11b746ceb5 4647
sepp_nepp 7:3a1115c2556b 4648 VL53L0X_Error VL53L0X::VL53L0X_read_byte( uint8_t index, uint8_t *p_data)
sepp_nepp 9:cb4c6d4e5030 4649 { return VL53L0X_i2c_read(index, p_data, 1); }
sepp_nepp 6:fb11b746ceb5 4650
sepp_nepp 7:3a1115c2556b 4651 VL53L0X_Error VL53L0X::VL53L0X_read_word( uint8_t index, uint16_t *p_data)
sepp_nepp 8:2fd7cb217068 4652 { int status;
sepp_nepp 6:fb11b746ceb5 4653 uint8_t buffer[2] = {0, 0};
sepp_nepp 6:fb11b746ceb5 4654
sepp_nepp 7:3a1115c2556b 4655 status = VL53L0X_i2c_read(index, buffer, 2);
sepp_nepp 9:cb4c6d4e5030 4656 if (!status) {*p_data = (buffer[0] << 8) + buffer[1];}
sepp_nepp 6:fb11b746ceb5 4657 return status;
sepp_nepp 6:fb11b746ceb5 4658 }
sepp_nepp 6:fb11b746ceb5 4659
sepp_nepp 7:3a1115c2556b 4660 VL53L0X_Error VL53L0X::VL53L0X_read_dword( uint8_t index, uint32_t *p_data)
sepp_nepp 8:2fd7cb217068 4661 { int status;
sepp_nepp 6:fb11b746ceb5 4662 uint8_t buffer[4] = {0, 0, 0, 0};
sepp_nepp 6:fb11b746ceb5 4663
sepp_nepp 7:3a1115c2556b 4664 status = VL53L0X_i2c_read(index, buffer, 4);
sepp_nepp 9:cb4c6d4e5030 4665 if (!status) { *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; }
sepp_nepp 6:fb11b746ceb5 4666 return status;
sepp_nepp 6:fb11b746ceb5 4667 }
sepp_nepp 6:fb11b746ceb5 4668
sepp_nepp 7:3a1115c2556b 4669 VL53L0X_Error VL53L0X::VL53L0X_update_byte( uint8_t index, uint8_t and_data, uint8_t or_data)
sepp_nepp 8:2fd7cb217068 4670 { int status;
sepp_nepp 6:fb11b746ceb5 4671 uint8_t buffer = 0;
sepp_nepp 6:fb11b746ceb5 4672
sepp_nepp 6:fb11b746ceb5 4673 /* read data direct onto buffer */
sepp_nepp 7:3a1115c2556b 4674 status = VL53L0X_i2c_read(index, &buffer, 1);
sepp_nepp 6:fb11b746ceb5 4675 if (!status) {
sepp_nepp 6:fb11b746ceb5 4676 buffer = (buffer & and_data) | or_data;
sepp_nepp 7:3a1115c2556b 4677 status = VL53L0X_i2c_write(index, &buffer, (uint8_t)1);
sepp_nepp 6:fb11b746ceb5 4678 }
sepp_nepp 6:fb11b746ceb5 4679 return status;
sepp_nepp 6:fb11b746ceb5 4680 }
sepp_nepp 6:fb11b746ceb5 4681
sepp_nepp 7:3a1115c2556b 4682 VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t RegisterAddr, uint8_t *p_data,
sepp_nepp 6:fb11b746ceb5 4683 uint16_t NumByteToWrite)
sepp_nepp 8:2fd7cb217068 4684 { /** Writes a buffer towards the I2C peripheral device. */
sepp_nepp 8:2fd7cb217068 4685 static uint8_t tmp[VL53L0X_MAX_I2C_XFER_SIZE];
sepp_nepp 6:fb11b746ceb5 4686
sepp_nepp 6:fb11b746ceb5 4687 if(NumByteToWrite >= VL53L0X_MAX_I2C_XFER_SIZE) return -2;
sepp_nepp 6:fb11b746ceb5 4688
sepp_nepp 6:fb11b746ceb5 4689 /* First, send device address. Then, send data and STOP condition */
sepp_nepp 6:fb11b746ceb5 4690 tmp[0] = RegisterAddr;
sepp_nepp 8:2fd7cb217068 4691 memcpy(tmp+1, p_data, NumByteToWrite);
sepp_nepp 8:2fd7cb217068 4692
sepp_nepp 8:2fd7cb217068 4693 if (_dev_i2c->write(I2cDevAddr, (const char*)tmp, NumByteToWrite+1, false) != 0 )
sepp_nepp 8:2fd7cb217068 4694 { return -1; }
sepp_nepp 8:2fd7cb217068 4695 return 0;
sepp_nepp 8:2fd7cb217068 4696 }
sepp_nepp 8:2fd7cb217068 4697
sepp_nepp 8:2fd7cb217068 4698 VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t RegisterAddr, uint8_t *p_data, uint16_t NumByteToRead)
sepp_nepp 8:2fd7cb217068 4699 { /** Reads a buffer from the I2C peripheral device. */
sepp_nepp 8:2fd7cb217068 4700
sepp_nepp 8:2fd7cb217068 4701 /* First Send device address, with no STOP condition */
sepp_nepp 8:2fd7cb217068 4702 int ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true);
sepp_nepp 8:2fd7cb217068 4703
sepp_nepp 8:2fd7cb217068 4704 /* all ok ? then Read data, with STOP condition */
sepp_nepp 9:cb4c6d4e5030 4705 if (ret == 0) { ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); }
sepp_nepp 8:2fd7cb217068 4706
sepp_nepp 6:fb11b746ceb5 4707 if (ret != 0 ){ return -1; }
sepp_nepp 6:fb11b746ceb5 4708 return 0;
sepp_nepp 6:fb11b746ceb5 4709 }
sepp_nepp 6:fb11b746ceb5 4710