Tarek Lule / VL53L0X_Condensed2

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

VL53L0X.cpp@6:fb11b746ceb5, 2019-06-20 (annotated)

Committer:
sepp_nepp
Date:
Thu Jun 20 12:00:59 2019 +0000
Revision:
6:fb11b746ceb5
Child:
7:3a1115c2556b
Greatly simplified version of the library.

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