ST Expansion SW Team / VL53L3CX_mbed

Rename library

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging

porting/vl53lx_platform.c@7:7f1bbf370283, 2021-07-21 (annotated)

Committer:
charlesmn
Date:
Wed Jul 21 14:07:59 2021 +0000
Revision:
7:7f1bbf370283
Parent:
5:89031b2f5316 
Moved vl53l3cx_class.cpp and .h to 53l3a2_RangingClass

Who changed what in which revision?

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