John Alexander / VL53L1X_mbed

VL53L1X sensor class, for ARM Mbed platform. Based on Ultra-lite, Mass-market C Driver.

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

vl53l1x_class.cpp@6:aa13392d16bb, 2019-05-17 (annotated)

Committer:
dmathew
Date:
Fri May 17 09:07:55 2019 +0000
Revision:
6:aa13392d16bb
Parent:
5:f16727052990 
Changes to class to allow interrupts

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
dmathew 6:aa13392d16bb 65 status = VL53L1_WrByte(Device, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);
JerrySzczurak 5:f16727052990 66 Device->I2cDevAddr = new_address;
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 status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */
JerrySzczurak 5:f16727052990 157 return status;
JerrySzczurak 5:f16727052990 158 }
JerrySzczurak 5:f16727052990 159
JerrySzczurak 5:f16727052990 160 VL53L1X_ERROR VL53L1X::VL53L1X_StopRanging()
JerrySzczurak 5:f16727052990 161 {
JerrySzczurak 5:f16727052990 162 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 163
JerrySzczurak 5:f16727052990 164 status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */
JerrySzczurak 5:f16727052990 165 return status;
JerrySzczurak 5:f16727052990 166 }
JerrySzczurak 5:f16727052990 167
JerrySzczurak 5:f16727052990 168
JerrySzczurak 5:f16727052990 169
JerrySzczurak 5:f16727052990 170 VL53L1X_ERROR VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady)
JerrySzczurak 5:f16727052990 171 {
JerrySzczurak 5:f16727052990 172 uint8_t Temp;
JerrySzczurak 5:f16727052990 173 uint8_t IntPol;
JerrySzczurak 5:f16727052990 174 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 175
JerrySzczurak 5:f16727052990 176 status = VL53L1X_GetInterruptPolarity(&IntPol);
JerrySzczurak 5:f16727052990 177 status = VL53L1_RdByte(Device, GPIO__TIO_HV_STATUS, &Temp);
JerrySzczurak 5:f16727052990 178 /* Read in the register to check if a new value is available */
JerrySzczurak 5:f16727052990 179 if (status == 0){
JerrySzczurak 5:f16727052990 180 if ((Temp & 1) == IntPol)
JerrySzczurak 5:f16727052990 181 *isDataReady = 1;
JerrySzczurak 5:f16727052990 182 else
JerrySzczurak 5:f16727052990 183 *isDataReady = 0;
JerrySzczurak 5:f16727052990 184 }
JerrySzczurak 5:f16727052990 185 return status;
JerrySzczurak 5:f16727052990 186 }
JerrySzczurak 5:f16727052990 187
JerrySzczurak 5:f16727052990 188
JerrySzczurak 5:f16727052990 189 VL53L1X_ERROR VL53L1X::VL53L1X_SetTimingBudgetInMs(uint16_t TimingBudgetInMs)
JerrySzczurak 5:f16727052990 190 {
JerrySzczurak 5:f16727052990 191 uint16_t DM;
JerrySzczurak 5:f16727052990 192 VL53L1X_ERROR status=0;
JerrySzczurak 5:f16727052990 193
JerrySzczurak 5:f16727052990 194 status = VL53L1X_GetDistanceMode(&DM);
JerrySzczurak 5:f16727052990 195 if (DM == 0)
JerrySzczurak 5:f16727052990 196 return 1;
JerrySzczurak 5:f16727052990 197 else if (DM == 1) { /* Short DistanceMode */
JerrySzczurak 5:f16727052990 198 switch (TimingBudgetInMs) {
JerrySzczurak 5:f16727052990 199 case 15: /* only available in short distance mode */
JerrySzczurak 5:f16727052990 200 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 201 0x01D);
JerrySzczurak 5:f16727052990 202 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 203 0x0027);
JerrySzczurak 5:f16727052990 204 break;
JerrySzczurak 5:f16727052990 205 case 20:
JerrySzczurak 5:f16727052990 206 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 207 0x0051);
JerrySzczurak 5:f16727052990 208 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 209 0x006E);
JerrySzczurak 5:f16727052990 210 break;
JerrySzczurak 5:f16727052990 211 case 33:
JerrySzczurak 5:f16727052990 212 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 213 0x00D6);
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 50:
JerrySzczurak 5:f16727052990 218 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 219 0x1AE);
JerrySzczurak 5:f16727052990 220 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 221 0x01E8);
JerrySzczurak 5:f16727052990 222 break;
JerrySzczurak 5:f16727052990 223 case 100:
JerrySzczurak 5:f16727052990 224 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 225 0x02E1);
JerrySzczurak 5:f16727052990 226 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 227 0x0388);
JerrySzczurak 5:f16727052990 228 break;
JerrySzczurak 5:f16727052990 229 case 200:
JerrySzczurak 5:f16727052990 230 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 231 0x03E1);
JerrySzczurak 5:f16727052990 232 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 233 0x0496);
JerrySzczurak 5:f16727052990 234 break;
JerrySzczurak 5:f16727052990 235 case 500:
JerrySzczurak 5:f16727052990 236 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 237 0x0591);
JerrySzczurak 5:f16727052990 238 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 239 0x05C1);
JerrySzczurak 5:f16727052990 240 break;
JerrySzczurak 5:f16727052990 241 default:
JerrySzczurak 5:f16727052990 242 status = 1;
JerrySzczurak 5:f16727052990 243 break;
JerrySzczurak 5:f16727052990 244 }
JerrySzczurak 5:f16727052990 245 } else {
JerrySzczurak 5:f16727052990 246 switch (TimingBudgetInMs) {
JerrySzczurak 5:f16727052990 247 case 20:
JerrySzczurak 5:f16727052990 248 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 249 0x001E);
JerrySzczurak 5:f16727052990 250 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 251 0x0022);
JerrySzczurak 5:f16727052990 252 break;
JerrySzczurak 5:f16727052990 253 case 33:
JerrySzczurak 5:f16727052990 254 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 255 0x0060);
JerrySzczurak 5:f16727052990 256 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 257 0x006E);
JerrySzczurak 5:f16727052990 258 break;
JerrySzczurak 5:f16727052990 259 case 50:
JerrySzczurak 5:f16727052990 260 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 261 0x00AD);
JerrySzczurak 5:f16727052990 262 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 263 0x00C6);
JerrySzczurak 5:f16727052990 264 break;
JerrySzczurak 5:f16727052990 265 case 100:
JerrySzczurak 5:f16727052990 266 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 267 0x01CC);
JerrySzczurak 5:f16727052990 268 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 269 0x01EA);
JerrySzczurak 5:f16727052990 270 break;
JerrySzczurak 5:f16727052990 271 case 200:
JerrySzczurak 5:f16727052990 272 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 273 0x02D9);
JerrySzczurak 5:f16727052990 274 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 275 0x02F8);
JerrySzczurak 5:f16727052990 276 break;
JerrySzczurak 5:f16727052990 277 case 500:
JerrySzczurak 5:f16727052990 278 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
JerrySzczurak 5:f16727052990 279 0x048F);
JerrySzczurak 5:f16727052990 280 VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
JerrySzczurak 5:f16727052990 281 0x04A4);
JerrySzczurak 5:f16727052990 282 break;
JerrySzczurak 5:f16727052990 283 default:
JerrySzczurak 5:f16727052990 284 status = 1;
JerrySzczurak 5:f16727052990 285 break;
JerrySzczurak 5:f16727052990 286 }
JerrySzczurak 5:f16727052990 287 }
JerrySzczurak 5:f16727052990 288 return status;
JerrySzczurak 5:f16727052990 289 }
JerrySzczurak 5:f16727052990 290
JerrySzczurak 5:f16727052990 291 VL53L1X_ERROR VL53L1X::VL53L1X_GetTimingBudgetInMs(uint16_t *pTimingBudget)
JerrySzczurak 5:f16727052990 292 {
JerrySzczurak 5:f16727052990 293 uint16_t Temp;
JerrySzczurak 5:f16727052990 294 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 295
JerrySzczurak 5:f16727052990 296 status = VL53L1_RdWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp);
JerrySzczurak 5:f16727052990 297 switch (Temp) {
JerrySzczurak 5:f16727052990 298 case 0x001D :
JerrySzczurak 5:f16727052990 299 *pTimingBudget = 15;
JerrySzczurak 5:f16727052990 300 break;
JerrySzczurak 5:f16727052990 301 case 0x0051 :
JerrySzczurak 5:f16727052990 302 case 0x001E :
JerrySzczurak 5:f16727052990 303 *pTimingBudget = 20;
JerrySzczurak 5:f16727052990 304 break;
JerrySzczurak 5:f16727052990 305 case 0x00D6 :
JerrySzczurak 5:f16727052990 306 case 0x0060 :
JerrySzczurak 5:f16727052990 307 *pTimingBudget = 33;
JerrySzczurak 5:f16727052990 308 break;
JerrySzczurak 5:f16727052990 309 case 0x1AE :
JerrySzczurak 5:f16727052990 310 case 0x00AD :
JerrySzczurak 5:f16727052990 311 *pTimingBudget = 50;
JerrySzczurak 5:f16727052990 312 break;
JerrySzczurak 5:f16727052990 313 case 0x02E1 :
JerrySzczurak 5:f16727052990 314 case 0x01CC :
JerrySzczurak 5:f16727052990 315 *pTimingBudget = 100;
JerrySzczurak 5:f16727052990 316 break;
JerrySzczurak 5:f16727052990 317 case 0x03E1 :
JerrySzczurak 5:f16727052990 318 case 0x02D9 :
JerrySzczurak 5:f16727052990 319 *pTimingBudget = 200;
JerrySzczurak 5:f16727052990 320 break;
JerrySzczurak 5:f16727052990 321 case 0x0591 :
JerrySzczurak 5:f16727052990 322 case 0x048F :
JerrySzczurak 5:f16727052990 323 *pTimingBudget = 500;
JerrySzczurak 5:f16727052990 324 break;
JerrySzczurak 5:f16727052990 325 default:
JerrySzczurak 5:f16727052990 326 *pTimingBudget = 0;
JerrySzczurak 5:f16727052990 327 break;
JerrySzczurak 5:f16727052990 328 }
JerrySzczurak 5:f16727052990 329 return status;
JerrySzczurak 5:f16727052990 330 }
JerrySzczurak 5:f16727052990 331
JerrySzczurak 5:f16727052990 332
JerrySzczurak 5:f16727052990 333 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceMode(uint16_t DM)
JerrySzczurak 5:f16727052990 334 {
JerrySzczurak 5:f16727052990 335 uint16_t TB;
JerrySzczurak 5:f16727052990 336 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 337
JerrySzczurak 5:f16727052990 338 status = VL53L1X_GetTimingBudgetInMs(&TB);
JerrySzczurak 5:f16727052990 339
JerrySzczurak 5:f16727052990 340
JerrySzczurak 5:f16727052990 341 switch (DM) {
JerrySzczurak 5:f16727052990 342 case 1:
JerrySzczurak 5:f16727052990 343 status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
JerrySzczurak 5:f16727052990 344 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
JerrySzczurak 5:f16727052990 345 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
JerrySzczurak 5:f16727052990 346 status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
JerrySzczurak 5:f16727052990 347 status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0705);
JerrySzczurak 5:f16727052990 348 status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
JerrySzczurak 5:f16727052990 349 break;
JerrySzczurak 5:f16727052990 350 case 2:
JerrySzczurak 5:f16727052990 351 status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
JerrySzczurak 5:f16727052990 352 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
JerrySzczurak 5:f16727052990 353 status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
JerrySzczurak 5:f16727052990 354 status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
JerrySzczurak 5:f16727052990 355 status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0F0D);
JerrySzczurak 5:f16727052990 356 status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
JerrySzczurak 5:f16727052990 357 break;
JerrySzczurak 5:f16727052990 358 default:
JerrySzczurak 5:f16727052990 359 break;
JerrySzczurak 5:f16727052990 360 }
JerrySzczurak 5:f16727052990 361 status = VL53L1X_SetTimingBudgetInMs(TB);
JerrySzczurak 5:f16727052990 362 return status;
JerrySzczurak 5:f16727052990 363 }
JerrySzczurak 5:f16727052990 364
JerrySzczurak 5:f16727052990 365
JerrySzczurak 5:f16727052990 366
JerrySzczurak 5:f16727052990 367
JerrySzczurak 5:f16727052990 368 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceMode(uint16_t *DM)
JerrySzczurak 5:f16727052990 369 {
JerrySzczurak 5:f16727052990 370 uint8_t TempDM, status=0;
JerrySzczurak 5:f16727052990 371
JerrySzczurak 5:f16727052990 372 status = VL53L1_RdByte(Device,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
JerrySzczurak 5:f16727052990 373 if (TempDM == 0x14)
JerrySzczurak 5:f16727052990 374 *DM=1;
JerrySzczurak 5:f16727052990 375 if(TempDM == 0x0A)
JerrySzczurak 5:f16727052990 376 *DM=2;
JerrySzczurak 5:f16727052990 377 return status;
JerrySzczurak 5:f16727052990 378 }
JerrySzczurak 5:f16727052990 379
JerrySzczurak 5:f16727052990 380
JerrySzczurak 5:f16727052990 381
JerrySzczurak 5:f16727052990 382 VL53L1X_ERROR VL53L1X::VL53L1X_SetInterMeasurementInMs(uint16_t InterMeasMs)
JerrySzczurak 5:f16727052990 383 {
JerrySzczurak 5:f16727052990 384 uint16_t ClockPLL;
JerrySzczurak 5:f16727052990 385 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 386
JerrySzczurak 5:f16727052990 387 status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
JerrySzczurak 5:f16727052990 388 ClockPLL = ClockPLL&0x3FF;
JerrySzczurak 5:f16727052990 389 VL53L1_WrDWord(Device, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
JerrySzczurak 5:f16727052990 390 (uint32_t)(ClockPLL * InterMeasMs * 1.075));
JerrySzczurak 5:f16727052990 391 return status;
JerrySzczurak 5:f16727052990 392
JerrySzczurak 5:f16727052990 393 }
JerrySzczurak 5:f16727052990 394
JerrySzczurak 5:f16727052990 395
JerrySzczurak 5:f16727052990 396 VL53L1X_ERROR VL53L1X::VL53L1X_GetInterMeasurementInMs(uint16_t *pIM)
JerrySzczurak 5:f16727052990 397 {
JerrySzczurak 5:f16727052990 398 uint16_t ClockPLL;
JerrySzczurak 5:f16727052990 399 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 400 uint32_t tmp;
JerrySzczurak 5:f16727052990 401
JerrySzczurak 5:f16727052990 402 status = VL53L1_RdDWord(Device,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
JerrySzczurak 5:f16727052990 403 *pIM = (uint16_t)tmp;
JerrySzczurak 5:f16727052990 404 status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
JerrySzczurak 5:f16727052990 405 ClockPLL = ClockPLL&0x3FF;
JerrySzczurak 5:f16727052990 406 *pIM= (uint16_t)(*pIM/(ClockPLL*1.065));
JerrySzczurak 5:f16727052990 407 return status;
JerrySzczurak 5:f16727052990 408 }
JerrySzczurak 5:f16727052990 409
JerrySzczurak 5:f16727052990 410
JerrySzczurak 5:f16727052990 411 VL53L1X_ERROR VL53L1X::VL53L1X_BootState(uint8_t *state)
JerrySzczurak 5:f16727052990 412 {
JerrySzczurak 5:f16727052990 413 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 414 uint8_t tmp = 0;
JerrySzczurak 5:f16727052990 415
JerrySzczurak 5:f16727052990 416 status = VL53L1_RdByte(Device,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
JerrySzczurak 5:f16727052990 417 *state = tmp;
JerrySzczurak 5:f16727052990 418 return status;
JerrySzczurak 5:f16727052990 419 }
JerrySzczurak 5:f16727052990 420
JerrySzczurak 5:f16727052990 421
JerrySzczurak 5:f16727052990 422 VL53L1X_ERROR VL53L1X::VL53L1X_GetSensorId(uint16_t *sensorId)
JerrySzczurak 5:f16727052990 423 {
JerrySzczurak 5:f16727052990 424 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 425 uint16_t tmp = 0;
JerrySzczurak 5:f16727052990 426
JerrySzczurak 5:f16727052990 427 status = VL53L1_RdWord(Device, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
JerrySzczurak 5:f16727052990 428 *sensorId = tmp;
JerrySzczurak 5:f16727052990 429 return status;
JerrySzczurak 5:f16727052990 430 }
JerrySzczurak 5:f16727052990 431
JerrySzczurak 5:f16727052990 432
JerrySzczurak 5:f16727052990 433 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistance(uint16_t *distance)
JerrySzczurak 5:f16727052990 434 {
JerrySzczurak 5:f16727052990 435 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 436 uint16_t tmp;
JerrySzczurak 5:f16727052990 437
JerrySzczurak 5:f16727052990 438 status = (VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 439 VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
JerrySzczurak 5:f16727052990 440 *distance = tmp;
JerrySzczurak 5:f16727052990 441 return status;
JerrySzczurak 5:f16727052990 442 }
JerrySzczurak 5:f16727052990 443
JerrySzczurak 5:f16727052990 444 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalPerSpad(uint16_t *signalRate)
JerrySzczurak 5:f16727052990 445 {
JerrySzczurak 5:f16727052990 446 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 447 uint16_t SpNb=1, signal;
JerrySzczurak 5:f16727052990 448
JerrySzczurak 5:f16727052990 449 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 450 VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
JerrySzczurak 5:f16727052990 451 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 452 VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
JerrySzczurak 5:f16727052990 453 *signalRate = (uint16_t) (2000.0*signal/SpNb);
JerrySzczurak 5:f16727052990 454 return status;
JerrySzczurak 5:f16727052990 455 }
JerrySzczurak 5:f16727052990 456
JerrySzczurak 5:f16727052990 457
JerrySzczurak 5:f16727052990 458 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientPerSpad(uint16_t *ambPerSp)
JerrySzczurak 5:f16727052990 459 {
JerrySzczurak 5:f16727052990 460 VL53L1X_ERROR status=0;
JerrySzczurak 5:f16727052990 461 uint16_t AmbientRate, SpNb=1;
JerrySzczurak 5:f16727052990 462
JerrySzczurak 5:f16727052990 463 status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
JerrySzczurak 5:f16727052990 464 status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
JerrySzczurak 5:f16727052990 465 *ambPerSp=(uint16_t) (2000.0 * AmbientRate / SpNb);
JerrySzczurak 5:f16727052990 466 return status;
JerrySzczurak 5:f16727052990 467 }
JerrySzczurak 5:f16727052990 468
JerrySzczurak 5:f16727052990 469
JerrySzczurak 5:f16727052990 470 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalRate(uint16_t *signal)
JerrySzczurak 5:f16727052990 471 {
JerrySzczurak 5:f16727052990 472 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 473 uint16_t tmp;
JerrySzczurak 5:f16727052990 474
JerrySzczurak 5:f16727052990 475 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 476 VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
JerrySzczurak 5:f16727052990 477 *signal = tmp*8;
JerrySzczurak 5:f16727052990 478 return status;
JerrySzczurak 5:f16727052990 479 }
JerrySzczurak 5:f16727052990 480
JerrySzczurak 5:f16727052990 481
JerrySzczurak 5:f16727052990 482 VL53L1X_ERROR VL53L1X::VL53L1X_GetSpadNb(uint16_t *spNb)
JerrySzczurak 5:f16727052990 483 {
JerrySzczurak 5:f16727052990 484 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 485 uint16_t tmp;
JerrySzczurak 5:f16727052990 486
JerrySzczurak 5:f16727052990 487 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 488 VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
JerrySzczurak 5:f16727052990 489 *spNb = tmp >> 8;
JerrySzczurak 5:f16727052990 490 return status;
JerrySzczurak 5:f16727052990 491 }
JerrySzczurak 5:f16727052990 492
JerrySzczurak 5:f16727052990 493
JerrySzczurak 5:f16727052990 494 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientRate(uint16_t *ambRate)
JerrySzczurak 5:f16727052990 495 {
JerrySzczurak 5:f16727052990 496 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 497 uint16_t tmp;
JerrySzczurak 5:f16727052990 498
JerrySzczurak 5:f16727052990 499 status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
JerrySzczurak 5:f16727052990 500 *ambRate = tmp*8;
JerrySzczurak 5:f16727052990 501 return status;
JerrySzczurak 5:f16727052990 502 }
JerrySzczurak 5:f16727052990 503
JerrySzczurak 5:f16727052990 504
JerrySzczurak 5:f16727052990 505 VL53L1X_ERROR VL53L1X::VL53L1X_GetRangeStatus(uint8_t *rangeStatus)
JerrySzczurak 5:f16727052990 506 {
JerrySzczurak 5:f16727052990 507 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 508 uint8_t RgSt;
JerrySzczurak 5:f16727052990 509
JerrySzczurak 5:f16727052990 510 status = VL53L1_RdByte(Device, VL53L1_RESULT__RANGE_STATUS, &RgSt);
JerrySzczurak 5:f16727052990 511 RgSt = RgSt&0x1F;
JerrySzczurak 5:f16727052990 512 switch (RgSt) {
JerrySzczurak 5:f16727052990 513 case 9:
JerrySzczurak 5:f16727052990 514 RgSt = 0;
JerrySzczurak 5:f16727052990 515 break;
JerrySzczurak 5:f16727052990 516 case 6:
JerrySzczurak 5:f16727052990 517 RgSt = 1;
JerrySzczurak 5:f16727052990 518 break;
JerrySzczurak 5:f16727052990 519 case 4:
JerrySzczurak 5:f16727052990 520 RgSt = 2;
JerrySzczurak 5:f16727052990 521 break;
JerrySzczurak 5:f16727052990 522 case 8:
JerrySzczurak 5:f16727052990 523 RgSt = 3;
JerrySzczurak 5:f16727052990 524 break;
JerrySzczurak 5:f16727052990 525 case 5:
JerrySzczurak 5:f16727052990 526 RgSt = 4;
JerrySzczurak 5:f16727052990 527 break;
JerrySzczurak 5:f16727052990 528 case 3:
JerrySzczurak 5:f16727052990 529 RgSt = 5;
JerrySzczurak 5:f16727052990 530 break;
JerrySzczurak 5:f16727052990 531 case 19:
JerrySzczurak 5:f16727052990 532 RgSt = 6;
JerrySzczurak 5:f16727052990 533 break;
JerrySzczurak 5:f16727052990 534 case 7:
JerrySzczurak 5:f16727052990 535 RgSt = 7;
JerrySzczurak 5:f16727052990 536 break;
JerrySzczurak 5:f16727052990 537 case 12:
JerrySzczurak 5:f16727052990 538 RgSt = 9;
JerrySzczurak 5:f16727052990 539 break;
JerrySzczurak 5:f16727052990 540 case 18:
JerrySzczurak 5:f16727052990 541 RgSt = 10;
JerrySzczurak 5:f16727052990 542 break;
JerrySzczurak 5:f16727052990 543 case 22:
JerrySzczurak 5:f16727052990 544 RgSt = 11;
JerrySzczurak 5:f16727052990 545 break;
JerrySzczurak 5:f16727052990 546 case 23:
JerrySzczurak 5:f16727052990 547 RgSt = 12;
JerrySzczurak 5:f16727052990 548 break;
JerrySzczurak 5:f16727052990 549 case 13:
JerrySzczurak 5:f16727052990 550 RgSt = 13;
JerrySzczurak 5:f16727052990 551 break;
JerrySzczurak 5:f16727052990 552 default:
JerrySzczurak 5:f16727052990 553 RgSt = 255;
JerrySzczurak 5:f16727052990 554 break;
JerrySzczurak 5:f16727052990 555 }
JerrySzczurak 5:f16727052990 556 *rangeStatus = RgSt;
JerrySzczurak 5:f16727052990 557 return status;
JerrySzczurak 5:f16727052990 558 }
JerrySzczurak 5:f16727052990 559
JerrySzczurak 5:f16727052990 560
JerrySzczurak 5:f16727052990 561 VL53L1X_ERROR VL53L1X::VL53L1X_SetOffset(int16_t OffsetValue)
JerrySzczurak 5:f16727052990 562 {
JerrySzczurak 5:f16727052990 563 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 564 int16_t Temp;
JerrySzczurak 5:f16727052990 565
JerrySzczurak 5:f16727052990 566 Temp = (OffsetValue*4);
JerrySzczurak 5:f16727052990 567 VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
JerrySzczurak 5:f16727052990 568 (uint16_t)Temp);
JerrySzczurak 5:f16727052990 569 VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 570 VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 571 return status;
JerrySzczurak 5:f16727052990 572 }
JerrySzczurak 5:f16727052990 573
JerrySzczurak 5:f16727052990 574
JerrySzczurak 5:f16727052990 575 VL53L1X_ERROR VL53L1X::VL53L1X_GetOffset(int16_t *offset)
JerrySzczurak 5:f16727052990 576 {
JerrySzczurak 5:f16727052990 577 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 578 uint16_t Temp;
JerrySzczurak 5:f16727052990 579
JerrySzczurak 5:f16727052990 580 status = VL53L1_RdWord(Device,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
JerrySzczurak 5:f16727052990 581 Temp = Temp<<3;
JerrySzczurak 5:f16727052990 582 Temp = Temp >>5;
JerrySzczurak 5:f16727052990 583 *offset = (int16_t)(Temp);
JerrySzczurak 5:f16727052990 584 return status;
JerrySzczurak 5:f16727052990 585 }
JerrySzczurak 5:f16727052990 586
JerrySzczurak 5:f16727052990 587 VL53L1X_ERROR VL53L1X::VL53L1X_SetXtalk(uint16_t XtalkValue)
JerrySzczurak 5:f16727052990 588 {
dmathew 6:aa13392d16bb 589 /* XTalkValue in count per second to avoid float type */
JerrySzczurak 5:f16727052990 590 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 591
JerrySzczurak 5:f16727052990 592 status = VL53L1_WrWord(Device,
JerrySzczurak 5:f16727052990 593 ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
JerrySzczurak 5:f16727052990 594 0x0000);
JerrySzczurak 5:f16727052990 595 status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
JerrySzczurak 5:f16727052990 596 0x0000);
JerrySzczurak 5:f16727052990 597 status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
JerrySzczurak 5:f16727052990 598 (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
JerrySzczurak 5:f16727052990 599 return status;
JerrySzczurak 5:f16727052990 600 }
JerrySzczurak 5:f16727052990 601
JerrySzczurak 5:f16727052990 602
JerrySzczurak 5:f16727052990 603 VL53L1X_ERROR VL53L1X::VL53L1X_GetXtalk(uint16_t *xtalk )
JerrySzczurak 5:f16727052990 604 {
JerrySzczurak 5:f16727052990 605 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 606 uint16_t tmp;
JerrySzczurak 5:f16727052990 607
JerrySzczurak 5:f16727052990 608 status = VL53L1_RdWord(Device,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, &tmp);
JerrySzczurak 5:f16727052990 609 *xtalk = (tmp*1000)>>9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
JerrySzczurak 5:f16727052990 610 return status;
JerrySzczurak 5:f16727052990 611 }
JerrySzczurak 5:f16727052990 612
JerrySzczurak 5:f16727052990 613
JerrySzczurak 5:f16727052990 614 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceThreshold(uint16_t ThreshLow,
JerrySzczurak 5:f16727052990 615 uint16_t ThreshHigh, uint8_t Window,
JerrySzczurak 5:f16727052990 616 uint8_t IntOnNoTarget)
JerrySzczurak 5:f16727052990 617 {
JerrySzczurak 5:f16727052990 618 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 619 uint8_t Temp = 0;
JerrySzczurak 5:f16727052990 620
JerrySzczurak 5:f16727052990 621 status = VL53L1_RdByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
JerrySzczurak 5:f16727052990 622 Temp = Temp & 0x47;
JerrySzczurak 5:f16727052990 623 if (IntOnNoTarget == 0) {
JerrySzczurak 5:f16727052990 624 status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
JerrySzczurak 5:f16727052990 625 (Temp | (Window & 0x07)));
JerrySzczurak 5:f16727052990 626 } else {
JerrySzczurak 5:f16727052990 627 status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
JerrySzczurak 5:f16727052990 628 ((Temp | (Window & 0x07)) | 0x40));
JerrySzczurak 5:f16727052990 629 }
JerrySzczurak 5:f16727052990 630 status = VL53L1_WrWord(Device, SYSTEM__THRESH_HIGH, ThreshHigh);
JerrySzczurak 5:f16727052990 631 status = VL53L1_WrWord(Device, SYSTEM__THRESH_LOW, ThreshLow);
JerrySzczurak 5:f16727052990 632 return status;
JerrySzczurak 5:f16727052990 633 }
JerrySzczurak 5:f16727052990 634
JerrySzczurak 5:f16727052990 635
JerrySzczurak 5:f16727052990 636 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdWindow(uint16_t *window)
JerrySzczurak 5:f16727052990 637 {
JerrySzczurak 5:f16727052990 638 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 639 uint8_t tmp;
JerrySzczurak 5:f16727052990 640 status = VL53L1_RdByte(Device,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
JerrySzczurak 5:f16727052990 641 *window = (uint16_t)(tmp & 0x7);
JerrySzczurak 5:f16727052990 642 return status;
JerrySzczurak 5:f16727052990 643 }
JerrySzczurak 5:f16727052990 644
JerrySzczurak 5:f16727052990 645
JerrySzczurak 5:f16727052990 646 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdLow(uint16_t *low)
JerrySzczurak 5:f16727052990 647 {
JerrySzczurak 5:f16727052990 648 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 649 uint16_t tmp;
JerrySzczurak 5:f16727052990 650
JerrySzczurak 5:f16727052990 651 status = VL53L1_RdWord(Device,SYSTEM__THRESH_LOW, &tmp);
JerrySzczurak 5:f16727052990 652 *low = tmp;
JerrySzczurak 5:f16727052990 653 return status;
JerrySzczurak 5:f16727052990 654 }
JerrySzczurak 5:f16727052990 655
JerrySzczurak 5:f16727052990 656 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdHigh(uint16_t *high)
JerrySzczurak 5:f16727052990 657 {
JerrySzczurak 5:f16727052990 658 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 659 uint16_t tmp;
JerrySzczurak 5:f16727052990 660
JerrySzczurak 5:f16727052990 661 status = VL53L1_RdWord(Device,SYSTEM__THRESH_HIGH, &tmp);
JerrySzczurak 5:f16727052990 662 *high = tmp;
JerrySzczurak 5:f16727052990 663 return status;
JerrySzczurak 5:f16727052990 664 }
JerrySzczurak 5:f16727052990 665
JerrySzczurak 5:f16727052990 666 VL53L1X_ERROR VL53L1X::VL53L1X_SetROI(uint16_t X, uint16_t Y)
JerrySzczurak 5:f16727052990 667 {
JerrySzczurak 5:f16727052990 668 uint8_t OpticalCenter;
JerrySzczurak 5:f16727052990 669 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 670
JerrySzczurak 5:f16727052990 671 status =VL53L1_RdByte(Device, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter);
JerrySzczurak 5:f16727052990 672 if (X > 16)
JerrySzczurak 5:f16727052990 673 X = 16;
JerrySzczurak 5:f16727052990 674 if (Y > 16)
JerrySzczurak 5:f16727052990 675 Y = 16;
JerrySzczurak 5:f16727052990 676 if (X > 10 || Y > 10){
JerrySzczurak 5:f16727052990 677 OpticalCenter = 199;
JerrySzczurak 5:f16727052990 678 }
JerrySzczurak 5:f16727052990 679 status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
JerrySzczurak 5:f16727052990 680 status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
JerrySzczurak 5:f16727052990 681 (Y - 1) << 4 | (X - 1));
JerrySzczurak 5:f16727052990 682 return status;
JerrySzczurak 5:f16727052990 683 }
JerrySzczurak 5:f16727052990 684
JerrySzczurak 5:f16727052990 685 VL53L1X_ERROR VL53L1X::VL53L1X_GetROI_XY(uint16_t *ROI_X, uint16_t *ROI_Y)
JerrySzczurak 5:f16727052990 686 {
JerrySzczurak 5:f16727052990 687 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 688 uint8_t tmp;
JerrySzczurak 5:f16727052990 689
JerrySzczurak 5:f16727052990 690 status = VL53L1_RdByte(Device,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp);
JerrySzczurak 5:f16727052990 691 *ROI_X = ((uint16_t)tmp & 0x0F) + 1;
JerrySzczurak 5:f16727052990 692 *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1;
JerrySzczurak 5:f16727052990 693 return status;
JerrySzczurak 5:f16727052990 694 }
JerrySzczurak 5:f16727052990 695
JerrySzczurak 5:f16727052990 696 VL53L1X_ERROR VL53L1X::VL53L1X_SetSignalThreshold(uint16_t Signal)
JerrySzczurak 5:f16727052990 697 {
JerrySzczurak 5:f16727052990 698 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 699
JerrySzczurak 5:f16727052990 700 VL53L1_WrWord(Device,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3);
JerrySzczurak 5:f16727052990 701 return status;
JerrySzczurak 5:f16727052990 702 }
JerrySzczurak 5:f16727052990 703
JerrySzczurak 5:f16727052990 704 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalThreshold(uint16_t *signal)
JerrySzczurak 5:f16727052990 705 {
JerrySzczurak 5:f16727052990 706 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 707 uint16_t tmp;
JerrySzczurak 5:f16727052990 708
JerrySzczurak 5:f16727052990 709 status = VL53L1_RdWord(Device,
JerrySzczurak 5:f16727052990 710 RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
JerrySzczurak 5:f16727052990 711 *signal = tmp <<3;
JerrySzczurak 5:f16727052990 712 return status;
JerrySzczurak 5:f16727052990 713 }
JerrySzczurak 5:f16727052990 714
JerrySzczurak 5:f16727052990 715
JerrySzczurak 5:f16727052990 716 VL53L1X_ERROR VL53L1X::VL53L1X_SetSigmaThreshold(uint16_t Sigma)
JerrySzczurak 5:f16727052990 717 {
JerrySzczurak 5:f16727052990 718 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 719
JerrySzczurak 5:f16727052990 720 if(Sigma>(0xFFFF>>2)){
JerrySzczurak 5:f16727052990 721 return 1;
JerrySzczurak 5:f16727052990 722 }
JerrySzczurak 5:f16727052990 723 /* 16 bits register 14.2 format */
JerrySzczurak 5:f16727052990 724 status = VL53L1_WrWord(Device,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2);
JerrySzczurak 5:f16727052990 725 return status;
JerrySzczurak 5:f16727052990 726 }
JerrySzczurak 5:f16727052990 727
JerrySzczurak 5:f16727052990 728 VL53L1X_ERROR VL53L1X::VL53L1X_GetSigmaThreshold(uint16_t *sigma)
JerrySzczurak 5:f16727052990 729 {
JerrySzczurak 5:f16727052990 730 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 731 uint16_t tmp;
JerrySzczurak 5:f16727052990 732
JerrySzczurak 5:f16727052990 733 status = VL53L1_RdWord(Device,RANGE_CONFIG__SIGMA_THRESH, &tmp);
JerrySzczurak 5:f16727052990 734 *sigma = tmp >> 2;
JerrySzczurak 5:f16727052990 735 return status;
JerrySzczurak 5:f16727052990 736
JerrySzczurak 5:f16727052990 737 }
JerrySzczurak 5:f16727052990 738
JerrySzczurak 5:f16727052990 739 VL53L1X_ERROR VL53L1X::VL53L1X_StartTemperatureUpdate()
JerrySzczurak 5:f16727052990 740 {
JerrySzczurak 5:f16727052990 741 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 742 uint8_t tmp=0;
JerrySzczurak 5:f16727052990 743
JerrySzczurak 5:f16727052990 744 status = VL53L1_WrByte(Device,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */
JerrySzczurak 5:f16727052990 745 status = VL53L1_WrByte(Device,0x0B,0x92);
JerrySzczurak 5:f16727052990 746 status = VL53L1X_StartRanging();
JerrySzczurak 5:f16727052990 747 while(tmp==0){
JerrySzczurak 5:f16727052990 748 status = VL53L1X_CheckForDataReady(&tmp);
JerrySzczurak 5:f16727052990 749 }
JerrySzczurak 5:f16727052990 750 tmp = 0;
JerrySzczurak 5:f16727052990 751 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 752 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 753 status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
JerrySzczurak 5:f16727052990 754 status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */
JerrySzczurak 5:f16727052990 755 return status;
JerrySzczurak 5:f16727052990 756 }
JerrySzczurak 5:f16727052990 757
JerrySzczurak 5:f16727052990 758 /* VL53L1X_calibration.h functions */
JerrySzczurak 5:f16727052990 759
JerrySzczurak 5:f16727052990 760 int8_t VL53L1X::VL53L1X_CalibrateOffset(uint16_t TargetDistInMm, int16_t *offset)
JerrySzczurak 5:f16727052990 761 {
JerrySzczurak 5:f16727052990 762 uint8_t i = 0, tmp;
JerrySzczurak 5:f16727052990 763 int16_t AverageDistance = 0;
JerrySzczurak 5:f16727052990 764 uint16_t distance;
JerrySzczurak 5:f16727052990 765 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 766
JerrySzczurak 5:f16727052990 767 status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 768 status = VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 769 status = VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
JerrySzczurak 5:f16727052990 770 status = VL53L1X_StartRanging(); /* Enable VL53L1X sensor */
JerrySzczurak 5:f16727052990 771 for (i = 0; i < 50; i++) {
JerrySzczurak 5:f16727052990 772 while (tmp == 0){
JerrySzczurak 5:f16727052990 773 status = VL53L1X_CheckForDataReady(&tmp);
JerrySzczurak 5:f16727052990 774 }
JerrySzczurak 5:f16727052990 775 tmp = 0;
JerrySzczurak 5:f16727052990 776 status = VL53L1X_GetDistance(&distance);
JerrySzczurak 5:f16727052990 777 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 778 AverageDistance = AverageDistance + distance;
JerrySzczurak 5:f16727052990 779 }
JerrySzczurak 5:f16727052990 780 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 781 AverageDistance = AverageDistance / 50;
JerrySzczurak 5:f16727052990 782 *offset = TargetDistInMm - AverageDistance;
JerrySzczurak 5:f16727052990 783 status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4);
JerrySzczurak 5:f16727052990 784 return status;
JerrySzczurak 5:f16727052990 785 }
JerrySzczurak 5:f16727052990 786
JerrySzczurak 5:f16727052990 787
JerrySzczurak 5:f16727052990 788 int8_t VL53L1X::VL53L1X_CalibrateXtalk(uint16_t TargetDistInMm, uint16_t *xtalk)
JerrySzczurak 5:f16727052990 789 {
JerrySzczurak 5:f16727052990 790 uint8_t i, tmp= 0;
JerrySzczurak 5:f16727052990 791 float AverageSignalRate = 0;
JerrySzczurak 5:f16727052990 792 float AverageDistance = 0;
JerrySzczurak 5:f16727052990 793 float AverageSpadNb = 0;
JerrySzczurak 5:f16727052990 794 uint16_t distance = 0, spadNum;
JerrySzczurak 5:f16727052990 795 uint16_t sr;
JerrySzczurak 5:f16727052990 796 VL53L1X_ERROR status = 0;
JerrySzczurak 5:f16727052990 797
JerrySzczurak 5:f16727052990 798 status = VL53L1_WrWord(Device, 0x0016,0);
JerrySzczurak 5:f16727052990 799 status = VL53L1X_StartRanging();
JerrySzczurak 5:f16727052990 800 for (i = 0; i < 50; i++) {
JerrySzczurak 5:f16727052990 801 while (tmp == 0){
JerrySzczurak 5:f16727052990 802 status = VL53L1X_CheckForDataReady(&tmp);
JerrySzczurak 5:f16727052990 803 }
JerrySzczurak 5:f16727052990 804 tmp=0;
JerrySzczurak 5:f16727052990 805 status= VL53L1X_GetSignalRate(&sr);
JerrySzczurak 5:f16727052990 806 status= VL53L1X_GetDistance(&distance);
JerrySzczurak 5:f16727052990 807 status = VL53L1X_ClearInterrupt();
JerrySzczurak 5:f16727052990 808 AverageDistance = AverageDistance + distance;
JerrySzczurak 5:f16727052990 809 status = VL53L1X_GetSpadNb(&spadNum);
JerrySzczurak 5:f16727052990 810 AverageSpadNb = AverageSpadNb + spadNum;
JerrySzczurak 5:f16727052990 811 AverageSignalRate =
JerrySzczurak 5:f16727052990 812 AverageSignalRate + sr;
JerrySzczurak 5:f16727052990 813 }
JerrySzczurak 5:f16727052990 814 status = VL53L1X_StopRanging();
JerrySzczurak 5:f16727052990 815 AverageDistance = AverageDistance / 50;
JerrySzczurak 5:f16727052990 816 AverageSpadNb = AverageSpadNb / 50;
JerrySzczurak 5:f16727052990 817 AverageSignalRate = AverageSignalRate / 50;
JerrySzczurak 5:f16727052990 818 /* Calculate Xtalk value */
JerrySzczurak 5:f16727052990 819 *xtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb);
JerrySzczurak 5:f16727052990 820 status = VL53L1_WrWord(Device, 0x0016, *xtalk);
JerrySzczurak 5:f16727052990 821 return status;
JerrySzczurak 5:f16727052990 822 }
JerrySzczurak 5:f16727052990 823
JerrySzczurak 5:f16727052990 824
JerrySzczurak 5:f16727052990 825
JerrySzczurak 5:f16727052990 826
JerrySzczurak 5:f16727052990 827 /* Write and read functions from I2C */
JerrySzczurak 5:f16727052990 828
JerrySzczurak 5:f16727052990 829
JerrySzczurak 5:f16727052990 830 VL53L1X_ERROR VL53L1X::VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
JerrySzczurak 5:f16727052990 831 {
JerrySzczurak 5:f16727052990 832 int status;
JerrySzczurak 5:f16727052990 833
JerrySzczurak 5:f16727052990 834 status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
JerrySzczurak 5:f16727052990 835 return status;
JerrySzczurak 5:f16727052990 836 }
JerrySzczurak 5:f16727052990 837
JerrySzczurak 5:f16727052990 838 VL53L1X_ERROR VL53L1X::VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
JerrySzczurak 5:f16727052990 839 {
JerrySzczurak 5:f16727052990 840 int status;
JerrySzczurak 5:f16727052990 841
JerrySzczurak 5:f16727052990 842 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
JerrySzczurak 5:f16727052990 843
JerrySzczurak 5:f16727052990 844 return status;
JerrySzczurak 5:f16727052990 845 }
JerrySzczurak 5:f16727052990 846
JerrySzczurak 5:f16727052990 847
JerrySzczurak 5:f16727052990 848 VL53L1X_ERROR VL53L1X::VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
JerrySzczurak 5:f16727052990 849 {
JerrySzczurak 5:f16727052990 850 int status;
JerrySzczurak 5:f16727052990 851
JerrySzczurak 5:f16727052990 852 status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, &data, 1);
JerrySzczurak 5:f16727052990 853 return status;
JerrySzczurak 5:f16727052990 854 }
JerrySzczurak 5:f16727052990 855
JerrySzczurak 5:f16727052990 856 VL53L1X_ERROR VL53L1X::VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data)
JerrySzczurak 5:f16727052990 857 {
JerrySzczurak 5:f16727052990 858 int status;
JerrySzczurak 5:f16727052990 859 uint8_t buffer[2];
JerrySzczurak 5:f16727052990 860
JerrySzczurak 5:f16727052990 861 buffer[0] = data >> 8;
JerrySzczurak 5:f16727052990 862 buffer[1] = data & 0x00FF;
JerrySzczurak 5:f16727052990 863 status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
JerrySzczurak 5:f16727052990 864 return status;
JerrySzczurak 5:f16727052990 865 }
JerrySzczurak 5:f16727052990 866
JerrySzczurak 5:f16727052990 867 VL53L1X_ERROR VL53L1X::VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data)
JerrySzczurak 5:f16727052990 868 {
JerrySzczurak 5:f16727052990 869 int status;
JerrySzczurak 5:f16727052990 870 uint8_t buffer[4];
JerrySzczurak 5:f16727052990 871
JerrySzczurak 5:f16727052990 872 buffer[0] = (data >> 24) & 0xFF;
JerrySzczurak 5:f16727052990 873 buffer[1] = (data >> 16) & 0xFF;
JerrySzczurak 5:f16727052990 874 buffer[2] = (data >> 8) & 0xFF;
JerrySzczurak 5:f16727052990 875 buffer[3] = (data >> 0) & 0xFF;
JerrySzczurak 5:f16727052990 876 status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
JerrySzczurak 5:f16727052990 877 return status;
JerrySzczurak 5:f16727052990 878 }
JerrySzczurak 5:f16727052990 879
JerrySzczurak 5:f16727052990 880
JerrySzczurak 5:f16727052990 881 VL53L1X_ERROR VL53L1X::VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
JerrySzczurak 5:f16727052990 882 {
JerrySzczurak 5:f16727052990 883 int status;
JerrySzczurak 5:f16727052990 884
JerrySzczurak 5:f16727052990 885 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, data, 1);
JerrySzczurak 5:f16727052990 886
JerrySzczurak 5:f16727052990 887 if(status)
JerrySzczurak 5:f16727052990 888 return -1;
JerrySzczurak 5:f16727052990 889
JerrySzczurak 5:f16727052990 890 return 0;
JerrySzczurak 5:f16727052990 891 }
JerrySzczurak 5:f16727052990 892
JerrySzczurak 5:f16727052990 893 VL53L1X_ERROR VL53L1X::VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
JerrySzczurak 5:f16727052990 894 {
JerrySzczurak 5:f16727052990 895 int status;
JerrySzczurak 5:f16727052990 896 uint8_t buffer[2] = {0,0};
JerrySzczurak 5:f16727052990 897
JerrySzczurak 5:f16727052990 898 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 2);
JerrySzczurak 5:f16727052990 899 if (!status)
JerrySzczurak 5:f16727052990 900 {
JerrySzczurak 5:f16727052990 901 *data = (buffer[0] << 8) + buffer[1];
JerrySzczurak 5:f16727052990 902 }
JerrySzczurak 5:f16727052990 903 return status;
JerrySzczurak 5:f16727052990 904
JerrySzczurak 5:f16727052990 905 }
JerrySzczurak 5:f16727052990 906
JerrySzczurak 5:f16727052990 907 VL53L1X_ERROR VL53L1X::VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
JerrySzczurak 5:f16727052990 908 {
JerrySzczurak 5:f16727052990 909 int status;
JerrySzczurak 5:f16727052990 910 uint8_t buffer[4] = {0,0,0,0};
JerrySzczurak 5:f16727052990 911
JerrySzczurak 5:f16727052990 912 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 4);
JerrySzczurak 5:f16727052990 913 if(!status)
JerrySzczurak 5:f16727052990 914 {
JerrySzczurak 5:f16727052990 915 *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
JerrySzczurak 5:f16727052990 916 }
JerrySzczurak 5:f16727052990 917 return status;
JerrySzczurak 5:f16727052990 918
JerrySzczurak 5:f16727052990 919 }
JerrySzczurak 5:f16727052990 920
JerrySzczurak 5:f16727052990 921 VL53L1X_ERROR VL53L1X::VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
JerrySzczurak 5:f16727052990 922 {
JerrySzczurak 5:f16727052990 923 int status;
JerrySzczurak 5:f16727052990 924 uint8_t buffer = 0;
JerrySzczurak 5:f16727052990 925
JerrySzczurak 5:f16727052990 926 /* read data direct onto buffer */
JerrySzczurak 5:f16727052990 927 status = VL53L1_I2CRead(Dev->I2cDevAddr, index, &buffer,1);
JerrySzczurak 5:f16727052990 928 if (!status)
JerrySzczurak 5:f16727052990 929 {
JerrySzczurak 5:f16727052990 930 buffer = (buffer & AndData) | OrData;
JerrySzczurak 5:f16727052990 931 status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint16_t)1);
JerrySzczurak 5:f16727052990 932 }
JerrySzczurak 5:f16727052990 933 return status;
JerrySzczurak 5:f16727052990 934 }
JerrySzczurak 5:f16727052990 935
JerrySzczurak 5:f16727052990 936 VL53L1X_ERROR VL53L1X::VL53L1_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
JerrySzczurak 5:f16727052990 937 {
JerrySzczurak 5:f16727052990 938 int ret;
JerrySzczurak 5:f16727052990 939 ret = dev_i2c->v53l1x_i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
JerrySzczurak 5:f16727052990 940 if (ret) {
JerrySzczurak 5:f16727052990 941 return -1;
JerrySzczurak 5:f16727052990 942 }
JerrySzczurak 5:f16727052990 943 return 0;
JerrySzczurak 5:f16727052990 944
JerrySzczurak 5:f16727052990 945 }
JerrySzczurak 5:f16727052990 946
JerrySzczurak 5:f16727052990 947 VL53L1X_ERROR VL53L1X::VL53L1_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
JerrySzczurak 5:f16727052990 948 {
JerrySzczurak 5:f16727052990 949 int ret;
JerrySzczurak 5:f16727052990 950
JerrySzczurak 5:f16727052990 951 ret = dev_i2c->v53l1x_i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
JerrySzczurak 5:f16727052990 952
JerrySzczurak 5:f16727052990 953 if (ret) {
JerrySzczurak 5:f16727052990 954 return -1;
JerrySzczurak 5:f16727052990 955 }
JerrySzczurak 5:f16727052990 956 return 0;
JerrySzczurak 5:f16727052990 957 }
JerrySzczurak 5:f16727052990 958
JerrySzczurak 5:f16727052990 959
JerrySzczurak 5:f16727052990 960 VL53L1X_ERROR VL53L1X::VL53L1_GetTickCount(
JerrySzczurak 5:f16727052990 961 uint32_t *ptick_count_ms)
JerrySzczurak 5:f16727052990 962 {
JerrySzczurak 5:f16727052990 963
JerrySzczurak 5:f16727052990 964 /* Returns current tick count in [ms] */
JerrySzczurak 5:f16727052990 965
JerrySzczurak 5:f16727052990 966 VL53L1X_ERROR status = VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 967
JerrySzczurak 5:f16727052990 968 *ptick_count_ms = 0;
JerrySzczurak 5:f16727052990 969
JerrySzczurak 5:f16727052990 970 return status;
JerrySzczurak 5:f16727052990 971 }
JerrySzczurak 5:f16727052990 972
JerrySzczurak 5:f16727052990 973
JerrySzczurak 5:f16727052990 974
JerrySzczurak 5:f16727052990 975 VL53L1X_ERROR VL53L1X::VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us)
JerrySzczurak 5:f16727052990 976 {
JerrySzczurak 5:f16727052990 977 return VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 978 }
JerrySzczurak 5:f16727052990 979
JerrySzczurak 5:f16727052990 980
JerrySzczurak 5:f16727052990 981 VL53L1X_ERROR VL53L1X::VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms)
JerrySzczurak 5:f16727052990 982 {
JerrySzczurak 5:f16727052990 983 return VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 984 }
JerrySzczurak 5:f16727052990 985
JerrySzczurak 5:f16727052990 986
JerrySzczurak 5:f16727052990 987 VL53L1X_ERROR VL53L1X::VL53L1_WaitValueMaskEx(
JerrySzczurak 5:f16727052990 988 VL53L1_Dev_t *pdev,
JerrySzczurak 5:f16727052990 989 uint32_t timeout_ms,
JerrySzczurak 5:f16727052990 990 uint16_t index,
JerrySzczurak 5:f16727052990 991 uint8_t value,
JerrySzczurak 5:f16727052990 992 uint8_t mask,
JerrySzczurak 5:f16727052990 993 uint32_t poll_delay_ms)
JerrySzczurak 5:f16727052990 994 {
JerrySzczurak 5:f16727052990 995
JerrySzczurak 5:f16727052990 996 /*
JerrySzczurak 5:f16727052990 997 * Platform implementation of WaitValueMaskEx V2WReg script command
JerrySzczurak 5:f16727052990 998 *
JerrySzczurak 5:f16727052990 999 * WaitValueMaskEx(
JerrySzczurak 5:f16727052990 1000 * duration_ms,
JerrySzczurak 5:f16727052990 1001 * index,
JerrySzczurak 5:f16727052990 1002 * value,
JerrySzczurak 5:f16727052990 1003 * mask,
JerrySzczurak 5:f16727052990 1004 * poll_delay_ms);
JerrySzczurak 5:f16727052990 1005 */
JerrySzczurak 5:f16727052990 1006
JerrySzczurak 5:f16727052990 1007 VL53L1_Error status = VL53L1_ERROR_NONE;
JerrySzczurak 5:f16727052990 1008 uint32_t start_time_ms = 0;
JerrySzczurak 5:f16727052990 1009 uint32_t current_time_ms = 0;
JerrySzczurak 5:f16727052990 1010 uint32_t polling_time_ms = 0;
JerrySzczurak 5:f16727052990 1011 uint8_t byte_value = 0;
JerrySzczurak 5:f16727052990 1012 uint8_t found = 0;
JerrySzczurak 5:f16727052990 1013
JerrySzczurak 5:f16727052990 1014
JerrySzczurak 5:f16727052990 1015
JerrySzczurak 5:f16727052990 1016 /* calculate time limit in absolute time */
JerrySzczurak 5:f16727052990 1017
JerrySzczurak 5:f16727052990 1018 VL53L1_GetTickCount(&start_time_ms);
JerrySzczurak 5:f16727052990 1019
JerrySzczurak 5:f16727052990 1020 /* remember current trace functions and temporarily disable
JerrySzczurak 5:f16727052990 1021 * function logging
JerrySzczurak 5:f16727052990 1022 */
JerrySzczurak 5:f16727052990 1023
JerrySzczurak 5:f16727052990 1024
JerrySzczurak 5:f16727052990 1025 /* wait until value is found, timeout reached on error occurred */
JerrySzczurak 5:f16727052990 1026
JerrySzczurak 5:f16727052990 1027 while ((status == VL53L1_ERROR_NONE) &&
JerrySzczurak 5:f16727052990 1028 (polling_time_ms < timeout_ms) &&
JerrySzczurak 5:f16727052990 1029 (found == 0)) {
JerrySzczurak 5:f16727052990 1030
JerrySzczurak 5:f16727052990 1031 if (status == VL53L1_ERROR_NONE)
JerrySzczurak 5:f16727052990 1032 status = VL53L1_RdByte(
JerrySzczurak 5:f16727052990 1033 pdev,
JerrySzczurak 5:f16727052990 1034 index,
JerrySzczurak 5:f16727052990 1035 &byte_value);
JerrySzczurak 5:f16727052990 1036
JerrySzczurak 5:f16727052990 1037 if ((byte_value & mask) == value)
JerrySzczurak 5:f16727052990 1038 found = 1;
JerrySzczurak 5:f16727052990 1039
JerrySzczurak 5:f16727052990 1040 if (status == VL53L1_ERROR_NONE &&
JerrySzczurak 5:f16727052990 1041 found == 0 &&
JerrySzczurak 5:f16727052990 1042 poll_delay_ms > 0)
JerrySzczurak 5:f16727052990 1043 status = VL53L1_WaitMs(
JerrySzczurak 5:f16727052990 1044 pdev,
JerrySzczurak 5:f16727052990 1045 poll_delay_ms);
JerrySzczurak 5:f16727052990 1046
JerrySzczurak 5:f16727052990 1047 /* Update polling time (Compare difference rather than absolute to
JerrySzczurak 5:f16727052990 1048 negate 32bit wrap around issue) */
JerrySzczurak 5:f16727052990 1049 VL53L1_GetTickCount(&current_time_ms);
JerrySzczurak 5:f16727052990 1050 polling_time_ms = current_time_ms - start_time_ms;
JerrySzczurak 5:f16727052990 1051
JerrySzczurak 5:f16727052990 1052 }
JerrySzczurak 5:f16727052990 1053
JerrySzczurak 5:f16727052990 1054
JerrySzczurak 5:f16727052990 1055 if (found == 0 && status == VL53L1_ERROR_NONE)
JerrySzczurak 5:f16727052990 1056 status = VL53L1_ERROR_TIME_OUT;
JerrySzczurak 5:f16727052990 1057
JerrySzczurak 5:f16727052990 1058 return status;
JerrySzczurak 5:f16727052990 1059 }
JerrySzczurak 5:f16727052990 1060
dmathew 6:aa13392d16bb 1061 int VL53L1X::handle_irq(uint16_t *distance)
dmathew 6:aa13392d16bb 1062 {
dmathew 6:aa13392d16bb 1063 int status;
dmathew 6:aa13392d16bb 1064 status = get_measurement(distance);
dmathew 6:aa13392d16bb 1065 enable_interrupt_measure_detection_irq();
dmathew 6:aa13392d16bb 1066 return status;
dmathew 6:aa13392d16bb 1067 }
JerrySzczurak 5:f16727052990 1068
dmathew 6:aa13392d16bb 1069 int VL53L1X::get_measurement(uint16_t *distance)
dmathew 6:aa13392d16bb 1070 {
dmathew 6:aa13392d16bb 1071 int status = 0;
JerrySzczurak 5:f16727052990 1072
dmathew 6:aa13392d16bb 1073 status = VL53L1X_GetDistance(distance);
dmathew 6:aa13392d16bb 1074 status = VL53L1X_ClearInterrupt();
dmathew 6:aa13392d16bb 1075
dmathew 6:aa13392d16bb 1076 return status;
dmathew 6:aa13392d16bb 1077 }
dmathew 6:aa13392d16bb 1078
dmathew 6:aa13392d16bb 1079 int VL53L1X::start_measurement(void (*fptr)(void))
dmathew 6:aa13392d16bb 1080 {
dmathew 6:aa13392d16bb 1081 int status = 0;
dmathew 6:aa13392d16bb 1082
dmathew 6:aa13392d16bb 1083 if (_gpio1Int == NULL) {
dmathew 6:aa13392d16bb 1084 printf("GPIO1 Error\r\n");
dmathew 6:aa13392d16bb 1085 return 1;
dmathew 6:aa13392d16bb 1086 }
dmathew 6:aa13392d16bb 1087
dmathew 6:aa13392d16bb 1088 status = VL53L1X_StopRanging(); // it is safer to do this while sensor is stopped
dmathew 6:aa13392d16bb 1089
dmathew 6:aa13392d16bb 1090 if (status == 0) {
dmathew 6:aa13392d16bb 1091 attach_interrupt_measure_detection_irq(fptr);
dmathew 6:aa13392d16bb 1092 enable_interrupt_measure_detection_irq();
dmathew 6:aa13392d16bb 1093 }
dmathew 6:aa13392d16bb 1094
dmathew 6:aa13392d16bb 1095 if (status == 0) {
dmathew 6:aa13392d16bb 1096 status = VL53L1X_StartRanging();
dmathew 6:aa13392d16bb 1097 }
dmathew 6:aa13392d16bb 1098
dmathew 6:aa13392d16bb 1099 return status;
dmathew 6:aa13392d16bb 1100 }
dmathew 6:aa13392d16bb 1101
dmathew 6:aa13392d16bb 1102 int VL53L1X::stop_measurement()
dmathew 6:aa13392d16bb 1103 {
dmathew 6:aa13392d16bb 1104 int status = 0;
dmathew 6:aa13392d16bb 1105
dmathew 6:aa13392d16bb 1106 if (status == 0) {
dmathew 6:aa13392d16bb 1107 printf("Call of VL53L0X_StopMeasurement\n");
dmathew 6:aa13392d16bb 1108 status = VL53L1X_StopRanging();
dmathew 6:aa13392d16bb 1109 }
dmathew 6:aa13392d16bb 1110
dmathew 6:aa13392d16bb 1111 if (status == 0)
dmathew 6:aa13392d16bb 1112 status = VL53L1X_ClearInterrupt();
dmathew 6:aa13392d16bb 1113
dmathew 6:aa13392d16bb 1114 return status;
dmathew 6:aa13392d16bb 1115 }