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