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
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?
User | Revision | Line number | New 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>© 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(¤t_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 |