ST / VL53L0X

VL53L0X World smallest Time-of-Flight (ToF) ranging sensor

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   HelloWorld_ST_Sensors mbed-os-mqtt-client Multi_VL53L0X DISCO-IOT01_HomeEnv ... more

Fork of VL53L0X by Nicola Capovilla

vl53l1x_class.cpp@5:f16727052990, 2019-01-28 (annotated)

Committer:
JerrySzczurak
Date:
Mon Jan 28 10:14:03 2019 +0000
Revision:
5:f16727052990
Initial Cpp mass market sensor class declaration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JerrySzczurak 5:f16727052990 1 /**
JerrySzczurak 5:f16727052990 2 ******************************************************************************
JerrySzczurak 5:f16727052990 3 * @file vl53l1x_class.cpp
JerrySzczurak 5:f16727052990 4 * @author JS
JerrySzczurak 5:f16727052990 5 * @version V0.0.1
JerrySzczurak 5:f16727052990 6 * @date 15-January-2019
JerrySzczurak 5:f16727052990 7 * @brief Implementation file for the VL53L1 sensor component driver class
JerrySzczurak 5:f16727052990 8 ******************************************************************************
JerrySzczurak 5:f16727052990 9 * @attention
JerrySzczurak 5:f16727052990 10 *
JerrySzczurak 5:f16727052990 11 * <h2><center>&copy; COPYRIGHT(c) 2018 STMicroelectronics</center></h2>
JerrySzczurak 5:f16727052990 12 *
JerrySzczurak 5:f16727052990 13 * Redistribution and use in source and binary forms, with or without modification,
JerrySzczurak 5:f16727052990 14 * are permitted provided that the following conditions are met:
JerrySzczurak 5:f16727052990 15 * 1. Redistributions of source code must retain the above copyright notice,
JerrySzczurak 5:f16727052990 16 * this list of conditions and the following disclaimer.
JerrySzczurak 5:f16727052990 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
JerrySzczurak 5:f16727052990 18 * this list of conditions and the following disclaimer in the documentation
JerrySzczurak 5:f16727052990 19 * and/or other materials provided with the distribution.
JerrySzczurak 5:f16727052990 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
JerrySzczurak 5:f16727052990 21 * may be used to endorse or promote products derived from this software
JerrySzczurak 5:f16727052990 22 * without specific prior written permission.
JerrySzczurak 5:f16727052990 23 *
JerrySzczurak 5:f16727052990 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
JerrySzczurak 5:f16727052990 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
JerrySzczurak 5:f16727052990 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
JerrySzczurak 5:f16727052990 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
JerrySzczurak 5:f16727052990 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
JerrySzczurak 5:f16727052990 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
JerrySzczurak 5:f16727052990 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
JerrySzczurak 5:f16727052990 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
JerrySzczurak 5:f16727052990 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
JerrySzczurak 5:f16727052990 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
JerrySzczurak 5:f16727052990 34 *
JerrySzczurak 5:f16727052990 35 ******************************************************************************
JerrySzczurak 5:f16727052990 36 */
JerrySzczurak 5:f16727052990 37
JerrySzczurak 5:f16727052990 38 /* Includes */
JerrySzczurak 5:f16727052990 39 #include <stdlib.h>
JerrySzczurak 5:f16727052990 40 #include "vl53l1x_class.h"
JerrySzczurak 5:f16727052990 41
JerrySzczurak 5:f16727052990 42
JerrySzczurak 5:f16727052990 43 #define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E
JerrySzczurak 5:f16727052990 44 #define MM_CONFIG__INNER_OFFSET_MM 0x0020
JerrySzczurak 5:f16727052990 45 #define MM_CONFIG__OUTER_OFFSET_MM 0x0022
JerrySzczurak 5:f16727052990 46
JerrySzczurak 5:f16727052990 47 #include "vl53l1x_configuration.h"
JerrySzczurak 5:f16727052990 48
JerrySzczurak 5:f16727052990 49
JerrySzczurak 5:f16727052990 50 VL53L1X_ERROR VL53L1X::VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion)
JerrySzczurak 5:f16727052990 51 {
JerrySzczurak 5:f16727052990 52 VL53L1X_ERROR Status = 0;
JerrySzczurak 5:f16727052990 53
JerrySzczurak 5:f16727052990 54 pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR;
JerrySzczurak 5:f16727052990 55 pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR;
JerrySzczurak 5:f16727052990 56 pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB;
JerrySzczurak 5:f16727052990 57 pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION;
JerrySzczurak 5:f16727052990 58 return Status;
JerrySzczurak 5:f16727052990 59 }
JerrySzczurak 5:f16727052990 60
JerrySzczurak 5:f16727052990 61 VL53L1X_ERROR VL53L1X::VL53L1X_SetI2CAddress(uint8_t new_address)
JerrySzczurak 5:f16727052990 62 {
JerrySzczurak 5:f16727052990 63 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 64
JerrySzczurak 5:f16727052990 65 Device->I2cDevAddr = new_address;
JerrySzczurak 5:f16727052990 66 status = VL53L1_WrByte(Device, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);
JerrySzczurak 5:f16727052990 67 return status;
JerrySzczurak 5:f16727052990 68 }
JerrySzczurak 5:f16727052990 69
JerrySzczurak 5:f16727052990 70 int VL53L1X::init_sensor(uint8_t new_addr)
JerrySzczurak 5:f16727052990 71 {
JerrySzczurak 5:f16727052990 72 Device->I2cDevAddr = new_addr;
JerrySzczurak 5:f16727052990 73 int status = 0;
JerrySzczurak 5:f16727052990 74 VL53L1_Off();
JerrySzczurak 5:f16727052990 75 VL53L1_On();
JerrySzczurak 5:f16727052990 76
JerrySzczurak 5:f16727052990 77 status = is_present();
JerrySzczurak 5:f16727052990 78 if (!status) {
JerrySzczurak 5:f16727052990 79 printf("Failed to init VL53L0X sensor!\n\r");
JerrySzczurak 5:f16727052990 80 return status;
JerrySzczurak 5:f16727052990 81 }
JerrySzczurak 5:f16727052990 82 return status;
JerrySzczurak 5:f16727052990 83 }
JerrySzczurak 5:f16727052990 84
JerrySzczurak 5:f16727052990 85
JerrySzczurak 5:f16727052990 86 VL53L1X_ERROR VL53L1X::VL53L1X_SensorInit()
JerrySzczurak 5:f16727052990 87 {
JerrySzczurak 5:f16727052990 88 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 89 uint8_t Addr = 0x00;
JerrySzczurak 5:f16727052990 90
JerrySzczurak 5:f16727052990 91 for (Addr = 0x2D; Addr <= 0x87; Addr++){
JerrySzczurak 5:f16727052990 92 status = VL53L1_WrByte(Device, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
JerrySzczurak 5:f16727052990 93 if (status != 0)
JerrySzczurak 5:f16727052990 94 {
JerrySzczurak 5:f16727052990 95 printf("Writing config failed - %d\r\n", status);
JerrySzczurak 5:f16727052990 96 }
JerrySzczurak 5:f16727052990 97 }
JerrySzczurak 5:f16727052990 98
JerrySzczurak 5:f16727052990 99 uint16_t sensorID= 0;
JerrySzczurak 5:f16727052990 100 status = VL53L1X_GetSensorId(&sensorID);
JerrySzczurak 5:f16727052990 101 printf("Sensor id is - %d (%X)\r\n", sensorID, sensorID);
JerrySzczurak 5:f16727052990 102
JerrySzczurak 5:f16727052990 103 status = VL53L1X_StartRanging();
JerrySzczurak 5:f16727052990 104 if (status != 0)
JerrySzczurak 5:f16727052990 105 {
JerrySzczurak 5:f16727052990 106 printf("start ranging failed - %d\r\n", status);
JerrySzczurak 5:f16727052990 107 }
JerrySzczurak 5:f16727052990 108
JerrySzczurak 5:f16727052990 109 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 110 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 111 status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
JerrySzczurak 5:f16727052990 112 status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */
JerrySzczurak 5:f16727052990 113 return status;
JerrySzczurak 5:f16727052990 114 }
JerrySzczurak 5:f16727052990 115
JerrySzczurak 5:f16727052990 116
JerrySzczurak 5:f16727052990 117 VL53L1X_ERROR VL53L1X::VL53L1X_ClearInterrupt()
JerrySzczurak 5:f16727052990 118 {
JerrySzczurak 5:f16727052990 119 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 120
JerrySzczurak 5:f16727052990 121 status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CLEAR, 0x01);
JerrySzczurak 5:f16727052990 122 return status;
JerrySzczurak 5:f16727052990 123 }
JerrySzczurak 5:f16727052990 124
JerrySzczurak 5:f16727052990 125
JerrySzczurak 5:f16727052990 126 VL53L1X_ERROR VL53L1X::VL53L1X_SetInterruptPolarity(uint8_t NewPolarity)
JerrySzczurak 5:f16727052990 127 {
JerrySzczurak 5:f16727052990 128 uint8_t Temp;
JerrySzczurak 5:f16727052990 129 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 130
JerrySzczurak 5:f16727052990 131 status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp);
JerrySzczurak 5:f16727052990 132 Temp = Temp & 0xEF;
JerrySzczurak 5:f16727052990 133 status = VL53L1_WrByte(Device, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4);
JerrySzczurak 5:f16727052990 134 return status;
JerrySzczurak 5:f16727052990 135 }
JerrySzczurak 5:f16727052990 136
JerrySzczurak 5:f16727052990 137
JerrySzczurak 5:f16727052990 138
JerrySzczurak 5:f16727052990 139 VL53L1X_ERROR VL53L1X::VL53L1X_GetInterruptPolarity(uint8_t *pInterruptPolarity)
JerrySzczurak 5:f16727052990 140 {
JerrySzczurak 5:f16727052990 141 uint8_t Temp;
JerrySzczurak 5:f16727052990 142 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 143
JerrySzczurak 5:f16727052990 144 status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp);
JerrySzczurak 5:f16727052990 145 Temp = Temp & 0x10;
JerrySzczurak 5:f16727052990 146 *pInterruptPolarity = !(Temp>>4);
JerrySzczurak 5:f16727052990 147 return status;
JerrySzczurak 5:f16727052990 148 }
JerrySzczurak 5:f16727052990 149
JerrySzczurak 5:f16727052990 150
JerrySzczurak 5:f16727052990 151
JerrySzczurak 5:f16727052990 152 VL53L1X_ERROR VL53L1X::VL53L1X_StartRanging()
JerrySzczurak 5:f16727052990 153 {
JerrySzczurak 5:f16727052990 154 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 155 /*
JerrySzczurak 5:f16727052990 156 uint8_t Addr = 0x00;
JerrySzczurak 5:f16727052990 157
JerrySzczurak 5:f16727052990 158 for (Addr = 0x2D; Addr <= 0x87; Addr++){
JerrySzczurak 5:f16727052990 159 status = VL53L1_WrByte(Device, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
JerrySzczurak 5:f16727052990 160 }
JerrySzczurak 5:f16727052990 161 */
JerrySzczurak 5:f16727052990 162 status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */
JerrySzczurak 5:f16727052990 163 return status;
JerrySzczurak 5:f16727052990 164 }
JerrySzczurak 5:f16727052990 165
JerrySzczurak 5:f16727052990 166 VL53L1X_ERROR VL53L1X::VL53L1X_StopRanging()
JerrySzczurak 5:f16727052990 167 {
JerrySzczurak 5:f16727052990 168 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 169
JerrySzczurak 5:f16727052990 170 status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */
JerrySzczurak 5:f16727052990 171 return status;
JerrySzczurak 5:f16727052990 172 }
JerrySzczurak 5:f16727052990 173
JerrySzczurak 5:f16727052990 174
JerrySzczurak 5:f16727052990 175
JerrySzczurak 5:f16727052990 176 VL53L1X_ERROR VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady)
JerrySzczurak 5:f16727052990 177 {
JerrySzczurak 5:f16727052990 178 uint8_t Temp;
JerrySzczurak 5:f16727052990 179 uint8_t IntPol;
JerrySzczurak 5:f16727052990 180 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 181
JerrySzczurak 5:f16727052990 182 status = VL53L1X_GetInterruptPolarity(&IntPol);
JerrySzczurak 5:f16727052990 183 status = VL53L1_RdByte(Device, GPIO__TIO_HV_STATUS, &Temp);
JerrySzczurak 5:f16727052990 184 /* Read in the register to check if a new value is available */
JerrySzczurak 5:f16727052990 185 if (status == 0){
JerrySzczurak 5:f16727052990 186 if ((Temp & 1) == IntPol)
JerrySzczurak 5:f16727052990 187 *isDataReady = 1;
JerrySzczurak 5:f16727052990 188 else
JerrySzczurak 5:f16727052990 189 *isDataReady = 0;
JerrySzczurak 5:f16727052990 190 }
JerrySzczurak 5:f16727052990 191 return status;
JerrySzczurak 5:f16727052990 192 }
JerrySzczurak 5:f16727052990 193
JerrySzczurak 5:f16727052990 194
JerrySzczurak 5:f16727052990 195 VL53L1X_ERROR VL53L1X::VL53L1X_SetTimingBudgetInMs(uint16_t TimingBudgetInMs)
JerrySzczurak 5:f16727052990 196 {
JerrySzczurak 5:f16727052990 197 uint16_t DM;
JerrySzczurak 5:f16727052990 198 VL53L1X_ERROR status=0;
JerrySzczurak 5:f16727052990 199
JerrySzczurak 5:f16727052990 200 status = VL53L1X_GetDistanceMode(&DM);
JerrySzczurak 5:f16727052990 201 if (DM == 0)
JerrySzczurak 5:f16727052990 202 return 1;
JerrySzczurak 5:f16727052990 203 else if (DM == 1) { /* Short DistanceMode */
JerrySzczurak 5:f16727052990 204 switch (TimingBudgetInMs) {
JerrySzczurak 5:f16727052990 205 case 15: /* only available in short distance mode */
JerrySzczurak 5:f16727052990 206 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 207 0x01D);
JerrySzczurak 5:f16727052990 208 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 209 0x0027);
JerrySzczurak 5:f16727052990 210 break;
JerrySzczurak 5:f16727052990 211 case 20:
JerrySzczurak 5:f16727052990 212 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 213 0x0051);
JerrySzczurak 5:f16727052990 214 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 215 0x006E);
JerrySzczurak 5:f16727052990 216 break;
JerrySzczurak 5:f16727052990 217 case 33:
JerrySzczurak 5:f16727052990 218 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 219 0x00D6);
JerrySzczurak 5:f16727052990 220 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 221 0x006E);
JerrySzczurak 5:f16727052990 222 break;
JerrySzczurak 5:f16727052990 223 case 50:
JerrySzczurak 5:f16727052990 224 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 225 0x1AE);
JerrySzczurak 5:f16727052990 226 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 227 0x01E8);
JerrySzczurak 5:f16727052990 228 break;
JerrySzczurak 5:f16727052990 229 case 100:
JerrySzczurak 5:f16727052990 230 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 231 0x02E1);
JerrySzczurak 5:f16727052990 232 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 233 0x0388);
JerrySzczurak 5:f16727052990 234 break;
JerrySzczurak 5:f16727052990 235 case 200:
JerrySzczurak 5:f16727052990 236 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 237 0x03E1);
JerrySzczurak 5:f16727052990 238 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 239 0x0496);
JerrySzczurak 5:f16727052990 240 break;
JerrySzczurak 5:f16727052990 241 case 500:
JerrySzczurak 5:f16727052990 242 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 243 0x0591);
JerrySzczurak 5:f16727052990 244 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 245 0x05C1);
JerrySzczurak 5:f16727052990 246 break;
JerrySzczurak 5:f16727052990 247 default:
JerrySzczurak 5:f16727052990 248 status = 1;
JerrySzczurak 5:f16727052990 249 break;
JerrySzczurak 5:f16727052990 250 }
JerrySzczurak 5:f16727052990 251 } else {
JerrySzczurak 5:f16727052990 252 switch (TimingBudgetInMs) {
JerrySzczurak 5:f16727052990 253 case 20:
JerrySzczurak 5:f16727052990 254 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 255 0x001E);
JerrySzczurak 5:f16727052990 256 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 257 0x0022);
JerrySzczurak 5:f16727052990 258 break;
JerrySzczurak 5:f16727052990 259 case 33:
JerrySzczurak 5:f16727052990 260 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 261 0x0060);
JerrySzczurak 5:f16727052990 262 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 263 0x006E);
JerrySzczurak 5:f16727052990 264 break;
JerrySzczurak 5:f16727052990 265 case 50:
JerrySzczurak 5:f16727052990 266 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 267 0x00AD);
JerrySzczurak 5:f16727052990 268 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 269 0x00C6);
JerrySzczurak 5:f16727052990 270 break;
JerrySzczurak 5:f16727052990 271 case 100:
JerrySzczurak 5:f16727052990 272 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 273 0x01CC);
JerrySzczurak 5:f16727052990 274 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 275 0x01EA);
JerrySzczurak 5:f16727052990 276 break;
JerrySzczurak 5:f16727052990 277 case 200:
JerrySzczurak 5:f16727052990 278 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 279 0x02D9);
JerrySzczurak 5:f16727052990 280 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 281 0x02F8);
JerrySzczurak 5:f16727052990 282 break;
JerrySzczurak 5:f16727052990 283 case 500:
JerrySzczurak 5:f16727052990 284 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 285 0x048F);
JerrySzczurak 5:f16727052990 286 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 287 0x04A4);
JerrySzczurak 5:f16727052990 288 break;
JerrySzczurak 5:f16727052990 289 default:
JerrySzczurak 5:f16727052990 290 status = 1;
JerrySzczurak 5:f16727052990 291 break;
JerrySzczurak 5:f16727052990 292 }
JerrySzczurak 5:f16727052990 293 }
JerrySzczurak 5:f16727052990 294 return status;
JerrySzczurak 5:f16727052990 295 }
JerrySzczurak 5:f16727052990 296
JerrySzczurak 5:f16727052990 297 VL53L1X_ERROR VL53L1X::VL53L1X_GetTimingBudgetInMs(uint16_t *pTimingBudget)
JerrySzczurak 5:f16727052990 298 {
JerrySzczurak 5:f16727052990 299 uint16_t Temp;
JerrySzczurak 5:f16727052990 300 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 301
JerrySzczurak 5:f16727052990 302 status = VL53L1_RdWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp);
JerrySzczurak 5:f16727052990 303 switch (Temp) {
JerrySzczurak 5:f16727052990 304 case 0x001D :
JerrySzczurak 5:f16727052990 305 *pTimingBudget = 15;
JerrySzczurak 5:f16727052990 306 break;
JerrySzczurak 5:f16727052990 307 case 0x0051 :
JerrySzczurak 5:f16727052990 308 case 0x001E :
JerrySzczurak 5:f16727052990 309 *pTimingBudget = 20;
JerrySzczurak 5:f16727052990 310 break;
JerrySzczurak 5:f16727052990 311 case 0x00D6 :
JerrySzczurak 5:f16727052990 312 case 0x0060 :
JerrySzczurak 5:f16727052990 313 *pTimingBudget = 33;
JerrySzczurak 5:f16727052990 314 break;
JerrySzczurak 5:f16727052990 315 case 0x1AE :
JerrySzczurak 5:f16727052990 316 case 0x00AD :
JerrySzczurak 5:f16727052990 317 *pTimingBudget = 50;
JerrySzczurak 5:f16727052990 318 break;
JerrySzczurak 5:f16727052990 319 case 0x02E1 :
JerrySzczurak 5:f16727052990 320 case 0x01CC :
JerrySzczurak 5:f16727052990 321 *pTimingBudget = 100;
JerrySzczurak 5:f16727052990 322 break;
JerrySzczurak 5:f16727052990 323 case 0x03E1 :
JerrySzczurak 5:f16727052990 324 case 0x02D9 :
JerrySzczurak 5:f16727052990 325 *pTimingBudget = 200;
JerrySzczurak 5:f16727052990 326 break;
JerrySzczurak 5:f16727052990 327 case 0x0591 :
JerrySzczurak 5:f16727052990 328 case 0x048F :
JerrySzczurak 5:f16727052990 329 *pTimingBudget = 500;
JerrySzczurak 5:f16727052990 330 break;
JerrySzczurak 5:f16727052990 331 default:
JerrySzczurak 5:f16727052990 332 *pTimingBudget = 0;
JerrySzczurak 5:f16727052990 333 break;
JerrySzczurak 5:f16727052990 334 }
JerrySzczurak 5:f16727052990 335 return status;
JerrySzczurak 5:f16727052990 336 }
JerrySzczurak 5:f16727052990 337
JerrySzczurak 5:f16727052990 338
JerrySzczurak 5:f16727052990 339 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceMode(uint16_t DM)
JerrySzczurak 5:f16727052990 340 {
JerrySzczurak 5:f16727052990 341 uint16_t TB;
JerrySzczurak 5:f16727052990 342 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 343
JerrySzczurak 5:f16727052990 344 status = VL53L1X_GetTimingBudgetInMs(&TB);
JerrySzczurak 5:f16727052990 345
JerrySzczurak 5:f16727052990 346
JerrySzczurak 5:f16727052990 347 switch (DM) {
JerrySzczurak 5:f16727052990 348 case 1:
JerrySzczurak 5:f16727052990 349 status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
JerrySzczurak 5:f16727052990 350 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
JerrySzczurak 5:f16727052990 351 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
JerrySzczurak 5:f16727052990 352 status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
JerrySzczurak 5:f16727052990 353 status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0705);
JerrySzczurak 5:f16727052990 354 status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
JerrySzczurak 5:f16727052990 355 break;
JerrySzczurak 5:f16727052990 356 case 2:
JerrySzczurak 5:f16727052990 357 status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
JerrySzczurak 5:f16727052990 358 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
JerrySzczurak 5:f16727052990 359 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
JerrySzczurak 5:f16727052990 360 status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
JerrySzczurak 5:f16727052990 361 status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0F0D);
JerrySzczurak 5:f16727052990 362 status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
JerrySzczurak 5:f16727052990 363 break;
JerrySzczurak 5:f16727052990 364 default:
JerrySzczurak 5:f16727052990 365 break;
JerrySzczurak 5:f16727052990 366 }
JerrySzczurak 5:f16727052990 367 status = VL53L1X_SetTimingBudgetInMs(TB);
JerrySzczurak 5:f16727052990 368 return status;
JerrySzczurak 5:f16727052990 369 }
JerrySzczurak 5:f16727052990 370
JerrySzczurak 5:f16727052990 371
JerrySzczurak 5:f16727052990 372
JerrySzczurak 5:f16727052990 373
JerrySzczurak 5:f16727052990 374 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceMode(uint16_t *DM)
JerrySzczurak 5:f16727052990 375 {
JerrySzczurak 5:f16727052990 376 uint8_t TempDM, status=0;
JerrySzczurak 5:f16727052990 377
JerrySzczurak 5:f16727052990 378 status = VL53L1_RdByte(Device,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
JerrySzczurak 5:f16727052990 379 if (TempDM == 0x14)
JerrySzczurak 5:f16727052990 380 *DM=1;
JerrySzczurak 5:f16727052990 381 if(TempDM == 0x0A)
JerrySzczurak 5:f16727052990 382 *DM=2;
JerrySzczurak 5:f16727052990 383 return status;
JerrySzczurak 5:f16727052990 384 }
JerrySzczurak 5:f16727052990 385
JerrySzczurak 5:f16727052990 386
JerrySzczurak 5:f16727052990 387
JerrySzczurak 5:f16727052990 388 VL53L1X_ERROR VL53L1X::VL53L1X_SetInterMeasurementInMs(uint16_t InterMeasMs)
JerrySzczurak 5:f16727052990 389 {
JerrySzczurak 5:f16727052990 390 uint16_t ClockPLL;
JerrySzczurak 5:f16727052990 391 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 392
JerrySzczurak 5:f16727052990 393 status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
JerrySzczurak 5:f16727052990 394 ClockPLL = ClockPLL&0x3FF;
JerrySzczurak 5:f16727052990 395 VL53L1_WrDWord(Device, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
JerrySzczurak 5:f16727052990 396 (uint32_t)(ClockPLL * InterMeasMs * 1.075));
JerrySzczurak 5:f16727052990 397 return status;
JerrySzczurak 5:f16727052990 398
JerrySzczurak 5:f16727052990 399 }
JerrySzczurak 5:f16727052990 400
JerrySzczurak 5:f16727052990 401
JerrySzczurak 5:f16727052990 402 VL53L1X_ERROR VL53L1X::VL53L1X_GetInterMeasurementInMs(uint16_t *pIM)
JerrySzczurak 5:f16727052990 403 {
JerrySzczurak 5:f16727052990 404 uint16_t ClockPLL;
JerrySzczurak 5:f16727052990 405 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 406 uint32_t tmp;
JerrySzczurak 5:f16727052990 407
JerrySzczurak 5:f16727052990 408 status = VL53L1_RdDWord(Device,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
JerrySzczurak 5:f16727052990 409 *pIM = (uint16_t)tmp;
JerrySzczurak 5:f16727052990 410 status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
JerrySzczurak 5:f16727052990 411 ClockPLL = ClockPLL&0x3FF;
JerrySzczurak 5:f16727052990 412 *pIM= (uint16_t)(*pIM/(ClockPLL*1.065));
JerrySzczurak 5:f16727052990 413 return status;
JerrySzczurak 5:f16727052990 414 }
JerrySzczurak 5:f16727052990 415
JerrySzczurak 5:f16727052990 416
JerrySzczurak 5:f16727052990 417 VL53L1X_ERROR VL53L1X::VL53L1X_BootState(uint8_t *state)
JerrySzczurak 5:f16727052990 418 {
JerrySzczurak 5:f16727052990 419 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 420 uint8_t tmp = 0;
JerrySzczurak 5:f16727052990 421
JerrySzczurak 5:f16727052990 422 status = VL53L1_RdByte(Device,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
JerrySzczurak 5:f16727052990 423 *state = tmp;
JerrySzczurak 5:f16727052990 424 return status;
JerrySzczurak 5:f16727052990 425 }
JerrySzczurak 5:f16727052990 426
JerrySzczurak 5:f16727052990 427
JerrySzczurak 5:f16727052990 428 VL53L1X_ERROR VL53L1X::VL53L1X_GetSensorId(uint16_t *sensorId)
JerrySzczurak 5:f16727052990 429 {
JerrySzczurak 5:f16727052990 430 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 431 uint16_t tmp = 0;
JerrySzczurak 5:f16727052990 432
JerrySzczurak 5:f16727052990 433 status = VL53L1_RdWord(Device, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
JerrySzczurak 5:f16727052990 434 *sensorId = tmp;
JerrySzczurak 5:f16727052990 435 return status;
JerrySzczurak 5:f16727052990 436 }
JerrySzczurak 5:f16727052990 437
JerrySzczurak 5:f16727052990 438
JerrySzczurak 5:f16727052990 439 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistance(uint16_t *distance)
JerrySzczurak 5:f16727052990 440 {
JerrySzczurak 5:f16727052990 441 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 442 uint16_t tmp;
JerrySzczurak 5:f16727052990 443
JerrySzczurak 5:f16727052990 444 status = (VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 445 VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
JerrySzczurak 5:f16727052990 446 *distance = tmp;
JerrySzczurak 5:f16727052990 447 return status;
JerrySzczurak 5:f16727052990 448 }
JerrySzczurak 5:f16727052990 449
JerrySzczurak 5:f16727052990 450 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalPerSpad(uint16_t *signalRate)
JerrySzczurak 5:f16727052990 451 {
JerrySzczurak 5:f16727052990 452 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 453 uint16_t SpNb=1, signal;
JerrySzczurak 5:f16727052990 454
JerrySzczurak 5:f16727052990 455 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 456 VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
JerrySzczurak 5:f16727052990 457 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 458 VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
JerrySzczurak 5:f16727052990 459 *signalRate = (uint16_t) (2000.0*signal/SpNb);
JerrySzczurak 5:f16727052990 460 return status;
JerrySzczurak 5:f16727052990 461 }
JerrySzczurak 5:f16727052990 462
JerrySzczurak 5:f16727052990 463
JerrySzczurak 5:f16727052990 464 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientPerSpad(uint16_t *ambPerSp)
JerrySzczurak 5:f16727052990 465 {
JerrySzczurak 5:f16727052990 466 VL53L1X_ERROR status=0;
JerrySzczurak 5:f16727052990 467 uint16_t AmbientRate, SpNb=1;
JerrySzczurak 5:f16727052990 468
JerrySzczurak 5:f16727052990 469 status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
JerrySzczurak 5:f16727052990 470 status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
JerrySzczurak 5:f16727052990 471 *ambPerSp=(uint16_t) (2000.0 * AmbientRate / SpNb);
JerrySzczurak 5:f16727052990 472 return status;
JerrySzczurak 5:f16727052990 473 }
JerrySzczurak 5:f16727052990 474
JerrySzczurak 5:f16727052990 475
JerrySzczurak 5:f16727052990 476 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalRate(uint16_t *signal)
JerrySzczurak 5:f16727052990 477 {
JerrySzczurak 5:f16727052990 478 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 479 uint16_t tmp;
JerrySzczurak 5:f16727052990 480
JerrySzczurak 5:f16727052990 481 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 482 VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
JerrySzczurak 5:f16727052990 483 *signal = tmp*8;
JerrySzczurak 5:f16727052990 484 return status;
JerrySzczurak 5:f16727052990 485 }
JerrySzczurak 5:f16727052990 486
JerrySzczurak 5:f16727052990 487
JerrySzczurak 5:f16727052990 488 VL53L1X_ERROR VL53L1X::VL53L1X_GetSpadNb(uint16_t *spNb)
JerrySzczurak 5:f16727052990 489 {
JerrySzczurak 5:f16727052990 490 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 491 uint16_t tmp;
JerrySzczurak 5:f16727052990 492
JerrySzczurak 5:f16727052990 493 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 494 VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
JerrySzczurak 5:f16727052990 495 *spNb = tmp >> 8;
JerrySzczurak 5:f16727052990 496 return status;
JerrySzczurak 5:f16727052990 497 }
JerrySzczurak 5:f16727052990 498
JerrySzczurak 5:f16727052990 499
JerrySzczurak 5:f16727052990 500 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientRate(uint16_t *ambRate)
JerrySzczurak 5:f16727052990 501 {
JerrySzczurak 5:f16727052990 502 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 503 uint16_t tmp;
JerrySzczurak 5:f16727052990 504
JerrySzczurak 5:f16727052990 505 status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
JerrySzczurak 5:f16727052990 506 *ambRate = tmp*8;
JerrySzczurak 5:f16727052990 507 return status;
JerrySzczurak 5:f16727052990 508 }
JerrySzczurak 5:f16727052990 509
JerrySzczurak 5:f16727052990 510
JerrySzczurak 5:f16727052990 511 VL53L1X_ERROR VL53L1X::VL53L1X_GetRangeStatus(uint8_t *rangeStatus)
JerrySzczurak 5:f16727052990 512 {
JerrySzczurak 5:f16727052990 513 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 514 uint8_t RgSt;
JerrySzczurak 5:f16727052990 515
JerrySzczurak 5:f16727052990 516 status = VL53L1_RdByte(Device, VL53L1_RESULT__RANGE_STATUS, &RgSt);
JerrySzczurak 5:f16727052990 517 RgSt = RgSt&0x1F;
JerrySzczurak 5:f16727052990 518 switch (RgSt) {
JerrySzczurak 5:f16727052990 519 case 9:
JerrySzczurak 5:f16727052990 520 RgSt = 0;
JerrySzczurak 5:f16727052990 521 break;
JerrySzczurak 5:f16727052990 522 case 6:
JerrySzczurak 5:f16727052990 523 RgSt = 1;
JerrySzczurak 5:f16727052990 524 break;
JerrySzczurak 5:f16727052990 525 case 4:
JerrySzczurak 5:f16727052990 526 RgSt = 2;
JerrySzczurak 5:f16727052990 527 break;
JerrySzczurak 5:f16727052990 528 case 8:
JerrySzczurak 5:f16727052990 529 RgSt = 3;
JerrySzczurak 5:f16727052990 530 break;
JerrySzczurak 5:f16727052990 531 case 5:
JerrySzczurak 5:f16727052990 532 RgSt = 4;
JerrySzczurak 5:f16727052990 533 break;
JerrySzczurak 5:f16727052990 534 case 3:
JerrySzczurak 5:f16727052990 535 RgSt = 5;
JerrySzczurak 5:f16727052990 536 break;
JerrySzczurak 5:f16727052990 537 case 19:
JerrySzczurak 5:f16727052990 538 RgSt = 6;
JerrySzczurak 5:f16727052990 539 break;
JerrySzczurak 5:f16727052990 540 case 7:
JerrySzczurak 5:f16727052990 541 RgSt = 7;
JerrySzczurak 5:f16727052990 542 break;
JerrySzczurak 5:f16727052990 543 case 12:
JerrySzczurak 5:f16727052990 544 RgSt = 9;
JerrySzczurak 5:f16727052990 545 break;
JerrySzczurak 5:f16727052990 546 case 18:
JerrySzczurak 5:f16727052990 547 RgSt = 10;
JerrySzczurak 5:f16727052990 548 break;
JerrySzczurak 5:f16727052990 549 case 22:
JerrySzczurak 5:f16727052990 550 RgSt = 11;
JerrySzczurak 5:f16727052990 551 break;
JerrySzczurak 5:f16727052990 552 case 23:
JerrySzczurak 5:f16727052990 553 RgSt = 12;
JerrySzczurak 5:f16727052990 554 break;
JerrySzczurak 5:f16727052990 555 case 13:
JerrySzczurak 5:f16727052990 556 RgSt = 13;
JerrySzczurak 5:f16727052990 557 break;
JerrySzczurak 5:f16727052990 558 default:
JerrySzczurak 5:f16727052990 559 RgSt = 255;
JerrySzczurak 5:f16727052990 560 break;
JerrySzczurak 5:f16727052990 561 }
JerrySzczurak 5:f16727052990 562 *rangeStatus = RgSt;
JerrySzczurak 5:f16727052990 563 return status;
JerrySzczurak 5:f16727052990 564 }
JerrySzczurak 5:f16727052990 565
JerrySzczurak 5:f16727052990 566
JerrySzczurak 5:f16727052990 567 VL53L1X_ERROR VL53L1X::VL53L1X_SetOffset(int16_t OffsetValue)
JerrySzczurak 5:f16727052990 568 {
JerrySzczurak 5:f16727052990 569 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 570 int16_t Temp;
JerrySzczurak 5:f16727052990 571
JerrySzczurak 5:f16727052990 572 Temp = (OffsetValue*4);
JerrySzczurak 5:f16727052990 573 VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
JerrySzczurak 5:f16727052990 574 (uint16_t)Temp);
JerrySzczurak 5:f16727052990 575 VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 576 VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 577 return status;
JerrySzczurak 5:f16727052990 578 }
JerrySzczurak 5:f16727052990 579
JerrySzczurak 5:f16727052990 580
JerrySzczurak 5:f16727052990 581 VL53L1X_ERROR VL53L1X::VL53L1X_GetOffset(int16_t *offset)
JerrySzczurak 5:f16727052990 582 {
JerrySzczurak 5:f16727052990 583 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 584 uint16_t Temp;
JerrySzczurak 5:f16727052990 585
JerrySzczurak 5:f16727052990 586 status = VL53L1_RdWord(Device,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
JerrySzczurak 5:f16727052990 587 Temp = Temp<<3;
JerrySzczurak 5:f16727052990 588 Temp = Temp >>5;
JerrySzczurak 5:f16727052990 589 *offset = (int16_t)(Temp);
JerrySzczurak 5:f16727052990 590 return status;
JerrySzczurak 5:f16727052990 591 }
JerrySzczurak 5:f16727052990 592
JerrySzczurak 5:f16727052990 593 VL53L1X_ERROR VL53L1X::VL53L1X_SetXtalk(uint16_t XtalkValue)
JerrySzczurak 5:f16727052990 594 {
JerrySzczurak 5:f16727052990 595 /* XTalkValue in count per second to avoid float type */
JerrySzczurak 5:f16727052990 596 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 597
JerrySzczurak 5:f16727052990 598 status = VL53L1_WrWord(Device,
JerrySzczurak 5:f16727052990 599 ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
JerrySzczurak 5:f16727052990 600 0x0000);
JerrySzczurak 5:f16727052990 601 status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
JerrySzczurak 5:f16727052990 602 0x0000);
JerrySzczurak 5:f16727052990 603 status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
JerrySzczurak 5:f16727052990 604 (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
JerrySzczurak 5:f16727052990 605 return status;
JerrySzczurak 5:f16727052990 606 }
JerrySzczurak 5:f16727052990 607
JerrySzczurak 5:f16727052990 608
JerrySzczurak 5:f16727052990 609 VL53L1X_ERROR VL53L1X::VL53L1X_GetXtalk(uint16_t *xtalk )
JerrySzczurak 5:f16727052990 610 {
JerrySzczurak 5:f16727052990 611 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 612 uint16_t tmp;
JerrySzczurak 5:f16727052990 613
JerrySzczurak 5:f16727052990 614 status = VL53L1_RdWord(Device,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, &tmp);
JerrySzczurak 5:f16727052990 615 *xtalk = (tmp*1000)>>9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
JerrySzczurak 5:f16727052990 616 return status;
JerrySzczurak 5:f16727052990 617 }
JerrySzczurak 5:f16727052990 618
JerrySzczurak 5:f16727052990 619
JerrySzczurak 5:f16727052990 620 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceThreshold(uint16_t ThreshLow,
JerrySzczurak 5:f16727052990 621 uint16_t ThreshHigh, uint8_t Window,
JerrySzczurak 5:f16727052990 622 uint8_t IntOnNoTarget)
JerrySzczurak 5:f16727052990 623 {
JerrySzczurak 5:f16727052990 624 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 625 uint8_t Temp = 0;
JerrySzczurak 5:f16727052990 626
JerrySzczurak 5:f16727052990 627 status = VL53L1_RdByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
JerrySzczurak 5:f16727052990 628 Temp = Temp & 0x47;
JerrySzczurak 5:f16727052990 629 if (IntOnNoTarget == 0) {
JerrySzczurak 5:f16727052990 630 status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
JerrySzczurak 5:f16727052990 631 (Temp | (Window & 0x07)));
JerrySzczurak 5:f16727052990 632 } else {
JerrySzczurak 5:f16727052990 633 status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
JerrySzczurak 5:f16727052990 634 ((Temp | (Window & 0x07)) | 0x40));
JerrySzczurak 5:f16727052990 635 }
JerrySzczurak 5:f16727052990 636 status = VL53L1_WrWord(Device, SYSTEM__THRESH_HIGH, ThreshHigh);
JerrySzczurak 5:f16727052990 637 status = VL53L1_WrWord(Device, SYSTEM__THRESH_LOW, ThreshLow);
JerrySzczurak 5:f16727052990 638 return status;
JerrySzczurak 5:f16727052990 639 }
JerrySzczurak 5:f16727052990 640
JerrySzczurak 5:f16727052990 641
JerrySzczurak 5:f16727052990 642 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdWindow(uint16_t *window)
JerrySzczurak 5:f16727052990 643 {
JerrySzczurak 5:f16727052990 644 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 645 uint8_t tmp;
JerrySzczurak 5:f16727052990 646 status = VL53L1_RdByte(Device,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
JerrySzczurak 5:f16727052990 647 *window = (uint16_t)(tmp & 0x7);
JerrySzczurak 5:f16727052990 648 return status;
JerrySzczurak 5:f16727052990 649 }
JerrySzczurak 5:f16727052990 650
JerrySzczurak 5:f16727052990 651
JerrySzczurak 5:f16727052990 652 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdLow(uint16_t *low)
JerrySzczurak 5:f16727052990 653 {
JerrySzczurak 5:f16727052990 654 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 655 uint16_t tmp;
JerrySzczurak 5:f16727052990 656
JerrySzczurak 5:f16727052990 657 status = VL53L1_RdWord(Device,SYSTEM__THRESH_LOW, &tmp);
JerrySzczurak 5:f16727052990 658 *low = tmp;
JerrySzczurak 5:f16727052990 659 return status;
JerrySzczurak 5:f16727052990 660 }
JerrySzczurak 5:f16727052990 661
JerrySzczurak 5:f16727052990 662 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdHigh(uint16_t *high)
JerrySzczurak 5:f16727052990 663 {
JerrySzczurak 5:f16727052990 664 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 665 uint16_t tmp;
JerrySzczurak 5:f16727052990 666
JerrySzczurak 5:f16727052990 667 status = VL53L1_RdWord(Device,SYSTEM__THRESH_HIGH, &tmp);
JerrySzczurak 5:f16727052990 668 *high = tmp;
JerrySzczurak 5:f16727052990 669 return status;
JerrySzczurak 5:f16727052990 670 }
JerrySzczurak 5:f16727052990 671
JerrySzczurak 5:f16727052990 672 VL53L1X_ERROR VL53L1X::VL53L1X_SetROI(uint16_t X, uint16_t Y)
JerrySzczurak 5:f16727052990 673 {
JerrySzczurak 5:f16727052990 674 uint8_t OpticalCenter;
JerrySzczurak 5:f16727052990 675 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 676
JerrySzczurak 5:f16727052990 677 status =VL53L1_RdByte(Device, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter);
JerrySzczurak 5:f16727052990 678 if (X > 16)
JerrySzczurak 5:f16727052990 679 X = 16;
JerrySzczurak 5:f16727052990 680 if (Y > 16)
JerrySzczurak 5:f16727052990 681 Y = 16;
JerrySzczurak 5:f16727052990 682 if (X > 10 || Y > 10){
JerrySzczurak 5:f16727052990 683 OpticalCenter = 199;
JerrySzczurak 5:f16727052990 684 }
JerrySzczurak 5:f16727052990 685 status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
JerrySzczurak 5:f16727052990 686 status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
JerrySzczurak 5:f16727052990 687 (Y - 1) << 4 | (X - 1));
JerrySzczurak 5:f16727052990 688 return status;
JerrySzczurak 5:f16727052990 689 }
JerrySzczurak 5:f16727052990 690
JerrySzczurak 5:f16727052990 691 VL53L1X_ERROR VL53L1X::VL53L1X_GetROI_XY(uint16_t *ROI_X, uint16_t *ROI_Y)
JerrySzczurak 5:f16727052990 692 {
JerrySzczurak 5:f16727052990 693 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 694 uint8_t tmp;
JerrySzczurak 5:f16727052990 695
JerrySzczurak 5:f16727052990 696 status = VL53L1_RdByte(Device,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp);
JerrySzczurak 5:f16727052990 697 *ROI_X = ((uint16_t)tmp & 0x0F) + 1;
JerrySzczurak 5:f16727052990 698 *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1;
JerrySzczurak 5:f16727052990 699 return status;
JerrySzczurak 5:f16727052990 700 }
JerrySzczurak 5:f16727052990 701
JerrySzczurak 5:f16727052990 702 VL53L1X_ERROR VL53L1X::VL53L1X_SetSignalThreshold(uint16_t Signal)
JerrySzczurak 5:f16727052990 703 {
JerrySzczurak 5:f16727052990 704 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 705
JerrySzczurak 5:f16727052990 706 VL53L1_WrWord(Device,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3);
JerrySzczurak 5:f16727052990 707 return status;
JerrySzczurak 5:f16727052990 708 }
JerrySzczurak 5:f16727052990 709
JerrySzczurak 5:f16727052990 710 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalThreshold(uint16_t *signal)
JerrySzczurak 5:f16727052990 711 {
JerrySzczurak 5:f16727052990 712 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 713 uint16_t tmp;
JerrySzczurak 5:f16727052990 714
JerrySzczurak 5:f16727052990 715 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 716 RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
JerrySzczurak 5:f16727052990 717 *signal = tmp <<3;
JerrySzczurak 5:f16727052990 718 return status;
JerrySzczurak 5:f16727052990 719 }
JerrySzczurak 5:f16727052990 720
JerrySzczurak 5:f16727052990 721
JerrySzczurak 5:f16727052990 722 VL53L1X_ERROR VL53L1X::VL53L1X_SetSigmaThreshold(uint16_t Sigma)
JerrySzczurak 5:f16727052990 723 {
JerrySzczurak 5:f16727052990 724 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 725
JerrySzczurak 5:f16727052990 726 if(Sigma>(0xFFFF>>2)){
JerrySzczurak 5:f16727052990 727 return 1;
JerrySzczurak 5:f16727052990 728 }
JerrySzczurak 5:f16727052990 729 /* 16 bits register 14.2 format */
JerrySzczurak 5:f16727052990 730 status = VL53L1_WrWord(Device,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2);
JerrySzczurak 5:f16727052990 731 return status;
JerrySzczurak 5:f16727052990 732 }
JerrySzczurak 5:f16727052990 733
JerrySzczurak 5:f16727052990 734 VL53L1X_ERROR VL53L1X::VL53L1X_GetSigmaThreshold(uint16_t *sigma)
JerrySzczurak 5:f16727052990 735 {
JerrySzczurak 5:f16727052990 736 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 737 uint16_t tmp;
JerrySzczurak 5:f16727052990 738
JerrySzczurak 5:f16727052990 739 status = VL53L1_RdWord(Device,RANGE_CONFIG__SIGMA_THRESH, &tmp);
JerrySzczurak 5:f16727052990 740 *sigma = tmp >> 2;
JerrySzczurak 5:f16727052990 741 return status;
JerrySzczurak 5:f16727052990 742
JerrySzczurak 5:f16727052990 743 }
JerrySzczurak 5:f16727052990 744
JerrySzczurak 5:f16727052990 745 VL53L1X_ERROR VL53L1X::VL53L1X_StartTemperatureUpdate()
JerrySzczurak 5:f16727052990 746 {
JerrySzczurak 5:f16727052990 747 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 748 uint8_t tmp=0;
JerrySzczurak 5:f16727052990 749
JerrySzczurak 5:f16727052990 750 status = VL53L1_WrByte(Device,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */
JerrySzczurak 5:f16727052990 751 status = VL53L1_WrByte(Device,0x0B,0x92);
JerrySzczurak 5:f16727052990 752 status = VL53L1X_StartRanging();
JerrySzczurak 5:f16727052990 753 while(tmp==0){
JerrySzczurak 5:f16727052990 754 status = VL53L1X_CheckForDataReady(&tmp);
JerrySzczurak 5:f16727052990 755 }
JerrySzczurak 5:f16727052990 756 tmp = 0;
JerrySzczurak 5:f16727052990 757 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 758 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 759 status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
JerrySzczurak 5:f16727052990 760 status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */
JerrySzczurak 5:f16727052990 761 return status;
JerrySzczurak 5:f16727052990 762 }
JerrySzczurak 5:f16727052990 763
JerrySzczurak 5:f16727052990 764 /* VL53L1X_calibration.h functions */
JerrySzczurak 5:f16727052990 765
JerrySzczurak 5:f16727052990 766 int8_t VL53L1X::VL53L1X_CalibrateOffset(uint16_t TargetDistInMm, int16_t *offset)
JerrySzczurak 5:f16727052990 767 {
JerrySzczurak 5:f16727052990 768 uint8_t i = 0, tmp;
JerrySzczurak 5:f16727052990 769 int16_t AverageDistance = 0;
JerrySzczurak 5:f16727052990 770 uint16_t distance;
JerrySzczurak 5:f16727052990 771 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 772
JerrySzczurak 5:f16727052990 773 status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 774 status = VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 775 status = VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 776 status = VL53L1X_StartRanging(); /* Enable VL53L1X sensor */
JerrySzczurak 5:f16727052990 777 for (i = 0; i < 50; i++) {
JerrySzczurak 5:f16727052990 778 while (tmp == 0){
JerrySzczurak 5:f16727052990 779 status = VL53L1X_CheckForDataReady(&tmp);
JerrySzczurak 5:f16727052990 780 }
JerrySzczurak 5:f16727052990 781 tmp = 0;
JerrySzczurak 5:f16727052990 782 status = VL53L1X_GetDistance(&distance);
JerrySzczurak 5:f16727052990 783 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 784 AverageDistance = AverageDistance + distance;
JerrySzczurak 5:f16727052990 785 }
JerrySzczurak 5:f16727052990 786 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 787 AverageDistance = AverageDistance / 50;
JerrySzczurak 5:f16727052990 788 *offset = TargetDistInMm - AverageDistance;
JerrySzczurak 5:f16727052990 789 status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4);
JerrySzczurak 5:f16727052990 790 return status;
JerrySzczurak 5:f16727052990 791 }
JerrySzczurak 5:f16727052990 792
JerrySzczurak 5:f16727052990 793
JerrySzczurak 5:f16727052990 794 int8_t VL53L1X::VL53L1X_CalibrateXtalk(uint16_t TargetDistInMm, uint16_t *xtalk)
JerrySzczurak 5:f16727052990 795 {
JerrySzczurak 5:f16727052990 796 uint8_t i, tmp= 0;
JerrySzczurak 5:f16727052990 797 float AverageSignalRate = 0;
JerrySzczurak 5:f16727052990 798 float AverageDistance = 0;
JerrySzczurak 5:f16727052990 799 float AverageSpadNb = 0;
JerrySzczurak 5:f16727052990 800 uint16_t distance = 0, spadNum;
JerrySzczurak 5:f16727052990 801 uint16_t sr;
JerrySzczurak 5:f16727052990 802 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 803
JerrySzczurak 5:f16727052990 804 status = VL53L1_WrWord(Device, 0x0016,0);
JerrySzczurak 5:f16727052990 805 status = VL53L1X_StartRanging();
JerrySzczurak 5:f16727052990 806 for (i = 0; i < 50; i++) {
JerrySzczurak 5:f16727052990 807 while (tmp == 0){
JerrySzczurak 5:f16727052990 808 status = VL53L1X_CheckForDataReady(&tmp);
JerrySzczurak 5:f16727052990 809 }
JerrySzczurak 5:f16727052990 810 tmp=0;
JerrySzczurak 5:f16727052990 811 status= VL53L1X_GetSignalRate(&sr);
JerrySzczurak 5:f16727052990 812 status= VL53L1X_GetDistance(&distance);
JerrySzczurak 5:f16727052990 813 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 814 AverageDistance = AverageDistance + distance;
JerrySzczurak 5:f16727052990 815 status = VL53L1X_GetSpadNb(&spadNum);
JerrySzczurak 5:f16727052990 816 AverageSpadNb = AverageSpadNb + spadNum;
JerrySzczurak 5:f16727052990 817 AverageSignalRate =
JerrySzczurak 5:f16727052990 818 AverageSignalRate + sr;
JerrySzczurak 5:f16727052990 819 }
JerrySzczurak 5:f16727052990 820 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 821 AverageDistance = AverageDistance / 50;
JerrySzczurak 5:f16727052990 822 AverageSpadNb = AverageSpadNb / 50;
JerrySzczurak 5:f16727052990 823 AverageSignalRate = AverageSignalRate / 50;
JerrySzczurak 5:f16727052990 824 /* Calculate Xtalk value */
JerrySzczurak 5:f16727052990 825 *xtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb);
JerrySzczurak 5:f16727052990 826 status = VL53L1_WrWord(Device, 0x0016, *xtalk);
JerrySzczurak 5:f16727052990 827 return status;
JerrySzczurak 5:f16727052990 828 }
JerrySzczurak 5:f16727052990 829
JerrySzczurak 5:f16727052990 830
JerrySzczurak 5:f16727052990 831
JerrySzczurak 5:f16727052990 832
JerrySzczurak 5:f16727052990 833 /* Write and read functions from I2C */
JerrySzczurak 5:f16727052990 834
JerrySzczurak 5:f16727052990 835
JerrySzczurak 5:f16727052990 836 VL53L1X_ERROR VL53L1X::VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
JerrySzczurak 5:f16727052990 837 {
JerrySzczurak 5:f16727052990 838 int status;
JerrySzczurak 5:f16727052990 839
JerrySzczurak 5:f16727052990 840 status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
JerrySzczurak 5:f16727052990 841 return status;
JerrySzczurak 5:f16727052990 842 }
JerrySzczurak 5:f16727052990 843
JerrySzczurak 5:f16727052990 844 VL53L1X_ERROR VL53L1X::VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
JerrySzczurak 5:f16727052990 845 {
JerrySzczurak 5:f16727052990 846 int status;
JerrySzczurak 5:f16727052990 847
JerrySzczurak 5:f16727052990 848 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
JerrySzczurak 5:f16727052990 849
JerrySzczurak 5:f16727052990 850 return status;
JerrySzczurak 5:f16727052990 851 }
JerrySzczurak 5:f16727052990 852
JerrySzczurak 5:f16727052990 853
JerrySzczurak 5:f16727052990 854 VL53L1X_ERROR VL53L1X::VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
JerrySzczurak 5:f16727052990 855 {
JerrySzczurak 5:f16727052990 856 int status;
JerrySzczurak 5:f16727052990 857
JerrySzczurak 5:f16727052990 858 status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, &data, 1);
JerrySzczurak 5:f16727052990 859 return status;
JerrySzczurak 5:f16727052990 860 }
JerrySzczurak 5:f16727052990 861
JerrySzczurak 5:f16727052990 862 VL53L1X_ERROR VL53L1X::VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data)
JerrySzczurak 5:f16727052990 863 {
JerrySzczurak 5:f16727052990 864 int status;
JerrySzczurak 5:f16727052990 865 uint8_t buffer[2];
JerrySzczurak 5:f16727052990 866
JerrySzczurak 5:f16727052990 867 buffer[0] = data >> 8;
JerrySzczurak 5:f16727052990 868 buffer[1] = data & 0x00FF;
JerrySzczurak 5:f16727052990 869 status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
JerrySzczurak 5:f16727052990 870 return status;
JerrySzczurak 5:f16727052990 871 }
JerrySzczurak 5:f16727052990 872
JerrySzczurak 5:f16727052990 873 VL53L1X_ERROR VL53L1X::VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data)
JerrySzczurak 5:f16727052990 874 {
JerrySzczurak 5:f16727052990 875 int status;
JerrySzczurak 5:f16727052990 876 uint8_t buffer[4];
JerrySzczurak 5:f16727052990 877
JerrySzczurak 5:f16727052990 878 buffer[0] = (data >> 24) & 0xFF;
JerrySzczurak 5:f16727052990 879 buffer[1] = (data >> 16) & 0xFF;
JerrySzczurak 5:f16727052990 880 buffer[2] = (data >> 8) & 0xFF;
JerrySzczurak 5:f16727052990 881 buffer[3] = (data >> 0) & 0xFF;
JerrySzczurak 5:f16727052990 882 status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
JerrySzczurak 5:f16727052990 883 return status;
JerrySzczurak 5:f16727052990 884 }
JerrySzczurak 5:f16727052990 885
JerrySzczurak 5:f16727052990 886
JerrySzczurak 5:f16727052990 887 VL53L1X_ERROR VL53L1X::VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
JerrySzczurak 5:f16727052990 888 {
JerrySzczurak 5:f16727052990 889 int status;
JerrySzczurak 5:f16727052990 890
JerrySzczurak 5:f16727052990 891 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, data, 1);
JerrySzczurak 5:f16727052990 892
JerrySzczurak 5:f16727052990 893 if(status)
JerrySzczurak 5:f16727052990 894 return -1;
JerrySzczurak 5:f16727052990 895
JerrySzczurak 5:f16727052990 896 return 0;
JerrySzczurak 5:f16727052990 897 }
JerrySzczurak 5:f16727052990 898
JerrySzczurak 5:f16727052990 899 VL53L1X_ERROR VL53L1X::VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
JerrySzczurak 5:f16727052990 900 {
JerrySzczurak 5:f16727052990 901 int status;
JerrySzczurak 5:f16727052990 902 uint8_t buffer[2] = {0,0};
JerrySzczurak 5:f16727052990 903
JerrySzczurak 5:f16727052990 904 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 2);
JerrySzczurak 5:f16727052990 905 if (!status)
JerrySzczurak 5:f16727052990 906 {
JerrySzczurak 5:f16727052990 907 *data = (buffer[0] << 8) + buffer[1];
JerrySzczurak 5:f16727052990 908 }
JerrySzczurak 5:f16727052990 909 return status;
JerrySzczurak 5:f16727052990 910
JerrySzczurak 5:f16727052990 911 }
JerrySzczurak 5:f16727052990 912
JerrySzczurak 5:f16727052990 913 VL53L1X_ERROR VL53L1X::VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
JerrySzczurak 5:f16727052990 914 {
JerrySzczurak 5:f16727052990 915 int status;
JerrySzczurak 5:f16727052990 916 uint8_t buffer[4] = {0,0,0,0};
JerrySzczurak 5:f16727052990 917
JerrySzczurak 5:f16727052990 918 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 4);
JerrySzczurak 5:f16727052990 919 if(!status)
JerrySzczurak 5:f16727052990 920 {
JerrySzczurak 5:f16727052990 921 *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
JerrySzczurak 5:f16727052990 922 }
JerrySzczurak 5:f16727052990 923 return status;
JerrySzczurak 5:f16727052990 924
JerrySzczurak 5:f16727052990 925 }
JerrySzczurak 5:f16727052990 926
JerrySzczurak 5:f16727052990 927 VL53L1X_ERROR VL53L1X::VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
JerrySzczurak 5:f16727052990 928 {
JerrySzczurak 5:f16727052990 929 int status;
JerrySzczurak 5:f16727052990 930 uint8_t buffer = 0;
JerrySzczurak 5:f16727052990 931
JerrySzczurak 5:f16727052990 932 /* read data direct onto buffer */
JerrySzczurak 5:f16727052990 933 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, &buffer,1);
JerrySzczurak 5:f16727052990 934 if (!status)
JerrySzczurak 5:f16727052990 935 {
JerrySzczurak 5:f16727052990 936 buffer = (buffer & AndData) | OrData;
JerrySzczurak 5:f16727052990 937 status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint16_t)1);
JerrySzczurak 5:f16727052990 938 }
JerrySzczurak 5:f16727052990 939 return status;
JerrySzczurak 5:f16727052990 940 }
JerrySzczurak 5:f16727052990 941
JerrySzczurak 5:f16727052990 942 VL53L1X_ERROR VL53L1X::VL53L1_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
JerrySzczurak 5:f16727052990 943 {
JerrySzczurak 5:f16727052990 944 int ret;
JerrySzczurak 5:f16727052990 945 ret = dev_i2c->v53l1x_i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
JerrySzczurak 5:f16727052990 946 if (ret) {
JerrySzczurak 5:f16727052990 947 return -1;
JerrySzczurak 5:f16727052990 948 }
JerrySzczurak 5:f16727052990 949 return 0;
JerrySzczurak 5:f16727052990 950
JerrySzczurak 5:f16727052990 951 }
JerrySzczurak 5:f16727052990 952
JerrySzczurak 5:f16727052990 953 VL53L1X_ERROR VL53L1X::VL53L1_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
JerrySzczurak 5:f16727052990 954 {
JerrySzczurak 5:f16727052990 955 int ret;
JerrySzczurak 5:f16727052990 956
JerrySzczurak 5:f16727052990 957 ret = dev_i2c->v53l1x_i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
JerrySzczurak 5:f16727052990 958 //ret = dev_i2c->i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
JerrySzczurak 5:f16727052990 959
JerrySzczurak 5:f16727052990 960 if (ret) {
JerrySzczurak 5:f16727052990 961 return -1;
JerrySzczurak 5:f16727052990 962 }
JerrySzczurak 5:f16727052990 963 return 0;
JerrySzczurak 5:f16727052990 964 }
JerrySzczurak 5:f16727052990 965
JerrySzczurak 5:f16727052990 966
JerrySzczurak 5:f16727052990 967 VL53L1X_ERROR VL53L1X::VL53L1_GetTickCount(
JerrySzczurak 5:f16727052990 968 uint32_t *ptick_count_ms)
JerrySzczurak 5:f16727052990 969 {
JerrySzczurak 5:f16727052990 970
JerrySzczurak 5:f16727052990 971 /* Returns current tick count in [ms] */
JerrySzczurak 5:f16727052990 972
JerrySzczurak 5:f16727052990 973 VL53L1X_ERROR status = VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 974
JerrySzczurak 5:f16727052990 975 //*ptick_count_ms = timeGetTime();
JerrySzczurak 5:f16727052990 976 *ptick_count_ms = 0;
JerrySzczurak 5:f16727052990 977
JerrySzczurak 5:f16727052990 978 return status;
JerrySzczurak 5:f16727052990 979 }
JerrySzczurak 5:f16727052990 980
JerrySzczurak 5:f16727052990 981
JerrySzczurak 5:f16727052990 982
JerrySzczurak 5:f16727052990 983 VL53L1X_ERROR VL53L1X::VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us)
JerrySzczurak 5:f16727052990 984 {
JerrySzczurak 5:f16727052990 985 //(void)pdev;
JerrySzczurak 5:f16727052990 986 //wait_ms(wait_us/1000);
JerrySzczurak 5:f16727052990 987 return VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 988 }
JerrySzczurak 5:f16727052990 989
JerrySzczurak 5:f16727052990 990
JerrySzczurak 5:f16727052990 991 VL53L1X_ERROR VL53L1X::VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms)
JerrySzczurak 5:f16727052990 992 {
JerrySzczurak 5:f16727052990 993 //(void)pdev;
JerrySzczurak 5:f16727052990 994 //wait_ms(wait_ms);
JerrySzczurak 5:f16727052990 995 return VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 996 }
JerrySzczurak 5:f16727052990 997
JerrySzczurak 5:f16727052990 998
JerrySzczurak 5:f16727052990 999 VL53L1X_ERROR VL53L1X::VL53L1_WaitValueMaskEx(
JerrySzczurak 5:f16727052990 1000 VL53L1_Dev_t *pdev,
JerrySzczurak 5:f16727052990 1001 uint32_t timeout_ms,
JerrySzczurak 5:f16727052990 1002 uint16_t index,
JerrySzczurak 5:f16727052990 1003 uint8_t value,
JerrySzczurak 5:f16727052990 1004 uint8_t mask,
JerrySzczurak 5:f16727052990 1005 uint32_t poll_delay_ms)
JerrySzczurak 5:f16727052990 1006 {
JerrySzczurak 5:f16727052990 1007
JerrySzczurak 5:f16727052990 1008 /*
JerrySzczurak 5:f16727052990 1009 * Platform implementation of WaitValueMaskEx V2WReg script command
JerrySzczurak 5:f16727052990 1010 *
JerrySzczurak 5:f16727052990 1011 * WaitValueMaskEx(
JerrySzczurak 5:f16727052990 1012 * duration_ms,
JerrySzczurak 5:f16727052990 1013 * index,
JerrySzczurak 5:f16727052990 1014 * value,
JerrySzczurak 5:f16727052990 1015 * mask,
JerrySzczurak 5:f16727052990 1016 * poll_delay_ms);
JerrySzczurak 5:f16727052990 1017 */
JerrySzczurak 5:f16727052990 1018
JerrySzczurak 5:f16727052990 1019 VL53L1_Error status = VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 1020 uint32_t start_time_ms = 0;
JerrySzczurak 5:f16727052990 1021 uint32_t current_time_ms = 0;
JerrySzczurak 5:f16727052990 1022 uint32_t polling_time_ms = 0;
JerrySzczurak 5:f16727052990 1023 uint8_t byte_value = 0;
JerrySzczurak 5:f16727052990 1024 uint8_t found = 0;
JerrySzczurak 5:f16727052990 1025
JerrySzczurak 5:f16727052990 1026
JerrySzczurak 5:f16727052990 1027
JerrySzczurak 5:f16727052990 1028 /* calculate time limit in absolute time */
JerrySzczurak 5:f16727052990 1029
JerrySzczurak 5:f16727052990 1030 VL53L1_GetTickCount(&start_time_ms);
JerrySzczurak 5:f16727052990 1031
JerrySzczurak 5:f16727052990 1032 /* remember current trace functions and temporarily disable
JerrySzczurak 5:f16727052990 1033 * function logging
JerrySzczurak 5:f16727052990 1034 */
JerrySzczurak 5:f16727052990 1035
JerrySzczurak 5:f16727052990 1036
JerrySzczurak 5:f16727052990 1037 /* wait until value is found, timeout reached on error occurred */
JerrySzczurak 5:f16727052990 1038
JerrySzczurak 5:f16727052990 1039 while ((status == VL53L1_ERROR_NONE) &&
JerrySzczurak 5:f16727052990 1040 (polling_time_ms < timeout_ms) &&
JerrySzczurak 5:f16727052990 1041 (found == 0)) {
JerrySzczurak 5:f16727052990 1042
JerrySzczurak 5:f16727052990 1043 if (status == VL53L1_ERROR_NONE)
JerrySzczurak 5:f16727052990 1044 status = VL53L1_RdByte(
JerrySzczurak 5:f16727052990 1045 pdev,
JerrySzczurak 5:f16727052990 1046 index,
JerrySzczurak 5:f16727052990 1047 &byte_value);
JerrySzczurak 5:f16727052990 1048
JerrySzczurak 5:f16727052990 1049 if ((byte_value & mask) == value)
JerrySzczurak 5:f16727052990 1050 found = 1;
JerrySzczurak 5:f16727052990 1051
JerrySzczurak 5:f16727052990 1052 if (status == VL53L1_ERROR_NONE &&
JerrySzczurak 5:f16727052990 1053 found == 0 &&
JerrySzczurak 5:f16727052990 1054 poll_delay_ms > 0)
JerrySzczurak 5:f16727052990 1055 status = VL53L1_WaitMs(
JerrySzczurak 5:f16727052990 1056 pdev,
JerrySzczurak 5:f16727052990 1057 poll_delay_ms);
JerrySzczurak 5:f16727052990 1058
JerrySzczurak 5:f16727052990 1059 /* Update polling time (Compare difference rather than absolute to
JerrySzczurak 5:f16727052990 1060 negate 32bit wrap around issue) */
JerrySzczurak 5:f16727052990 1061 VL53L1_GetTickCount(&current_time_ms);
JerrySzczurak 5:f16727052990 1062 polling_time_ms = current_time_ms - start_time_ms;
JerrySzczurak 5:f16727052990 1063
JerrySzczurak 5:f16727052990 1064 }
JerrySzczurak 5:f16727052990 1065
JerrySzczurak 5:f16727052990 1066
JerrySzczurak 5:f16727052990 1067 if (found == 0 && status == VL53L1_ERROR_NONE)
JerrySzczurak 5:f16727052990 1068 status = VL53L1_ERROR_TIME_OUT;
JerrySzczurak 5:f16727052990 1069
JerrySzczurak 5:f16727052990 1070 return status;
JerrySzczurak 5:f16727052990 1071 }
JerrySzczurak 5:f16727052990 1072
JerrySzczurak 5:f16727052990 1073
JerrySzczurak 5:f16727052990 1074