Tarek Lule / VL53L0X_Condensed2

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

VL53L0X.cpp@11:d8dbe3b87f9f, 2019-07-02 (annotated)

Committer:
sepp_nepp
Date:
Tue Jul 02 12:38:07 2019 +0000
Revision:
11:d8dbe3b87f9f
Parent:
10:cd1758e186a4
Child:
12:81f37e50f8f8
Demo Version for TOF_Synth

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