ST Expansion SW Team / VL53L1CB

The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.

src/vl53l1_platform.c@0:3ac96e360672, 2020-11-06 (annotated)

Committer:
charlesmn
Date:
Fri Nov 06 10:06:37 2020 +0000
Revision:
0:3ac96e360672
Child:
18:0696efe39d08
Library for ST Vl53L1A1 time of flight sensor.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
charlesmn 0:3ac96e360672 2 /*
charlesmn 0:3ac96e360672 3 * This file is part of VL53L1 Platform
charlesmn 0:3ac96e360672 4 *
charlesmn 0:3ac96e360672 5 * Copyright (c) 2016, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 6 *
charlesmn 0:3ac96e360672 7 * License terms: BSD 3-clause "New" or "Revised" License.
charlesmn 0:3ac96e360672 8 *
charlesmn 0:3ac96e360672 9 * Redistribution and use in source and binary forms, with or without
charlesmn 0:3ac96e360672 10 * modification, are permitted provided that the following conditions are met:
charlesmn 0:3ac96e360672 11 *
charlesmn 0:3ac96e360672 12 * 1. Redistributions of source code must retain the above copyright notice, this
charlesmn 0:3ac96e360672 13 * list of conditions and the following disclaimer.
charlesmn 0:3ac96e360672 14 *
charlesmn 0:3ac96e360672 15 * 2. Redistributions in binary form must reproduce the above copyright notice,
charlesmn 0:3ac96e360672 16 * this list of conditions and the following disclaimer in the documentation
charlesmn 0:3ac96e360672 17 * and/or other materials provided with the distribution.
charlesmn 0:3ac96e360672 18 *
charlesmn 0:3ac96e360672 19 * 3. Neither the name of the copyright holder nor the names of its contributors
charlesmn 0:3ac96e360672 20 * may be used to endorse or promote products derived from this software
charlesmn 0:3ac96e360672 21 * without specific prior written permission.
charlesmn 0:3ac96e360672 22 *
charlesmn 0:3ac96e360672 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
charlesmn 0:3ac96e360672 24 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
charlesmn 0:3ac96e360672 25 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
charlesmn 0:3ac96e360672 26 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
charlesmn 0:3ac96e360672 27 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
charlesmn 0:3ac96e360672 28 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
charlesmn 0:3ac96e360672 29 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
charlesmn 0:3ac96e360672 30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
charlesmn 0:3ac96e360672 31 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
charlesmn 0:3ac96e360672 32 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
charlesmn 0:3ac96e360672 33 *
charlesmn 0:3ac96e360672 34 */
charlesmn 0:3ac96e360672 35
charlesmn 0:3ac96e360672 36 unsigned int i2creadCount = 0;
charlesmn 0:3ac96e360672 37 unsigned int i2cwriteCount = 0;
charlesmn 0:3ac96e360672 38 unsigned char SPI2C_Buffer[256];
charlesmn 0:3ac96e360672 39
charlesmn 0:3ac96e360672 40
charlesmn 0:3ac96e360672 41 #include <mbed_wait_api.h>
charlesmn 0:3ac96e360672 42 #include "vl53l1_platform.h"
charlesmn 0:3ac96e360672 43 #ifndef SMALL_FOOTPRINT
charlesmn 0:3ac96e360672 44 #endif
charlesmn 0:3ac96e360672 45 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 46 #include "vl53l1_api.h"
charlesmn 0:3ac96e360672 47 #include "spi_interface.h"
charlesmn 0:3ac96e360672 48 #include <string.h>
charlesmn 0:3ac96e360672 49 #include <time.h>
charlesmn 0:3ac96e360672 50 #include <math.h>
charlesmn 0:3ac96e360672 51
charlesmn 0:3ac96e360672 52
charlesmn 0:3ac96e360672 53
charlesmn 0:3ac96e360672 54 #define I2C_TIME_OUT_BASE 10
charlesmn 0:3ac96e360672 55 #define I2C_TIME_OUT_BYTE 1
charlesmn 0:3ac96e360672 56
charlesmn 0:3ac96e360672 57 #ifdef VL53L1_LOG_ENABLE
charlesmn 0:3ac96e360672 58 #define trace_print(level, ...) VL53L1_trace_print_module_function(VL53L1_TRACE_MODULE_PLATFORM, level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 59 #define trace_i2c(...) VL53L1_trace_print_module_function(VL53L1_TRACE_MODULE_NONE, VL53L1_TRACE_LEVEL_NONE, VL53L1_TRACE_FUNCTION_I2C, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 60 #endif
charlesmn 0:3ac96e360672 61
charlesmn 0:3ac96e360672 62 /* when not customized by application define dummy one */
charlesmn 0:3ac96e360672 63 #ifndef VL53L1_GetI2cBus
charlesmn 0:3ac96e360672 64 /** This macro can be overloaded by user to enforce i2c sharing in RTOS context
charlesmn 0:3ac96e360672 65 */
charlesmn 0:3ac96e360672 66 # define VL53L1_GetI2cBus(...) (void)0
charlesmn 0:3ac96e360672 67 #endif
charlesmn 0:3ac96e360672 68
charlesmn 0:3ac96e360672 69 #ifndef VL53L1_PutI2cBus
charlesmn 0:3ac96e360672 70 /** This macro can be overloaded by user to enforce i2c sharing in RTOS context
charlesmn 0:3ac96e360672 71 */
charlesmn 0:3ac96e360672 72 # define VL53L1_PutI2cBus(...) (void)0
charlesmn 0:3ac96e360672 73 #endif
charlesmn 0:3ac96e360672 74
charlesmn 0:3ac96e360672 75 uint8_t _I2CBuffer[256];
charlesmn 0:3ac96e360672 76
charlesmn 0:3ac96e360672 77
charlesmn 0:3ac96e360672 78 VL53L1_Error VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
charlesmn 0:3ac96e360672 79 {
charlesmn 0:3ac96e360672 80 int status;
charlesmn 0:3ac96e360672 81 // printf("VL53L1_WriteMulti %d %d %d \n",Dev->I2cDevAddr,index,count);
charlesmn 0:3ac96e360672 82 status = v53l1x_i2c_write_if(pdata,Dev->i2c_slave_address, index,count);
charlesmn 0:3ac96e360672 83 return status;
charlesmn 0:3ac96e360672 84 }
charlesmn 0:3ac96e360672 85
charlesmn 0:3ac96e360672 86
charlesmn 0:3ac96e360672 87 VL53L1_Error VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
charlesmn 0:3ac96e360672 88 {
charlesmn 0:3ac96e360672 89 int status;
charlesmn 0:3ac96e360672 90
charlesmn 0:3ac96e360672 91 status = v53l1x_i2c_read_if(pdata,Dev->i2c_slave_address, index,count);
charlesmn 0:3ac96e360672 92
charlesmn 0:3ac96e360672 93 return status;
charlesmn 0:3ac96e360672 94 }
charlesmn 0:3ac96e360672 95
charlesmn 0:3ac96e360672 96
charlesmn 0:3ac96e360672 97 VL53L1_Error VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
charlesmn 0:3ac96e360672 98 {
charlesmn 0:3ac96e360672 99 int status;
charlesmn 0:3ac96e360672 100
charlesmn 0:3ac96e360672 101 status = v53l1x_i2c_write_if(&data,Dev->i2c_slave_address, index,1);
charlesmn 0:3ac96e360672 102 return status;
charlesmn 0:3ac96e360672 103 }
charlesmn 0:3ac96e360672 104
charlesmn 0:3ac96e360672 105
charlesmn 0:3ac96e360672 106 VL53L1_Error VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data)
charlesmn 0:3ac96e360672 107 {
charlesmn 0:3ac96e360672 108 int status;
charlesmn 0:3ac96e360672 109 uint8_t buffer[2];
charlesmn 0:3ac96e360672 110
charlesmn 0:3ac96e360672 111 buffer[0] = data >> 8;
charlesmn 0:3ac96e360672 112 buffer[1] = data & 0x00FF;
charlesmn 0:3ac96e360672 113 status = v53l1x_i2c_write_if((uint8_t *)buffer,Dev->i2c_slave_address, index,2);
charlesmn 0:3ac96e360672 114 return status;
charlesmn 0:3ac96e360672 115 }
charlesmn 0:3ac96e360672 116
charlesmn 0:3ac96e360672 117
charlesmn 0:3ac96e360672 118 VL53L1_Error VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data)
charlesmn 0:3ac96e360672 119 {
charlesmn 0:3ac96e360672 120 int status;
charlesmn 0:3ac96e360672 121 uint8_t buffer[4];
charlesmn 0:3ac96e360672 122
charlesmn 0:3ac96e360672 123 buffer[0] = (data >> 24) & 0xFF;
charlesmn 0:3ac96e360672 124 buffer[1] = (data >> 16) & 0xFF;
charlesmn 0:3ac96e360672 125 buffer[2] = (data >> 8) & 0xFF;
charlesmn 0:3ac96e360672 126 buffer[3] = (data >> 0) & 0xFF;
charlesmn 0:3ac96e360672 127 status = v53l1x_i2c_write_if((uint8_t *)buffer,Dev->i2c_slave_address, index,4);
charlesmn 0:3ac96e360672 128 return status;
charlesmn 0:3ac96e360672 129 }
charlesmn 0:3ac96e360672 130
charlesmn 0:3ac96e360672 131 VL53L1_Error VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
charlesmn 0:3ac96e360672 132 {
charlesmn 0:3ac96e360672 133 int status;
charlesmn 0:3ac96e360672 134 uint8_t buffer = 0;
charlesmn 0:3ac96e360672 135
charlesmn 0:3ac96e360672 136 /* read data direct onto buffer */
charlesmn 0:3ac96e360672 137 status = v53l1x_i2c_read_if(&buffer,Dev->i2c_slave_address, index,1);
charlesmn 0:3ac96e360672 138 if (!status)
charlesmn 0:3ac96e360672 139 {
charlesmn 0:3ac96e360672 140 buffer = (buffer & AndData) | OrData;
charlesmn 0:3ac96e360672 141 status = v53l1x_i2c_write_if(&buffer,Dev->i2c_slave_address, index,1);
charlesmn 0:3ac96e360672 142 }
charlesmn 0:3ac96e360672 143 return status;
charlesmn 0:3ac96e360672 144 }
charlesmn 0:3ac96e360672 145
charlesmn 0:3ac96e360672 146 VL53L1_Error VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
charlesmn 0:3ac96e360672 147 {
charlesmn 0:3ac96e360672 148 int status;
charlesmn 0:3ac96e360672 149
charlesmn 0:3ac96e360672 150 status = v53l1x_i2c_read_if(data,Dev->i2c_slave_address, index,1); //is this correct
charlesmn 0:3ac96e360672 151 // printf("VL53L1_RdByte %d %d %d\n",Dev->i2c_slave_address, status,*data);
charlesmn 0:3ac96e360672 152 if(status)
charlesmn 0:3ac96e360672 153 return -1;
charlesmn 0:3ac96e360672 154
charlesmn 0:3ac96e360672 155 return 0;
charlesmn 0:3ac96e360672 156 }
charlesmn 0:3ac96e360672 157
charlesmn 0:3ac96e360672 158
charlesmn 0:3ac96e360672 159 VL53L1_Error VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
charlesmn 0:3ac96e360672 160 {
charlesmn 0:3ac96e360672 161 int status;
charlesmn 0:3ac96e360672 162 uint8_t buffer[2] = {0,0};
charlesmn 0:3ac96e360672 163
charlesmn 0:3ac96e360672 164 status = v53l1x_i2c_read_if(buffer,Dev->i2c_slave_address, index,2); //is this correct
charlesmn 0:3ac96e360672 165 if (!status)
charlesmn 0:3ac96e360672 166 {
charlesmn 0:3ac96e360672 167 *data = (buffer[0] << 8) + buffer[1];
charlesmn 0:3ac96e360672 168 }
charlesmn 0:3ac96e360672 169 return status;
charlesmn 0:3ac96e360672 170
charlesmn 0:3ac96e360672 171 }
charlesmn 0:3ac96e360672 172
charlesmn 0:3ac96e360672 173
charlesmn 0:3ac96e360672 174
charlesmn 0:3ac96e360672 175 VL53L1_Error VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
charlesmn 0:3ac96e360672 176 {
charlesmn 0:3ac96e360672 177 int status;
charlesmn 0:3ac96e360672 178 uint8_t buffer[4] = {0,0,0,0};
charlesmn 0:3ac96e360672 179
charlesmn 0:3ac96e360672 180 status = v53l1x_i2c_read_if(buffer,Dev->i2c_slave_address, index,4);
charlesmn 0:3ac96e360672 181 if(!status)
charlesmn 0:3ac96e360672 182 {
charlesmn 0:3ac96e360672 183 *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
charlesmn 0:3ac96e360672 184 }
charlesmn 0:3ac96e360672 185 return status;
charlesmn 0:3ac96e360672 186
charlesmn 0:3ac96e360672 187 }
charlesmn 0:3ac96e360672 188
charlesmn 0:3ac96e360672 189
charlesmn 0:3ac96e360672 190 VL53L1_Error VL53L1_GetTickCount(
charlesmn 0:3ac96e360672 191 uint32_t *ptick_count_ms)
charlesmn 0:3ac96e360672 192 {
charlesmn 0:3ac96e360672 193
charlesmn 0:3ac96e360672 194 /* Returns current tick count in [ms] */
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 197
charlesmn 0:3ac96e360672 198 GetTickCount( ptick_count_ms);
charlesmn 0:3ac96e360672 199
charlesmn 0:3ac96e360672 200 #ifdef VL53L1_LOG_ENABLE
charlesmn 0:3ac96e360672 201 trace_print(
charlesmn 0:3ac96e360672 202 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 203 "VL53L1_GetTickCount() = %5u ms;\n",
charlesmn 0:3ac96e360672 204 *ptick_count_ms);
charlesmn 0:3ac96e360672 205 #endif
charlesmn 0:3ac96e360672 206
charlesmn 0:3ac96e360672 207 return status;
charlesmn 0:3ac96e360672 208 }
charlesmn 0:3ac96e360672 209
charlesmn 0:3ac96e360672 210
charlesmn 0:3ac96e360672 211 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 212 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_PLATFORM, \
charlesmn 0:3ac96e360672 213 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 214
charlesmn 0:3ac96e360672 215 #define trace_i2c(...) \
charlesmn 0:3ac96e360672 216 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_NONE, \
charlesmn 0:3ac96e360672 217 VL53L1_TRACE_LEVEL_NONE, VL53L1_TRACE_FUNCTION_I2C, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 218
charlesmn 0:3ac96e360672 219
charlesmn 0:3ac96e360672 220 VL53L1_Error VL53L1_GetTimerFrequency(int32_t *ptimer_freq_hz)
charlesmn 0:3ac96e360672 221 {
charlesmn 0:3ac96e360672 222 *ptimer_freq_hz = 0;
charlesmn 0:3ac96e360672 223
charlesmn 0:3ac96e360672 224 trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GetTimerFrequency: Freq : %dHz\n", *ptimer_freq_hz);
charlesmn 0:3ac96e360672 225 return VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 226 }
charlesmn 0:3ac96e360672 227
charlesmn 0:3ac96e360672 228
charlesmn 0:3ac96e360672 229 VL53L1_Error VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_time){
charlesmn 0:3ac96e360672 230 (void)pdev;
charlesmn 0:3ac96e360672 231 wait_ms(wait_time);
charlesmn 0:3ac96e360672 232 return VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 233 }
charlesmn 0:3ac96e360672 234
charlesmn 0:3ac96e360672 235
charlesmn 0:3ac96e360672 236 VL53L1_Error VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_time){
charlesmn 0:3ac96e360672 237 (void)pdev;
charlesmn 0:3ac96e360672 238 wait_us(wait_time);
charlesmn 0:3ac96e360672 239 return VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 240 }
charlesmn 0:3ac96e360672 241
charlesmn 0:3ac96e360672 242 VL53L1_Error VL53L1_WaitValueMaskEx(
charlesmn 0:3ac96e360672 243 VL53L1_Dev_t *pdev,
charlesmn 0:3ac96e360672 244 uint32_t timeout_ms,
charlesmn 0:3ac96e360672 245 uint16_t index,
charlesmn 0:3ac96e360672 246 uint8_t value,
charlesmn 0:3ac96e360672 247 uint8_t mask,
charlesmn 0:3ac96e360672 248 uint32_t poll_delay_ms)
charlesmn 0:3ac96e360672 249 {
charlesmn 0:3ac96e360672 250
charlesmn 0:3ac96e360672 251 /*
charlesmn 0:3ac96e360672 252 * Platform implementation of WaitValueMaskEx V2WReg script command
charlesmn 0:3ac96e360672 253 *
charlesmn 0:3ac96e360672 254 * WaitValueMaskEx(
charlesmn 0:3ac96e360672 255 * duration_ms,
charlesmn 0:3ac96e360672 256 * index,
charlesmn 0:3ac96e360672 257 * value,
charlesmn 0:3ac96e360672 258 * mask,
charlesmn 0:3ac96e360672 259 * poll_delay_ms);
charlesmn 0:3ac96e360672 260 */
charlesmn 0:3ac96e360672 261
charlesmn 0:3ac96e360672 262 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 263 uint32_t start_time_ms = 0;
charlesmn 0:3ac96e360672 264 uint32_t current_time_ms = 0;
charlesmn 0:3ac96e360672 265 uint32_t polling_time_ms = 0;
charlesmn 0:3ac96e360672 266 uint8_t byte_value = 0;
charlesmn 0:3ac96e360672 267 uint8_t found = 0;
charlesmn 0:3ac96e360672 268 #ifdef VL53L1_LOG_ENABLE
charlesmn 0:3ac96e360672 269 uint8_t trace_functions = VL53L1_TRACE_FUNCTION_NONE;
charlesmn 0:3ac96e360672 270 #endif
charlesmn 0:3ac96e360672 271
charlesmn 0:3ac96e360672 272 char register_name[VL53L1_MAX_STRING_LENGTH];
charlesmn 0:3ac96e360672 273
charlesmn 0:3ac96e360672 274 /* look up register name */
charlesmn 0:3ac96e360672 275 #ifdef PAL_EXTENDED
charlesmn 0:3ac96e360672 276 VL53L1_get_register_name(
charlesmn 0:3ac96e360672 277 index,
charlesmn 0:3ac96e360672 278 register_name);
charlesmn 0:3ac96e360672 279 #else
charlesmn 0:3ac96e360672 280 VL53L1_COPYSTRING(register_name, "");
charlesmn 0:3ac96e360672 281 #endif
charlesmn 0:3ac96e360672 282
charlesmn 0:3ac96e360672 283 /* Output to I2C logger for FMT/DFT */
charlesmn 0:3ac96e360672 284
charlesmn 0:3ac96e360672 285 /*trace_i2c("WaitValueMaskEx(%5d, 0x%04X, 0x%02X, 0x%02X, %5d);\n",
charlesmn 0:3ac96e360672 286 timeout_ms, index, value, mask, poll_delay_ms); */
charlesmn 0:3ac96e360672 287 trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n",
charlesmn 0:3ac96e360672 288 timeout_ms, register_name, value, mask, poll_delay_ms);
charlesmn 0:3ac96e360672 289
charlesmn 0:3ac96e360672 290 /* calculate time limit in absolute time */
charlesmn 0:3ac96e360672 291
charlesmn 0:3ac96e360672 292 VL53L1_GetTickCount(&start_time_ms);
charlesmn 0:3ac96e360672 293
charlesmn 0:3ac96e360672 294
charlesmn 0:3ac96e360672 295 wait_ms(10);
charlesmn 0:3ac96e360672 296
charlesmn 0:3ac96e360672 297 /* remember current trace functions and temporarily disable
charlesmn 0:3ac96e360672 298 * function logging
charlesmn 0:3ac96e360672 299 */
charlesmn 0:3ac96e360672 300
charlesmn 0:3ac96e360672 301 #ifdef VL53L1_LOG_ENABLE
charlesmn 0:3ac96e360672 302 trace_functions = VL53L1_get_trace_functions();
charlesmn 0:3ac96e360672 303 VL53L1_set_trace_functions(VL53L1_TRACE_FUNCTION_NONE);
charlesmn 0:3ac96e360672 304 #endif
charlesmn 0:3ac96e360672 305
charlesmn 0:3ac96e360672 306 /* wait until value is found, timeout reached on error occurred */
charlesmn 0:3ac96e360672 307 while ((status == VL53L1_ERROR_NONE) &&
charlesmn 0:3ac96e360672 308 (polling_time_ms < timeout_ms) &&
charlesmn 0:3ac96e360672 309 (found == 0)) {
charlesmn 0:3ac96e360672 310
charlesmn 0:3ac96e360672 311 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 312 status = VL53L1_RdByte(
charlesmn 0:3ac96e360672 313 pdev,
charlesmn 0:3ac96e360672 314 index,
charlesmn 0:3ac96e360672 315 &byte_value);
charlesmn 0:3ac96e360672 316
charlesmn 0:3ac96e360672 317 if ((byte_value & mask) == value)
charlesmn 0:3ac96e360672 318 found = 1;
charlesmn 0:3ac96e360672 319 if (status)
charlesmn 0:3ac96e360672 320 {
charlesmn 0:3ac96e360672 321 printf("VL53L1_WaitValueMaskEx 1 %d %d\n",pdev->i2c_slave_address,status);
charlesmn 0:3ac96e360672 322 }
charlesmn 0:3ac96e360672 323
charlesmn 0:3ac96e360672 324 if (status == VL53L1_ERROR_NONE &&
charlesmn 0:3ac96e360672 325 found == 0 &&
charlesmn 0:3ac96e360672 326 poll_delay_ms > 0)
charlesmn 0:3ac96e360672 327 status = VL53L1_WaitMs(
charlesmn 0:3ac96e360672 328 pdev,
charlesmn 0:3ac96e360672 329 poll_delay_ms);
charlesmn 0:3ac96e360672 330 /* Update polling time (Compare difference rather than absolute to
charlesmn 0:3ac96e360672 331 negate 32bit wrap around issue) */
charlesmn 0:3ac96e360672 332 if (status)
charlesmn 0:3ac96e360672 333 {
charlesmn 0:3ac96e360672 334 printf("VL53L1_WaitValueMaskEx 2 %d\n",status);
charlesmn 0:3ac96e360672 335 }
charlesmn 0:3ac96e360672 336 VL53L1_GetTickCount(&current_time_ms);
charlesmn 0:3ac96e360672 337 polling_time_ms = current_time_ms - start_time_ms;
charlesmn 0:3ac96e360672 338
charlesmn 0:3ac96e360672 339 }
charlesmn 0:3ac96e360672 340 // printf("polling_time_ms %d \n",polling_time_ms);
charlesmn 0:3ac96e360672 341 #ifdef VL53L1_LOG_ENABLE
charlesmn 0:3ac96e360672 342 /* Restore function logging */
charlesmn 0:3ac96e360672 343 VL53L1_set_trace_functions(trace_functions);
charlesmn 0:3ac96e360672 344 #endif
charlesmn 0:3ac96e360672 345
charlesmn 0:3ac96e360672 346 if (found == 0 && status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 347 status = VL53L1_ERROR_TIME_OUT;
charlesmn 0:3ac96e360672 348
charlesmn 0:3ac96e360672 349 return status;
charlesmn 0:3ac96e360672 350 }
charlesmn 0:3ac96e360672 351
charlesmn 0:3ac96e360672 352
charlesmn 0:3ac96e360672 353
charlesmn 0:3ac96e360672 354