Tarek Lule / VL53L0X_Condensed

Condensed Version of Public VL53L0X

Dependents:   ToF-Only-Tryout

VL53L0X.cpp@5:b95f6951f7d5, 2019-03-23 (annotated)

Committer:
sepp_nepp
Date:
Sat Mar 23 21:28:30 2019 +0000
Revision:
5:b95f6951f7d5
Parent:
3:e9269ff624ed
Child:
6:1976f4afed97
First Version of My Variant of VL53L0X

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nikapov 0:a1a69d32f310 1 /**
nikapov 0:a1a69d32f310 2 ******************************************************************************
nikapov 0:a1a69d32f310 3 * @file VL53L0X_class.cpp
nikapov 0:a1a69d32f310 4 * @author IMG
nikapov 0:a1a69d32f310 5 * @version V0.0.1
nikapov 0:a1a69d32f310 6 * @date 28-June-2016
nikapov 0:a1a69d32f310 7 * @brief Implementation file for the VL53L0X driver class
nikapov 0:a1a69d32f310 8 ******************************************************************************
nikapov 0:a1a69d32f310 9 * @attention
nikapov 0:a1a69d32f310 10 *
nikapov 0:a1a69d32f310 11 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
nikapov 0:a1a69d32f310 12 *
nikapov 0:a1a69d32f310 13 * Redistribution and use in source and binary forms, with or without modification,
nikapov 0:a1a69d32f310 14 * are permitted provided that the following conditions are met:
nikapov 0:a1a69d32f310 15 * 1. Redistributions of source code must retain the above copyright notice,
nikapov 0:a1a69d32f310 16 * this list of conditions and the following disclaimer.
nikapov 0:a1a69d32f310 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
nikapov 0:a1a69d32f310 18 * this list of conditions and the following disclaimer in the documentation
nikapov 0:a1a69d32f310 19 * and/or other materials provided with the distribution.
nikapov 0:a1a69d32f310 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
nikapov 0:a1a69d32f310 21 * may be used to endorse or promote products derived from this software
nikapov 0:a1a69d32f310 22 * without specific prior written permission.
nikapov 0:a1a69d32f310 23 *
nikapov 0:a1a69d32f310 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
nikapov 0:a1a69d32f310 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
nikapov 0:a1a69d32f310 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
nikapov 0:a1a69d32f310 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
nikapov 0:a1a69d32f310 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
nikapov 0:a1a69d32f310 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
nikapov 0:a1a69d32f310 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
nikapov 0:a1a69d32f310 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
nikapov 0:a1a69d32f310 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
nikapov 0:a1a69d32f310 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
nikapov 0:a1a69d32f310 34 *
nikapov 0:a1a69d32f310 35 ******************************************************************************
nikapov 0:a1a69d32f310 36 */
nikapov 0:a1a69d32f310 37
nikapov 0:a1a69d32f310 38 /* Includes */
nikapov 0:a1a69d32f310 39 #include <stdlib.h>
nikapov 0:a1a69d32f310 40
nikapov 0:a1a69d32f310 41 #include "VL53L0X.h"
nikapov 0:a1a69d32f310 42 #include "VL53L0X_tuning.h"
nikapov 0:a1a69d32f310 43
nikapov 0:a1a69d32f310 44 #define REF_ARRAY_SPAD_0 0
nikapov 0:a1a69d32f310 45 #define REF_ARRAY_SPAD_5 5
nikapov 0:a1a69d32f310 46 #define REF_ARRAY_SPAD_10 10
nikapov 0:a1a69d32f310 47
nikapov 0:a1a69d32f310 48 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5,
sepp_nepp 5:b95f6951f7d5 49 REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 };
nikapov 0:a1a69d32f310 50
nikapov 0:a1a69d32f310 51 VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 52 {
nikapov 0:a1a69d32f310 53 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 54 uint8_t strobe;
nikapov 0:a1a69d32f310 55 uint32_t loop_nb;
nikapov 0:a1a69d32f310 56
nikapov 0:a1a69d32f310 57 status |= VL53L0X_write_byte(dev, 0x83, 0x00);
nikapov 0:a1a69d32f310 58
nikapov 0:a1a69d32f310 59 /* polling
nikapov 0:a1a69d32f310 60 * use timeout to avoid deadlock*/
nikapov 0:a1a69d32f310 61 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 62 loop_nb = 0;
nikapov 0:a1a69d32f310 63 do {
nikapov 0:a1a69d32f310 64 status = VL53L0X_read_byte(dev, 0x83, &strobe);
nikapov 0:a1a69d32f310 65 if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 66 break;
nikapov 0:a1a69d32f310 67 }
nikapov 0:a1a69d32f310 68
nikapov 0:a1a69d32f310 69 loop_nb = loop_nb + 1;
nikapov 0:a1a69d32f310 70 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov 0:a1a69d32f310 71
nikapov 0:a1a69d32f310 72 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov 0:a1a69d32f310 73 status = VL53L0X_ERROR_TIME_OUT;
nikapov 0:a1a69d32f310 74 }
nikapov 0:a1a69d32f310 75 }
nikapov 0:a1a69d32f310 76
nikapov 0:a1a69d32f310 77 status |= VL53L0X_write_byte(dev, 0x83, 0x01);
nikapov 0:a1a69d32f310 78
nikapov 0:a1a69d32f310 79 return status;
nikapov 0:a1a69d32f310 80 }
nikapov 0:a1a69d32f310 81
nikapov 0:a1a69d32f310 82 VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(VL53L0X_DEV dev, uint8_t option)
nikapov 0:a1a69d32f310 83 {
nikapov 0:a1a69d32f310 84 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 85 uint8_t byte;
nikapov 0:a1a69d32f310 86 uint32_t tmp_dword;
nikapov 0:a1a69d32f310 87 uint8_t module_id;
nikapov 0:a1a69d32f310 88 uint8_t revision;
nikapov 0:a1a69d32f310 89 uint8_t reference_spad_count = 0;
nikapov 0:a1a69d32f310 90 uint8_t reference_spad_type = 0;
nikapov 0:a1a69d32f310 91 uint32_t part_uid_upper = 0;
nikapov 0:a1a69d32f310 92 uint32_t part_uid_lower = 0;
nikapov 0:a1a69d32f310 93 uint32_t offset_fixed1104_mm = 0;
nikapov 0:a1a69d32f310 94 int16_t offset_micro_meters = 0;
nikapov 0:a1a69d32f310 95 uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
nikapov 0:a1a69d32f310 96 uint32_t dist_meas_fixed1104_400_mm = 0;
nikapov 0:a1a69d32f310 97 uint32_t signal_rate_meas_fixed1104_400_mm = 0;
nikapov 0:a1a69d32f310 98 char product_id[19];
nikapov 0:a1a69d32f310 99 char *product_id_tmp;
nikapov 0:a1a69d32f310 100 uint8_t read_data_from_device_done;
nikapov 0:a1a69d32f310 101 FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0;
nikapov 0:a1a69d32f310 102 uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE];
nikapov 0:a1a69d32f310 103 int i;
sepp_nepp 5:b95f6951f7d5 104
nikapov 0:a1a69d32f310 105
nikapov 0:a1a69d32f310 106 read_data_from_device_done = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 107 ReadDataFromDeviceDone);
nikapov 0:a1a69d32f310 108
nikapov 0:a1a69d32f310 109 /* This access is done only once after that a GetDeviceInfo or
nikapov 0:a1a69d32f310 110 * datainit is done*/
nikapov 0:a1a69d32f310 111 if (read_data_from_device_done != 7) {
nikapov 0:a1a69d32f310 112
nikapov 0:a1a69d32f310 113 status |= VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov 0:a1a69d32f310 114 status |= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 115 status |= VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov 0:a1a69d32f310 116
nikapov 0:a1a69d32f310 117 status |= VL53L0X_write_byte(dev, 0xFF, 0x06);
nikapov 0:a1a69d32f310 118 status |= VL53L0X_read_byte(dev, 0x83, &byte);
nikapov 0:a1a69d32f310 119 status |= VL53L0X_write_byte(dev, 0x83, byte | 4);
nikapov 0:a1a69d32f310 120 status |= VL53L0X_write_byte(dev, 0xFF, 0x07);
nikapov 0:a1a69d32f310 121 status |= VL53L0X_write_byte(dev, 0x81, 0x01);
nikapov 0:a1a69d32f310 122
nikapov 0:a1a69d32f310 123 status |= VL53L0X_polling_delay(dev);
nikapov 0:a1a69d32f310 124
nikapov 0:a1a69d32f310 125 status |= VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov 0:a1a69d32f310 126
nikapov 0:a1a69d32f310 127 if (((option & 1) == 1) &&
nikapov 0:a1a69d32f310 128 ((read_data_from_device_done & 1) == 0)) {
nikapov 0:a1a69d32f310 129 status |= VL53L0X_write_byte(dev, 0x94, 0x6b);
nikapov 0:a1a69d32f310 130 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 131 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 132
nikapov 0:a1a69d32f310 133 reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f);
nikapov 0:a1a69d32f310 134 reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01);
nikapov 0:a1a69d32f310 135
nikapov 0:a1a69d32f310 136 status |= VL53L0X_write_byte(dev, 0x94, 0x24);
nikapov 0:a1a69d32f310 137 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 138 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 139
nikapov 0:a1a69d32f310 140
nikapov 0:a1a69d32f310 141 nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24)
nikapov 0:a1a69d32f310 142 & 0xff);
nikapov 0:a1a69d32f310 143 nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16)
nikapov 0:a1a69d32f310 144 & 0xff);
nikapov 0:a1a69d32f310 145 nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8)
nikapov 0:a1a69d32f310 146 & 0xff);
nikapov 0:a1a69d32f310 147 nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff);
nikapov 0:a1a69d32f310 148
nikapov 0:a1a69d32f310 149 status |= VL53L0X_write_byte(dev, 0x94, 0x25);
nikapov 0:a1a69d32f310 150 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 151 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 152
nikapov 0:a1a69d32f310 153 nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24)
nikapov 0:a1a69d32f310 154 & 0xff);
nikapov 0:a1a69d32f310 155 nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16)
nikapov 0:a1a69d32f310 156 & 0xff);
nikapov 0:a1a69d32f310 157 }
nikapov 0:a1a69d32f310 158
nikapov 0:a1a69d32f310 159 if (((option & 2) == 2) &&
nikapov 0:a1a69d32f310 160 ((read_data_from_device_done & 2) == 0)) {
nikapov 0:a1a69d32f310 161
nikapov 0:a1a69d32f310 162 status |= VL53L0X_write_byte(dev, 0x94, 0x02);
nikapov 0:a1a69d32f310 163 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 164 status |= VL53L0X_read_byte(dev, 0x90, &module_id);
nikapov 0:a1a69d32f310 165
nikapov 0:a1a69d32f310 166 status |= VL53L0X_write_byte(dev, 0x94, 0x7B);
nikapov 0:a1a69d32f310 167 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 168 status |= VL53L0X_read_byte(dev, 0x90, &revision);
nikapov 0:a1a69d32f310 169
nikapov 0:a1a69d32f310 170 status |= VL53L0X_write_byte(dev, 0x94, 0x77);
nikapov 0:a1a69d32f310 171 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 172 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 173
nikapov 0:a1a69d32f310 174 product_id[0] = (char)((tmp_dword >> 25) & 0x07f);
nikapov 0:a1a69d32f310 175 product_id[1] = (char)((tmp_dword >> 18) & 0x07f);
nikapov 0:a1a69d32f310 176 product_id[2] = (char)((tmp_dword >> 11) & 0x07f);
nikapov 0:a1a69d32f310 177 product_id[3] = (char)((tmp_dword >> 4) & 0x07f);
nikapov 0:a1a69d32f310 178
nikapov 0:a1a69d32f310 179 byte = (uint8_t)((tmp_dword & 0x00f) << 3);
nikapov 0:a1a69d32f310 180
nikapov 0:a1a69d32f310 181 status |= VL53L0X_write_byte(dev, 0x94, 0x78);
nikapov 0:a1a69d32f310 182 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 183 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 184
nikapov 0:a1a69d32f310 185 product_id[4] = (char)(byte +
nikapov 0:a1a69d32f310 186 ((tmp_dword >> 29) & 0x07f));
nikapov 0:a1a69d32f310 187 product_id[5] = (char)((tmp_dword >> 22) & 0x07f);
nikapov 0:a1a69d32f310 188 product_id[6] = (char)((tmp_dword >> 15) & 0x07f);
nikapov 0:a1a69d32f310 189 product_id[7] = (char)((tmp_dword >> 8) & 0x07f);
nikapov 0:a1a69d32f310 190 product_id[8] = (char)((tmp_dword >> 1) & 0x07f);
nikapov 0:a1a69d32f310 191
nikapov 0:a1a69d32f310 192 byte = (uint8_t)((tmp_dword & 0x001) << 6);
nikapov 0:a1a69d32f310 193
nikapov 0:a1a69d32f310 194 status |= VL53L0X_write_byte(dev, 0x94, 0x79);
nikapov 0:a1a69d32f310 195
nikapov 0:a1a69d32f310 196 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 197
nikapov 0:a1a69d32f310 198 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 199
nikapov 0:a1a69d32f310 200 product_id[9] = (char)(byte +
nikapov 0:a1a69d32f310 201 ((tmp_dword >> 26) & 0x07f));
nikapov 0:a1a69d32f310 202 product_id[10] = (char)((tmp_dword >> 19) & 0x07f);
nikapov 0:a1a69d32f310 203 product_id[11] = (char)((tmp_dword >> 12) & 0x07f);
nikapov 0:a1a69d32f310 204 product_id[12] = (char)((tmp_dword >> 5) & 0x07f);
nikapov 0:a1a69d32f310 205
nikapov 0:a1a69d32f310 206 byte = (uint8_t)((tmp_dword & 0x01f) << 2);
nikapov 0:a1a69d32f310 207
nikapov 0:a1a69d32f310 208 status |= VL53L0X_write_byte(dev, 0x94, 0x7A);
nikapov 0:a1a69d32f310 209
nikapov 0:a1a69d32f310 210 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 211
nikapov 0:a1a69d32f310 212 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 213
nikapov 0:a1a69d32f310 214 product_id[13] = (char)(byte +
nikapov 0:a1a69d32f310 215 ((tmp_dword >> 30) & 0x07f));
nikapov 0:a1a69d32f310 216 product_id[14] = (char)((tmp_dword >> 23) & 0x07f);
nikapov 0:a1a69d32f310 217 product_id[15] = (char)((tmp_dword >> 16) & 0x07f);
nikapov 0:a1a69d32f310 218 product_id[16] = (char)((tmp_dword >> 9) & 0x07f);
nikapov 0:a1a69d32f310 219 product_id[17] = (char)((tmp_dword >> 2) & 0x07f);
nikapov 0:a1a69d32f310 220 product_id[18] = '\0';
nikapov 0:a1a69d32f310 221
nikapov 0:a1a69d32f310 222 }
nikapov 0:a1a69d32f310 223
nikapov 0:a1a69d32f310 224 if (((option & 4) == 4) &&
nikapov 0:a1a69d32f310 225 ((read_data_from_device_done & 4) == 0)) {
nikapov 0:a1a69d32f310 226
nikapov 0:a1a69d32f310 227 status |= VL53L0X_write_byte(dev, 0x94, 0x7B);
nikapov 0:a1a69d32f310 228 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 229 status |= VL53L0X_read_dword(dev, 0x90, &part_uid_upper);
nikapov 0:a1a69d32f310 230
nikapov 0:a1a69d32f310 231 status |= VL53L0X_write_byte(dev, 0x94, 0x7C);
nikapov 0:a1a69d32f310 232 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 233 status |= VL53L0X_read_dword(dev, 0x90, &part_uid_lower);
nikapov 0:a1a69d32f310 234
nikapov 0:a1a69d32f310 235 status |= VL53L0X_write_byte(dev, 0x94, 0x73);
nikapov 0:a1a69d32f310 236 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 237 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 238
nikapov 0:a1a69d32f310 239 signal_rate_meas_fixed1104_400_mm = (tmp_dword &
nikapov 0:a1a69d32f310 240 0x0000000ff) << 8;
nikapov 0:a1a69d32f310 241
nikapov 0:a1a69d32f310 242 status |= VL53L0X_write_byte(dev, 0x94, 0x74);
nikapov 0:a1a69d32f310 243 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 244 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 245
nikapov 0:a1a69d32f310 246 signal_rate_meas_fixed1104_400_mm |= ((tmp_dword &
nikapov 0:a1a69d32f310 247 0xff000000) >> 24);
nikapov 0:a1a69d32f310 248
nikapov 0:a1a69d32f310 249 status |= VL53L0X_write_byte(dev, 0x94, 0x75);
nikapov 0:a1a69d32f310 250 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 251 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 252
nikapov 0:a1a69d32f310 253 dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)
nikapov 0:a1a69d32f310 254 << 8;
nikapov 0:a1a69d32f310 255
nikapov 0:a1a69d32f310 256 status |= VL53L0X_write_byte(dev, 0x94, 0x76);
nikapov 0:a1a69d32f310 257 status |= VL53L0X_device_read_strobe(dev);
nikapov 0:a1a69d32f310 258 status |= VL53L0X_read_dword(dev, 0x90, &tmp_dword);
nikapov 0:a1a69d32f310 259
nikapov 0:a1a69d32f310 260 dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000)
nikapov 0:a1a69d32f310 261 >> 24);
nikapov 0:a1a69d32f310 262 }
nikapov 0:a1a69d32f310 263
nikapov 0:a1a69d32f310 264 status |= VL53L0X_write_byte(dev, 0x81, 0x00);
nikapov 0:a1a69d32f310 265 status |= VL53L0X_write_byte(dev, 0xFF, 0x06);
nikapov 0:a1a69d32f310 266 status |= VL53L0X_read_byte(dev, 0x83, &byte);
nikapov 0:a1a69d32f310 267 status |= VL53L0X_write_byte(dev, 0x83, byte & 0xfb);
nikapov 0:a1a69d32f310 268 status |= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 269 status |= VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov 0:a1a69d32f310 270
nikapov 0:a1a69d32f310 271 status |= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 272 status |= VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov 0:a1a69d32f310 273 }
nikapov 0:a1a69d32f310 274
nikapov 0:a1a69d32f310 275 if ((status == VL53L0X_ERROR_NONE) &&
nikapov 0:a1a69d32f310 276 (read_data_from_device_done != 7)) {
nikapov 0:a1a69d32f310 277 /* Assign to variable if status is ok */
nikapov 0:a1a69d32f310 278 if (((option & 1) == 1) &&
nikapov 0:a1a69d32f310 279 ((read_data_from_device_done & 1) == 0)) {
nikapov 0:a1a69d32f310 280 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 281 ReferenceSpadCount, reference_spad_count);
nikapov 0:a1a69d32f310 282
nikapov 0:a1a69d32f310 283 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 284 ReferenceSpadType, reference_spad_type);
nikapov 0:a1a69d32f310 285
nikapov 0:a1a69d32f310 286 for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
sepp_nepp 5:b95f6951f7d5 287 dev->SpadData.RefGoodSpadMap[i] =
nikapov 0:a1a69d32f310 288 nvm_ref_good_spad_map[i];
nikapov 0:a1a69d32f310 289 }
nikapov 0:a1a69d32f310 290 }
nikapov 0:a1a69d32f310 291
nikapov 0:a1a69d32f310 292 if (((option & 2) == 2) &&
nikapov 0:a1a69d32f310 293 ((read_data_from_device_done & 2) == 0)) {
nikapov 0:a1a69d32f310 294 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 295 ModuleId, module_id);
nikapov 0:a1a69d32f310 296
nikapov 0:a1a69d32f310 297 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 298 Revision, revision);
nikapov 0:a1a69d32f310 299
nikapov 0:a1a69d32f310 300 product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 301 ProductId);
sepp_nepp 5:b95f6951f7d5 302 strcpy(product_id_tmp, product_id);
nikapov 0:a1a69d32f310 303
nikapov 0:a1a69d32f310 304 }
nikapov 0:a1a69d32f310 305
nikapov 0:a1a69d32f310 306 if (((option & 4) == 4) &&
nikapov 0:a1a69d32f310 307 ((read_data_from_device_done & 4) == 0)) {
nikapov 0:a1a69d32f310 308 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 309 PartUIDUpper, part_uid_upper);
nikapov 0:a1a69d32f310 310
nikapov 0:a1a69d32f310 311 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 312 PartUIDLower, part_uid_lower);
nikapov 0:a1a69d32f310 313
nikapov 0:a1a69d32f310 314 signal_rate_meas_fixed400_mm_fix =
nikapov 0:a1a69d32f310 315 VL53L0X_FIXPOINT97TOFIXPOINT1616(
nikapov 0:a1a69d32f310 316 signal_rate_meas_fixed1104_400_mm);
nikapov 0:a1a69d32f310 317
nikapov 0:a1a69d32f310 318 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 319 SignalRateMeasFixed400mm,
nikapov 0:a1a69d32f310 320 signal_rate_meas_fixed400_mm_fix);
nikapov 0:a1a69d32f310 321
nikapov 0:a1a69d32f310 322 offset_micro_meters = 0;
nikapov 0:a1a69d32f310 323 if (dist_meas_fixed1104_400_mm != 0) {
nikapov 0:a1a69d32f310 324 offset_fixed1104_mm =
nikapov 0:a1a69d32f310 325 dist_meas_fixed1104_400_mm -
nikapov 0:a1a69d32f310 326 dist_meas_tgt_fixed1104_mm;
nikapov 0:a1a69d32f310 327 offset_micro_meters = (offset_fixed1104_mm
nikapov 0:a1a69d32f310 328 * 1000) >> 4;
nikapov 0:a1a69d32f310 329 offset_micro_meters *= -1;
nikapov 0:a1a69d32f310 330 }
nikapov 0:a1a69d32f310 331
nikapov 0:a1a69d32f310 332 PALDevDataSet(dev,
nikapov 0:a1a69d32f310 333 Part2PartOffsetAdjustmentNVMMicroMeter,
nikapov 0:a1a69d32f310 334 offset_micro_meters);
nikapov 0:a1a69d32f310 335 }
nikapov 0:a1a69d32f310 336 byte = (uint8_t)(read_data_from_device_done | option);
nikapov 0:a1a69d32f310 337 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone,
nikapov 0:a1a69d32f310 338 byte);
nikapov 0:a1a69d32f310 339 }
nikapov 0:a1a69d32f310 340
sepp_nepp 5:b95f6951f7d5 341
nikapov 0:a1a69d32f310 342 return status;
nikapov 0:a1a69d32f310 343 }
nikapov 0:a1a69d32f310 344
nikapov 0:a1a69d32f310 345 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 346 int32_t *p_offset_calibration_data_micro_meter)
nikapov 0:a1a69d32f310 347 {
nikapov 0:a1a69d32f310 348 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 349 uint16_t range_offset_register;
nikapov 0:a1a69d32f310 350 int16_t c_max_offset = 2047;
nikapov 0:a1a69d32f310 351 int16_t c_offset_range = 4096;
nikapov 0:a1a69d32f310 352
nikapov 0:a1a69d32f310 353 /* Note that offset has 10.2 format */
nikapov 0:a1a69d32f310 354
nikapov 0:a1a69d32f310 355 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 356 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
nikapov 0:a1a69d32f310 357 &range_offset_register);
nikapov 0:a1a69d32f310 358
nikapov 0:a1a69d32f310 359 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 360 range_offset_register = (range_offset_register & 0x0fff);
nikapov 0:a1a69d32f310 361
nikapov 0:a1a69d32f310 362 /* Apply 12 bit 2's compliment conversion */
nikapov 0:a1a69d32f310 363 if (range_offset_register > c_max_offset) {
nikapov 0:a1a69d32f310 364 *p_offset_calibration_data_micro_meter =
nikapov 0:a1a69d32f310 365 (int16_t)(range_offset_register - c_offset_range)
nikapov 0:a1a69d32f310 366 * 250;
nikapov 0:a1a69d32f310 367 } else {
nikapov 0:a1a69d32f310 368 *p_offset_calibration_data_micro_meter =
nikapov 0:a1a69d32f310 369 (int16_t)range_offset_register * 250;
nikapov 0:a1a69d32f310 370 }
nikapov 0:a1a69d32f310 371
nikapov 0:a1a69d32f310 372 }
nikapov 0:a1a69d32f310 373
nikapov 0:a1a69d32f310 374 return status;
nikapov 0:a1a69d32f310 375 }
nikapov 0:a1a69d32f310 376
nikapov 0:a1a69d32f310 377 VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 378 int32_t *p_offset_calibration_data_micro_meter)
nikapov 0:a1a69d32f310 379 {
nikapov 0:a1a69d32f310 380 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 381
nikapov 0:a1a69d32f310 382
nikapov 0:a1a69d32f310 383 status = wrapped_VL53L0X_get_offset_calibration_data_micro_meter(dev,
nikapov 0:a1a69d32f310 384 p_offset_calibration_data_micro_meter);
nikapov 0:a1a69d32f310 385
sepp_nepp 5:b95f6951f7d5 386
nikapov 0:a1a69d32f310 387 return status;
nikapov 0:a1a69d32f310 388 }
nikapov 0:a1a69d32f310 389
nikapov 0:a1a69d32f310 390 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 391 int32_t offset_calibration_data_micro_meter)
nikapov 0:a1a69d32f310 392 {
nikapov 0:a1a69d32f310 393 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 394 int32_t c_max_offset_micro_meter = 511000;
nikapov 0:a1a69d32f310 395 int32_t c_min_offset_micro_meter = -512000;
nikapov 0:a1a69d32f310 396 int16_t c_offset_range = 4096;
nikapov 0:a1a69d32f310 397 uint32_t encoded_offset_val;
nikapov 0:a1a69d32f310 398
sepp_nepp 5:b95f6951f7d5 399
nikapov 0:a1a69d32f310 400
nikapov 0:a1a69d32f310 401 if (offset_calibration_data_micro_meter > c_max_offset_micro_meter) {
nikapov 0:a1a69d32f310 402 offset_calibration_data_micro_meter = c_max_offset_micro_meter;
nikapov 0:a1a69d32f310 403 } else {
nikapov 0:a1a69d32f310 404 if (offset_calibration_data_micro_meter < c_min_offset_micro_meter) {
Davidroid 2:d07edeaff6f1 405 offset_calibration_data_micro_meter = c_min_offset_micro_meter;
Davidroid 2:d07edeaff6f1 406 }
Davidroid 2:d07edeaff6f1 407 }
nikapov 0:a1a69d32f310 408
nikapov 0:a1a69d32f310 409 /* The offset register is 10.2 format and units are mm
nikapov 0:a1a69d32f310 410 * therefore conversion is applied by a division of
nikapov 0:a1a69d32f310 411 * 250.
nikapov 0:a1a69d32f310 412 */
nikapov 0:a1a69d32f310 413 if (offset_calibration_data_micro_meter >= 0) {
nikapov 0:a1a69d32f310 414 encoded_offset_val =
nikapov 0:a1a69d32f310 415 offset_calibration_data_micro_meter / 250;
nikapov 0:a1a69d32f310 416 } else {
nikapov 0:a1a69d32f310 417 encoded_offset_val =
nikapov 0:a1a69d32f310 418 c_offset_range +
nikapov 0:a1a69d32f310 419 offset_calibration_data_micro_meter / 250;
nikapov 0:a1a69d32f310 420 }
nikapov 0:a1a69d32f310 421
nikapov 0:a1a69d32f310 422 status = VL53L0X_write_word(dev,
nikapov 0:a1a69d32f310 423 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
nikapov 0:a1a69d32f310 424 encoded_offset_val);
nikapov 0:a1a69d32f310 425
sepp_nepp 5:b95f6951f7d5 426
nikapov 0:a1a69d32f310 427 return status;
nikapov 0:a1a69d32f310 428 }
nikapov 0:a1a69d32f310 429
nikapov 0:a1a69d32f310 430 VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 431 int32_t offset_calibration_data_micro_meter)
nikapov 0:a1a69d32f310 432 {
nikapov 0:a1a69d32f310 433 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 434
nikapov 0:a1a69d32f310 435
nikapov 0:a1a69d32f310 436 status = wrapped_VL53L0X_set_offset_calibration_data_micro_meter(dev,
nikapov 0:a1a69d32f310 437 offset_calibration_data_micro_meter);
nikapov 0:a1a69d32f310 438
sepp_nepp 5:b95f6951f7d5 439
nikapov 0:a1a69d32f310 440 return status;
nikapov 0:a1a69d32f310 441 }
nikapov 0:a1a69d32f310 442
nikapov 0:a1a69d32f310 443 VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 444 {
nikapov 0:a1a69d32f310 445 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 446 int32_t corrected_offset_micro_meters;
nikapov 0:a1a69d32f310 447 int32_t current_offset_micro_meters;
nikapov 0:a1a69d32f310 448
nikapov 0:a1a69d32f310 449 /* if we run on this function we can read all the NVM info
nikapov 0:a1a69d32f310 450 * used by the API */
nikapov 0:a1a69d32f310 451 status = VL53L0X_get_info_from_device(dev, 7);
nikapov 0:a1a69d32f310 452
nikapov 0:a1a69d32f310 453 /* Read back current device offset */
nikapov 0:a1a69d32f310 454 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 455 status = VL53L0X_get_offset_calibration_data_micro_meter(dev,
nikapov 0:a1a69d32f310 456 &current_offset_micro_meters);
nikapov 0:a1a69d32f310 457 }
nikapov 0:a1a69d32f310 458
nikapov 0:a1a69d32f310 459 /* Apply Offset Adjustment derived from 400mm measurements */
nikapov 0:a1a69d32f310 460 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 461
nikapov 0:a1a69d32f310 462 /* Store initial device offset */
nikapov 0:a1a69d32f310 463 PALDevDataSet(dev, Part2PartOffsetNVMMicroMeter,
nikapov 0:a1a69d32f310 464 current_offset_micro_meters);
nikapov 0:a1a69d32f310 465
nikapov 0:a1a69d32f310 466 corrected_offset_micro_meters = current_offset_micro_meters +
nikapov 0:a1a69d32f310 467 (int32_t)PALDevDataGet(dev,
nikapov 0:a1a69d32f310 468 Part2PartOffsetAdjustmentNVMMicroMeter);
nikapov 0:a1a69d32f310 469
nikapov 0:a1a69d32f310 470 status = VL53L0X_set_offset_calibration_data_micro_meter(dev,
nikapov 0:a1a69d32f310 471 corrected_offset_micro_meters);
nikapov 0:a1a69d32f310 472
nikapov 0:a1a69d32f310 473 /* store current, adjusted offset */
nikapov 0:a1a69d32f310 474 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 475 VL53L0X_SETPARAMETERFIELD(dev, RangeOffsetMicroMeters,
nikapov 0:a1a69d32f310 476 corrected_offset_micro_meters);
nikapov 0:a1a69d32f310 477 }
nikapov 0:a1a69d32f310 478 }
nikapov 0:a1a69d32f310 479
nikapov 0:a1a69d32f310 480 return status;
nikapov 0:a1a69d32f310 481 }
nikapov 0:a1a69d32f310 482
nikapov 0:a1a69d32f310 483 VL53L0X_Error VL53L0X::VL53L0X_get_device_mode(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 484 VL53L0X_DeviceModes *p_device_mode)
nikapov 0:a1a69d32f310 485 {
nikapov 0:a1a69d32f310 486 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 487
nikapov 0:a1a69d32f310 488
nikapov 0:a1a69d32f310 489 VL53L0X_GETPARAMETERFIELD(dev, DeviceMode, *p_device_mode);
nikapov 0:a1a69d32f310 490
sepp_nepp 5:b95f6951f7d5 491
nikapov 0:a1a69d32f310 492 return status;
nikapov 0:a1a69d32f310 493 }
nikapov 0:a1a69d32f310 494
nikapov 0:a1a69d32f310 495 VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_milli_seconds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 496 uint32_t *p_inter_measurement_period_milli_seconds)
nikapov 0:a1a69d32f310 497 {
nikapov 0:a1a69d32f310 498 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 499 uint16_t osc_calibrate_val;
nikapov 0:a1a69d32f310 500 uint32_t im_period_milli_seconds;
nikapov 0:a1a69d32f310 501
sepp_nepp 5:b95f6951f7d5 502
nikapov 0:a1a69d32f310 503
nikapov 0:a1a69d32f310 504 status = VL53L0X_read_word(dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
nikapov 0:a1a69d32f310 505 &osc_calibrate_val);
nikapov 0:a1a69d32f310 506
nikapov 0:a1a69d32f310 507 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 508 status = VL53L0X_read_dword(dev,
nikapov 0:a1a69d32f310 509 VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
nikapov 0:a1a69d32f310 510 &im_period_milli_seconds);
nikapov 0:a1a69d32f310 511 }
nikapov 0:a1a69d32f310 512
nikapov 0:a1a69d32f310 513 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 514 if (osc_calibrate_val != 0) {
nikapov 0:a1a69d32f310 515 *p_inter_measurement_period_milli_seconds =
nikapov 0:a1a69d32f310 516 im_period_milli_seconds / osc_calibrate_val;
nikapov 0:a1a69d32f310 517 }
nikapov 0:a1a69d32f310 518 VL53L0X_SETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 519 InterMeasurementPeriodMilliSeconds,
nikapov 0:a1a69d32f310 520 *p_inter_measurement_period_milli_seconds);
nikapov 0:a1a69d32f310 521 }
nikapov 0:a1a69d32f310 522
sepp_nepp 5:b95f6951f7d5 523
nikapov 0:a1a69d32f310 524 return status;
nikapov 0:a1a69d32f310 525 }
nikapov 0:a1a69d32f310 526
nikapov 0:a1a69d32f310 527 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_mega_cps(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 528 FixPoint1616_t *p_xtalk_compensation_rate_mega_cps)
nikapov 0:a1a69d32f310 529 {
nikapov 0:a1a69d32f310 530 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 531 uint16_t value;
nikapov 0:a1a69d32f310 532 FixPoint1616_t temp_fix1616;
nikapov 0:a1a69d32f310 533
sepp_nepp 5:b95f6951f7d5 534
nikapov 0:a1a69d32f310 535
nikapov 0:a1a69d32f310 536 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 537 VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&value);
nikapov 0:a1a69d32f310 538 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 539 if (value == 0) {
nikapov 0:a1a69d32f310 540 /* the Xtalk is disabled return value from memory */
nikapov 0:a1a69d32f310 541 VL53L0X_GETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 542 XTalkCompensationRateMegaCps, temp_fix1616);
nikapov 0:a1a69d32f310 543 *p_xtalk_compensation_rate_mega_cps = temp_fix1616;
nikapov 0:a1a69d32f310 544 VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationEnable,
nikapov 0:a1a69d32f310 545 0);
nikapov 0:a1a69d32f310 546 } else {
nikapov 0:a1a69d32f310 547 temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value);
nikapov 0:a1a69d32f310 548 *p_xtalk_compensation_rate_mega_cps = temp_fix1616;
nikapov 0:a1a69d32f310 549 VL53L0X_SETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 550 XTalkCompensationRateMegaCps, temp_fix1616);
nikapov 0:a1a69d32f310 551 VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationEnable,
nikapov 0:a1a69d32f310 552 1);
nikapov 0:a1a69d32f310 553 }
nikapov 0:a1a69d32f310 554 }
nikapov 0:a1a69d32f310 555
sepp_nepp 5:b95f6951f7d5 556
nikapov 0:a1a69d32f310 557 return status;
nikapov 0:a1a69d32f310 558 }
nikapov 0:a1a69d32f310 559
nikapov 0:a1a69d32f310 560 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value(VL53L0X_DEV dev, uint16_t limit_check_id,
nikapov 0:a1a69d32f310 561 FixPoint1616_t *p_limit_check_value)
nikapov 0:a1a69d32f310 562 {
nikapov 0:a1a69d32f310 563 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 564 uint8_t enable_zero_value = 0;
nikapov 0:a1a69d32f310 565 uint16_t temp16;
nikapov 0:a1a69d32f310 566 FixPoint1616_t temp_fix1616;
nikapov 0:a1a69d32f310 567
sepp_nepp 5:b95f6951f7d5 568
nikapov 0:a1a69d32f310 569
nikapov 0:a1a69d32f310 570 switch (limit_check_id) {
nikapov 0:a1a69d32f310 571
nikapov 0:a1a69d32f310 572 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov 0:a1a69d32f310 573 /* internal computation: */
nikapov 0:a1a69d32f310 574 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 575 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp_fix1616);
nikapov 0:a1a69d32f310 576 enable_zero_value = 0;
nikapov 0:a1a69d32f310 577 break;
nikapov 0:a1a69d32f310 578
nikapov 0:a1a69d32f310 579 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
nikapov 0:a1a69d32f310 580 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 581 VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
nikapov 0:a1a69d32f310 582 &temp16);
Davidroid 3:e9269ff624ed 583 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 584 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
Davidroid 3:e9269ff624ed 585 }
nikapov 0:a1a69d32f310 586
nikapov 0:a1a69d32f310 587
nikapov 0:a1a69d32f310 588 enable_zero_value = 1;
nikapov 0:a1a69d32f310 589 break;
nikapov 0:a1a69d32f310 590
nikapov 0:a1a69d32f310 591 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov 0:a1a69d32f310 592 /* internal computation: */
nikapov 0:a1a69d32f310 593 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 594 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp_fix1616);
nikapov 0:a1a69d32f310 595 enable_zero_value = 0;
nikapov 0:a1a69d32f310 596 break;
nikapov 0:a1a69d32f310 597
nikapov 0:a1a69d32f310 598 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov 0:a1a69d32f310 599 /* internal computation: */
nikapov 0:a1a69d32f310 600 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 601 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, temp_fix1616);
nikapov 0:a1a69d32f310 602 enable_zero_value = 0;
nikapov 0:a1a69d32f310 603 break;
nikapov 0:a1a69d32f310 604
nikapov 0:a1a69d32f310 605 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov 0:a1a69d32f310 606 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
nikapov 0:a1a69d32f310 607 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 608 VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
nikapov 0:a1a69d32f310 609 &temp16);
Davidroid 3:e9269ff624ed 610 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 611 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
Davidroid 3:e9269ff624ed 612 }
nikapov 0:a1a69d32f310 613
nikapov 0:a1a69d32f310 614
nikapov 0:a1a69d32f310 615 enable_zero_value = 0;
nikapov 0:a1a69d32f310 616 break;
nikapov 0:a1a69d32f310 617
nikapov 0:a1a69d32f310 618 default:
nikapov 0:a1a69d32f310 619 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 620
nikapov 0:a1a69d32f310 621 }
nikapov 0:a1a69d32f310 622
nikapov 0:a1a69d32f310 623 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 624
nikapov 0:a1a69d32f310 625 if (enable_zero_value == 1) {
nikapov 0:a1a69d32f310 626
nikapov 0:a1a69d32f310 627 if (temp_fix1616 == 0) {
nikapov 0:a1a69d32f310 628 /* disabled: return value from memory */
nikapov 0:a1a69d32f310 629 VL53L0X_GETARRAYPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 630 LimitChecksValue, limit_check_id,
nikapov 0:a1a69d32f310 631 temp_fix1616);
nikapov 0:a1a69d32f310 632 *p_limit_check_value = temp_fix1616;
nikapov 0:a1a69d32f310 633 VL53L0X_SETARRAYPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 634 LimitChecksEnable, limit_check_id, 0);
nikapov 0:a1a69d32f310 635 } else {
nikapov 0:a1a69d32f310 636 *p_limit_check_value = temp_fix1616;
nikapov 0:a1a69d32f310 637 VL53L0X_SETARRAYPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 638 LimitChecksValue, limit_check_id,
nikapov 0:a1a69d32f310 639 temp_fix1616);
nikapov 0:a1a69d32f310 640 VL53L0X_SETARRAYPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 641 LimitChecksEnable, limit_check_id, 1);
nikapov 0:a1a69d32f310 642 }
nikapov 0:a1a69d32f310 643 } else {
nikapov 0:a1a69d32f310 644 *p_limit_check_value = temp_fix1616;
nikapov 0:a1a69d32f310 645 }
nikapov 0:a1a69d32f310 646 }
nikapov 0:a1a69d32f310 647
sepp_nepp 5:b95f6951f7d5 648
nikapov 0:a1a69d32f310 649 return status;
nikapov 0:a1a69d32f310 650
nikapov 0:a1a69d32f310 651 }
nikapov 0:a1a69d32f310 652
nikapov 0:a1a69d32f310 653 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id,
nikapov 0:a1a69d32f310 654 uint8_t *p_limit_check_enable)
nikapov 0:a1a69d32f310 655 {
nikapov 0:a1a69d32f310 656 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 657 uint8_t temp8;
nikapov 0:a1a69d32f310 658
sepp_nepp 5:b95f6951f7d5 659
nikapov 0:a1a69d32f310 660
nikapov 0:a1a69d32f310 661 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
nikapov 0:a1a69d32f310 662 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 663 *p_limit_check_enable = 0;
nikapov 0:a1a69d32f310 664 } else {
nikapov 0:a1a69d32f310 665 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov 0:a1a69d32f310 666 limit_check_id, temp8);
nikapov 0:a1a69d32f310 667 *p_limit_check_enable = temp8;
nikapov 0:a1a69d32f310 668 }
nikapov 0:a1a69d32f310 669
sepp_nepp 5:b95f6951f7d5 670
nikapov 0:a1a69d32f310 671 return status;
nikapov 0:a1a69d32f310 672 }
nikapov 0:a1a69d32f310 673
nikapov 0:a1a69d32f310 674 VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 675 uint8_t *p_wrap_around_check_enable)
nikapov 0:a1a69d32f310 676 {
nikapov 0:a1a69d32f310 677 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 678 uint8_t data;
nikapov 0:a1a69d32f310 679
sepp_nepp 5:b95f6951f7d5 680
nikapov 0:a1a69d32f310 681
nikapov 0:a1a69d32f310 682 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data);
nikapov 0:a1a69d32f310 683 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 684 PALDevDataSet(dev, SequenceConfig, data);
Davidroid 3:e9269ff624ed 685 if (data & (0x01 << 7)) {
nikapov 0:a1a69d32f310 686 *p_wrap_around_check_enable = 0x01;
Davidroid 3:e9269ff624ed 687 } else {
nikapov 0:a1a69d32f310 688 *p_wrap_around_check_enable = 0x00;
Davidroid 3:e9269ff624ed 689 }
nikapov 0:a1a69d32f310 690 }
nikapov 0:a1a69d32f310 691 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 692 VL53L0X_SETPARAMETERFIELD(dev, WrapAroundCheckEnable,
nikapov 0:a1a69d32f310 693 *p_wrap_around_check_enable);
nikapov 0:a1a69d32f310 694 }
nikapov 0:a1a69d32f310 695
sepp_nepp 5:b95f6951f7d5 696
nikapov 0:a1a69d32f310 697 return status;
nikapov 0:a1a69d32f310 698 }
nikapov 0:a1a69d32f310 699
nikapov 0:a1a69d32f310 700 VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 701 VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config,
nikapov 0:a1a69d32f310 702 uint8_t *p_sequence_step_enabled)
nikapov 0:a1a69d32f310 703 {
nikapov 0:a1a69d32f310 704 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 705 *p_sequence_step_enabled = 0;
sepp_nepp 5:b95f6951f7d5 706
nikapov 0:a1a69d32f310 707
nikapov 0:a1a69d32f310 708 switch (sequence_step_id) {
nikapov 0:a1a69d32f310 709 case VL53L0X_SEQUENCESTEP_TCC:
nikapov 0:a1a69d32f310 710 *p_sequence_step_enabled = (sequence_config & 0x10) >> 4;
nikapov 0:a1a69d32f310 711 break;
nikapov 0:a1a69d32f310 712 case VL53L0X_SEQUENCESTEP_DSS:
nikapov 0:a1a69d32f310 713 *p_sequence_step_enabled = (sequence_config & 0x08) >> 3;
nikapov 0:a1a69d32f310 714 break;
nikapov 0:a1a69d32f310 715 case VL53L0X_SEQUENCESTEP_MSRC:
nikapov 0:a1a69d32f310 716 *p_sequence_step_enabled = (sequence_config & 0x04) >> 2;
nikapov 0:a1a69d32f310 717 break;
nikapov 0:a1a69d32f310 718 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov 0:a1a69d32f310 719 *p_sequence_step_enabled = (sequence_config & 0x40) >> 6;
nikapov 0:a1a69d32f310 720 break;
nikapov 0:a1a69d32f310 721 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov 0:a1a69d32f310 722 *p_sequence_step_enabled = (sequence_config & 0x80) >> 7;
nikapov 0:a1a69d32f310 723 break;
nikapov 0:a1a69d32f310 724 default:
nikapov 0:a1a69d32f310 725 Status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 726 }
nikapov 0:a1a69d32f310 727
sepp_nepp 5:b95f6951f7d5 728
nikapov 0:a1a69d32f310 729 return Status;
nikapov 0:a1a69d32f310 730 }
nikapov 0:a1a69d32f310 731
nikapov 0:a1a69d32f310 732 VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 733 VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps)
nikapov 0:a1a69d32f310 734 {
nikapov 0:a1a69d32f310 735 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 736 uint8_t sequence_config = 0;
sepp_nepp 5:b95f6951f7d5 737
nikapov 0:a1a69d32f310 738
nikapov 0:a1a69d32f310 739 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 740 &sequence_config);
nikapov 0:a1a69d32f310 741
nikapov 0:a1a69d32f310 742 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 743 status = sequence_step_enabled(dev,
nikapov 0:a1a69d32f310 744 VL53L0X_SEQUENCESTEP_TCC, sequence_config,
nikapov 0:a1a69d32f310 745 &p_scheduler_sequence_steps->TccOn);
nikapov 0:a1a69d32f310 746 }
nikapov 0:a1a69d32f310 747 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 748 status = sequence_step_enabled(dev,
nikapov 0:a1a69d32f310 749 VL53L0X_SEQUENCESTEP_DSS, sequence_config,
nikapov 0:a1a69d32f310 750 &p_scheduler_sequence_steps->DssOn);
nikapov 0:a1a69d32f310 751 }
nikapov 0:a1a69d32f310 752 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 753 status = sequence_step_enabled(dev,
nikapov 0:a1a69d32f310 754 VL53L0X_SEQUENCESTEP_MSRC, sequence_config,
nikapov 0:a1a69d32f310 755 &p_scheduler_sequence_steps->MsrcOn);
nikapov 0:a1a69d32f310 756 }
nikapov 0:a1a69d32f310 757 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 758 status = sequence_step_enabled(dev,
nikapov 0:a1a69d32f310 759 VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config,
nikapov 0:a1a69d32f310 760 &p_scheduler_sequence_steps->PreRangeOn);
nikapov 0:a1a69d32f310 761 }
nikapov 0:a1a69d32f310 762 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 763 status = sequence_step_enabled(dev,
nikapov 0:a1a69d32f310 764 VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config,
nikapov 0:a1a69d32f310 765 &p_scheduler_sequence_steps->FinalRangeOn);
nikapov 0:a1a69d32f310 766 }
nikapov 0:a1a69d32f310 767
sepp_nepp 5:b95f6951f7d5 768
nikapov 0:a1a69d32f310 769 return status;
nikapov 0:a1a69d32f310 770 }
nikapov 0:a1a69d32f310 771
nikapov 0:a1a69d32f310 772 uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
nikapov 0:a1a69d32f310 773 {
nikapov 0:a1a69d32f310 774 /*!
nikapov 0:a1a69d32f310 775 * Converts the encoded VCSEL period register value into the real
nikapov 0:a1a69d32f310 776 * period in PLL clocks
nikapov 0:a1a69d32f310 777 */
nikapov 0:a1a69d32f310 778
nikapov 0:a1a69d32f310 779 uint8_t vcsel_period_pclks = 0;
nikapov 0:a1a69d32f310 780
nikapov 0:a1a69d32f310 781 vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
nikapov 0:a1a69d32f310 782
nikapov 0:a1a69d32f310 783 return vcsel_period_pclks;
nikapov 0:a1a69d32f310 784 }
nikapov 0:a1a69d32f310 785
nikapov 0:a1a69d32f310 786 uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks)
nikapov 0:a1a69d32f310 787 {
nikapov 0:a1a69d32f310 788 /*!
nikapov 0:a1a69d32f310 789 * Converts the encoded VCSEL period register value into the real period
nikapov 0:a1a69d32f310 790 * in PLL clocks
nikapov 0:a1a69d32f310 791 */
nikapov 0:a1a69d32f310 792
nikapov 0:a1a69d32f310 793 uint8_t vcsel_period_reg = 0;
nikapov 0:a1a69d32f310 794
nikapov 0:a1a69d32f310 795 vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
nikapov 0:a1a69d32f310 796
nikapov 0:a1a69d32f310 797 return vcsel_period_reg;
nikapov 0:a1a69d32f310 798 }
nikapov 0:a1a69d32f310 799
nikapov 0:a1a69d32f310 800
nikapov 0:a1a69d32f310 801 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 802 VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk)
nikapov 0:a1a69d32f310 803 {
nikapov 0:a1a69d32f310 804 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 805 uint8_t vcsel_period_reg;
nikapov 0:a1a69d32f310 806 uint8_t min_pre_vcsel_period_pclk = 12;
nikapov 0:a1a69d32f310 807 uint8_t max_pre_vcsel_period_pclk = 18;
nikapov 0:a1a69d32f310 808 uint8_t min_final_vcsel_period_pclk = 8;
nikapov 0:a1a69d32f310 809 uint8_t max_final_vcsel_period_pclk = 14;
nikapov 0:a1a69d32f310 810 uint32_t measurement_timing_budget_micro_seconds;
nikapov 0:a1a69d32f310 811 uint32_t final_range_timeout_micro_seconds;
nikapov 0:a1a69d32f310 812 uint32_t pre_range_timeout_micro_seconds;
nikapov 0:a1a69d32f310 813 uint32_t msrc_timeout_micro_seconds;
nikapov 0:a1a69d32f310 814 uint8_t phase_cal_int = 0;
nikapov 0:a1a69d32f310 815
nikapov 0:a1a69d32f310 816 /* Check if valid clock period requested */
nikapov 0:a1a69d32f310 817
nikapov 0:a1a69d32f310 818 if ((vcsel_pulse_period_pclk % 2) != 0) {
nikapov 0:a1a69d32f310 819 /* Value must be an even number */
nikapov 0:a1a69d32f310 820 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 821 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
nikapov 0:a1a69d32f310 822 (vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk ||
nikapov 0:a1a69d32f310 823 vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) {
nikapov 0:a1a69d32f310 824 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 825 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
nikapov 0:a1a69d32f310 826 (vcsel_pulse_period_pclk < min_final_vcsel_period_pclk ||
nikapov 0:a1a69d32f310 827 vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) {
nikapov 0:a1a69d32f310 828
nikapov 0:a1a69d32f310 829 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 830 }
nikapov 0:a1a69d32f310 831
nikapov 0:a1a69d32f310 832 /* Apply specific settings for the requested clock period */
nikapov 0:a1a69d32f310 833
Davidroid 3:e9269ff624ed 834 if (status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 835 return status;
Davidroid 3:e9269ff624ed 836 }
nikapov 0:a1a69d32f310 837
nikapov 0:a1a69d32f310 838
nikapov 0:a1a69d32f310 839 if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
nikapov 0:a1a69d32f310 840
nikapov 0:a1a69d32f310 841 /* Set phase check limits */
nikapov 0:a1a69d32f310 842 if (vcsel_pulse_period_pclk == 12) {
nikapov 0:a1a69d32f310 843
nikapov 0:a1a69d32f310 844 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 845 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 846 0x18);
nikapov 0:a1a69d32f310 847 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 848 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 849 0x08);
nikapov 0:a1a69d32f310 850 } else if (vcsel_pulse_period_pclk == 14) {
nikapov 0:a1a69d32f310 851
nikapov 0:a1a69d32f310 852 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 853 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 854 0x30);
nikapov 0:a1a69d32f310 855 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 856 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 857 0x08);
nikapov 0:a1a69d32f310 858 } else if (vcsel_pulse_period_pclk == 16) {
nikapov 0:a1a69d32f310 859
nikapov 0:a1a69d32f310 860 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 861 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 862 0x40);
nikapov 0:a1a69d32f310 863 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 864 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 865 0x08);
nikapov 0:a1a69d32f310 866 } else if (vcsel_pulse_period_pclk == 18) {
nikapov 0:a1a69d32f310 867
nikapov 0:a1a69d32f310 868 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 869 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 870 0x50);
nikapov 0:a1a69d32f310 871 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 872 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 873 0x08);
nikapov 0:a1a69d32f310 874 }
nikapov 0:a1a69d32f310 875 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
nikapov 0:a1a69d32f310 876
nikapov 0:a1a69d32f310 877 if (vcsel_pulse_period_pclk == 8) {
nikapov 0:a1a69d32f310 878
nikapov 0:a1a69d32f310 879 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 880 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 881 0x10);
nikapov 0:a1a69d32f310 882 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 883 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 884 0x08);
nikapov 0:a1a69d32f310 885
nikapov 0:a1a69d32f310 886 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 887 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
nikapov 0:a1a69d32f310 888 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 889 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
nikapov 0:a1a69d32f310 890
nikapov 0:a1a69d32f310 891 status |= VL53L0X_write_byte(dev, 0xff, 0x01);
nikapov 0:a1a69d32f310 892 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 893 VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov 0:a1a69d32f310 894 0x30);
nikapov 0:a1a69d32f310 895 status |= VL53L0X_write_byte(dev, 0xff, 0x00);
nikapov 0:a1a69d32f310 896 } else if (vcsel_pulse_period_pclk == 10) {
nikapov 0:a1a69d32f310 897
nikapov 0:a1a69d32f310 898 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 899 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 900 0x28);
nikapov 0:a1a69d32f310 901 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 902 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 903 0x08);
nikapov 0:a1a69d32f310 904
nikapov 0:a1a69d32f310 905 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 906 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
nikapov 0:a1a69d32f310 907 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 908 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
nikapov 0:a1a69d32f310 909
nikapov 0:a1a69d32f310 910 status |= VL53L0X_write_byte(dev, 0xff, 0x01);
nikapov 0:a1a69d32f310 911 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 912 VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov 0:a1a69d32f310 913 0x20);
nikapov 0:a1a69d32f310 914 status |= VL53L0X_write_byte(dev, 0xff, 0x00);
nikapov 0:a1a69d32f310 915 } else if (vcsel_pulse_period_pclk == 12) {
nikapov 0:a1a69d32f310 916
nikapov 0:a1a69d32f310 917 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 918 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 919 0x38);
nikapov 0:a1a69d32f310 920 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 921 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 922 0x08);
nikapov 0:a1a69d32f310 923
nikapov 0:a1a69d32f310 924 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 925 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
nikapov 0:a1a69d32f310 926 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 927 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
nikapov 0:a1a69d32f310 928
nikapov 0:a1a69d32f310 929 status |= VL53L0X_write_byte(dev, 0xff, 0x01);
nikapov 0:a1a69d32f310 930 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 931 VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov 0:a1a69d32f310 932 0x20);
nikapov 0:a1a69d32f310 933 status |= VL53L0X_write_byte(dev, 0xff, 0x00);
nikapov 0:a1a69d32f310 934 } else if (vcsel_pulse_period_pclk == 14) {
nikapov 0:a1a69d32f310 935
nikapov 0:a1a69d32f310 936 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 937 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov 0:a1a69d32f310 938 0x048);
nikapov 0:a1a69d32f310 939 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 940 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov 0:a1a69d32f310 941 0x08);
nikapov 0:a1a69d32f310 942
nikapov 0:a1a69d32f310 943 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 944 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
nikapov 0:a1a69d32f310 945 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 946 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
nikapov 0:a1a69d32f310 947
nikapov 0:a1a69d32f310 948 status |= VL53L0X_write_byte(dev, 0xff, 0x01);
nikapov 0:a1a69d32f310 949 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 950 VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov 0:a1a69d32f310 951 0x20);
nikapov 0:a1a69d32f310 952 status |= VL53L0X_write_byte(dev, 0xff, 0x00);
nikapov 0:a1a69d32f310 953 }
nikapov 0:a1a69d32f310 954 }
nikapov 0:a1a69d32f310 955
nikapov 0:a1a69d32f310 956
nikapov 0:a1a69d32f310 957 /* Re-calculate and apply timeouts, in macro periods */
nikapov 0:a1a69d32f310 958
nikapov 0:a1a69d32f310 959 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 960 vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t)
nikapov 0:a1a69d32f310 961 vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 962
nikapov 0:a1a69d32f310 963 /* When the VCSEL period for the pre or final range is changed,
nikapov 0:a1a69d32f310 964 * the corresponding timeout must be read from the device using
nikapov 0:a1a69d32f310 965 * the current VCSEL period, then the new VCSEL period can be
nikapov 0:a1a69d32f310 966 * applied. The timeout then must be written back to the device
nikapov 0:a1a69d32f310 967 * using the new VCSEL period.
nikapov 0:a1a69d32f310 968 *
nikapov 0:a1a69d32f310 969 * For the MSRC timeout, the same applies - this timeout being
nikapov 0:a1a69d32f310 970 * dependant on the pre-range vcsel period.
nikapov 0:a1a69d32f310 971 */
nikapov 0:a1a69d32f310 972 switch (vcsel_period_type) {
nikapov 0:a1a69d32f310 973 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
nikapov 0:a1a69d32f310 974 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 975 VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov 0:a1a69d32f310 976 &pre_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 977
nikapov 0:a1a69d32f310 978 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 979 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 980 VL53L0X_SEQUENCESTEP_MSRC,
nikapov 0:a1a69d32f310 981 &msrc_timeout_micro_seconds);
nikapov 0:a1a69d32f310 982
nikapov 0:a1a69d32f310 983 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 984 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 985 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
nikapov 0:a1a69d32f310 986 vcsel_period_reg);
nikapov 0:a1a69d32f310 987
nikapov 0:a1a69d32f310 988
nikapov 0:a1a69d32f310 989 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 990 status = set_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 991 VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov 0:a1a69d32f310 992 pre_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 993
nikapov 0:a1a69d32f310 994
nikapov 0:a1a69d32f310 995 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 996 status = set_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 997 VL53L0X_SEQUENCESTEP_MSRC,
nikapov 0:a1a69d32f310 998 msrc_timeout_micro_seconds);
nikapov 0:a1a69d32f310 999
nikapov 0:a1a69d32f310 1000 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 1001 dev,
nikapov 0:a1a69d32f310 1002 PreRangeVcselPulsePeriod,
nikapov 0:a1a69d32f310 1003 vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 1004 break;
nikapov 0:a1a69d32f310 1005 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
nikapov 0:a1a69d32f310 1006 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 1007 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov 0:a1a69d32f310 1008 &final_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 1009
nikapov 0:a1a69d32f310 1010 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1011 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 1012 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
nikapov 0:a1a69d32f310 1013 vcsel_period_reg);
nikapov 0:a1a69d32f310 1014
nikapov 0:a1a69d32f310 1015
nikapov 0:a1a69d32f310 1016 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1017 status = set_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 1018 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov 0:a1a69d32f310 1019 final_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 1020
nikapov 0:a1a69d32f310 1021 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 1022 dev,
nikapov 0:a1a69d32f310 1023 FinalRangeVcselPulsePeriod,
nikapov 0:a1a69d32f310 1024 vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 1025 break;
nikapov 0:a1a69d32f310 1026 default:
nikapov 0:a1a69d32f310 1027 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 1028 }
nikapov 0:a1a69d32f310 1029 }
nikapov 0:a1a69d32f310 1030
nikapov 0:a1a69d32f310 1031 /* Finally, the timing budget must be re-applied */
nikapov 0:a1a69d32f310 1032 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1033 VL53L0X_GETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 1034 MeasurementTimingBudgetMicroSeconds,
nikapov 0:a1a69d32f310 1035 measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 1036
nikapov 0:a1a69d32f310 1037 status = VL53L0X_set_measurement_timing_budget_micro_seconds(dev,
nikapov 0:a1a69d32f310 1038 measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 1039 }
nikapov 0:a1a69d32f310 1040
nikapov 0:a1a69d32f310 1041 /* Perform the phase calibration. This is needed after changing on
nikapov 0:a1a69d32f310 1042 * vcsel period.
nikapov 0:a1a69d32f310 1043 * get_data_enable = 0, restore_config = 1 */
nikapov 0:a1a69d32f310 1044 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1045 status = VL53L0X_perform_phase_calibration(
nikapov 0:a1a69d32f310 1046 dev, &phase_cal_int, 0, 1);
nikapov 0:a1a69d32f310 1047
nikapov 0:a1a69d32f310 1048 return status;
nikapov 0:a1a69d32f310 1049 }
nikapov 0:a1a69d32f310 1050
nikapov 0:a1a69d32f310 1051 VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1052 VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period)
nikapov 0:a1a69d32f310 1053 {
nikapov 0:a1a69d32f310 1054 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 1055
nikapov 0:a1a69d32f310 1056
nikapov 0:a1a69d32f310 1057 status = wrapped_VL53L0X_set_vcsel_pulse_period(dev, vcsel_period_type,
nikapov 0:a1a69d32f310 1058 vcsel_pulse_period);
nikapov 0:a1a69d32f310 1059
sepp_nepp 5:b95f6951f7d5 1060
nikapov 0:a1a69d32f310 1061 return status;
nikapov 0:a1a69d32f310 1062 }
nikapov 0:a1a69d32f310 1063
nikapov 0:a1a69d32f310 1064 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1065 VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk)
nikapov 0:a1a69d32f310 1066 {
nikapov 0:a1a69d32f310 1067 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1068 uint8_t vcsel_period_reg;
nikapov 0:a1a69d32f310 1069
nikapov 0:a1a69d32f310 1070 switch (vcsel_period_type) {
nikapov 0:a1a69d32f310 1071 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
nikapov 0:a1a69d32f310 1072 status = VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 1073 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
nikapov 0:a1a69d32f310 1074 &vcsel_period_reg);
nikapov 0:a1a69d32f310 1075 break;
nikapov 0:a1a69d32f310 1076 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
nikapov 0:a1a69d32f310 1077 status = VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 1078 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
nikapov 0:a1a69d32f310 1079 &vcsel_period_reg);
nikapov 0:a1a69d32f310 1080 break;
nikapov 0:a1a69d32f310 1081 default:
nikapov 0:a1a69d32f310 1082 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 1083 }
nikapov 0:a1a69d32f310 1084
nikapov 0:a1a69d32f310 1085 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1086 *p_vcsel_pulse_period_pclk =
nikapov 0:a1a69d32f310 1087 VL53L0X_decode_vcsel_period(vcsel_period_reg);
nikapov 0:a1a69d32f310 1088
nikapov 0:a1a69d32f310 1089 return status;
nikapov 0:a1a69d32f310 1090 }
nikapov 0:a1a69d32f310 1091
nikapov 0:a1a69d32f310 1092 VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1093 VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk)
nikapov 0:a1a69d32f310 1094 {
nikapov 0:a1a69d32f310 1095 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 1096
nikapov 0:a1a69d32f310 1097
nikapov 0:a1a69d32f310 1098 status = wrapped_VL53L0X_get_vcsel_pulse_period(dev, vcsel_period_type,
nikapov 0:a1a69d32f310 1099 p_vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 1100
sepp_nepp 5:b95f6951f7d5 1101
nikapov 0:a1a69d32f310 1102 return status;
nikapov 0:a1a69d32f310 1103 }
nikapov 0:a1a69d32f310 1104
nikapov 0:a1a69d32f310 1105 uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout)
nikapov 0:a1a69d32f310 1106 {
nikapov 0:a1a69d32f310 1107 /*!
nikapov 0:a1a69d32f310 1108 * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
nikapov 0:a1a69d32f310 1109 */
nikapov 0:a1a69d32f310 1110
nikapov 0:a1a69d32f310 1111 uint32_t timeout_macro_clks = 0;
nikapov 0:a1a69d32f310 1112
nikapov 0:a1a69d32f310 1113 timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF)
nikapov 0:a1a69d32f310 1114 << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
nikapov 0:a1a69d32f310 1115
nikapov 0:a1a69d32f310 1116 return timeout_macro_clks;
nikapov 0:a1a69d32f310 1117 }
nikapov 0:a1a69d32f310 1118
nikapov 0:a1a69d32f310 1119 uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(VL53L0X_DEV dev, uint8_t vcsel_period_pclks)
nikapov 0:a1a69d32f310 1120 {
nikapov 0:a1a69d32f310 1121 uint64_t pll_period_ps;
nikapov 0:a1a69d32f310 1122 uint32_t macro_period_vclks;
nikapov 0:a1a69d32f310 1123 uint32_t macro_period_ps;
nikapov 0:a1a69d32f310 1124
sepp_nepp 5:b95f6951f7d5 1125
nikapov 0:a1a69d32f310 1126
nikapov 0:a1a69d32f310 1127 /* The above calculation will produce rounding errors,
nikapov 0:a1a69d32f310 1128 therefore set fixed value
nikapov 0:a1a69d32f310 1129 */
nikapov 0:a1a69d32f310 1130 pll_period_ps = 1655;
nikapov 0:a1a69d32f310 1131
nikapov 0:a1a69d32f310 1132 macro_period_vclks = 2304;
nikapov 0:a1a69d32f310 1133 macro_period_ps = (uint32_t)(macro_period_vclks
nikapov 0:a1a69d32f310 1134 * vcsel_period_pclks * pll_period_ps);
nikapov 0:a1a69d32f310 1135 return macro_period_ps;
nikapov 0:a1a69d32f310 1136 }
nikapov 0:a1a69d32f310 1137
nikapov 0:a1a69d32f310 1138 /* To convert register value into us */
nikapov 0:a1a69d32f310 1139 uint32_t VL53L0X::VL53L0X_calc_timeout_us(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1140 uint16_t timeout_period_mclks,
nikapov 0:a1a69d32f310 1141 uint8_t vcsel_period_pclks)
nikapov 0:a1a69d32f310 1142 {
nikapov 0:a1a69d32f310 1143 uint32_t macro_period_ps;
nikapov 0:a1a69d32f310 1144 uint32_t macro_period_ns;
nikapov 0:a1a69d32f310 1145 uint32_t actual_timeout_period_us = 0;
nikapov 0:a1a69d32f310 1146
nikapov 0:a1a69d32f310 1147 macro_period_ps = VL53L0X_calc_macro_period_ps(dev, vcsel_period_pclks);
nikapov 0:a1a69d32f310 1148 macro_period_ns = (macro_period_ps + 500) / 1000;
nikapov 0:a1a69d32f310 1149
nikapov 0:a1a69d32f310 1150 actual_timeout_period_us =
nikapov 0:a1a69d32f310 1151 ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
nikapov 0:a1a69d32f310 1152
nikapov 0:a1a69d32f310 1153 return actual_timeout_period_us;
nikapov 0:a1a69d32f310 1154 }
nikapov 0:a1a69d32f310 1155
nikapov 0:a1a69d32f310 1156 VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1157 VL53L0X_SequenceStepId sequence_step_id,
nikapov 0:a1a69d32f310 1158 uint32_t *p_time_out_micro_secs)
nikapov 0:a1a69d32f310 1159 {
nikapov 0:a1a69d32f310 1160 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1161 uint8_t current_vcsel_pulse_period_p_clk;
nikapov 0:a1a69d32f310 1162 uint8_t encoded_time_out_byte = 0;
nikapov 0:a1a69d32f310 1163 uint32_t timeout_micro_seconds = 0;
nikapov 0:a1a69d32f310 1164 uint16_t pre_range_encoded_time_out = 0;
nikapov 0:a1a69d32f310 1165 uint16_t msrc_time_out_m_clks;
nikapov 0:a1a69d32f310 1166 uint16_t pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 1167 uint16_t final_range_time_out_m_clks = 0;
nikapov 0:a1a69d32f310 1168 uint16_t final_range_encoded_time_out;
nikapov 0:a1a69d32f310 1169 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov 0:a1a69d32f310 1170
nikapov 0:a1a69d32f310 1171 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
nikapov 0:a1a69d32f310 1172 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
nikapov 0:a1a69d32f310 1173 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
nikapov 0:a1a69d32f310 1174
nikapov 0:a1a69d32f310 1175 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 1176 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 1177 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1178 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1179 status = VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 1180 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
nikapov 0:a1a69d32f310 1181 &encoded_time_out_byte);
nikapov 0:a1a69d32f310 1182 }
nikapov 0:a1a69d32f310 1183 msrc_time_out_m_clks = VL53L0X_decode_timeout(encoded_time_out_byte);
nikapov 0:a1a69d32f310 1184
nikapov 0:a1a69d32f310 1185 timeout_micro_seconds = VL53L0X_calc_timeout_us(dev,
nikapov 0:a1a69d32f310 1186 msrc_time_out_m_clks,
nikapov 0:a1a69d32f310 1187 current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1188 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
nikapov 0:a1a69d32f310 1189 /* Retrieve PRE-RANGE VCSEL Period */
nikapov 0:a1a69d32f310 1190 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 1191 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 1192 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1193
nikapov 0:a1a69d32f310 1194 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
nikapov 0:a1a69d32f310 1195 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1196
nikapov 0:a1a69d32f310 1197 /* Retrieve PRE-RANGE VCSEL Period */
nikapov 0:a1a69d32f310 1198 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 1199 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 1200 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1201
nikapov 0:a1a69d32f310 1202 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1203 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 1204 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov 0:a1a69d32f310 1205 &pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 1206 }
nikapov 0:a1a69d32f310 1207
nikapov 0:a1a69d32f310 1208 pre_range_time_out_m_clks = VL53L0X_decode_timeout(
nikapov 0:a1a69d32f310 1209 pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 1210
nikapov 0:a1a69d32f310 1211 timeout_micro_seconds = VL53L0X_calc_timeout_us(dev,
nikapov 0:a1a69d32f310 1212 pre_range_time_out_m_clks,
nikapov 0:a1a69d32f310 1213 current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1214 }
nikapov 0:a1a69d32f310 1215 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov 0:a1a69d32f310 1216
nikapov 0:a1a69d32f310 1217 VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps);
nikapov 0:a1a69d32f310 1218 pre_range_time_out_m_clks = 0;
nikapov 0:a1a69d32f310 1219
nikapov 0:a1a69d32f310 1220 if (scheduler_sequence_steps.PreRangeOn) {
nikapov 0:a1a69d32f310 1221 /* Retrieve PRE-RANGE VCSEL Period */
nikapov 0:a1a69d32f310 1222 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 1223 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 1224 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1225
nikapov 0:a1a69d32f310 1226 /* Retrieve PRE-RANGE Timeout in Macro periods
nikapov 0:a1a69d32f310 1227 * (MCLKS) */
nikapov 0:a1a69d32f310 1228 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1229 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 1230 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov 0:a1a69d32f310 1231 &pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 1232 pre_range_time_out_m_clks = VL53L0X_decode_timeout(
nikapov 0:a1a69d32f310 1233 pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 1234 }
nikapov 0:a1a69d32f310 1235 }
nikapov 0:a1a69d32f310 1236
nikapov 0:a1a69d32f310 1237 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1238 /* Retrieve FINAL-RANGE VCSEL Period */
nikapov 0:a1a69d32f310 1239 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 1240 VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov 0:a1a69d32f310 1241 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1242 }
nikapov 0:a1a69d32f310 1243
nikapov 0:a1a69d32f310 1244 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
nikapov 0:a1a69d32f310 1245 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1246 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 1247 VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov 0:a1a69d32f310 1248 &final_range_encoded_time_out);
nikapov 0:a1a69d32f310 1249 final_range_time_out_m_clks = VL53L0X_decode_timeout(
nikapov 0:a1a69d32f310 1250 final_range_encoded_time_out);
nikapov 0:a1a69d32f310 1251 }
nikapov 0:a1a69d32f310 1252
nikapov 0:a1a69d32f310 1253 final_range_time_out_m_clks -= pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 1254 timeout_micro_seconds = VL53L0X_calc_timeout_us(dev,
nikapov 0:a1a69d32f310 1255 final_range_time_out_m_clks,
nikapov 0:a1a69d32f310 1256 current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 1257 }
nikapov 0:a1a69d32f310 1258
nikapov 0:a1a69d32f310 1259 *p_time_out_micro_secs = timeout_micro_seconds;
nikapov 0:a1a69d32f310 1260
nikapov 0:a1a69d32f310 1261 return status;
nikapov 0:a1a69d32f310 1262 }
nikapov 0:a1a69d32f310 1263
nikapov 0:a1a69d32f310 1264 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1265 uint32_t *p_measurement_timing_budget_micro_seconds)
nikapov 0:a1a69d32f310 1266 {
nikapov 0:a1a69d32f310 1267 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1268 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov 0:a1a69d32f310 1269 uint32_t final_range_timeout_micro_seconds;
nikapov 0:a1a69d32f310 1270 uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
nikapov 0:a1a69d32f310 1271 uint32_t start_overhead_micro_seconds = 1910;
nikapov 0:a1a69d32f310 1272 uint32_t end_overhead_micro_seconds = 960;
nikapov 0:a1a69d32f310 1273 uint32_t msrc_overhead_micro_seconds = 660;
nikapov 0:a1a69d32f310 1274 uint32_t tcc_overhead_micro_seconds = 590;
nikapov 0:a1a69d32f310 1275 uint32_t dss_overhead_micro_seconds = 690;
nikapov 0:a1a69d32f310 1276 uint32_t pre_range_overhead_micro_seconds = 660;
nikapov 0:a1a69d32f310 1277 uint32_t final_range_overhead_micro_seconds = 550;
nikapov 0:a1a69d32f310 1278 uint32_t pre_range_timeout_micro_seconds = 0;
nikapov 0:a1a69d32f310 1279
sepp_nepp 5:b95f6951f7d5 1280
nikapov 0:a1a69d32f310 1281
nikapov 0:a1a69d32f310 1282 /* Start and end overhead times always present */
nikapov 0:a1a69d32f310 1283 *p_measurement_timing_budget_micro_seconds
nikapov 0:a1a69d32f310 1284 = start_overhead_micro_seconds + end_overhead_micro_seconds;
nikapov 0:a1a69d32f310 1285
nikapov 0:a1a69d32f310 1286 status = VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps);
nikapov 0:a1a69d32f310 1287
nikapov 0:a1a69d32f310 1288 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 5:b95f6951f7d5 1289
nikapov 0:a1a69d32f310 1290 return status;
nikapov 0:a1a69d32f310 1291 }
nikapov 0:a1a69d32f310 1292
nikapov 0:a1a69d32f310 1293
nikapov 0:a1a69d32f310 1294 if (scheduler_sequence_steps.TccOn ||
nikapov 0:a1a69d32f310 1295 scheduler_sequence_steps.MsrcOn ||
nikapov 0:a1a69d32f310 1296 scheduler_sequence_steps.DssOn) {
nikapov 0:a1a69d32f310 1297
nikapov 0:a1a69d32f310 1298 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 1299 VL53L0X_SEQUENCESTEP_MSRC,
nikapov 0:a1a69d32f310 1300 &msrc_dcc_tcc_timeout_micro_seconds);
nikapov 0:a1a69d32f310 1301
nikapov 0:a1a69d32f310 1302 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1303 if (scheduler_sequence_steps.TccOn) {
nikapov 0:a1a69d32f310 1304 *p_measurement_timing_budget_micro_seconds +=
nikapov 0:a1a69d32f310 1305 msrc_dcc_tcc_timeout_micro_seconds +
nikapov 0:a1a69d32f310 1306 tcc_overhead_micro_seconds;
nikapov 0:a1a69d32f310 1307 }
nikapov 0:a1a69d32f310 1308
nikapov 0:a1a69d32f310 1309 if (scheduler_sequence_steps.DssOn) {
nikapov 0:a1a69d32f310 1310 *p_measurement_timing_budget_micro_seconds +=
nikapov 0:a1a69d32f310 1311 2 * (msrc_dcc_tcc_timeout_micro_seconds +
nikapov 0:a1a69d32f310 1312 dss_overhead_micro_seconds);
nikapov 0:a1a69d32f310 1313 } else if (scheduler_sequence_steps.MsrcOn) {
nikapov 0:a1a69d32f310 1314 *p_measurement_timing_budget_micro_seconds +=
nikapov 0:a1a69d32f310 1315 msrc_dcc_tcc_timeout_micro_seconds +
nikapov 0:a1a69d32f310 1316 msrc_overhead_micro_seconds;
nikapov 0:a1a69d32f310 1317 }
nikapov 0:a1a69d32f310 1318 }
nikapov 0:a1a69d32f310 1319 }
nikapov 0:a1a69d32f310 1320
nikapov 0:a1a69d32f310 1321 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1322 if (scheduler_sequence_steps.PreRangeOn) {
nikapov 0:a1a69d32f310 1323 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 1324 VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov 0:a1a69d32f310 1325 &pre_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 1326 *p_measurement_timing_budget_micro_seconds +=
nikapov 0:a1a69d32f310 1327 pre_range_timeout_micro_seconds +
nikapov 0:a1a69d32f310 1328 pre_range_overhead_micro_seconds;
nikapov 0:a1a69d32f310 1329 }
nikapov 0:a1a69d32f310 1330 }
nikapov 0:a1a69d32f310 1331
nikapov 0:a1a69d32f310 1332 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1333 if (scheduler_sequence_steps.FinalRangeOn) {
nikapov 0:a1a69d32f310 1334 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 1335 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov 0:a1a69d32f310 1336 &final_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 1337 *p_measurement_timing_budget_micro_seconds +=
nikapov 0:a1a69d32f310 1338 (final_range_timeout_micro_seconds +
nikapov 0:a1a69d32f310 1339 final_range_overhead_micro_seconds);
nikapov 0:a1a69d32f310 1340 }
nikapov 0:a1a69d32f310 1341 }
nikapov 0:a1a69d32f310 1342
nikapov 0:a1a69d32f310 1343 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1344 VL53L0X_SETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 1345 MeasurementTimingBudgetMicroSeconds,
nikapov 0:a1a69d32f310 1346 *p_measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 1347 }
nikapov 0:a1a69d32f310 1348
sepp_nepp 5:b95f6951f7d5 1349
nikapov 0:a1a69d32f310 1350 return status;
nikapov 0:a1a69d32f310 1351 }
nikapov 0:a1a69d32f310 1352
nikapov 0:a1a69d32f310 1353 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1354 uint32_t *p_measurement_timing_budget_micro_seconds)
nikapov 0:a1a69d32f310 1355 {
nikapov 0:a1a69d32f310 1356 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 1357
nikapov 0:a1a69d32f310 1358
nikapov 0:a1a69d32f310 1359 status = wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(dev,
nikapov 0:a1a69d32f310 1360 p_measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 1361
sepp_nepp 5:b95f6951f7d5 1362
nikapov 0:a1a69d32f310 1363 return status;
nikapov 0:a1a69d32f310 1364 }
nikapov 0:a1a69d32f310 1365
nikapov 0:a1a69d32f310 1366 VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1367 VL53L0X_DeviceParameters_t *p_device_parameters)
nikapov 0:a1a69d32f310 1368 {
nikapov 0:a1a69d32f310 1369 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1370 int i;
nikapov 0:a1a69d32f310 1371
sepp_nepp 5:b95f6951f7d5 1372
nikapov 0:a1a69d32f310 1373
nikapov 0:a1a69d32f310 1374 status = VL53L0X_get_device_mode(dev, &(p_device_parameters->DeviceMode));
nikapov 0:a1a69d32f310 1375
nikapov 0:a1a69d32f310 1376 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1377 status = VL53L0X_get_inter_measurement_period_milli_seconds(dev,
nikapov 0:a1a69d32f310 1378 &(p_device_parameters->InterMeasurementPeriodMilliSeconds));
nikapov 0:a1a69d32f310 1379
nikapov 0:a1a69d32f310 1380
Davidroid 3:e9269ff624ed 1381 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1382 p_device_parameters->XTalkCompensationEnable = 0;
Davidroid 3:e9269ff624ed 1383 }
nikapov 0:a1a69d32f310 1384
nikapov 0:a1a69d32f310 1385 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1386 status = VL53L0X_get_x_talk_compensation_rate_mega_cps(dev,
nikapov 0:a1a69d32f310 1387 &(p_device_parameters->XTalkCompensationRateMegaCps));
nikapov 0:a1a69d32f310 1388
nikapov 0:a1a69d32f310 1389
nikapov 0:a1a69d32f310 1390 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1391 status = VL53L0X_get_offset_calibration_data_micro_meter(dev,
nikapov 0:a1a69d32f310 1392 &(p_device_parameters->RangeOffsetMicroMeters));
nikapov 0:a1a69d32f310 1393
nikapov 0:a1a69d32f310 1394
nikapov 0:a1a69d32f310 1395 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1396 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
nikapov 0:a1a69d32f310 1397 /* get first the values, then the enables.
nikapov 0:a1a69d32f310 1398 * VL53L0X_GetLimitCheckValue will modify the enable
nikapov 0:a1a69d32f310 1399 * flags
nikapov 0:a1a69d32f310 1400 */
nikapov 0:a1a69d32f310 1401 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1402 status |= VL53L0X_get_limit_check_value(dev, i,
nikapov 0:a1a69d32f310 1403 &(p_device_parameters->LimitChecksValue[i]));
nikapov 0:a1a69d32f310 1404 } else {
nikapov 0:a1a69d32f310 1405 break;
nikapov 0:a1a69d32f310 1406 }
nikapov 0:a1a69d32f310 1407 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1408 status |= VL53L0X_get_limit_check_enable(dev, i,
nikapov 0:a1a69d32f310 1409 &(p_device_parameters->LimitChecksEnable[i]));
nikapov 0:a1a69d32f310 1410 } else {
nikapov 0:a1a69d32f310 1411 break;
nikapov 0:a1a69d32f310 1412 }
nikapov 0:a1a69d32f310 1413 }
nikapov 0:a1a69d32f310 1414 }
nikapov 0:a1a69d32f310 1415
nikapov 0:a1a69d32f310 1416 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1417 status = VL53L0X_get_wrap_around_check_enable(dev,
nikapov 0:a1a69d32f310 1418 &(p_device_parameters->WrapAroundCheckEnable));
nikapov 0:a1a69d32f310 1419 }
nikapov 0:a1a69d32f310 1420
nikapov 0:a1a69d32f310 1421 /* Need to be done at the end as it uses VCSELPulsePeriod */
nikapov 0:a1a69d32f310 1422 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1423 status = VL53L0X_get_measurement_timing_budget_micro_seconds(dev,
nikapov 0:a1a69d32f310 1424 &(p_device_parameters->MeasurementTimingBudgetMicroSeconds));
nikapov 0:a1a69d32f310 1425 }
nikapov 0:a1a69d32f310 1426
sepp_nepp 5:b95f6951f7d5 1427
nikapov 0:a1a69d32f310 1428 return status;
nikapov 0:a1a69d32f310 1429 }
nikapov 0:a1a69d32f310 1430
nikapov 0:a1a69d32f310 1431 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value(VL53L0X_DEV dev, uint16_t limit_check_id,
nikapov 0:a1a69d32f310 1432 FixPoint1616_t limit_check_value)
nikapov 0:a1a69d32f310 1433 {
nikapov 0:a1a69d32f310 1434 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1435 uint8_t temp8;
nikapov 0:a1a69d32f310 1436
sepp_nepp 5:b95f6951f7d5 1437
nikapov 0:a1a69d32f310 1438
nikapov 0:a1a69d32f310 1439 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksEnable, limit_check_id,
nikapov 0:a1a69d32f310 1440 temp8);
nikapov 0:a1a69d32f310 1441
nikapov 0:a1a69d32f310 1442 if (temp8 == 0) { /* disabled write only internal value */
nikapov 0:a1a69d32f310 1443 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 1444 limit_check_id, limit_check_value);
nikapov 0:a1a69d32f310 1445 } else {
nikapov 0:a1a69d32f310 1446
nikapov 0:a1a69d32f310 1447 switch (limit_check_id) {
nikapov 0:a1a69d32f310 1448
nikapov 0:a1a69d32f310 1449 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov 0:a1a69d32f310 1450 /* internal computation: */
nikapov 0:a1a69d32f310 1451 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 1452 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov 0:a1a69d32f310 1453 limit_check_value);
nikapov 0:a1a69d32f310 1454 break;
nikapov 0:a1a69d32f310 1455
nikapov 0:a1a69d32f310 1456 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
nikapov 0:a1a69d32f310 1457
nikapov 0:a1a69d32f310 1458 status = VL53L0X_write_word(dev,
nikapov 0:a1a69d32f310 1459 VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
nikapov 0:a1a69d32f310 1460 VL53L0X_FIXPOINT1616TOFIXPOINT97(
nikapov 0:a1a69d32f310 1461 limit_check_value));
nikapov 0:a1a69d32f310 1462
nikapov 0:a1a69d32f310 1463 break;
nikapov 0:a1a69d32f310 1464
nikapov 0:a1a69d32f310 1465 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov 0:a1a69d32f310 1466
nikapov 0:a1a69d32f310 1467 /* internal computation: */
nikapov 0:a1a69d32f310 1468 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 1469 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov 0:a1a69d32f310 1470 limit_check_value);
nikapov 0:a1a69d32f310 1471
nikapov 0:a1a69d32f310 1472 break;
nikapov 0:a1a69d32f310 1473
nikapov 0:a1a69d32f310 1474 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov 0:a1a69d32f310 1475
nikapov 0:a1a69d32f310 1476 /* internal computation: */
nikapov 0:a1a69d32f310 1477 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 1478 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov 0:a1a69d32f310 1479 limit_check_value);
nikapov 0:a1a69d32f310 1480
nikapov 0:a1a69d32f310 1481 break;
nikapov 0:a1a69d32f310 1482
nikapov 0:a1a69d32f310 1483 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov 0:a1a69d32f310 1484 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
nikapov 0:a1a69d32f310 1485
nikapov 0:a1a69d32f310 1486 status = VL53L0X_write_word(dev,
nikapov 0:a1a69d32f310 1487 VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
nikapov 0:a1a69d32f310 1488 VL53L0X_FIXPOINT1616TOFIXPOINT97(
nikapov 0:a1a69d32f310 1489 limit_check_value));
nikapov 0:a1a69d32f310 1490
nikapov 0:a1a69d32f310 1491 break;
nikapov 0:a1a69d32f310 1492
nikapov 0:a1a69d32f310 1493 default:
nikapov 0:a1a69d32f310 1494 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 1495
nikapov 0:a1a69d32f310 1496 }
nikapov 0:a1a69d32f310 1497
nikapov 0:a1a69d32f310 1498 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1499 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 1500 limit_check_id, limit_check_value);
nikapov 0:a1a69d32f310 1501 }
nikapov 0:a1a69d32f310 1502 }
nikapov 0:a1a69d32f310 1503
sepp_nepp 5:b95f6951f7d5 1504
nikapov 0:a1a69d32f310 1505 return status;
nikapov 0:a1a69d32f310 1506 }
nikapov 0:a1a69d32f310 1507
nikapov 0:a1a69d32f310 1508 VL53L0X_Error VL53L0X::VL53L0X_data_init(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 1509 {
nikapov 0:a1a69d32f310 1510 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1511 VL53L0X_DeviceParameters_t CurrentParameters;
nikapov 0:a1a69d32f310 1512 int i;
nikapov 0:a1a69d32f310 1513 uint8_t StopVariable;
nikapov 0:a1a69d32f310 1514
sepp_nepp 5:b95f6951f7d5 1515
nikapov 0:a1a69d32f310 1516
nikapov 0:a1a69d32f310 1517 /* by default the I2C is running at 1V8 if you want to change it you
nikapov 0:a1a69d32f310 1518 * need to include this define at compilation level. */
nikapov 0:a1a69d32f310 1519 #ifdef USE_I2C_2V8
nikapov 0:a1a69d32f310 1520 Status = VL53L0X_UpdateByte(Dev,
nikapov 0:a1a69d32f310 1521 VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
nikapov 0:a1a69d32f310 1522 0xFE,
nikapov 0:a1a69d32f310 1523 0x01);
nikapov 0:a1a69d32f310 1524 #endif
nikapov 0:a1a69d32f310 1525
nikapov 0:a1a69d32f310 1526 /* Set I2C standard mode */
Davidroid 3:e9269ff624ed 1527 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1528 status = VL53L0X_write_byte(dev, 0x88, 0x00);
Davidroid 3:e9269ff624ed 1529 }
nikapov 0:a1a69d32f310 1530
nikapov 0:a1a69d32f310 1531 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone, 0);
nikapov 0:a1a69d32f310 1532
nikapov 0:a1a69d32f310 1533 #ifdef USE_IQC_STATION
Davidroid 3:e9269ff624ed 1534 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1535 Status = VL53L0X_apply_offset_adjustment(Dev);
Davidroid 3:e9269ff624ed 1536 }
nikapov 0:a1a69d32f310 1537 #endif
nikapov 0:a1a69d32f310 1538
nikapov 0:a1a69d32f310 1539 /* Default value is 1000 for Linearity Corrective Gain */
nikapov 0:a1a69d32f310 1540 PALDevDataSet(dev, LinearityCorrectiveGain, 1000);
nikapov 0:a1a69d32f310 1541
nikapov 0:a1a69d32f310 1542 /* Dmax default Parameter */
nikapov 0:a1a69d32f310 1543 PALDevDataSet(dev, DmaxCalRangeMilliMeter, 400);
nikapov 0:a1a69d32f310 1544 PALDevDataSet(dev, DmaxCalSignalRateRtnMegaCps,
nikapov 0:a1a69d32f310 1545 (FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/
nikapov 0:a1a69d32f310 1546
nikapov 0:a1a69d32f310 1547 /* Set Default static parameters
nikapov 0:a1a69d32f310 1548 *set first temporary values 9.44MHz * 65536 = 618660 */
nikapov 0:a1a69d32f310 1549 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz, 618660);
nikapov 0:a1a69d32f310 1550
nikapov 0:a1a69d32f310 1551 /* Set Default XTalkCompensationRateMegaCps to 0 */
nikapov 0:a1a69d32f310 1552 VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps, 0);
nikapov 0:a1a69d32f310 1553
nikapov 0:a1a69d32f310 1554 /* Get default parameters */
nikapov 0:a1a69d32f310 1555 status = VL53L0X_get_device_parameters(dev, &CurrentParameters);
nikapov 0:a1a69d32f310 1556 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1557 /* initialize PAL values */
nikapov 0:a1a69d32f310 1558 CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
nikapov 0:a1a69d32f310 1559 CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
nikapov 0:a1a69d32f310 1560 PALDevDataSet(dev, CurrentParameters, CurrentParameters);
nikapov 0:a1a69d32f310 1561 }
nikapov 0:a1a69d32f310 1562
nikapov 0:a1a69d32f310 1563 /* Sigma estimator variable */
nikapov 0:a1a69d32f310 1564 PALDevDataSet(dev, SigmaEstRefArray, 100);
nikapov 0:a1a69d32f310 1565 PALDevDataSet(dev, SigmaEstEffPulseWidth, 900);
nikapov 0:a1a69d32f310 1566 PALDevDataSet(dev, SigmaEstEffAmbWidth, 500);
nikapov 0:a1a69d32f310 1567 PALDevDataSet(dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */
nikapov 0:a1a69d32f310 1568
nikapov 0:a1a69d32f310 1569 /* Use internal default settings */
nikapov 0:a1a69d32f310 1570 PALDevDataSet(dev, UseInternalTuningSettings, 1);
nikapov 0:a1a69d32f310 1571
nikapov 0:a1a69d32f310 1572 status |= VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov 0:a1a69d32f310 1573 status |= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 1574 status |= VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov 0:a1a69d32f310 1575 status |= VL53L0X_read_byte(dev, 0x91, &StopVariable);
nikapov 0:a1a69d32f310 1576 PALDevDataSet(dev, StopVariable, StopVariable);
nikapov 0:a1a69d32f310 1577 status |= VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov 0:a1a69d32f310 1578 status |= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 1579 status |= VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov 0:a1a69d32f310 1580
nikapov 0:a1a69d32f310 1581 /* Enable all check */
nikapov 0:a1a69d32f310 1582 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
Davidroid 3:e9269ff624ed 1583 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1584 status |= VL53L0X_set_limit_check_enable(dev, i, 1);
Davidroid 3:e9269ff624ed 1585 } else {
nikapov 0:a1a69d32f310 1586 break;
Davidroid 3:e9269ff624ed 1587 }
nikapov 0:a1a69d32f310 1588
nikapov 0:a1a69d32f310 1589 }
nikapov 0:a1a69d32f310 1590
nikapov 0:a1a69d32f310 1591 /* Disable the following checks */
nikapov 0:a1a69d32f310 1592 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1593 status = VL53L0X_set_limit_check_enable(dev,
nikapov 0:a1a69d32f310 1594 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
nikapov 0:a1a69d32f310 1595
nikapov 0:a1a69d32f310 1596 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1597 status = VL53L0X_set_limit_check_enable(dev,
nikapov 0:a1a69d32f310 1598 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
nikapov 0:a1a69d32f310 1599
nikapov 0:a1a69d32f310 1600 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1601 status = VL53L0X_set_limit_check_enable(dev,
nikapov 0:a1a69d32f310 1602 VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
nikapov 0:a1a69d32f310 1603
nikapov 0:a1a69d32f310 1604 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 1605 status = VL53L0X_set_limit_check_enable(dev,
nikapov 0:a1a69d32f310 1606 VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
nikapov 0:a1a69d32f310 1607
nikapov 0:a1a69d32f310 1608 /* Limit default values */
nikapov 0:a1a69d32f310 1609 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1610 status = VL53L0X_set_limit_check_value(dev,
nikapov 0:a1a69d32f310 1611 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov 0:a1a69d32f310 1612 (FixPoint1616_t)(18 * 65536));
nikapov 0:a1a69d32f310 1613 }
nikapov 0:a1a69d32f310 1614 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1615 status = VL53L0X_set_limit_check_value(dev,
nikapov 0:a1a69d32f310 1616 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov 0:a1a69d32f310 1617 (FixPoint1616_t)(25 * 65536 / 100));
nikapov 0:a1a69d32f310 1618 /* 0.25 * 65536 */
nikapov 0:a1a69d32f310 1619 }
nikapov 0:a1a69d32f310 1620
nikapov 0:a1a69d32f310 1621 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1622 status = VL53L0X_set_limit_check_value(dev,
nikapov 0:a1a69d32f310 1623 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov 0:a1a69d32f310 1624 (FixPoint1616_t)(35 * 65536));
nikapov 0:a1a69d32f310 1625 }
nikapov 0:a1a69d32f310 1626
nikapov 0:a1a69d32f310 1627 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1628 status = VL53L0X_set_limit_check_value(dev,
nikapov 0:a1a69d32f310 1629 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov 0:a1a69d32f310 1630 (FixPoint1616_t)(0 * 65536));
nikapov 0:a1a69d32f310 1631 }
nikapov 0:a1a69d32f310 1632
nikapov 0:a1a69d32f310 1633 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1634
nikapov 0:a1a69d32f310 1635 PALDevDataSet(dev, SequenceConfig, 0xFF);
nikapov 0:a1a69d32f310 1636 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 1637 0xFF);
nikapov 0:a1a69d32f310 1638
nikapov 0:a1a69d32f310 1639 /* Set PAL state to tell that we are waiting for call to
nikapov 0:a1a69d32f310 1640 * VL53L0X_StaticInit */
nikapov 0:a1a69d32f310 1641 PALDevDataSet(dev, PalState, VL53L0X_STATE_WAIT_STATICINIT);
nikapov 0:a1a69d32f310 1642 }
nikapov 0:a1a69d32f310 1643
Davidroid 3:e9269ff624ed 1644 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1645 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 0);
Davidroid 3:e9269ff624ed 1646 }
nikapov 0:a1a69d32f310 1647
nikapov 0:a1a69d32f310 1648
sepp_nepp 5:b95f6951f7d5 1649
nikapov 0:a1a69d32f310 1650 return status;
nikapov 0:a1a69d32f310 1651 }
nikapov 0:a1a69d32f310 1652
nikapov 0:a1a69d32f310 1653 VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1654 uint8_t *revision,
nikapov 0:a1a69d32f310 1655 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov 0:a1a69d32f310 1656 {
nikapov 0:a1a69d32f310 1657 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1658 uint8_t module_id_int;
nikapov 0:a1a69d32f310 1659 char *product_id_tmp;
nikapov 0:a1a69d32f310 1660
sepp_nepp 5:b95f6951f7d5 1661
nikapov 0:a1a69d32f310 1662
nikapov 0:a1a69d32f310 1663 status = VL53L0X_get_info_from_device(dev, 2);
nikapov 0:a1a69d32f310 1664
nikapov 0:a1a69d32f310 1665 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1666 module_id_int = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, ModuleId);
nikapov 0:a1a69d32f310 1667
nikapov 0:a1a69d32f310 1668 if (module_id_int == 0) {
nikapov 0:a1a69d32f310 1669 *revision = 0;
sepp_nepp 5:b95f6951f7d5 1670 strcpy(p_VL53L0X_device_info->ProductId, "");
nikapov 0:a1a69d32f310 1671 } else {
nikapov 0:a1a69d32f310 1672 *revision = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, Revision);
nikapov 0:a1a69d32f310 1673 product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 1674 ProductId);
sepp_nepp 5:b95f6951f7d5 1675 strcpy(p_VL53L0X_device_info->ProductId, product_id_tmp);
nikapov 0:a1a69d32f310 1676 }
nikapov 0:a1a69d32f310 1677 }
nikapov 0:a1a69d32f310 1678
sepp_nepp 5:b95f6951f7d5 1679
nikapov 0:a1a69d32f310 1680 return status;
nikapov 0:a1a69d32f310 1681 }
nikapov 0:a1a69d32f310 1682
nikapov 0:a1a69d32f310 1683 VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_device_info(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1684 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov 0:a1a69d32f310 1685 {
nikapov 0:a1a69d32f310 1686 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1687 uint8_t revision_id;
nikapov 0:a1a69d32f310 1688 uint8_t revision;
nikapov 0:a1a69d32f310 1689
nikapov 0:a1a69d32f310 1690 status = VL53L0X_check_part_used(dev, &revision, p_VL53L0X_device_info);
nikapov 0:a1a69d32f310 1691
nikapov 0:a1a69d32f310 1692 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1693 if (revision == 0) {
sepp_nepp 5:b95f6951f7d5 1694 strcpy(p_VL53L0X_device_info->Name,
nikapov 0:a1a69d32f310 1695 VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
nikapov 0:a1a69d32f310 1696 } else if ((revision <= 34) && (revision != 32)) {
sepp_nepp 5:b95f6951f7d5 1697 strcpy(p_VL53L0X_device_info->Name,
nikapov 0:a1a69d32f310 1698 VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
nikapov 0:a1a69d32f310 1699 } else if (revision < 39) {
sepp_nepp 5:b95f6951f7d5 1700 strcpy(p_VL53L0X_device_info->Name,
nikapov 0:a1a69d32f310 1701 VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
nikapov 0:a1a69d32f310 1702 } else {
sepp_nepp 5:b95f6951f7d5 1703 strcpy(p_VL53L0X_device_info->Name,
nikapov 0:a1a69d32f310 1704 VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
nikapov 0:a1a69d32f310 1705 }
nikapov 0:a1a69d32f310 1706
sepp_nepp 5:b95f6951f7d5 1707 strcpy(p_VL53L0X_device_info->Type,
nikapov 0:a1a69d32f310 1708 VL53L0X_STRING_DEVICE_INFO_TYPE);
nikapov 0:a1a69d32f310 1709
nikapov 0:a1a69d32f310 1710 }
nikapov 0:a1a69d32f310 1711
nikapov 0:a1a69d32f310 1712 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1713 status = VL53L0X_read_byte(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
nikapov 0:a1a69d32f310 1714 &p_VL53L0X_device_info->ProductType);
nikapov 0:a1a69d32f310 1715 }
nikapov 0:a1a69d32f310 1716
nikapov 0:a1a69d32f310 1717 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1718 status = VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 1719 VL53L0X_REG_IDENTIFICATION_REVISION_ID,
nikapov 0:a1a69d32f310 1720 &revision_id);
nikapov 0:a1a69d32f310 1721 p_VL53L0X_device_info->ProductRevisionMajor = 1;
nikapov 0:a1a69d32f310 1722 p_VL53L0X_device_info->ProductRevisionMinor =
nikapov 0:a1a69d32f310 1723 (revision_id & 0xF0) >> 4;
nikapov 0:a1a69d32f310 1724 }
nikapov 0:a1a69d32f310 1725
nikapov 0:a1a69d32f310 1726 return status;
nikapov 0:a1a69d32f310 1727 }
nikapov 0:a1a69d32f310 1728
nikapov 0:a1a69d32f310 1729 VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1730 VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov 0:a1a69d32f310 1731 {
nikapov 0:a1a69d32f310 1732 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1733
nikapov 0:a1a69d32f310 1734 status = wrapped_VL53L0X_get_device_info(dev, p_VL53L0X_device_info);
nikapov 0:a1a69d32f310 1735
nikapov 0:a1a69d32f310 1736 return status;
nikapov 0:a1a69d32f310 1737 }
nikapov 0:a1a69d32f310 1738
nikapov 0:a1a69d32f310 1739 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1740 uint32_t *p_interrupt_mask_status)
nikapov 0:a1a69d32f310 1741 {
nikapov 0:a1a69d32f310 1742 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1743 uint8_t byte;
sepp_nepp 5:b95f6951f7d5 1744
nikapov 0:a1a69d32f310 1745
nikapov 0:a1a69d32f310 1746 status = VL53L0X_read_byte(dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
nikapov 0:a1a69d32f310 1747 *p_interrupt_mask_status = byte & 0x07;
nikapov 0:a1a69d32f310 1748
nikapov 0:a1a69d32f310 1749 if (byte & 0x18) {
nikapov 0:a1a69d32f310 1750 status = VL53L0X_ERROR_RANGE_ERROR;
nikapov 0:a1a69d32f310 1751 }
nikapov 0:a1a69d32f310 1752
sepp_nepp 5:b95f6951f7d5 1753
nikapov 0:a1a69d32f310 1754 return status;
nikapov 0:a1a69d32f310 1755 }
nikapov 0:a1a69d32f310 1756
nikapov 0:a1a69d32f310 1757 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1758 uint8_t *p_measurement_data_ready)
nikapov 0:a1a69d32f310 1759 {
nikapov 0:a1a69d32f310 1760 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1761 uint8_t sys_range_status_register;
nikapov 0:a1a69d32f310 1762 uint8_t interrupt_config;
nikapov 0:a1a69d32f310 1763 uint32_t interrupt_mask;
sepp_nepp 5:b95f6951f7d5 1764
nikapov 0:a1a69d32f310 1765
nikapov 0:a1a69d32f310 1766 interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 1767 Pin0GpioFunctionality);
nikapov 0:a1a69d32f310 1768
nikapov 0:a1a69d32f310 1769 if (interrupt_config ==
nikapov 0:a1a69d32f310 1770 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
nikapov 0:a1a69d32f310 1771 status = VL53L0X_get_interrupt_mask_status(dev, &interrupt_mask);
nikapov 0:a1a69d32f310 1772 if (interrupt_mask ==
nikapov 0:a1a69d32f310 1773 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
nikapov 0:a1a69d32f310 1774 *p_measurement_data_ready = 1;
nikapov 0:a1a69d32f310 1775 } else {
nikapov 0:a1a69d32f310 1776 *p_measurement_data_ready = 0;
nikapov 0:a1a69d32f310 1777 }
nikapov 0:a1a69d32f310 1778 } else {
nikapov 0:a1a69d32f310 1779 status = VL53L0X_read_byte(dev, VL53L0X_REG_RESULT_RANGE_STATUS,
nikapov 0:a1a69d32f310 1780 &sys_range_status_register);
nikapov 0:a1a69d32f310 1781 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1782 if (sys_range_status_register & 0x01) {
nikapov 0:a1a69d32f310 1783 *p_measurement_data_ready = 1;
nikapov 0:a1a69d32f310 1784 } else {
nikapov 0:a1a69d32f310 1785 *p_measurement_data_ready = 0;
nikapov 0:a1a69d32f310 1786 }
nikapov 0:a1a69d32f310 1787 }
nikapov 0:a1a69d32f310 1788 }
nikapov 0:a1a69d32f310 1789
sepp_nepp 5:b95f6951f7d5 1790
nikapov 0:a1a69d32f310 1791 return status;
nikapov 0:a1a69d32f310 1792 }
nikapov 0:a1a69d32f310 1793
nikapov 0:a1a69d32f310 1794 VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 1795 {
nikapov 0:a1a69d32f310 1796 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1797 uint8_t new_data_ready = 0;
nikapov 0:a1a69d32f310 1798 uint32_t loop_nb;
nikapov 0:a1a69d32f310 1799
nikapov 0:a1a69d32f310 1800 loop_nb = 0;
sepp_nepp 5:b95f6951f7d5 1801
sepp_nepp 5:b95f6951f7d5 1802 status = VL53L0X_get_measurement_data_ready(dev, &new_data_ready);
sepp_nepp 5:b95f6951f7d5 1803
sepp_nepp 5:b95f6951f7d5 1804 while ( (status==0) && (new_data_ready != 1) &&
sepp_nepp 5:b95f6951f7d5 1805 (loop_nb < VL53L0X_DEFAULT_MAX_LOOP) )
sepp_nepp 5:b95f6951f7d5 1806 {
sepp_nepp 5:b95f6951f7d5 1807 VL53L0X_polling_delay(dev);
nikapov 0:a1a69d32f310 1808 status = VL53L0X_get_measurement_data_ready(dev, &new_data_ready);
sepp_nepp 5:b95f6951f7d5 1809 loop_nb++;
sepp_nepp 5:b95f6951f7d5 1810 } // while ;
sepp_nepp 5:b95f6951f7d5 1811
sepp_nepp 5:b95f6951f7d5 1812 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp 5:b95f6951f7d5 1813
nikapov 0:a1a69d32f310 1814 return status;
nikapov 0:a1a69d32f310 1815 }
nikapov 0:a1a69d32f310 1816
nikapov 0:a1a69d32f310 1817 /* Group PAL Interrupt Functions */
nikapov 0:a1a69d32f310 1818 VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask(VL53L0X_DEV dev, uint32_t interrupt_mask)
nikapov 0:a1a69d32f310 1819 {
nikapov 0:a1a69d32f310 1820 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1821 uint8_t loop_count;
nikapov 0:a1a69d32f310 1822 uint8_t byte;
sepp_nepp 5:b95f6951f7d5 1823
nikapov 0:a1a69d32f310 1824
nikapov 0:a1a69d32f310 1825 /* clear bit 0 range interrupt, bit 1 error interrupt */
nikapov 0:a1a69d32f310 1826 loop_count = 0;
nikapov 0:a1a69d32f310 1827 do {
nikapov 0:a1a69d32f310 1828 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 1829 VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
nikapov 0:a1a69d32f310 1830 status |= VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 1831 VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
nikapov 0:a1a69d32f310 1832 status |= VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 1833 VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
nikapov 0:a1a69d32f310 1834 loop_count++;
nikapov 0:a1a69d32f310 1835 } while (((byte & 0x07) != 0x00)
nikapov 0:a1a69d32f310 1836 && (loop_count < 3)
nikapov 0:a1a69d32f310 1837 && (status == VL53L0X_ERROR_NONE));
nikapov 0:a1a69d32f310 1838
nikapov 0:a1a69d32f310 1839
nikapov 0:a1a69d32f310 1840 if (loop_count >= 3) {
nikapov 0:a1a69d32f310 1841 status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
nikapov 0:a1a69d32f310 1842 }
nikapov 0:a1a69d32f310 1843
sepp_nepp 5:b95f6951f7d5 1844
nikapov 0:a1a69d32f310 1845 return status;
nikapov 0:a1a69d32f310 1846 }
nikapov 0:a1a69d32f310 1847
nikapov 0:a1a69d32f310 1848 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1849 uint8_t vhv_init_byte)
nikapov 0:a1a69d32f310 1850 {
nikapov 0:a1a69d32f310 1851 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1852
nikapov 0:a1a69d32f310 1853 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1854 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START,
nikapov 0:a1a69d32f310 1855 VL53L0X_REG_SYSRANGE_MODE_START_STOP |
nikapov 0:a1a69d32f310 1856 vhv_init_byte);
nikapov 0:a1a69d32f310 1857 }
nikapov 0:a1a69d32f310 1858
nikapov 0:a1a69d32f310 1859 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1860 status = VL53L0X_measurement_poll_for_completion(dev);
nikapov 0:a1a69d32f310 1861 }
nikapov 0:a1a69d32f310 1862
nikapov 0:a1a69d32f310 1863 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1864 status = VL53L0X_clear_interrupt_mask(dev, 0);
nikapov 0:a1a69d32f310 1865 }
nikapov 0:a1a69d32f310 1866
nikapov 0:a1a69d32f310 1867 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1868 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x00);
nikapov 0:a1a69d32f310 1869 }
nikapov 0:a1a69d32f310 1870
nikapov 0:a1a69d32f310 1871 return status;
nikapov 0:a1a69d32f310 1872 }
nikapov 0:a1a69d32f310 1873
nikapov 0:a1a69d32f310 1874 VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(VL53L0X_DEV dev, uint8_t read_not_write,
nikapov 0:a1a69d32f310 1875 uint8_t vhv_settings, uint8_t phase_cal,
nikapov 0:a1a69d32f310 1876 uint8_t *p_vhv_settings, uint8_t *p_phase_cal,
nikapov 0:a1a69d32f310 1877 const uint8_t vhv_enable, const uint8_t phase_enable)
nikapov 0:a1a69d32f310 1878 {
nikapov 0:a1a69d32f310 1879 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1880 uint8_t phase_calint = 0;
nikapov 0:a1a69d32f310 1881
nikapov 0:a1a69d32f310 1882 /* Read VHV from device */
nikapov 0:a1a69d32f310 1883 status |= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 1884 status |= VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov 0:a1a69d32f310 1885 status |= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 1886
nikapov 0:a1a69d32f310 1887 if (read_not_write) {
nikapov 0:a1a69d32f310 1888 if (vhv_enable) {
nikapov 0:a1a69d32f310 1889 status |= VL53L0X_read_byte(dev, 0xCB, p_vhv_settings);
nikapov 0:a1a69d32f310 1890 }
nikapov 0:a1a69d32f310 1891 if (phase_enable) {
nikapov 0:a1a69d32f310 1892 status |= VL53L0X_read_byte(dev, 0xEE, &phase_calint);
nikapov 0:a1a69d32f310 1893 }
nikapov 0:a1a69d32f310 1894 } else {
nikapov 0:a1a69d32f310 1895 if (vhv_enable) {
nikapov 0:a1a69d32f310 1896 status |= VL53L0X_write_byte(dev, 0xCB, vhv_settings);
nikapov 0:a1a69d32f310 1897 }
nikapov 0:a1a69d32f310 1898 if (phase_enable) {
nikapov 0:a1a69d32f310 1899 status |= VL53L0X_update_byte(dev, 0xEE, 0x80, phase_cal);
nikapov 0:a1a69d32f310 1900 }
nikapov 0:a1a69d32f310 1901 }
nikapov 0:a1a69d32f310 1902
nikapov 0:a1a69d32f310 1903 status |= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 1904 status |= VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov 0:a1a69d32f310 1905 status |= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 1906
nikapov 0:a1a69d32f310 1907 *p_phase_cal = (uint8_t)(phase_calint & 0xEF);
nikapov 0:a1a69d32f310 1908
nikapov 0:a1a69d32f310 1909 return status;
nikapov 0:a1a69d32f310 1910 }
nikapov 0:a1a69d32f310 1911
nikapov 0:a1a69d32f310 1912 VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1913 uint8_t *p_vhv_settings, const uint8_t get_data_enable,
nikapov 0:a1a69d32f310 1914 const uint8_t restore_config)
nikapov 0:a1a69d32f310 1915 {
nikapov 0:a1a69d32f310 1916 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1917 uint8_t sequence_config = 0;
nikapov 0:a1a69d32f310 1918 uint8_t vhv_settings = 0;
nikapov 0:a1a69d32f310 1919 uint8_t phase_cal = 0;
nikapov 0:a1a69d32f310 1920 uint8_t phase_cal_int = 0;
nikapov 0:a1a69d32f310 1921
nikapov 0:a1a69d32f310 1922 /* store the value of the sequence config,
nikapov 0:a1a69d32f310 1923 * this will be reset before the end of the function
nikapov 0:a1a69d32f310 1924 */
nikapov 0:a1a69d32f310 1925
nikapov 0:a1a69d32f310 1926 if (restore_config) {
nikapov 0:a1a69d32f310 1927 sequence_config = PALDevDataGet(dev, SequenceConfig);
nikapov 0:a1a69d32f310 1928 }
nikapov 0:a1a69d32f310 1929
nikapov 0:a1a69d32f310 1930 /* Run VHV */
nikapov 0:a1a69d32f310 1931 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
nikapov 0:a1a69d32f310 1932
nikapov 0:a1a69d32f310 1933 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1934 status = VL53L0X_perform_single_ref_calibration(dev, 0x40);
nikapov 0:a1a69d32f310 1935 }
nikapov 0:a1a69d32f310 1936
nikapov 0:a1a69d32f310 1937 /* Read VHV from device */
nikapov 0:a1a69d32f310 1938 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
nikapov 0:a1a69d32f310 1939 status = VL53L0X_ref_calibration_io(dev, 1,
nikapov 0:a1a69d32f310 1940 vhv_settings, phase_cal, /* Not used here */
nikapov 0:a1a69d32f310 1941 p_vhv_settings, &phase_cal_int,
nikapov 0:a1a69d32f310 1942 1, 0);
nikapov 0:a1a69d32f310 1943 } else {
nikapov 0:a1a69d32f310 1944 *p_vhv_settings = 0;
nikapov 0:a1a69d32f310 1945 }
nikapov 0:a1a69d32f310 1946
nikapov 0:a1a69d32f310 1947
nikapov 0:a1a69d32f310 1948 if ((status == VL53L0X_ERROR_NONE) && restore_config) {
nikapov 0:a1a69d32f310 1949 /* restore the previous Sequence Config */
nikapov 0:a1a69d32f310 1950 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 1951 sequence_config);
nikapov 0:a1a69d32f310 1952 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1953 PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov 0:a1a69d32f310 1954 }
nikapov 0:a1a69d32f310 1955
nikapov 0:a1a69d32f310 1956 }
nikapov 0:a1a69d32f310 1957
nikapov 0:a1a69d32f310 1958 return status;
nikapov 0:a1a69d32f310 1959 }
nikapov 0:a1a69d32f310 1960
nikapov 0:a1a69d32f310 1961 VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 1962 uint8_t *p_phase_cal, const uint8_t get_data_enable,
nikapov 0:a1a69d32f310 1963 const uint8_t restore_config)
nikapov 0:a1a69d32f310 1964 {
nikapov 0:a1a69d32f310 1965 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 1966 uint8_t sequence_config = 0;
nikapov 0:a1a69d32f310 1967 uint8_t vhv_settings = 0;
nikapov 0:a1a69d32f310 1968 uint8_t phase_cal = 0;
nikapov 0:a1a69d32f310 1969 uint8_t vhv_settingsint;
nikapov 0:a1a69d32f310 1970
nikapov 0:a1a69d32f310 1971 /* store the value of the sequence config,
nikapov 0:a1a69d32f310 1972 * this will be reset before the end of the function
nikapov 0:a1a69d32f310 1973 */
nikapov 0:a1a69d32f310 1974
Davidroid 3:e9269ff624ed 1975 if (restore_config) {
nikapov 0:a1a69d32f310 1976 sequence_config = PALDevDataGet(dev, SequenceConfig);
Davidroid 3:e9269ff624ed 1977 }
nikapov 0:a1a69d32f310 1978
nikapov 0:a1a69d32f310 1979 /* Run PhaseCal */
nikapov 0:a1a69d32f310 1980 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
nikapov 0:a1a69d32f310 1981
nikapov 0:a1a69d32f310 1982 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 1983 status = VL53L0X_perform_single_ref_calibration(dev, 0x0);
nikapov 0:a1a69d32f310 1984 }
nikapov 0:a1a69d32f310 1985
nikapov 0:a1a69d32f310 1986 /* Read PhaseCal from device */
nikapov 0:a1a69d32f310 1987 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
nikapov 0:a1a69d32f310 1988 status = VL53L0X_ref_calibration_io(dev, 1,
nikapov 0:a1a69d32f310 1989 vhv_settings, phase_cal, /* Not used here */
nikapov 0:a1a69d32f310 1990 &vhv_settingsint, p_phase_cal,
nikapov 0:a1a69d32f310 1991 0, 1);
nikapov 0:a1a69d32f310 1992 } else {
nikapov 0:a1a69d32f310 1993 *p_phase_cal = 0;
nikapov 0:a1a69d32f310 1994 }
nikapov 0:a1a69d32f310 1995
nikapov 0:a1a69d32f310 1996
nikapov 0:a1a69d32f310 1997 if ((status == VL53L0X_ERROR_NONE) && restore_config) {
nikapov 0:a1a69d32f310 1998 /* restore the previous Sequence Config */
nikapov 0:a1a69d32f310 1999 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 2000 sequence_config);
nikapov 0:a1a69d32f310 2001 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2002 PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov 0:a1a69d32f310 2003 }
nikapov 0:a1a69d32f310 2004
nikapov 0:a1a69d32f310 2005 }
nikapov 0:a1a69d32f310 2006
nikapov 0:a1a69d32f310 2007 return status;
nikapov 0:a1a69d32f310 2008 }
nikapov 0:a1a69d32f310 2009
nikapov 0:a1a69d32f310 2010 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2011 uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable)
nikapov 0:a1a69d32f310 2012 {
nikapov 0:a1a69d32f310 2013 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2014 uint8_t sequence_config = 0;
nikapov 0:a1a69d32f310 2015
nikapov 0:a1a69d32f310 2016 /* store the value of the sequence config,
nikapov 0:a1a69d32f310 2017 * this will be reset before the end of the function
nikapov 0:a1a69d32f310 2018 */
nikapov 0:a1a69d32f310 2019
nikapov 0:a1a69d32f310 2020 sequence_config = PALDevDataGet(dev, SequenceConfig);
nikapov 0:a1a69d32f310 2021
nikapov 0:a1a69d32f310 2022 /* In the following function we don't save the config to optimize
nikapov 0:a1a69d32f310 2023 * writes on device. Config is saved and restored only once. */
nikapov 0:a1a69d32f310 2024 status = VL53L0X_perform_vhv_calibration(
nikapov 0:a1a69d32f310 2025 dev, p_vhv_settings, get_data_enable, 0);
nikapov 0:a1a69d32f310 2026
nikapov 0:a1a69d32f310 2027 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2028 status = VL53L0X_perform_phase_calibration(
nikapov 0:a1a69d32f310 2029 dev, p_phase_cal, get_data_enable, 0);
Davidroid 2:d07edeaff6f1 2030 }
nikapov 0:a1a69d32f310 2031
nikapov 0:a1a69d32f310 2032
nikapov 0:a1a69d32f310 2033 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2034 /* restore the previous Sequence Config */
nikapov 0:a1a69d32f310 2035 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 2036 sequence_config);
nikapov 0:a1a69d32f310 2037 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2038 PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov 0:a1a69d32f310 2039 }
nikapov 0:a1a69d32f310 2040
nikapov 0:a1a69d32f310 2041 }
nikapov 0:a1a69d32f310 2042
nikapov 0:a1a69d32f310 2043 return status;
nikapov 0:a1a69d32f310 2044 }
nikapov 0:a1a69d32f310 2045
nikapov 0:a1a69d32f310 2046 void VL53L0X::get_next_good_spad(uint8_t good_spad_array[], uint32_t size,
nikapov 0:a1a69d32f310 2047 uint32_t curr, int32_t *p_next)
nikapov 0:a1a69d32f310 2048 {
nikapov 0:a1a69d32f310 2049 uint32_t start_index;
nikapov 0:a1a69d32f310 2050 uint32_t fine_offset;
nikapov 0:a1a69d32f310 2051 uint32_t c_spads_per_byte = 8;
nikapov 0:a1a69d32f310 2052 uint32_t coarse_index;
nikapov 0:a1a69d32f310 2053 uint32_t fine_index;
nikapov 0:a1a69d32f310 2054 uint8_t data_byte;
nikapov 0:a1a69d32f310 2055 uint8_t success = 0;
nikapov 0:a1a69d32f310 2056
nikapov 0:a1a69d32f310 2057 /*
nikapov 0:a1a69d32f310 2058 * Starting with the current good spad, loop through the array to find
nikapov 0:a1a69d32f310 2059 * the next. i.e. the next bit set in the sequence.
nikapov 0:a1a69d32f310 2060 *
nikapov 0:a1a69d32f310 2061 * The coarse index is the byte index of the array and the fine index is
nikapov 0:a1a69d32f310 2062 * the index of the bit within each byte.
nikapov 0:a1a69d32f310 2063 */
nikapov 0:a1a69d32f310 2064
nikapov 0:a1a69d32f310 2065 *p_next = -1;
nikapov 0:a1a69d32f310 2066
nikapov 0:a1a69d32f310 2067 start_index = curr / c_spads_per_byte;
nikapov 0:a1a69d32f310 2068 fine_offset = curr % c_spads_per_byte;
nikapov 0:a1a69d32f310 2069
nikapov 0:a1a69d32f310 2070 for (coarse_index = start_index; ((coarse_index < size) && !success);
nikapov 0:a1a69d32f310 2071 coarse_index++) {
nikapov 0:a1a69d32f310 2072 fine_index = 0;
nikapov 0:a1a69d32f310 2073 data_byte = good_spad_array[coarse_index];
nikapov 0:a1a69d32f310 2074
nikapov 0:a1a69d32f310 2075 if (coarse_index == start_index) {
nikapov 0:a1a69d32f310 2076 /* locate the bit position of the provided current
nikapov 0:a1a69d32f310 2077 * spad bit before iterating */
nikapov 0:a1a69d32f310 2078 data_byte >>= fine_offset;
nikapov 0:a1a69d32f310 2079 fine_index = fine_offset;
nikapov 0:a1a69d32f310 2080 }
nikapov 0:a1a69d32f310 2081
nikapov 0:a1a69d32f310 2082 while (fine_index < c_spads_per_byte) {
nikapov 0:a1a69d32f310 2083 if ((data_byte & 0x1) == 1) {
nikapov 0:a1a69d32f310 2084 success = 1;
nikapov 0:a1a69d32f310 2085 *p_next = coarse_index * c_spads_per_byte + fine_index;
nikapov 0:a1a69d32f310 2086 break;
nikapov 0:a1a69d32f310 2087 }
nikapov 0:a1a69d32f310 2088 data_byte >>= 1;
nikapov 0:a1a69d32f310 2089 fine_index++;
nikapov 0:a1a69d32f310 2090 }
nikapov 0:a1a69d32f310 2091 }
nikapov 0:a1a69d32f310 2092 }
nikapov 0:a1a69d32f310 2093
nikapov 0:a1a69d32f310 2094 uint8_t VL53L0X::is_aperture(uint32_t spad_index)
nikapov 0:a1a69d32f310 2095 {
nikapov 0:a1a69d32f310 2096 /*
nikapov 0:a1a69d32f310 2097 * This function reports if a given spad index is an aperture SPAD by
nikapov 0:a1a69d32f310 2098 * deriving the quadrant.
nikapov 0:a1a69d32f310 2099 */
nikapov 0:a1a69d32f310 2100 uint32_t quadrant;
nikapov 0:a1a69d32f310 2101 uint8_t is_aperture = 1;
nikapov 0:a1a69d32f310 2102 quadrant = spad_index >> 6;
nikapov 0:a1a69d32f310 2103 if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) {
nikapov 0:a1a69d32f310 2104 is_aperture = 0;
Davidroid 2:d07edeaff6f1 2105 }
nikapov 0:a1a69d32f310 2106
nikapov 0:a1a69d32f310 2107 return is_aperture;
nikapov 0:a1a69d32f310 2108 }
nikapov 0:a1a69d32f310 2109
nikapov 0:a1a69d32f310 2110 VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[], uint32_t size,
nikapov 0:a1a69d32f310 2111 uint32_t spad_index)
nikapov 0:a1a69d32f310 2112 {
nikapov 0:a1a69d32f310 2113 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2114 uint32_t c_spads_per_byte = 8;
nikapov 0:a1a69d32f310 2115 uint32_t coarse_index;
nikapov 0:a1a69d32f310 2116 uint32_t fine_index;
nikapov 0:a1a69d32f310 2117
nikapov 0:a1a69d32f310 2118 coarse_index = spad_index / c_spads_per_byte;
nikapov 0:a1a69d32f310 2119 fine_index = spad_index % c_spads_per_byte;
nikapov 0:a1a69d32f310 2120 if (coarse_index >= size) {
nikapov 0:a1a69d32f310 2121 status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 2122 } else {
nikapov 0:a1a69d32f310 2123 spad_array[coarse_index] |= (1 << fine_index);
nikapov 0:a1a69d32f310 2124 }
nikapov 0:a1a69d32f310 2125
nikapov 0:a1a69d32f310 2126 return status;
nikapov 0:a1a69d32f310 2127 }
nikapov 0:a1a69d32f310 2128
nikapov 0:a1a69d32f310 2129 VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array)
nikapov 0:a1a69d32f310 2130 {
nikapov 0:a1a69d32f310 2131 VL53L0X_Error status = VL53L0X_write_multi(dev,
nikapov 0:a1a69d32f310 2132 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
nikapov 0:a1a69d32f310 2133 p_ref_spad_array, 6);
nikapov 0:a1a69d32f310 2134
nikapov 0:a1a69d32f310 2135 return status;
nikapov 0:a1a69d32f310 2136 }
nikapov 0:a1a69d32f310 2137
nikapov 0:a1a69d32f310 2138 VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array)
nikapov 0:a1a69d32f310 2139 {
nikapov 0:a1a69d32f310 2140 VL53L0X_Error status = VL53L0X_read_multi(dev,
nikapov 0:a1a69d32f310 2141 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
nikapov 0:a1a69d32f310 2142 p_ref_spad_array,
nikapov 0:a1a69d32f310 2143 6);
nikapov 0:a1a69d32f310 2144 // VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2145 // uint8_t count=0;
nikapov 0:a1a69d32f310 2146
nikapov 0:a1a69d32f310 2147 // for (count = 0; count < 6; count++)
nikapov 0:a1a69d32f310 2148 // status = VL53L0X_RdByte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]);
nikapov 0:a1a69d32f310 2149 return status;
nikapov 0:a1a69d32f310 2150 }
nikapov 0:a1a69d32f310 2151
nikapov 0:a1a69d32f310 2152 VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2153 uint8_t aperture_spads,
nikapov 0:a1a69d32f310 2154 uint8_t good_spad_array[],
nikapov 0:a1a69d32f310 2155 uint8_t spad_array[],
nikapov 0:a1a69d32f310 2156 uint32_t size,
nikapov 0:a1a69d32f310 2157 uint32_t start,
nikapov 0:a1a69d32f310 2158 uint32_t offset,
nikapov 0:a1a69d32f310 2159 uint32_t spad_count,
nikapov 0:a1a69d32f310 2160 uint32_t *p_last_spad)
nikapov 0:a1a69d32f310 2161 {
nikapov 0:a1a69d32f310 2162 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2163 uint32_t index;
nikapov 0:a1a69d32f310 2164 uint32_t i;
nikapov 0:a1a69d32f310 2165 int32_t next_good_spad = offset;
nikapov 0:a1a69d32f310 2166 uint32_t current_spad;
nikapov 0:a1a69d32f310 2167 uint8_t check_spad_array[6];
nikapov 0:a1a69d32f310 2168
nikapov 0:a1a69d32f310 2169 /*
nikapov 0:a1a69d32f310 2170 * This function takes in a spad array which may or may not have SPADS
nikapov 0:a1a69d32f310 2171 * already enabled and appends from a given offset a requested number
nikapov 0:a1a69d32f310 2172 * of new SPAD enables. The 'good spad map' is applied to
nikapov 0:a1a69d32f310 2173 * determine the next SPADs to enable.
nikapov 0:a1a69d32f310 2174 *
nikapov 0:a1a69d32f310 2175 * This function applies to only aperture or only non-aperture spads.
nikapov 0:a1a69d32f310 2176 * Checks are performed to ensure this.
nikapov 0:a1a69d32f310 2177 */
nikapov 0:a1a69d32f310 2178
nikapov 0:a1a69d32f310 2179 current_spad = offset;
nikapov 0:a1a69d32f310 2180 for (index = 0; index < spad_count; index++) {
nikapov 0:a1a69d32f310 2181 get_next_good_spad(good_spad_array, size, current_spad,
nikapov 0:a1a69d32f310 2182 &next_good_spad);
nikapov 0:a1a69d32f310 2183
nikapov 0:a1a69d32f310 2184 if (next_good_spad == -1) {
nikapov 0:a1a69d32f310 2185 status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 2186 break;
nikapov 0:a1a69d32f310 2187 }
nikapov 0:a1a69d32f310 2188
nikapov 0:a1a69d32f310 2189 /* Confirm that the next good SPAD is non-aperture */
nikapov 0:a1a69d32f310 2190 if (is_aperture(start + next_good_spad) != aperture_spads) {
nikapov 0:a1a69d32f310 2191 /* if we can't get the required number of good aperture
nikapov 0:a1a69d32f310 2192 * spads from the current quadrant then this is an error
nikapov 0:a1a69d32f310 2193 */
nikapov 0:a1a69d32f310 2194 status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 2195 break;
nikapov 0:a1a69d32f310 2196 }
nikapov 0:a1a69d32f310 2197 current_spad = (uint32_t)next_good_spad;
nikapov 0:a1a69d32f310 2198 enable_spad_bit(spad_array, size, current_spad);
nikapov 0:a1a69d32f310 2199 current_spad++;
nikapov 0:a1a69d32f310 2200 }
nikapov 0:a1a69d32f310 2201 *p_last_spad = current_spad;
nikapov 0:a1a69d32f310 2202
nikapov 0:a1a69d32f310 2203 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2204 status = set_ref_spad_map(dev, spad_array);
nikapov 0:a1a69d32f310 2205 }
nikapov 0:a1a69d32f310 2206
nikapov 0:a1a69d32f310 2207
nikapov 0:a1a69d32f310 2208 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2209 status = get_ref_spad_map(dev, check_spad_array);
nikapov 0:a1a69d32f310 2210
nikapov 0:a1a69d32f310 2211 i = 0;
nikapov 0:a1a69d32f310 2212
nikapov 0:a1a69d32f310 2213 /* Compare spad maps. If not equal report error. */
nikapov 0:a1a69d32f310 2214 while (i < size) {
nikapov 0:a1a69d32f310 2215 if (spad_array[i] != check_spad_array[i]) {
nikapov 0:a1a69d32f310 2216 status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 2217 break;
nikapov 0:a1a69d32f310 2218 }
nikapov 0:a1a69d32f310 2219 i++;
nikapov 0:a1a69d32f310 2220 }
nikapov 0:a1a69d32f310 2221 }
nikapov 0:a1a69d32f310 2222 return status;
nikapov 0:a1a69d32f310 2223 }
nikapov 0:a1a69d32f310 2224
nikapov 0:a1a69d32f310 2225 VL53L0X_Error VL53L0X::VL53L0X_set_device_mode(VL53L0X_DEV dev, VL53L0X_DeviceModes device_mode)
nikapov 0:a1a69d32f310 2226 {
nikapov 0:a1a69d32f310 2227 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2228
nikapov 0:a1a69d32f310 2229 switch (device_mode) {
nikapov 0:a1a69d32f310 2230 case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov 0:a1a69d32f310 2231 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov 0:a1a69d32f310 2232 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov 0:a1a69d32f310 2233 case VL53L0X_DEVICEMODE_GPIO_DRIVE:
nikapov 0:a1a69d32f310 2234 case VL53L0X_DEVICEMODE_GPIO_OSC:
nikapov 0:a1a69d32f310 2235 /* Supported modes */
nikapov 0:a1a69d32f310 2236 VL53L0X_SETPARAMETERFIELD(dev, DeviceMode, device_mode);
nikapov 0:a1a69d32f310 2237 break;
nikapov 0:a1a69d32f310 2238 default:
nikapov 0:a1a69d32f310 2239 /* Unsupported mode */
nikapov 0:a1a69d32f310 2240 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov 0:a1a69d32f310 2241 }
nikapov 0:a1a69d32f310 2242
sepp_nepp 5:b95f6951f7d5 2243
nikapov 0:a1a69d32f310 2244 return status;
nikapov 0:a1a69d32f310 2245 }
nikapov 0:a1a69d32f310 2246
nikapov 0:a1a69d32f310 2247 VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2248 VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low,
nikapov 0:a1a69d32f310 2249 FixPoint1616_t threshold_high)
nikapov 0:a1a69d32f310 2250 {
nikapov 0:a1a69d32f310 2251 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2252 uint16_t threshold16;
sepp_nepp 5:b95f6951f7d5 2253
sepp_nepp 5:b95f6951f7d5 2254
sepp_nepp 5:b95f6951f7d5 2255 /* no dependency on DeviceMode for FlightSense */
nikapov 0:a1a69d32f310 2256 /* Need to divide by 2 because the FW will apply a x2 */
nikapov 0:a1a69d32f310 2257 threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
nikapov 0:a1a69d32f310 2258 status = VL53L0X_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16);
nikapov 0:a1a69d32f310 2259
nikapov 0:a1a69d32f310 2260 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2261 /* Need to divide by 2 because the FW will apply a x2 */
nikapov 0:a1a69d32f310 2262 threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
nikapov 0:a1a69d32f310 2263 status = VL53L0X_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
nikapov 0:a1a69d32f310 2264 threshold16);
nikapov 0:a1a69d32f310 2265 }
nikapov 0:a1a69d32f310 2266
sepp_nepp 5:b95f6951f7d5 2267
nikapov 0:a1a69d32f310 2268 return status;
nikapov 0:a1a69d32f310 2269 }
nikapov 0:a1a69d32f310 2270
nikapov 0:a1a69d32f310 2271 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2272 VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low,
nikapov 0:a1a69d32f310 2273 FixPoint1616_t *p_threshold_high)
nikapov 0:a1a69d32f310 2274 {
nikapov 0:a1a69d32f310 2275 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2276 uint16_t threshold16;
sepp_nepp 5:b95f6951f7d5 2277
sepp_nepp 5:b95f6951f7d5 2278
sepp_nepp 5:b95f6951f7d5 2279 /* no dependency on DeviceMode for FlightSense */
nikapov 0:a1a69d32f310 2280
nikapov 0:a1a69d32f310 2281 status = VL53L0X_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16);
nikapov 0:a1a69d32f310 2282 /* Need to multiply by 2 because the FW will apply a x2 */
nikapov 0:a1a69d32f310 2283 *p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17);
nikapov 0:a1a69d32f310 2284
nikapov 0:a1a69d32f310 2285 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2286 status = VL53L0X_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
nikapov 0:a1a69d32f310 2287 &threshold16);
nikapov 0:a1a69d32f310 2288 /* Need to multiply by 2 because the FW will apply a x2 */
nikapov 0:a1a69d32f310 2289 *p_threshold_high =
nikapov 0:a1a69d32f310 2290 (FixPoint1616_t)((0x00fff & threshold16) << 17);
nikapov 0:a1a69d32f310 2291 }
nikapov 0:a1a69d32f310 2292
sepp_nepp 5:b95f6951f7d5 2293
nikapov 0:a1a69d32f310 2294 return status;
nikapov 0:a1a69d32f310 2295 }
nikapov 0:a1a69d32f310 2296
nikapov 0:a1a69d32f310 2297 VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2298 uint8_t *p_tuning_setting_buffer)
nikapov 0:a1a69d32f310 2299 {
nikapov 0:a1a69d32f310 2300 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2301 int i;
nikapov 0:a1a69d32f310 2302 int index;
nikapov 0:a1a69d32f310 2303 uint8_t msb;
nikapov 0:a1a69d32f310 2304 uint8_t lsb;
nikapov 0:a1a69d32f310 2305 uint8_t select_param;
nikapov 0:a1a69d32f310 2306 uint8_t number_of_writes;
nikapov 0:a1a69d32f310 2307 uint8_t address;
nikapov 0:a1a69d32f310 2308 uint8_t local_buffer[4]; /* max */
nikapov 0:a1a69d32f310 2309 uint16_t temp16;
nikapov 0:a1a69d32f310 2310
sepp_nepp 5:b95f6951f7d5 2311
nikapov 0:a1a69d32f310 2312
nikapov 0:a1a69d32f310 2313 index = 0;
nikapov 0:a1a69d32f310 2314
nikapov 0:a1a69d32f310 2315 while ((*(p_tuning_setting_buffer + index) != 0) &&
nikapov 0:a1a69d32f310 2316 (status == VL53L0X_ERROR_NONE)) {
nikapov 0:a1a69d32f310 2317 number_of_writes = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2318 index++;
nikapov 0:a1a69d32f310 2319 if (number_of_writes == 0xFF) {
nikapov 0:a1a69d32f310 2320 /* internal parameters */
nikapov 0:a1a69d32f310 2321 select_param = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2322 index++;
nikapov 0:a1a69d32f310 2323 switch (select_param) {
nikapov 0:a1a69d32f310 2324 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
nikapov 0:a1a69d32f310 2325 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2326 index++;
nikapov 0:a1a69d32f310 2327 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2328 index++;
nikapov 0:a1a69d32f310 2329 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov 0:a1a69d32f310 2330 PALDevDataSet(dev, SigmaEstRefArray, temp16);
nikapov 0:a1a69d32f310 2331 break;
nikapov 0:a1a69d32f310 2332 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
nikapov 0:a1a69d32f310 2333 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2334 index++;
nikapov 0:a1a69d32f310 2335 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2336 index++;
nikapov 0:a1a69d32f310 2337 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov 0:a1a69d32f310 2338 PALDevDataSet(dev, SigmaEstEffPulseWidth,
nikapov 0:a1a69d32f310 2339 temp16);
nikapov 0:a1a69d32f310 2340 break;
nikapov 0:a1a69d32f310 2341 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
nikapov 0:a1a69d32f310 2342 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2343 index++;
nikapov 0:a1a69d32f310 2344 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2345 index++;
nikapov 0:a1a69d32f310 2346 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov 0:a1a69d32f310 2347 PALDevDataSet(dev, SigmaEstEffAmbWidth, temp16);
nikapov 0:a1a69d32f310 2348 break;
nikapov 0:a1a69d32f310 2349 case 3: /* uint16_t targetRefRate -> 2 bytes */
nikapov 0:a1a69d32f310 2350 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2351 index++;
nikapov 0:a1a69d32f310 2352 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2353 index++;
nikapov 0:a1a69d32f310 2354 temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov 0:a1a69d32f310 2355 PALDevDataSet(dev, targetRefRate, temp16);
nikapov 0:a1a69d32f310 2356 break;
nikapov 0:a1a69d32f310 2357 default: /* invalid parameter */
nikapov 0:a1a69d32f310 2358 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 2359 }
nikapov 0:a1a69d32f310 2360
nikapov 0:a1a69d32f310 2361 } else if (number_of_writes <= 4) {
nikapov 0:a1a69d32f310 2362 address = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 2363 index++;
nikapov 0:a1a69d32f310 2364
nikapov 0:a1a69d32f310 2365 for (i = 0; i < number_of_writes; i++) {
nikapov 0:a1a69d32f310 2366 local_buffer[i] = *(p_tuning_setting_buffer +
nikapov 0:a1a69d32f310 2367 index);
nikapov 0:a1a69d32f310 2368 index++;
nikapov 0:a1a69d32f310 2369 }
nikapov 0:a1a69d32f310 2370
nikapov 0:a1a69d32f310 2371 status = VL53L0X_write_multi(dev, address, local_buffer,
nikapov 0:a1a69d32f310 2372 number_of_writes);
nikapov 0:a1a69d32f310 2373
nikapov 0:a1a69d32f310 2374 } else {
nikapov 0:a1a69d32f310 2375 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 2376 }
nikapov 0:a1a69d32f310 2377 }
nikapov 0:a1a69d32f310 2378
sepp_nepp 5:b95f6951f7d5 2379
nikapov 0:a1a69d32f310 2380 return status;
nikapov 0:a1a69d32f310 2381 }
nikapov 0:a1a69d32f310 2382
nikapov 0:a1a69d32f310 2383 VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2384 uint8_t start_not_stopflag)
nikapov 0:a1a69d32f310 2385 {
nikapov 0:a1a69d32f310 2386 uint8_t interrupt_config;
nikapov 0:a1a69d32f310 2387 FixPoint1616_t threshold_low;
nikapov 0:a1a69d32f310 2388 FixPoint1616_t threshold_high;
nikapov 0:a1a69d32f310 2389 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2390
nikapov 0:a1a69d32f310 2391 interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 2392 Pin0GpioFunctionality);
nikapov 0:a1a69d32f310 2393
nikapov 0:a1a69d32f310 2394 if ((interrupt_config ==
nikapov 0:a1a69d32f310 2395 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) ||
nikapov 0:a1a69d32f310 2396 (interrupt_config ==
nikapov 0:a1a69d32f310 2397 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) ||
nikapov 0:a1a69d32f310 2398 (interrupt_config ==
nikapov 0:a1a69d32f310 2399 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
nikapov 0:a1a69d32f310 2400
nikapov 0:a1a69d32f310 2401 status = VL53L0X_get_interrupt_thresholds(dev,
nikapov 0:a1a69d32f310 2402 VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov 0:a1a69d32f310 2403 &threshold_low, &threshold_high);
nikapov 0:a1a69d32f310 2404
nikapov 0:a1a69d32f310 2405 if (((threshold_low > 255 * 65536) ||
nikapov 0:a1a69d32f310 2406 (threshold_high > 255 * 65536)) &&
nikapov 0:a1a69d32f310 2407 (status == VL53L0X_ERROR_NONE)) {
nikapov 0:a1a69d32f310 2408
nikapov 0:a1a69d32f310 2409 if (start_not_stopflag != 0) {
nikapov 0:a1a69d32f310 2410 status = VL53L0X_load_tuning_settings(dev,
nikapov 0:a1a69d32f310 2411 InterruptThresholdSettings);
nikapov 0:a1a69d32f310 2412 } else {
nikapov 0:a1a69d32f310 2413 status |= VL53L0X_write_byte(dev, 0xFF, 0x04);
nikapov 0:a1a69d32f310 2414 status |= VL53L0X_write_byte(dev, 0x70, 0x00);
nikapov 0:a1a69d32f310 2415 status |= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 2416 status |= VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov 0:a1a69d32f310 2417 }
nikapov 0:a1a69d32f310 2418
nikapov 0:a1a69d32f310 2419 }
nikapov 0:a1a69d32f310 2420
nikapov 0:a1a69d32f310 2421
nikapov 0:a1a69d32f310 2422 }
nikapov 0:a1a69d32f310 2423
nikapov 0:a1a69d32f310 2424 return status;
nikapov 0:a1a69d32f310 2425
nikapov 0:a1a69d32f310 2426 }
nikapov 0:a1a69d32f310 2427
nikapov 0:a1a69d32f310 2428 VL53L0X_Error VL53L0X::VL53L0X_start_measurement(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 2429 {
nikapov 0:a1a69d32f310 2430 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2431 VL53L0X_DeviceModes device_mode;
nikapov 0:a1a69d32f310 2432 uint8_t byte;
nikapov 0:a1a69d32f310 2433 uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
nikapov 0:a1a69d32f310 2434 uint32_t loop_nb;
sepp_nepp 5:b95f6951f7d5 2435
nikapov 0:a1a69d32f310 2436
nikapov 0:a1a69d32f310 2437 /* Get Current DeviceMode */
nikapov 0:a1a69d32f310 2438 VL53L0X_get_device_mode(dev, &device_mode);
nikapov 0:a1a69d32f310 2439
nikapov 0:a1a69d32f310 2440 status = VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov 0:a1a69d32f310 2441 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 2442 status = VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov 0:a1a69d32f310 2443 status = VL53L0X_write_byte(dev, 0x91, PALDevDataGet(dev, StopVariable));
nikapov 0:a1a69d32f310 2444 status = VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov 0:a1a69d32f310 2445 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 2446 status = VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov 0:a1a69d32f310 2447
nikapov 0:a1a69d32f310 2448 switch (device_mode) {
nikapov 0:a1a69d32f310 2449 case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov 0:a1a69d32f310 2450 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x01);
nikapov 0:a1a69d32f310 2451
nikapov 0:a1a69d32f310 2452 byte = start_stop_byte;
nikapov 0:a1a69d32f310 2453 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2454 /* Wait until start bit has been cleared */
nikapov 0:a1a69d32f310 2455 loop_nb = 0;
nikapov 0:a1a69d32f310 2456 do {
nikapov 0:a1a69d32f310 2457 if (loop_nb > 0)
nikapov 0:a1a69d32f310 2458 status = VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 2459 VL53L0X_REG_SYSRANGE_START, &byte);
nikapov 0:a1a69d32f310 2460 loop_nb = loop_nb + 1;
nikapov 0:a1a69d32f310 2461 } while (((byte & start_stop_byte) == start_stop_byte)
nikapov 0:a1a69d32f310 2462 && (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 2463 && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
nikapov 0:a1a69d32f310 2464
Davidroid 3:e9269ff624ed 2465 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov 0:a1a69d32f310 2466 status = VL53L0X_ERROR_TIME_OUT;
Davidroid 3:e9269ff624ed 2467 }
nikapov 0:a1a69d32f310 2468
nikapov 0:a1a69d32f310 2469 }
nikapov 0:a1a69d32f310 2470
nikapov 0:a1a69d32f310 2471 break;
nikapov 0:a1a69d32f310 2472 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov 0:a1a69d32f310 2473 /* Back-to-back mode */
nikapov 0:a1a69d32f310 2474
nikapov 0:a1a69d32f310 2475 /* Check if need to apply interrupt settings */
Davidroid 3:e9269ff624ed 2476 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2477 status = VL53L0X_check_and_load_interrupt_settings(dev, 1);
Davidroid 3:e9269ff624ed 2478 }
nikapov 0:a1a69d32f310 2479
nikapov 0:a1a69d32f310 2480 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 2481 VL53L0X_REG_SYSRANGE_START,
nikapov 0:a1a69d32f310 2482 VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
nikapov 0:a1a69d32f310 2483 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2484 /* Set PAL State to Running */
nikapov 0:a1a69d32f310 2485 PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING);
nikapov 0:a1a69d32f310 2486 }
nikapov 0:a1a69d32f310 2487 break;
nikapov 0:a1a69d32f310 2488 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov 0:a1a69d32f310 2489 /* Continuous mode */
nikapov 0:a1a69d32f310 2490 /* Check if need to apply interrupt settings */
Davidroid 3:e9269ff624ed 2491 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2492 status = VL53L0X_check_and_load_interrupt_settings(dev, 1);
Davidroid 3:e9269ff624ed 2493 }
nikapov 0:a1a69d32f310 2494
nikapov 0:a1a69d32f310 2495 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 2496 VL53L0X_REG_SYSRANGE_START,
nikapov 0:a1a69d32f310 2497 VL53L0X_REG_SYSRANGE_MODE_TIMED);
nikapov 0:a1a69d32f310 2498
nikapov 0:a1a69d32f310 2499 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2500 /* Set PAL State to Running */
nikapov 0:a1a69d32f310 2501 PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING);
nikapov 0:a1a69d32f310 2502 }
nikapov 0:a1a69d32f310 2503 break;
nikapov 0:a1a69d32f310 2504 default:
nikapov 0:a1a69d32f310 2505 /* Selected mode not supported */
nikapov 0:a1a69d32f310 2506 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov 0:a1a69d32f310 2507 }
nikapov 0:a1a69d32f310 2508
nikapov 0:a1a69d32f310 2509
sepp_nepp 5:b95f6951f7d5 2510
nikapov 0:a1a69d32f310 2511 return status;
nikapov 0:a1a69d32f310 2512 }
nikapov 0:a1a69d32f310 2513
nikapov 0:a1a69d32f310 2514 /* Group PAL Measurement Functions */
nikapov 0:a1a69d32f310 2515 VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 2516 {
nikapov 0:a1a69d32f310 2517 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2518 VL53L0X_DeviceModes device_mode;
nikapov 0:a1a69d32f310 2519
sepp_nepp 5:b95f6951f7d5 2520
nikapov 0:a1a69d32f310 2521
nikapov 0:a1a69d32f310 2522 /* Get Current DeviceMode */
nikapov 0:a1a69d32f310 2523 status = VL53L0X_get_device_mode(dev, &device_mode);
nikapov 0:a1a69d32f310 2524
nikapov 0:a1a69d32f310 2525 /* Start immediately to run a single ranging measurement in case of
nikapov 0:a1a69d32f310 2526 * single ranging or single histogram */
nikapov 0:a1a69d32f310 2527 if (status == VL53L0X_ERROR_NONE
nikapov 0:a1a69d32f310 2528 && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
nikapov 0:a1a69d32f310 2529 status = VL53L0X_start_measurement(dev);
nikapov 0:a1a69d32f310 2530 }
nikapov 0:a1a69d32f310 2531
nikapov 0:a1a69d32f310 2532
nikapov 0:a1a69d32f310 2533 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2534 status = VL53L0X_measurement_poll_for_completion(dev);
nikapov 0:a1a69d32f310 2535 }
nikapov 0:a1a69d32f310 2536
nikapov 0:a1a69d32f310 2537
nikapov 0:a1a69d32f310 2538 /* Change PAL State in case of single ranging or single histogram */
nikapov 0:a1a69d32f310 2539 if (status == VL53L0X_ERROR_NONE
Davidroid 3:e9269ff624ed 2540 && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
nikapov 0:a1a69d32f310 2541 PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
Davidroid 3:e9269ff624ed 2542 }
nikapov 0:a1a69d32f310 2543
nikapov 0:a1a69d32f310 2544
sepp_nepp 5:b95f6951f7d5 2545
nikapov 0:a1a69d32f310 2546 return status;
nikapov 0:a1a69d32f310 2547 }
nikapov 0:a1a69d32f310 2548
nikapov 0:a1a69d32f310 2549 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2550 uint8_t *p_x_talk_compensation_enable)
nikapov 0:a1a69d32f310 2551 {
nikapov 0:a1a69d32f310 2552 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2553 uint8_t temp8;
sepp_nepp 5:b95f6951f7d5 2554
nikapov 0:a1a69d32f310 2555
nikapov 0:a1a69d32f310 2556 VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable, temp8);
nikapov 0:a1a69d32f310 2557 *p_x_talk_compensation_enable = temp8;
nikapov 0:a1a69d32f310 2558
sepp_nepp 5:b95f6951f7d5 2559
nikapov 0:a1a69d32f310 2560 return status;
nikapov 0:a1a69d32f310 2561 }
nikapov 0:a1a69d32f310 2562
nikapov 0:a1a69d32f310 2563 VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2564 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov 0:a1a69d32f310 2565 FixPoint1616_t *p_total_xtalk_rate_mcps)
nikapov 0:a1a69d32f310 2566 {
nikapov 0:a1a69d32f310 2567 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2568
nikapov 0:a1a69d32f310 2569 uint8_t xtalk_comp_enable;
nikapov 0:a1a69d32f310 2570 FixPoint1616_t total_xtalk_mega_cps;
nikapov 0:a1a69d32f310 2571 FixPoint1616_t xtalk_per_spad_mega_cps;
nikapov 0:a1a69d32f310 2572
nikapov 0:a1a69d32f310 2573 *p_total_xtalk_rate_mcps = 0;
nikapov 0:a1a69d32f310 2574
nikapov 0:a1a69d32f310 2575 status = VL53L0X_get_x_talk_compensation_enable(dev, &xtalk_comp_enable);
nikapov 0:a1a69d32f310 2576 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2577
nikapov 0:a1a69d32f310 2578 if (xtalk_comp_enable) {
nikapov 0:a1a69d32f310 2579
nikapov 0:a1a69d32f310 2580 VL53L0X_GETPARAMETERFIELD(
nikapov 0:a1a69d32f310 2581 dev,
nikapov 0:a1a69d32f310 2582 XTalkCompensationRateMegaCps,
nikapov 0:a1a69d32f310 2583 xtalk_per_spad_mega_cps);
nikapov 0:a1a69d32f310 2584
nikapov 0:a1a69d32f310 2585 /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
nikapov 0:a1a69d32f310 2586 total_xtalk_mega_cps =
nikapov 0:a1a69d32f310 2587 p_ranging_measurement_data->EffectiveSpadRtnCount *
nikapov 0:a1a69d32f310 2588 xtalk_per_spad_mega_cps;
nikapov 0:a1a69d32f310 2589
nikapov 0:a1a69d32f310 2590 /* FixPoint0824 >> 8 = FixPoint1616 */
nikapov 0:a1a69d32f310 2591 *p_total_xtalk_rate_mcps =
nikapov 0:a1a69d32f310 2592 (total_xtalk_mega_cps + 0x80) >> 8;
nikapov 0:a1a69d32f310 2593 }
nikapov 0:a1a69d32f310 2594 }
nikapov 0:a1a69d32f310 2595
nikapov 0:a1a69d32f310 2596 return status;
nikapov 0:a1a69d32f310 2597 }
nikapov 0:a1a69d32f310 2598
nikapov 0:a1a69d32f310 2599 VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2600 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov 0:a1a69d32f310 2601 FixPoint1616_t *p_total_signal_rate_mcps)
nikapov 0:a1a69d32f310 2602 {
nikapov 0:a1a69d32f310 2603 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2604 FixPoint1616_t total_xtalk_mega_cps;
nikapov 0:a1a69d32f310 2605
sepp_nepp 5:b95f6951f7d5 2606
nikapov 0:a1a69d32f310 2607
nikapov 0:a1a69d32f310 2608 *p_total_signal_rate_mcps =
nikapov 0:a1a69d32f310 2609 p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov 0:a1a69d32f310 2610
nikapov 0:a1a69d32f310 2611 status = VL53L0X_get_total_xtalk_rate(
nikapov 0:a1a69d32f310 2612 dev, p_ranging_measurement_data, &total_xtalk_mega_cps);
nikapov 0:a1a69d32f310 2613
nikapov 0:a1a69d32f310 2614 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2615 *p_total_signal_rate_mcps += total_xtalk_mega_cps;
nikapov 0:a1a69d32f310 2616 }
nikapov 0:a1a69d32f310 2617
nikapov 0:a1a69d32f310 2618 return status;
nikapov 0:a1a69d32f310 2619 }
nikapov 0:a1a69d32f310 2620
nikapov 0:a1a69d32f310 2621 /* To convert ms into register value */
nikapov 0:a1a69d32f310 2622 uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2623 uint32_t timeout_period_us,
nikapov 0:a1a69d32f310 2624 uint8_t vcsel_period_pclks)
nikapov 0:a1a69d32f310 2625 {
nikapov 0:a1a69d32f310 2626 uint32_t macro_period_ps;
nikapov 0:a1a69d32f310 2627 uint32_t macro_period_ns;
nikapov 0:a1a69d32f310 2628 uint32_t timeout_period_mclks = 0;
nikapov 0:a1a69d32f310 2629
nikapov 0:a1a69d32f310 2630 macro_period_ps = VL53L0X_calc_macro_period_ps(dev, vcsel_period_pclks);
nikapov 0:a1a69d32f310 2631 macro_period_ns = (macro_period_ps + 500) / 1000;
nikapov 0:a1a69d32f310 2632
nikapov 0:a1a69d32f310 2633 timeout_period_mclks =
nikapov 0:a1a69d32f310 2634 (uint32_t)(((timeout_period_us * 1000)
nikapov 0:a1a69d32f310 2635 + (macro_period_ns / 2)) / macro_period_ns);
nikapov 0:a1a69d32f310 2636
nikapov 0:a1a69d32f310 2637 return timeout_period_mclks;
nikapov 0:a1a69d32f310 2638 }
nikapov 0:a1a69d32f310 2639
nikapov 0:a1a69d32f310 2640 uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
nikapov 0:a1a69d32f310 2641 {
nikapov 0:a1a69d32f310 2642 /*
nikapov 0:a1a69d32f310 2643 * Implements an integer square root
nikapov 0:a1a69d32f310 2644 *
nikapov 0:a1a69d32f310 2645 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
nikapov 0:a1a69d32f310 2646 */
nikapov 0:a1a69d32f310 2647
nikapov 0:a1a69d32f310 2648 uint32_t res = 0;
nikapov 0:a1a69d32f310 2649 uint32_t bit = 1 << 30;
nikapov 0:a1a69d32f310 2650 /* The second-to-top bit is set:
nikapov 0:a1a69d32f310 2651 * 1 << 14 for 16-bits, 1 << 30 for 32 bits */
nikapov 0:a1a69d32f310 2652
nikapov 0:a1a69d32f310 2653 /* "bit" starts at the highest power of four <= the argument. */
nikapov 0:a1a69d32f310 2654 while (bit > num) {
nikapov 0:a1a69d32f310 2655 bit >>= 2;
nikapov 0:a1a69d32f310 2656 }
nikapov 0:a1a69d32f310 2657
nikapov 0:a1a69d32f310 2658
nikapov 0:a1a69d32f310 2659 while (bit != 0) {
nikapov 0:a1a69d32f310 2660 if (num >= res + bit) {
nikapov 0:a1a69d32f310 2661 num -= res + bit;
nikapov 0:a1a69d32f310 2662 res = (res >> 1) + bit;
Davidroid 3:e9269ff624ed 2663 } else {
nikapov 0:a1a69d32f310 2664 res >>= 1;
Davidroid 3:e9269ff624ed 2665 }
nikapov 0:a1a69d32f310 2666
nikapov 0:a1a69d32f310 2667 bit >>= 2;
nikapov 0:a1a69d32f310 2668 }
nikapov 0:a1a69d32f310 2669
nikapov 0:a1a69d32f310 2670 return res;
nikapov 0:a1a69d32f310 2671 }
nikapov 0:a1a69d32f310 2672
nikapov 0:a1a69d32f310 2673 VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(
nikapov 0:a1a69d32f310 2674 VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2675 FixPoint1616_t total_signal_rate_mcps,
nikapov 0:a1a69d32f310 2676 FixPoint1616_t total_corr_signal_rate_mcps,
nikapov 0:a1a69d32f310 2677 FixPoint1616_t pw_mult,
nikapov 0:a1a69d32f310 2678 uint32_t sigma_estimate_p1,
nikapov 0:a1a69d32f310 2679 FixPoint1616_t sigma_estimate_p2,
nikapov 0:a1a69d32f310 2680 uint32_t peak_vcsel_duration_us,
nikapov 0:a1a69d32f310 2681 uint32_t *pd_max_mm)
nikapov 0:a1a69d32f310 2682 {
nikapov 0:a1a69d32f310 2683 const uint32_t c_sigma_limit = 18;
nikapov 0:a1a69d32f310 2684 const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
nikapov 0:a1a69d32f310 2685 const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */
nikapov 0:a1a69d32f310 2686 const uint32_t c_amb_eff_width_sigma_est_ns = 6;
nikapov 0:a1a69d32f310 2687 const uint32_t c_amb_eff_width_d_max_ns = 7;
nikapov 0:a1a69d32f310 2688 uint32_t dmax_cal_range_mm;
nikapov 0:a1a69d32f310 2689 FixPoint1616_t dmax_cal_signal_rate_rtn_mcps;
nikapov 0:a1a69d32f310 2690 FixPoint1616_t min_signal_needed;
nikapov 0:a1a69d32f310 2691 FixPoint1616_t min_signal_needed_p1;
nikapov 0:a1a69d32f310 2692 FixPoint1616_t min_signal_needed_p2;
nikapov 0:a1a69d32f310 2693 FixPoint1616_t min_signal_needed_p3;
nikapov 0:a1a69d32f310 2694 FixPoint1616_t min_signal_needed_p4;
nikapov 0:a1a69d32f310 2695 FixPoint1616_t sigma_limit_tmp;
nikapov 0:a1a69d32f310 2696 FixPoint1616_t sigma_est_sq_tmp;
nikapov 0:a1a69d32f310 2697 FixPoint1616_t signal_limit_tmp;
nikapov 0:a1a69d32f310 2698 FixPoint1616_t signal_at0_mm;
nikapov 0:a1a69d32f310 2699 FixPoint1616_t dmax_dark;
nikapov 0:a1a69d32f310 2700 FixPoint1616_t dmax_ambient;
nikapov 0:a1a69d32f310 2701 FixPoint1616_t dmax_dark_tmp;
nikapov 0:a1a69d32f310 2702 FixPoint1616_t sigma_est_p2_tmp;
nikapov 0:a1a69d32f310 2703 uint32_t signal_rate_temp_mcps;
nikapov 0:a1a69d32f310 2704
nikapov 0:a1a69d32f310 2705 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2706
sepp_nepp 5:b95f6951f7d5 2707
nikapov 0:a1a69d32f310 2708
nikapov 0:a1a69d32f310 2709 dmax_cal_range_mm =
nikapov 0:a1a69d32f310 2710 PALDevDataGet(dev, DmaxCalRangeMilliMeter);
nikapov 0:a1a69d32f310 2711
nikapov 0:a1a69d32f310 2712 dmax_cal_signal_rate_rtn_mcps =
nikapov 0:a1a69d32f310 2713 PALDevDataGet(dev, DmaxCalSignalRateRtnMegaCps);
nikapov 0:a1a69d32f310 2714
nikapov 0:a1a69d32f310 2715 /* uint32 * FixPoint1616 = FixPoint1616 */
nikapov 0:a1a69d32f310 2716 signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
nikapov 0:a1a69d32f310 2717
nikapov 0:a1a69d32f310 2718 /* FixPoint1616 >> 8 = FixPoint2408 */
nikapov 0:a1a69d32f310 2719 signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
nikapov 0:a1a69d32f310 2720 signal_at0_mm *= dmax_cal_range_mm;
nikapov 0:a1a69d32f310 2721
nikapov 0:a1a69d32f310 2722 min_signal_needed_p1 = 0;
nikapov 0:a1a69d32f310 2723 if (total_corr_signal_rate_mcps > 0) {
nikapov 0:a1a69d32f310 2724
nikapov 0:a1a69d32f310 2725 /* Shift by 10 bits to increase resolution prior to the
nikapov 0:a1a69d32f310 2726 * division */
nikapov 0:a1a69d32f310 2727 signal_rate_temp_mcps = total_signal_rate_mcps << 10;
nikapov 0:a1a69d32f310 2728
nikapov 0:a1a69d32f310 2729 /* Add rounding value prior to division */
nikapov 0:a1a69d32f310 2730 min_signal_needed_p1 = signal_rate_temp_mcps +
nikapov 0:a1a69d32f310 2731 (total_corr_signal_rate_mcps / 2);
nikapov 0:a1a69d32f310 2732
nikapov 0:a1a69d32f310 2733 /* FixPoint0626/FixPoint1616 = FixPoint2210 */
nikapov 0:a1a69d32f310 2734 min_signal_needed_p1 /= total_corr_signal_rate_mcps;
nikapov 0:a1a69d32f310 2735
nikapov 0:a1a69d32f310 2736 /* Apply a factored version of the speed of light.
nikapov 0:a1a69d32f310 2737 Correction to be applied at the end */
nikapov 0:a1a69d32f310 2738 min_signal_needed_p1 *= 3;
nikapov 0:a1a69d32f310 2739
nikapov 0:a1a69d32f310 2740 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
nikapov 0:a1a69d32f310 2741 min_signal_needed_p1 *= min_signal_needed_p1;
nikapov 0:a1a69d32f310 2742
nikapov 0:a1a69d32f310 2743 /* FixPoint1220 >> 16 = FixPoint2804 */
nikapov 0:a1a69d32f310 2744 min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 2745 }
nikapov 0:a1a69d32f310 2746
nikapov 0:a1a69d32f310 2747 min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
nikapov 0:a1a69d32f310 2748
nikapov 0:a1a69d32f310 2749 /* FixPoint1616 >> 16 = uint32 */
nikapov 0:a1a69d32f310 2750 min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 2751
nikapov 0:a1a69d32f310 2752 /* uint32 * uint32 = uint32 */
nikapov 0:a1a69d32f310 2753 min_signal_needed_p2 *= min_signal_needed_p2;
nikapov 0:a1a69d32f310 2754
nikapov 0:a1a69d32f310 2755 /* Check sigmaEstimateP2
nikapov 0:a1a69d32f310 2756 * If this value is too high there is not enough signal rate
nikapov 0:a1a69d32f310 2757 * to calculate dmax value so set a suitable value to ensure
nikapov 0:a1a69d32f310 2758 * a very small dmax.
nikapov 0:a1a69d32f310 2759 */
nikapov 0:a1a69d32f310 2760 sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 2761 sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
nikapov 0:a1a69d32f310 2762 c_amb_eff_width_sigma_est_ns;
nikapov 0:a1a69d32f310 2763 sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
nikapov 0:a1a69d32f310 2764
nikapov 0:a1a69d32f310 2765 if (sigma_est_p2_tmp > 0xffff) {
nikapov 0:a1a69d32f310 2766 min_signal_needed_p3 = 0xfff00000;
nikapov 0:a1a69d32f310 2767 } else {
nikapov 0:a1a69d32f310 2768
nikapov 0:a1a69d32f310 2769 /* DMAX uses a different ambient width from sigma, so apply
nikapov 0:a1a69d32f310 2770 * correction.
nikapov 0:a1a69d32f310 2771 * Perform division before multiplication to prevent overflow.
nikapov 0:a1a69d32f310 2772 */
nikapov 0:a1a69d32f310 2773 sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
nikapov 0:a1a69d32f310 2774 c_amb_eff_width_sigma_est_ns;
nikapov 0:a1a69d32f310 2775 sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
nikapov 0:a1a69d32f310 2776
nikapov 0:a1a69d32f310 2777 /* FixPoint1616 >> 16 = uint32 */
nikapov 0:a1a69d32f310 2778 min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 2779
nikapov 0:a1a69d32f310 2780 min_signal_needed_p3 *= min_signal_needed_p3;
nikapov 0:a1a69d32f310 2781
nikapov 0:a1a69d32f310 2782 }
nikapov 0:a1a69d32f310 2783
nikapov 0:a1a69d32f310 2784 /* FixPoint1814 / uint32 = FixPoint1814 */
nikapov 0:a1a69d32f310 2785 sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
nikapov 0:a1a69d32f310 2786
nikapov 0:a1a69d32f310 2787 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
nikapov 0:a1a69d32f310 2788 sigma_limit_tmp *= sigma_limit_tmp;
nikapov 0:a1a69d32f310 2789
nikapov 0:a1a69d32f310 2790 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov 0:a1a69d32f310 2791 sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
nikapov 0:a1a69d32f310 2792
nikapov 0:a1a69d32f310 2793 /* FixPoint3232 >> 4 = FixPoint0428 */
nikapov 0:a1a69d32f310 2794 sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
nikapov 0:a1a69d32f310 2795
nikapov 0:a1a69d32f310 2796 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */
nikapov 0:a1a69d32f310 2797 sigma_limit_tmp -= sigma_est_sq_tmp;
nikapov 0:a1a69d32f310 2798
nikapov 0:a1a69d32f310 2799 /* uint32_t * FixPoint0428 = FixPoint0428 */
nikapov 0:a1a69d32f310 2800 min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
nikapov 0:a1a69d32f310 2801
nikapov 0:a1a69d32f310 2802 /* FixPoint0428 >> 14 = FixPoint1814 */
nikapov 0:a1a69d32f310 2803 min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14;
nikapov 0:a1a69d32f310 2804
nikapov 0:a1a69d32f310 2805 /* uint32 + uint32 = uint32 */
nikapov 0:a1a69d32f310 2806 min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3);
nikapov 0:a1a69d32f310 2807
nikapov 0:a1a69d32f310 2808 /* uint32 / uint32 = uint32 */
nikapov 0:a1a69d32f310 2809 min_signal_needed += (peak_vcsel_duration_us / 2);
nikapov 0:a1a69d32f310 2810 min_signal_needed /= peak_vcsel_duration_us;
nikapov 0:a1a69d32f310 2811
nikapov 0:a1a69d32f310 2812 /* uint32 << 14 = FixPoint1814 */
nikapov 0:a1a69d32f310 2813 min_signal_needed <<= 14;
nikapov 0:a1a69d32f310 2814
nikapov 0:a1a69d32f310 2815 /* FixPoint1814 / FixPoint1814 = uint32 */
nikapov 0:a1a69d32f310 2816 min_signal_needed += (min_signal_needed_p4 / 2);
nikapov 0:a1a69d32f310 2817 min_signal_needed /= min_signal_needed_p4;
nikapov 0:a1a69d32f310 2818
nikapov 0:a1a69d32f310 2819 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
nikapov 0:a1a69d32f310 2820 min_signal_needed *= min_signal_needed_p1;
nikapov 0:a1a69d32f310 2821
nikapov 0:a1a69d32f310 2822 /* Apply correction by dividing by 1000000.
nikapov 0:a1a69d32f310 2823 * This assumes 10E16 on the numerator of the equation
nikapov 0:a1a69d32f310 2824 * and 10E-22 on the denominator.
nikapov 0:a1a69d32f310 2825 * We do this because 32bit fix point calculation can't
nikapov 0:a1a69d32f310 2826 * handle the larger and smaller elements of this equation,
nikapov 0:a1a69d32f310 2827 * i.e. speed of light and pulse widths.
nikapov 0:a1a69d32f310 2828 */
nikapov 0:a1a69d32f310 2829 min_signal_needed = (min_signal_needed + 500) / 1000;
nikapov 0:a1a69d32f310 2830 min_signal_needed <<= 4;
nikapov 0:a1a69d32f310 2831
nikapov 0:a1a69d32f310 2832 min_signal_needed = (min_signal_needed + 500) / 1000;
nikapov 0:a1a69d32f310 2833
nikapov 0:a1a69d32f310 2834 /* FixPoint1616 >> 8 = FixPoint2408 */
nikapov 0:a1a69d32f310 2835 signal_limit_tmp = (c_signal_limit + 0x80) >> 8;
nikapov 0:a1a69d32f310 2836
nikapov 0:a1a69d32f310 2837 /* FixPoint2408/FixPoint2408 = uint32 */
nikapov 0:a1a69d32f310 2838 if (signal_limit_tmp != 0) {
nikapov 0:a1a69d32f310 2839 dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
nikapov 0:a1a69d32f310 2840 / signal_limit_tmp;
nikapov 0:a1a69d32f310 2841 } else {
nikapov 0:a1a69d32f310 2842 dmax_dark_tmp = 0;
nikapov 0:a1a69d32f310 2843 }
nikapov 0:a1a69d32f310 2844
nikapov 0:a1a69d32f310 2845 dmax_dark = VL53L0X_isqrt(dmax_dark_tmp);
nikapov 0:a1a69d32f310 2846
nikapov 0:a1a69d32f310 2847 /* FixPoint2408/FixPoint2408 = uint32 */
nikapov 0:a1a69d32f310 2848 if (min_signal_needed != 0) {
nikapov 0:a1a69d32f310 2849 dmax_ambient = (signal_at0_mm + min_signal_needed / 2)
nikapov 0:a1a69d32f310 2850 / min_signal_needed;
nikapov 0:a1a69d32f310 2851 } else {
nikapov 0:a1a69d32f310 2852 dmax_ambient = 0;
nikapov 0:a1a69d32f310 2853 }
nikapov 0:a1a69d32f310 2854
nikapov 0:a1a69d32f310 2855 dmax_ambient = VL53L0X_isqrt(dmax_ambient);
nikapov 0:a1a69d32f310 2856
nikapov 0:a1a69d32f310 2857 *pd_max_mm = dmax_dark;
nikapov 0:a1a69d32f310 2858 if (dmax_dark > dmax_ambient) {
nikapov 0:a1a69d32f310 2859 *pd_max_mm = dmax_ambient;
nikapov 0:a1a69d32f310 2860 }
nikapov 0:a1a69d32f310 2861
sepp_nepp 5:b95f6951f7d5 2862
nikapov 0:a1a69d32f310 2863
nikapov 0:a1a69d32f310 2864 return status;
nikapov 0:a1a69d32f310 2865 }
nikapov 0:a1a69d32f310 2866
nikapov 0:a1a69d32f310 2867 VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 2868 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov 0:a1a69d32f310 2869 FixPoint1616_t *p_sigma_estimate,
nikapov 0:a1a69d32f310 2870 uint32_t *p_dmax_mm)
nikapov 0:a1a69d32f310 2871 {
nikapov 0:a1a69d32f310 2872 /* Expressed in 100ths of a ns, i.e. centi-ns */
nikapov 0:a1a69d32f310 2873 const uint32_t c_pulse_effective_width_centi_ns = 800;
nikapov 0:a1a69d32f310 2874 /* Expressed in 100ths of a ns, i.e. centi-ns */
nikapov 0:a1a69d32f310 2875 const uint32_t c_ambient_effective_width_centi_ns = 600;
nikapov 0:a1a69d32f310 2876 const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
nikapov 0:a1a69d32f310 2877 const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
nikapov 0:a1a69d32f310 2878 const FixPoint1616_t c_sigma_est_max = 0x028F87AE;
nikapov 0:a1a69d32f310 2879 const FixPoint1616_t c_sigma_est_rtn_max = 0xF000;
nikapov 0:a1a69d32f310 2880 const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
nikapov 0:a1a69d32f310 2881 c_ambient_effective_width_centi_ns;
nikapov 0:a1a69d32f310 2882 /* Time Of Flight per mm (6.6 pico secs) */
nikapov 0:a1a69d32f310 2883 const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A;
nikapov 0:a1a69d32f310 2884 const uint32_t c_16bit_rounding_param = 0x00008000;
nikapov 0:a1a69d32f310 2885 const FixPoint1616_t c_max_x_talk_kcps = 0x00320000;
nikapov 0:a1a69d32f310 2886 const uint32_t c_pll_period_ps = 1655;
nikapov 0:a1a69d32f310 2887
nikapov 0:a1a69d32f310 2888 uint32_t vcsel_total_events_rtn;
nikapov 0:a1a69d32f310 2889 uint32_t final_range_timeout_micro_secs;
nikapov 0:a1a69d32f310 2890 uint32_t pre_range_timeout_micro_secs;
nikapov 0:a1a69d32f310 2891 uint32_t final_range_integration_time_milli_secs;
nikapov 0:a1a69d32f310 2892 FixPoint1616_t sigma_estimate_p1;
nikapov 0:a1a69d32f310 2893 FixPoint1616_t sigma_estimate_p2;
nikapov 0:a1a69d32f310 2894 FixPoint1616_t sigma_estimate_p3;
nikapov 0:a1a69d32f310 2895 FixPoint1616_t delta_t_ps;
nikapov 0:a1a69d32f310 2896 FixPoint1616_t pw_mult;
nikapov 0:a1a69d32f310 2897 FixPoint1616_t sigma_est_rtn;
nikapov 0:a1a69d32f310 2898 FixPoint1616_t sigma_estimate;
nikapov 0:a1a69d32f310 2899 FixPoint1616_t x_talk_correction;
nikapov 0:a1a69d32f310 2900 FixPoint1616_t ambient_rate_kcps;
nikapov 0:a1a69d32f310 2901 FixPoint1616_t peak_signal_rate_kcps;
nikapov 0:a1a69d32f310 2902 FixPoint1616_t x_talk_comp_rate_mcps;
nikapov 0:a1a69d32f310 2903 uint32_t x_talk_comp_rate_kcps;
nikapov 0:a1a69d32f310 2904 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 2905 FixPoint1616_t diff1_mcps;
nikapov 0:a1a69d32f310 2906 FixPoint1616_t diff2_mcps;
nikapov 0:a1a69d32f310 2907 FixPoint1616_t sqr1;
nikapov 0:a1a69d32f310 2908 FixPoint1616_t sqr2;
nikapov 0:a1a69d32f310 2909 FixPoint1616_t sqr_sum;
nikapov 0:a1a69d32f310 2910 FixPoint1616_t sqrt_result_centi_ns;
nikapov 0:a1a69d32f310 2911 FixPoint1616_t sqrt_result;
nikapov 0:a1a69d32f310 2912 FixPoint1616_t total_signal_rate_mcps;
nikapov 0:a1a69d32f310 2913 FixPoint1616_t corrected_signal_rate_mcps;
nikapov 0:a1a69d32f310 2914 FixPoint1616_t sigma_est_ref;
nikapov 0:a1a69d32f310 2915 uint32_t vcsel_width;
nikapov 0:a1a69d32f310 2916 uint32_t final_range_macro_pclks;
nikapov 0:a1a69d32f310 2917 uint32_t pre_range_macro_pclks;
nikapov 0:a1a69d32f310 2918 uint32_t peak_vcsel_duration_us;
nikapov 0:a1a69d32f310 2919 uint8_t final_range_vcsel_pclks;
nikapov 0:a1a69d32f310 2920 uint8_t pre_range_vcsel_pclks;
nikapov 0:a1a69d32f310 2921 /*! \addtogroup calc_sigma_estimate
nikapov 0:a1a69d32f310 2922 * @{
nikapov 0:a1a69d32f310 2923 *
nikapov 0:a1a69d32f310 2924 * Estimates the range sigma
nikapov 0:a1a69d32f310 2925 */
nikapov 0:a1a69d32f310 2926
sepp_nepp 5:b95f6951f7d5 2927
nikapov 0:a1a69d32f310 2928
nikapov 0:a1a69d32f310 2929 VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps,
nikapov 0:a1a69d32f310 2930 x_talk_comp_rate_mcps);
nikapov 0:a1a69d32f310 2931
nikapov 0:a1a69d32f310 2932 /*
nikapov 0:a1a69d32f310 2933 * We work in kcps rather than mcps as this helps keep within the
nikapov 0:a1a69d32f310 2934 * confines of the 32 Fix1616 type.
nikapov 0:a1a69d32f310 2935 */
nikapov 0:a1a69d32f310 2936
nikapov 0:a1a69d32f310 2937 ambient_rate_kcps =
nikapov 0:a1a69d32f310 2938 (p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16;
nikapov 0:a1a69d32f310 2939
nikapov 0:a1a69d32f310 2940 corrected_signal_rate_mcps =
nikapov 0:a1a69d32f310 2941 p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov 0:a1a69d32f310 2942
nikapov 0:a1a69d32f310 2943
nikapov 0:a1a69d32f310 2944 status = VL53L0X_get_total_signal_rate(
nikapov 0:a1a69d32f310 2945 dev, p_ranging_measurement_data, &total_signal_rate_mcps);
nikapov 0:a1a69d32f310 2946 status = VL53L0X_get_total_xtalk_rate(
nikapov 0:a1a69d32f310 2947 dev, p_ranging_measurement_data, &x_talk_comp_rate_mcps);
nikapov 0:a1a69d32f310 2948
nikapov 0:a1a69d32f310 2949
nikapov 0:a1a69d32f310 2950 /* Signal rate measurement provided by device is the
nikapov 0:a1a69d32f310 2951 * peak signal rate, not average.
nikapov 0:a1a69d32f310 2952 */
nikapov 0:a1a69d32f310 2953 peak_signal_rate_kcps = (total_signal_rate_mcps * 1000);
nikapov 0:a1a69d32f310 2954 peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16;
nikapov 0:a1a69d32f310 2955
nikapov 0:a1a69d32f310 2956 x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
nikapov 0:a1a69d32f310 2957
nikapov 0:a1a69d32f310 2958 if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) {
nikapov 0:a1a69d32f310 2959 x_talk_comp_rate_kcps = c_max_x_talk_kcps;
nikapov 0:a1a69d32f310 2960 }
nikapov 0:a1a69d32f310 2961
nikapov 0:a1a69d32f310 2962 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 2963
nikapov 0:a1a69d32f310 2964 /* Calculate final range macro periods */
nikapov 0:a1a69d32f310 2965 final_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 2966 dev, FinalRangeTimeoutMicroSecs);
nikapov 0:a1a69d32f310 2967
nikapov 0:a1a69d32f310 2968 final_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 2969 dev, FinalRangeVcselPulsePeriod);
nikapov 0:a1a69d32f310 2970
nikapov 0:a1a69d32f310 2971 final_range_macro_pclks = VL53L0X_calc_timeout_mclks(
nikapov 0:a1a69d32f310 2972 dev, final_range_timeout_micro_secs, final_range_vcsel_pclks);
nikapov 0:a1a69d32f310 2973
nikapov 0:a1a69d32f310 2974 /* Calculate pre-range macro periods */
nikapov 0:a1a69d32f310 2975 pre_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 2976 dev, PreRangeTimeoutMicroSecs);
nikapov 0:a1a69d32f310 2977
nikapov 0:a1a69d32f310 2978 pre_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 2979 dev, PreRangeVcselPulsePeriod);
nikapov 0:a1a69d32f310 2980
nikapov 0:a1a69d32f310 2981 pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(
nikapov 0:a1a69d32f310 2982 dev, pre_range_timeout_micro_secs, pre_range_vcsel_pclks);
nikapov 0:a1a69d32f310 2983
nikapov 0:a1a69d32f310 2984 vcsel_width = 3;
nikapov 0:a1a69d32f310 2985 if (final_range_vcsel_pclks == 8) {
nikapov 0:a1a69d32f310 2986 vcsel_width = 2;
nikapov 0:a1a69d32f310 2987 }
nikapov 0:a1a69d32f310 2988
nikapov 0:a1a69d32f310 2989
nikapov 0:a1a69d32f310 2990 peak_vcsel_duration_us = vcsel_width * 2048 *
nikapov 0:a1a69d32f310 2991 (pre_range_macro_pclks + final_range_macro_pclks);
nikapov 0:a1a69d32f310 2992 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov 0:a1a69d32f310 2993 peak_vcsel_duration_us *= c_pll_period_ps;
nikapov 0:a1a69d32f310 2994 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov 0:a1a69d32f310 2995
nikapov 0:a1a69d32f310 2996 /* Fix1616 >> 8 = Fix2408 */
nikapov 0:a1a69d32f310 2997 total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8;
nikapov 0:a1a69d32f310 2998
nikapov 0:a1a69d32f310 2999 /* Fix2408 * uint32 = Fix2408 */
nikapov 0:a1a69d32f310 3000 vcsel_total_events_rtn = total_signal_rate_mcps *
nikapov 0:a1a69d32f310 3001 peak_vcsel_duration_us;
nikapov 0:a1a69d32f310 3002
nikapov 0:a1a69d32f310 3003 /* Fix2408 >> 8 = uint32 */
nikapov 0:a1a69d32f310 3004 vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
nikapov 0:a1a69d32f310 3005
nikapov 0:a1a69d32f310 3006 /* Fix2408 << 8 = Fix1616 = */
nikapov 0:a1a69d32f310 3007 total_signal_rate_mcps <<= 8;
nikapov 0:a1a69d32f310 3008 }
nikapov 0:a1a69d32f310 3009
nikapov 0:a1a69d32f310 3010 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 5:b95f6951f7d5 3011
nikapov 0:a1a69d32f310 3012 return status;
nikapov 0:a1a69d32f310 3013 }
nikapov 0:a1a69d32f310 3014
nikapov 0:a1a69d32f310 3015 if (peak_signal_rate_kcps == 0) {
nikapov 0:a1a69d32f310 3016 *p_sigma_estimate = c_sigma_est_max;
nikapov 0:a1a69d32f310 3017 PALDevDataSet(dev, SigmaEstimate, c_sigma_est_max);
nikapov 0:a1a69d32f310 3018 *p_dmax_mm = 0;
nikapov 0:a1a69d32f310 3019 } else {
nikapov 0:a1a69d32f310 3020 if (vcsel_total_events_rtn < 1) {
nikapov 0:a1a69d32f310 3021 vcsel_total_events_rtn = 1;
nikapov 0:a1a69d32f310 3022 }
nikapov 0:a1a69d32f310 3023
nikapov 0:a1a69d32f310 3024 sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
nikapov 0:a1a69d32f310 3025
nikapov 0:a1a69d32f310 3026 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
nikapov 0:a1a69d32f310 3027 sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps;
nikapov 0:a1a69d32f310 3028 if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) {
nikapov 0:a1a69d32f310 3029 /* Clip to prevent overflow. Will ensure safe
nikapov 0:a1a69d32f310 3030 * max result. */
nikapov 0:a1a69d32f310 3031 sigma_estimate_p2 = c_amb_to_signal_ratio_max;
nikapov 0:a1a69d32f310 3032 }
nikapov 0:a1a69d32f310 3033 sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
nikapov 0:a1a69d32f310 3034
nikapov 0:a1a69d32f310 3035 sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12);
nikapov 0:a1a69d32f310 3036
nikapov 0:a1a69d32f310 3037 /* uint32 * FixPoint1616 = FixPoint1616 */
nikapov 0:a1a69d32f310 3038 delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
nikapov 0:a1a69d32f310 3039 c_tof_per_mm_ps;
nikapov 0:a1a69d32f310 3040
nikapov 0:a1a69d32f310 3041 /*
nikapov 0:a1a69d32f310 3042 * vcselRate - xtalkCompRate
nikapov 0:a1a69d32f310 3043 * (uint32 << 16) - FixPoint1616 = FixPoint1616.
nikapov 0:a1a69d32f310 3044 * Divide result by 1000 to convert to mcps.
nikapov 0:a1a69d32f310 3045 * 500 is added to ensure rounding when integer division
nikapov 0:a1a69d32f310 3046 * truncates.
nikapov 0:a1a69d32f310 3047 */
nikapov 0:a1a69d32f310 3048 diff1_mcps = (((peak_signal_rate_kcps << 16) -
nikapov 0:a1a69d32f310 3049 2 * x_talk_comp_rate_kcps) + 500) / 1000;
nikapov 0:a1a69d32f310 3050
nikapov 0:a1a69d32f310 3051 /* vcselRate + xtalkCompRate */
nikapov 0:a1a69d32f310 3052 diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000;
nikapov 0:a1a69d32f310 3053
nikapov 0:a1a69d32f310 3054 /* Shift by 8 bits to increase resolution prior to the
nikapov 0:a1a69d32f310 3055 * division */
nikapov 0:a1a69d32f310 3056 diff1_mcps <<= 8;
nikapov 0:a1a69d32f310 3057
nikapov 0:a1a69d32f310 3058 /* FixPoint0824/FixPoint1616 = FixPoint2408 */
nikapov 0:a1a69d32f310 3059 // xTalkCorrection = abs(diff1_mcps/diff2_mcps);
nikapov 0:a1a69d32f310 3060 // abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
nikapov 0:a1a69d32f310 3061 x_talk_correction = diff1_mcps / diff2_mcps;
nikapov 0:a1a69d32f310 3062
nikapov 0:a1a69d32f310 3063 /* FixPoint2408 << 8 = FixPoint1616 */
nikapov 0:a1a69d32f310 3064 x_talk_correction <<= 8;
nikapov 0:a1a69d32f310 3065
nikapov 0:a1a69d32f310 3066 if (p_ranging_measurement_data->RangeStatus != 0) {
nikapov 0:a1a69d32f310 3067 pw_mult = 1 << 16;
nikapov 0:a1a69d32f310 3068 } else {
nikapov 0:a1a69d32f310 3069 /* FixPoint1616/uint32 = FixPoint1616 */
nikapov 0:a1a69d32f310 3070 pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
nikapov 0:a1a69d32f310 3071
nikapov 0:a1a69d32f310 3072 /*
nikapov 0:a1a69d32f310 3073 * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
nikapov 0:a1a69d32f310 3074 * values are small enough such that32 bits will not be
nikapov 0:a1a69d32f310 3075 * exceeded.
nikapov 0:a1a69d32f310 3076 */
nikapov 0:a1a69d32f310 3077 pw_mult *= ((1 << 16) - x_talk_correction);
nikapov 0:a1a69d32f310 3078
nikapov 0:a1a69d32f310 3079 /* (FixPoint3232 >> 16) = FixPoint1616 */
nikapov 0:a1a69d32f310 3080 pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
nikapov 0:a1a69d32f310 3081
nikapov 0:a1a69d32f310 3082 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
nikapov 0:a1a69d32f310 3083 pw_mult += (1 << 16);
nikapov 0:a1a69d32f310 3084
nikapov 0:a1a69d32f310 3085 /*
nikapov 0:a1a69d32f310 3086 * At this point the value will be 1.xx, therefore if we square
nikapov 0:a1a69d32f310 3087 * the value this will exceed 32 bits. To address this perform
nikapov 0:a1a69d32f310 3088 * a single shift to the right before the multiplication.
nikapov 0:a1a69d32f310 3089 */
nikapov 0:a1a69d32f310 3090 pw_mult >>= 1;
nikapov 0:a1a69d32f310 3091 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
nikapov 0:a1a69d32f310 3092 pw_mult = pw_mult * pw_mult;
nikapov 0:a1a69d32f310 3093
nikapov 0:a1a69d32f310 3094 /* (FixPoint3430 >> 14) = Fix1616 */
nikapov 0:a1a69d32f310 3095 pw_mult >>= 14;
nikapov 0:a1a69d32f310 3096 }
nikapov 0:a1a69d32f310 3097
nikapov 0:a1a69d32f310 3098 /* FixPoint1616 * uint32 = FixPoint1616 */
nikapov 0:a1a69d32f310 3099 sqr1 = pw_mult * sigma_estimate_p1;
nikapov 0:a1a69d32f310 3100
nikapov 0:a1a69d32f310 3101 /* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov 0:a1a69d32f310 3102 sqr1 = (sqr1 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 3103
nikapov 0:a1a69d32f310 3104 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov 0:a1a69d32f310 3105 sqr1 *= sqr1;
nikapov 0:a1a69d32f310 3106
nikapov 0:a1a69d32f310 3107 sqr2 = sigma_estimate_p2;
nikapov 0:a1a69d32f310 3108
nikapov 0:a1a69d32f310 3109 /* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov 0:a1a69d32f310 3110 sqr2 = (sqr2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 3111
nikapov 0:a1a69d32f310 3112 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov 0:a1a69d32f310 3113 sqr2 *= sqr2;
nikapov 0:a1a69d32f310 3114
nikapov 0:a1a69d32f310 3115 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
nikapov 0:a1a69d32f310 3116 sqr_sum = sqr1 + sqr2;
nikapov 0:a1a69d32f310 3117
nikapov 0:a1a69d32f310 3118 /* SQRT(FixPoin6400) = FixPoint3200 */
nikapov 0:a1a69d32f310 3119 sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum);
nikapov 0:a1a69d32f310 3120
nikapov 0:a1a69d32f310 3121 /* (FixPoint3200 << 16) = FixPoint1616 */
nikapov 0:a1a69d32f310 3122 sqrt_result_centi_ns <<= 16;
nikapov 0:a1a69d32f310 3123
nikapov 0:a1a69d32f310 3124 /*
nikapov 0:a1a69d32f310 3125 * Note that the Speed Of Light is expressed in um per 1E-10
nikapov 0:a1a69d32f310 3126 * seconds (2997) Therefore to get mm/ns we have to divide by
nikapov 0:a1a69d32f310 3127 * 10000
nikapov 0:a1a69d32f310 3128 */
nikapov 0:a1a69d32f310 3129 sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
nikapov 0:a1a69d32f310 3130 sigma_estimate_p3);
nikapov 0:a1a69d32f310 3131 sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
nikapov 0:a1a69d32f310 3132
nikapov 0:a1a69d32f310 3133 /* Add 5000 before dividing by 10000 to ensure rounding. */
nikapov 0:a1a69d32f310 3134 sigma_est_rtn += 5000;
nikapov 0:a1a69d32f310 3135 sigma_est_rtn /= 10000;
nikapov 0:a1a69d32f310 3136
nikapov 0:a1a69d32f310 3137 if (sigma_est_rtn > c_sigma_est_rtn_max) {
nikapov 0:a1a69d32f310 3138 /* Clip to prevent overflow. Will ensure safe
nikapov 0:a1a69d32f310 3139 * max result. */
nikapov 0:a1a69d32f310 3140 sigma_est_rtn = c_sigma_est_rtn_max;
nikapov 0:a1a69d32f310 3141 }
nikapov 0:a1a69d32f310 3142 final_range_integration_time_milli_secs =
nikapov 0:a1a69d32f310 3143 (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
nikapov 0:a1a69d32f310 3144
nikapov 0:a1a69d32f310 3145 /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
nikapov 0:a1a69d32f310 3146 * sqrt(FixPoint1616/int) = FixPoint2408)
nikapov 0:a1a69d32f310 3147 */
nikapov 0:a1a69d32f310 3148 sigma_est_ref =
nikapov 0:a1a69d32f310 3149 VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs +
nikapov 0:a1a69d32f310 3150 final_range_integration_time_milli_secs / 2) /
nikapov 0:a1a69d32f310 3151 final_range_integration_time_milli_secs);
nikapov 0:a1a69d32f310 3152
nikapov 0:a1a69d32f310 3153 /* FixPoint2408 << 8 = FixPoint1616 */
nikapov 0:a1a69d32f310 3154 sigma_est_ref <<= 8;
nikapov 0:a1a69d32f310 3155 sigma_est_ref = (sigma_est_ref + 500) / 1000;
nikapov 0:a1a69d32f310 3156
nikapov 0:a1a69d32f310 3157 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov 0:a1a69d32f310 3158 sqr1 = sigma_est_rtn * sigma_est_rtn;
nikapov 0:a1a69d32f310 3159 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov 0:a1a69d32f310 3160 sqr2 = sigma_est_ref * sigma_est_ref;
nikapov 0:a1a69d32f310 3161
nikapov 0:a1a69d32f310 3162 /* sqrt(FixPoint3232) = FixPoint1616 */
nikapov 0:a1a69d32f310 3163 sqrt_result = VL53L0X_isqrt((sqr1 + sqr2));
nikapov 0:a1a69d32f310 3164 /*
nikapov 0:a1a69d32f310 3165 * Note that the Shift by 4 bits increases resolution prior to
nikapov 0:a1a69d32f310 3166 * the sqrt, therefore the result must be shifted by 2 bits to
nikapov 0:a1a69d32f310 3167 * the right to revert back to the FixPoint1616 format.
nikapov 0:a1a69d32f310 3168 */
nikapov 0:a1a69d32f310 3169
nikapov 0:a1a69d32f310 3170 sigma_estimate = 1000 * sqrt_result;
nikapov 0:a1a69d32f310 3171
nikapov 0:a1a69d32f310 3172 if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
nikapov 0:a1a69d32f310 3173 (sigma_estimate > c_sigma_est_max)) {
nikapov 0:a1a69d32f310 3174 sigma_estimate = c_sigma_est_max;
nikapov 0:a1a69d32f310 3175 }
nikapov 0:a1a69d32f310 3176
nikapov 0:a1a69d32f310 3177 *p_sigma_estimate = (uint32_t)(sigma_estimate);
nikapov 0:a1a69d32f310 3178 PALDevDataSet(dev, SigmaEstimate, *p_sigma_estimate);
nikapov 0:a1a69d32f310 3179 status = VL53L0X_calc_dmax(
nikapov 0:a1a69d32f310 3180 dev,
nikapov 0:a1a69d32f310 3181 total_signal_rate_mcps,
nikapov 0:a1a69d32f310 3182 corrected_signal_rate_mcps,
nikapov 0:a1a69d32f310 3183 pw_mult,
nikapov 0:a1a69d32f310 3184 sigma_estimate_p1,
nikapov 0:a1a69d32f310 3185 sigma_estimate_p2,
nikapov 0:a1a69d32f310 3186 peak_vcsel_duration_us,
nikapov 0:a1a69d32f310 3187 p_dmax_mm);
nikapov 0:a1a69d32f310 3188 }
nikapov 0:a1a69d32f310 3189
sepp_nepp 5:b95f6951f7d5 3190
nikapov 0:a1a69d32f310 3191 return status;
nikapov 0:a1a69d32f310 3192 }
nikapov 0:a1a69d32f310 3193
nikapov 0:a1a69d32f310 3194 VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 3195 uint8_t device_range_status,
nikapov 0:a1a69d32f310 3196 FixPoint1616_t signal_rate,
nikapov 0:a1a69d32f310 3197 uint16_t effective_spad_rtn_count,
nikapov 0:a1a69d32f310 3198 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov 0:a1a69d32f310 3199 uint8_t *p_pal_range_status)
nikapov 0:a1a69d32f310 3200 {
nikapov 0:a1a69d32f310 3201 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 3202 uint8_t none_flag;
nikapov 0:a1a69d32f310 3203 uint8_t sigma_limitflag = 0;
nikapov 0:a1a69d32f310 3204 uint8_t signal_ref_clipflag = 0;
nikapov 0:a1a69d32f310 3205 uint8_t range_ignore_thresholdflag = 0;
nikapov 0:a1a69d32f310 3206 uint8_t sigma_limit_check_enable = 0;
nikapov 0:a1a69d32f310 3207 uint8_t signal_rate_final_range_limit_check_enable = 0;
nikapov 0:a1a69d32f310 3208 uint8_t signal_ref_clip_limit_check_enable = 0;
nikapov 0:a1a69d32f310 3209 uint8_t range_ignore_threshold_limit_check_enable = 0;
nikapov 0:a1a69d32f310 3210 FixPoint1616_t sigma_estimate;
nikapov 0:a1a69d32f310 3211 FixPoint1616_t sigma_limit_value;
nikapov 0:a1a69d32f310 3212 FixPoint1616_t signal_ref_clip_value;
nikapov 0:a1a69d32f310 3213 FixPoint1616_t range_ignore_threshold_value;
nikapov 0:a1a69d32f310 3214 FixPoint1616_t signal_rate_per_spad;
nikapov 0:a1a69d32f310 3215 uint8_t device_range_status_internal = 0;
nikapov 0:a1a69d32f310 3216 uint16_t tmp_word = 0;
nikapov 0:a1a69d32f310 3217 uint8_t temp8;
nikapov 0:a1a69d32f310 3218 uint32_t dmax_mm = 0;
nikapov 0:a1a69d32f310 3219 FixPoint1616_t last_signal_ref_mcps;
nikapov 0:a1a69d32f310 3220
sepp_nepp 5:b95f6951f7d5 3221
nikapov 0:a1a69d32f310 3222
nikapov 0:a1a69d32f310 3223
nikapov 0:a1a69d32f310 3224 /*
nikapov 0:a1a69d32f310 3225 * VL53L0X has a good ranging when the value of the
nikapov 0:a1a69d32f310 3226 * DeviceRangeStatus = 11. This function will replace the value 0 with
nikapov 0:a1a69d32f310 3227 * the value 11 in the DeviceRangeStatus.
nikapov 0:a1a69d32f310 3228 * In addition, the SigmaEstimator is not included in the VL53L0X
nikapov 0:a1a69d32f310 3229 * DeviceRangeStatus, this will be added in the PalRangeStatus.
nikapov 0:a1a69d32f310 3230 */
nikapov 0:a1a69d32f310 3231
nikapov 0:a1a69d32f310 3232 device_range_status_internal = ((device_range_status & 0x78) >> 3);
nikapov 0:a1a69d32f310 3233
nikapov 0:a1a69d32f310 3234 if (device_range_status_internal == 0 ||
nikapov 0:a1a69d32f310 3235 device_range_status_internal == 5 ||
nikapov 0:a1a69d32f310 3236 device_range_status_internal == 7 ||
nikapov 0:a1a69d32f310 3237 device_range_status_internal == 12 ||
nikapov 0:a1a69d32f310 3238 device_range_status_internal == 13 ||
nikapov 0:a1a69d32f310 3239 device_range_status_internal == 14 ||
nikapov 0:a1a69d32f310 3240 device_range_status_internal == 15
nikapov 0:a1a69d32f310 3241 ) {
nikapov 0:a1a69d32f310 3242 none_flag = 1;
nikapov 0:a1a69d32f310 3243 } else {
nikapov 0:a1a69d32f310 3244 none_flag = 0;
nikapov 0:a1a69d32f310 3245 }
nikapov 0:a1a69d32f310 3246
nikapov 0:a1a69d32f310 3247 /*
nikapov 0:a1a69d32f310 3248 * Check if Sigma limit is enabled, if yes then do comparison with limit
nikapov 0:a1a69d32f310 3249 * value and put the result back into pPalRangeStatus.
nikapov 0:a1a69d32f310 3250 */
nikapov 0:a1a69d32f310 3251 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3252 status = VL53L0X_get_limit_check_enable(dev,
nikapov 0:a1a69d32f310 3253 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov 0:a1a69d32f310 3254 &sigma_limit_check_enable);
Davidroid 2:d07edeaff6f1 3255 }
nikapov 0:a1a69d32f310 3256
nikapov 0:a1a69d32f310 3257 if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
nikapov 0:a1a69d32f310 3258 /*
nikapov 0:a1a69d32f310 3259 * compute the Sigma and check with limit
nikapov 0:a1a69d32f310 3260 */
nikapov 0:a1a69d32f310 3261 status = VL53L0X_calc_sigma_estimate(
nikapov 0:a1a69d32f310 3262 dev,
nikapov 0:a1a69d32f310 3263 p_ranging_measurement_data,
nikapov 0:a1a69d32f310 3264 &sigma_estimate,
nikapov 0:a1a69d32f310 3265 &dmax_mm);
nikapov 0:a1a69d32f310 3266 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3267 p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
nikapov 0:a1a69d32f310 3268 }
nikapov 0:a1a69d32f310 3269
nikapov 0:a1a69d32f310 3270 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3271 status = VL53L0X_get_limit_check_value(dev,
nikapov 0:a1a69d32f310 3272 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov 0:a1a69d32f310 3273 &sigma_limit_value);
nikapov 0:a1a69d32f310 3274
nikapov 0:a1a69d32f310 3275 if ((sigma_limit_value > 0) &&
nikapov 0:a1a69d32f310 3276 (sigma_estimate > sigma_limit_value)) {
nikapov 0:a1a69d32f310 3277 /* Limit Fail */
nikapov 0:a1a69d32f310 3278 sigma_limitflag = 1;
nikapov 0:a1a69d32f310 3279 }
nikapov 0:a1a69d32f310 3280 }
nikapov 0:a1a69d32f310 3281 }
nikapov 0:a1a69d32f310 3282
nikapov 0:a1a69d32f310 3283 /*
nikapov 0:a1a69d32f310 3284 * Check if Signal ref clip limit is enabled, if yes then do comparison
nikapov 0:a1a69d32f310 3285 * with limit value and put the result back into pPalRangeStatus.
nikapov 0:a1a69d32f310 3286 */
nikapov 0:a1a69d32f310 3287 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3288 status = VL53L0X_get_limit_check_enable(dev,
nikapov 0:a1a69d32f310 3289 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov 0:a1a69d32f310 3290 &signal_ref_clip_limit_check_enable);
nikapov 0:a1a69d32f310 3291 }
nikapov 0:a1a69d32f310 3292
nikapov 0:a1a69d32f310 3293 if ((signal_ref_clip_limit_check_enable != 0) &&
nikapov 0:a1a69d32f310 3294 (status == VL53L0X_ERROR_NONE)) {
nikapov 0:a1a69d32f310 3295
nikapov 0:a1a69d32f310 3296 status = VL53L0X_get_limit_check_value(dev,
nikapov 0:a1a69d32f310 3297 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov 0:a1a69d32f310 3298 &signal_ref_clip_value);
nikapov 0:a1a69d32f310 3299
nikapov 0:a1a69d32f310 3300 /* Read LastSignalRefMcps from device */
nikapov 0:a1a69d32f310 3301 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3302 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 3303 }
nikapov 0:a1a69d32f310 3304
nikapov 0:a1a69d32f310 3305 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3306 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 3307 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
nikapov 0:a1a69d32f310 3308 &tmp_word);
nikapov 0:a1a69d32f310 3309 }
nikapov 0:a1a69d32f310 3310
nikapov 0:a1a69d32f310 3311 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3312 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 3313 }
nikapov 0:a1a69d32f310 3314
nikapov 0:a1a69d32f310 3315 last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word);
nikapov 0:a1a69d32f310 3316 PALDevDataSet(dev, LastSignalRefMcps, last_signal_ref_mcps);
nikapov 0:a1a69d32f310 3317
nikapov 0:a1a69d32f310 3318 if ((signal_ref_clip_value > 0) &&
nikapov 0:a1a69d32f310 3319 (last_signal_ref_mcps > signal_ref_clip_value)) {
nikapov 0:a1a69d32f310 3320 /* Limit Fail */
nikapov 0:a1a69d32f310 3321 signal_ref_clipflag = 1;
nikapov 0:a1a69d32f310 3322 }
nikapov 0:a1a69d32f310 3323 }
nikapov 0:a1a69d32f310 3324
nikapov 0:a1a69d32f310 3325 /*
nikapov 0:a1a69d32f310 3326 * Check if Signal ref clip limit is enabled, if yes then do comparison
nikapov 0:a1a69d32f310 3327 * with limit value and put the result back into pPalRangeStatus.
nikapov 0:a1a69d32f310 3328 * EffectiveSpadRtnCount has a format 8.8
nikapov 0:a1a69d32f310 3329 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
nikapov 0:a1a69d32f310 3330 */
Davidroid 2:d07edeaff6f1 3331 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3332 status = VL53L0X_get_limit_check_enable(dev,
nikapov 0:a1a69d32f310 3333 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov 0:a1a69d32f310 3334 &range_ignore_threshold_limit_check_enable);
Davidroid 2:d07edeaff6f1 3335 }
nikapov 0:a1a69d32f310 3336
nikapov 0:a1a69d32f310 3337 if ((range_ignore_threshold_limit_check_enable != 0) &&
nikapov 0:a1a69d32f310 3338 (status == VL53L0X_ERROR_NONE)) {
nikapov 0:a1a69d32f310 3339
nikapov 0:a1a69d32f310 3340 /* Compute the signal rate per spad */
nikapov 0:a1a69d32f310 3341 if (effective_spad_rtn_count == 0) {
nikapov 0:a1a69d32f310 3342 signal_rate_per_spad = 0;
nikapov 0:a1a69d32f310 3343 } else {
nikapov 0:a1a69d32f310 3344 signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate)
nikapov 0:a1a69d32f310 3345 / effective_spad_rtn_count);
nikapov 0:a1a69d32f310 3346 }
nikapov 0:a1a69d32f310 3347
nikapov 0:a1a69d32f310 3348 status = VL53L0X_get_limit_check_value(dev,
nikapov 0:a1a69d32f310 3349 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov 0:a1a69d32f310 3350 &range_ignore_threshold_value);
nikapov 0:a1a69d32f310 3351
nikapov 0:a1a69d32f310 3352 if ((range_ignore_threshold_value > 0) &&
nikapov 0:a1a69d32f310 3353 (signal_rate_per_spad < range_ignore_threshold_value)) {
nikapov 0:a1a69d32f310 3354 /* Limit Fail add 2^6 to range status */
nikapov 0:a1a69d32f310 3355 range_ignore_thresholdflag = 1;
nikapov 0:a1a69d32f310 3356 }
nikapov 0:a1a69d32f310 3357 }
nikapov 0:a1a69d32f310 3358
nikapov 0:a1a69d32f310 3359 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3360 if (none_flag == 1) {
nikapov 0:a1a69d32f310 3361 *p_pal_range_status = 255; /* NONE */
nikapov 0:a1a69d32f310 3362 } else if (device_range_status_internal == 1 ||
nikapov 0:a1a69d32f310 3363 device_range_status_internal == 2 ||
nikapov 0:a1a69d32f310 3364 device_range_status_internal == 3) {
nikapov 0:a1a69d32f310 3365 *p_pal_range_status = 5; /* HW fail */
nikapov 0:a1a69d32f310 3366 } else if (device_range_status_internal == 6 ||
nikapov 0:a1a69d32f310 3367 device_range_status_internal == 9) {
nikapov 0:a1a69d32f310 3368 *p_pal_range_status = 4; /* Phase fail */
nikapov 0:a1a69d32f310 3369 } else if (device_range_status_internal == 8 ||
nikapov 0:a1a69d32f310 3370 device_range_status_internal == 10 ||
nikapov 0:a1a69d32f310 3371 signal_ref_clipflag == 1) {
nikapov 0:a1a69d32f310 3372 *p_pal_range_status = 3; /* Min range */
nikapov 0:a1a69d32f310 3373 } else if (device_range_status_internal == 4 ||
nikapov 0:a1a69d32f310 3374 range_ignore_thresholdflag == 1) {
nikapov 0:a1a69d32f310 3375 *p_pal_range_status = 2; /* Signal Fail */
nikapov 0:a1a69d32f310 3376 } else if (sigma_limitflag == 1) {
nikapov 0:a1a69d32f310 3377 *p_pal_range_status = 1; /* Sigma Fail */
nikapov 0:a1a69d32f310 3378 } else {
nikapov 0:a1a69d32f310 3379 *p_pal_range_status = 0; /* Range Valid */
nikapov 0:a1a69d32f310 3380 }
nikapov 0:a1a69d32f310 3381 }
nikapov 0:a1a69d32f310 3382
nikapov 0:a1a69d32f310 3383 /* DMAX only relevant during range error */
Davidroid 3:e9269ff624ed 3384 if (*p_pal_range_status == 0) {
nikapov 0:a1a69d32f310 3385 p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
Davidroid 3:e9269ff624ed 3386 }
nikapov 0:a1a69d32f310 3387
nikapov 0:a1a69d32f310 3388 /* fill the Limit Check Status */
nikapov 0:a1a69d32f310 3389
nikapov 0:a1a69d32f310 3390 status = VL53L0X_get_limit_check_enable(dev,
nikapov 0:a1a69d32f310 3391 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov 0:a1a69d32f310 3392 &signal_rate_final_range_limit_check_enable);
nikapov 0:a1a69d32f310 3393
nikapov 0:a1a69d32f310 3394 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3395 if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1)) {
nikapov 0:a1a69d32f310 3396 temp8 = 1;
nikapov 0:a1a69d32f310 3397 } else {
nikapov 0:a1a69d32f310 3398 temp8 = 0;
nikapov 0:a1a69d32f310 3399 }
nikapov 0:a1a69d32f310 3400 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov 0:a1a69d32f310 3401 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp8);
nikapov 0:a1a69d32f310 3402
nikapov 0:a1a69d32f310 3403 if ((device_range_status_internal == 4) ||
nikapov 0:a1a69d32f310 3404 (signal_rate_final_range_limit_check_enable == 0)) {
nikapov 0:a1a69d32f310 3405 temp8 = 1;
nikapov 0:a1a69d32f310 3406 } else {
nikapov 0:a1a69d32f310 3407 temp8 = 0;
nikapov 0:a1a69d32f310 3408 }
nikapov 0:a1a69d32f310 3409 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov 0:a1a69d32f310 3410 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov 0:a1a69d32f310 3411 temp8);
nikapov 0:a1a69d32f310 3412
nikapov 0:a1a69d32f310 3413 if ((signal_ref_clip_limit_check_enable == 0) ||
nikapov 0:a1a69d32f310 3414 (signal_ref_clipflag == 1)) {
nikapov 0:a1a69d32f310 3415 temp8 = 1;
nikapov 0:a1a69d32f310 3416 } else {
nikapov 0:a1a69d32f310 3417 temp8 = 0;
nikapov 0:a1a69d32f310 3418 }
nikapov 0:a1a69d32f310 3419
nikapov 0:a1a69d32f310 3420 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov 0:a1a69d32f310 3421 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp8);
nikapov 0:a1a69d32f310 3422
nikapov 0:a1a69d32f310 3423 if ((range_ignore_threshold_limit_check_enable == 0) ||
nikapov 0:a1a69d32f310 3424 (range_ignore_thresholdflag == 1)) {
nikapov 0:a1a69d32f310 3425 temp8 = 1;
nikapov 0:a1a69d32f310 3426 } else {
nikapov 0:a1a69d32f310 3427 temp8 = 0;
nikapov 0:a1a69d32f310 3428 }
nikapov 0:a1a69d32f310 3429
nikapov 0:a1a69d32f310 3430 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov 0:a1a69d32f310 3431 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov 0:a1a69d32f310 3432 temp8);
nikapov 0:a1a69d32f310 3433 }
nikapov 0:a1a69d32f310 3434
sepp_nepp 5:b95f6951f7d5 3435
nikapov 0:a1a69d32f310 3436 return status;
nikapov 0:a1a69d32f310 3437
nikapov 0:a1a69d32f310 3438 }
nikapov 0:a1a69d32f310 3439
nikapov 0:a1a69d32f310 3440 VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 3441 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
nikapov 0:a1a69d32f310 3442 {
nikapov 0:a1a69d32f310 3443 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 3444 uint8_t device_range_status;
nikapov 0:a1a69d32f310 3445 uint8_t range_fractional_enable;
nikapov 0:a1a69d32f310 3446 uint8_t pal_range_status;
nikapov 0:a1a69d32f310 3447 uint8_t x_talk_compensation_enable;
nikapov 0:a1a69d32f310 3448 uint16_t ambient_rate;
nikapov 0:a1a69d32f310 3449 FixPoint1616_t signal_rate;
nikapov 0:a1a69d32f310 3450 uint16_t x_talk_compensation_rate_mega_cps;
nikapov 0:a1a69d32f310 3451 uint16_t effective_spad_rtn_count;
nikapov 0:a1a69d32f310 3452 uint16_t tmpuint16;
nikapov 0:a1a69d32f310 3453 uint16_t xtalk_range_milli_meter;
nikapov 0:a1a69d32f310 3454 uint16_t linearity_corrective_gain;
nikapov 0:a1a69d32f310 3455 uint8_t localBuffer[12];
nikapov 0:a1a69d32f310 3456 VL53L0X_RangingMeasurementData_t last_range_data_buffer;
nikapov 0:a1a69d32f310 3457
sepp_nepp 5:b95f6951f7d5 3458
nikapov 0:a1a69d32f310 3459
nikapov 0:a1a69d32f310 3460 /*
nikapov 0:a1a69d32f310 3461 * use multi read even if some registers are not useful, result will
nikapov 0:a1a69d32f310 3462 * be more efficient
nikapov 0:a1a69d32f310 3463 * start reading at 0x14 dec20
nikapov 0:a1a69d32f310 3464 * end reading at 0x21 dec33 total 14 bytes to read
nikapov 0:a1a69d32f310 3465 */
nikapov 0:a1a69d32f310 3466 status = VL53L0X_read_multi(dev, 0x14, localBuffer, 12);
nikapov 0:a1a69d32f310 3467
nikapov 0:a1a69d32f310 3468 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3469
nikapov 0:a1a69d32f310 3470 p_ranging_measurement_data->ZoneId = 0; /* Only one zone */
nikapov 0:a1a69d32f310 3471 p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */
nikapov 0:a1a69d32f310 3472
nikapov 0:a1a69d32f310 3473 tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]);
nikapov 0:a1a69d32f310 3474 /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
nikapov 0:a1a69d32f310 3475 *(format 11.2) else no fractional
nikapov 0:a1a69d32f310 3476 */
nikapov 0:a1a69d32f310 3477
nikapov 0:a1a69d32f310 3478 p_ranging_measurement_data->MeasurementTimeUsec = 0;
nikapov 0:a1a69d32f310 3479
nikapov 0:a1a69d32f310 3480 signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
nikapov 0:a1a69d32f310 3481 VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
nikapov 0:a1a69d32f310 3482 /* peak_signal_count_rate_rtn_mcps */
nikapov 0:a1a69d32f310 3483 p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
nikapov 0:a1a69d32f310 3484
nikapov 0:a1a69d32f310 3485 ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
nikapov 0:a1a69d32f310 3486 p_ranging_measurement_data->AmbientRateRtnMegaCps =
nikapov 0:a1a69d32f310 3487 VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate);
nikapov 0:a1a69d32f310 3488
nikapov 0:a1a69d32f310 3489 effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3],
nikapov 0:a1a69d32f310 3490 localBuffer[2]);
nikapov 0:a1a69d32f310 3491 /* EffectiveSpadRtnCount is 8.8 format */
nikapov 0:a1a69d32f310 3492 p_ranging_measurement_data->EffectiveSpadRtnCount =
nikapov 0:a1a69d32f310 3493 effective_spad_rtn_count;
nikapov 0:a1a69d32f310 3494
nikapov 0:a1a69d32f310 3495 device_range_status = localBuffer[0];
nikapov 0:a1a69d32f310 3496
nikapov 0:a1a69d32f310 3497 /* Get Linearity Corrective Gain */
nikapov 0:a1a69d32f310 3498 linearity_corrective_gain = PALDevDataGet(dev,
nikapov 0:a1a69d32f310 3499 LinearityCorrectiveGain);
nikapov 0:a1a69d32f310 3500
nikapov 0:a1a69d32f310 3501 /* Get ranging configuration */
nikapov 0:a1a69d32f310 3502 range_fractional_enable = PALDevDataGet(dev,
nikapov 0:a1a69d32f310 3503 RangeFractionalEnable);
nikapov 0:a1a69d32f310 3504
nikapov 0:a1a69d32f310 3505 if (linearity_corrective_gain != 1000) {
nikapov 0:a1a69d32f310 3506
nikapov 0:a1a69d32f310 3507 tmpuint16 = (uint16_t)((linearity_corrective_gain
nikapov 0:a1a69d32f310 3508 * tmpuint16 + 500) / 1000);
nikapov 0:a1a69d32f310 3509
nikapov 0:a1a69d32f310 3510 /* Implement Xtalk */
nikapov 0:a1a69d32f310 3511 VL53L0X_GETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 3512 XTalkCompensationRateMegaCps,
nikapov 0:a1a69d32f310 3513 x_talk_compensation_rate_mega_cps);
nikapov 0:a1a69d32f310 3514 VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable,
nikapov 0:a1a69d32f310 3515 x_talk_compensation_enable);
nikapov 0:a1a69d32f310 3516
nikapov 0:a1a69d32f310 3517 if (x_talk_compensation_enable) {
nikapov 0:a1a69d32f310 3518
nikapov 0:a1a69d32f310 3519 if ((signal_rate
nikapov 0:a1a69d32f310 3520 - ((x_talk_compensation_rate_mega_cps
nikapov 0:a1a69d32f310 3521 * effective_spad_rtn_count) >> 8))
nikapov 0:a1a69d32f310 3522 <= 0) {
nikapov 0:a1a69d32f310 3523 if (range_fractional_enable) {
nikapov 0:a1a69d32f310 3524 xtalk_range_milli_meter = 8888;
nikapov 0:a1a69d32f310 3525 } else {
nikapov 0:a1a69d32f310 3526 xtalk_range_milli_meter = 8888 << 2;
nikapov 0:a1a69d32f310 3527 }
nikapov 0:a1a69d32f310 3528 } else {
nikapov 0:a1a69d32f310 3529 xtalk_range_milli_meter =
nikapov 0:a1a69d32f310 3530 (tmpuint16 * signal_rate)
nikapov 0:a1a69d32f310 3531 / (signal_rate
nikapov 0:a1a69d32f310 3532 - ((x_talk_compensation_rate_mega_cps
nikapov 0:a1a69d32f310 3533 * effective_spad_rtn_count)
nikapov 0:a1a69d32f310 3534 >> 8));
nikapov 0:a1a69d32f310 3535 }
nikapov 0:a1a69d32f310 3536
nikapov 0:a1a69d32f310 3537 tmpuint16 = xtalk_range_milli_meter;
nikapov 0:a1a69d32f310 3538 }
nikapov 0:a1a69d32f310 3539
nikapov 0:a1a69d32f310 3540 }
nikapov 0:a1a69d32f310 3541
nikapov 0:a1a69d32f310 3542 if (range_fractional_enable) {
nikapov 0:a1a69d32f310 3543 p_ranging_measurement_data->RangeMilliMeter =
nikapov 0:a1a69d32f310 3544 (uint16_t)((tmpuint16) >> 2);
nikapov 0:a1a69d32f310 3545 p_ranging_measurement_data->RangeFractionalPart =
nikapov 0:a1a69d32f310 3546 (uint8_t)((tmpuint16 & 0x03) << 6);
nikapov 0:a1a69d32f310 3547 } else {
nikapov 0:a1a69d32f310 3548 p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
nikapov 0:a1a69d32f310 3549 p_ranging_measurement_data->RangeFractionalPart = 0;
nikapov 0:a1a69d32f310 3550 }
nikapov 0:a1a69d32f310 3551
nikapov 0:a1a69d32f310 3552 /*
nikapov 0:a1a69d32f310 3553 * For a standard definition of RangeStatus, this should
nikapov 0:a1a69d32f310 3554 * return 0 in case of good result after a ranging
nikapov 0:a1a69d32f310 3555 * The range status depends on the device so call a device
nikapov 0:a1a69d32f310 3556 * specific function to obtain the right Status.
nikapov 0:a1a69d32f310 3557 */
nikapov 0:a1a69d32f310 3558 status |= VL53L0X_get_pal_range_status(dev, device_range_status,
nikapov 0:a1a69d32f310 3559 signal_rate, effective_spad_rtn_count,
nikapov 0:a1a69d32f310 3560 p_ranging_measurement_data, &pal_range_status);
nikapov 0:a1a69d32f310 3561
nikapov 0:a1a69d32f310 3562 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3563 p_ranging_measurement_data->RangeStatus = pal_range_status;
nikapov 0:a1a69d32f310 3564 }
nikapov 0:a1a69d32f310 3565
nikapov 0:a1a69d32f310 3566 }
nikapov 0:a1a69d32f310 3567
nikapov 0:a1a69d32f310 3568 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3569 /* Copy last read data into Dev buffer */
nikapov 0:a1a69d32f310 3570 last_range_data_buffer = PALDevDataGet(dev, LastRangeMeasure);
nikapov 0:a1a69d32f310 3571
nikapov 0:a1a69d32f310 3572 last_range_data_buffer.RangeMilliMeter =
nikapov 0:a1a69d32f310 3573 p_ranging_measurement_data->RangeMilliMeter;
nikapov 0:a1a69d32f310 3574 last_range_data_buffer.RangeFractionalPart =
nikapov 0:a1a69d32f310 3575 p_ranging_measurement_data->RangeFractionalPart;
nikapov 0:a1a69d32f310 3576 last_range_data_buffer.RangeDMaxMilliMeter =
nikapov 0:a1a69d32f310 3577 p_ranging_measurement_data->RangeDMaxMilliMeter;
nikapov 0:a1a69d32f310 3578 last_range_data_buffer.MeasurementTimeUsec =
nikapov 0:a1a69d32f310 3579 p_ranging_measurement_data->MeasurementTimeUsec;
nikapov 0:a1a69d32f310 3580 last_range_data_buffer.SignalRateRtnMegaCps =
nikapov 0:a1a69d32f310 3581 p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov 0:a1a69d32f310 3582 last_range_data_buffer.AmbientRateRtnMegaCps =
nikapov 0:a1a69d32f310 3583 p_ranging_measurement_data->AmbientRateRtnMegaCps;
nikapov 0:a1a69d32f310 3584 last_range_data_buffer.EffectiveSpadRtnCount =
nikapov 0:a1a69d32f310 3585 p_ranging_measurement_data->EffectiveSpadRtnCount;
nikapov 0:a1a69d32f310 3586 last_range_data_buffer.RangeStatus =
nikapov 0:a1a69d32f310 3587 p_ranging_measurement_data->RangeStatus;
nikapov 0:a1a69d32f310 3588
nikapov 0:a1a69d32f310 3589 PALDevDataSet(dev, LastRangeMeasure, last_range_data_buffer);
nikapov 0:a1a69d32f310 3590 }
nikapov 0:a1a69d32f310 3591
sepp_nepp 5:b95f6951f7d5 3592
nikapov 0:a1a69d32f310 3593 return status;
nikapov 0:a1a69d32f310 3594 }
nikapov 0:a1a69d32f310 3595
nikapov 0:a1a69d32f310 3596 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 3597 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
nikapov 0:a1a69d32f310 3598 {
nikapov 0:a1a69d32f310 3599 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 3600
sepp_nepp 5:b95f6951f7d5 3601
nikapov 0:a1a69d32f310 3602
nikapov 0:a1a69d32f310 3603 /* This function will do a complete single ranging
nikapov 0:a1a69d32f310 3604 * Here we fix the mode! */
nikapov 0:a1a69d32f310 3605 status = VL53L0X_set_device_mode(dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
nikapov 0:a1a69d32f310 3606
nikapov 0:a1a69d32f310 3607 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3608 status = VL53L0X_perform_single_measurement(dev);
nikapov 0:a1a69d32f310 3609 }
nikapov 0:a1a69d32f310 3610
nikapov 0:a1a69d32f310 3611 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3612 status = VL53L0X_get_ranging_measurement_data(dev,
nikapov 0:a1a69d32f310 3613 p_ranging_measurement_data);
nikapov 0:a1a69d32f310 3614 }
nikapov 0:a1a69d32f310 3615
nikapov 0:a1a69d32f310 3616 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3617 status = VL53L0X_clear_interrupt_mask(dev, 0);
nikapov 0:a1a69d32f310 3618 }
nikapov 0:a1a69d32f310 3619
sepp_nepp 5:b95f6951f7d5 3620
nikapov 0:a1a69d32f310 3621 return status;
nikapov 0:a1a69d32f310 3622 }
nikapov 0:a1a69d32f310 3623
nikapov 0:a1a69d32f310 3624 VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 3625 uint16_t *p_ref_signal_rate)
nikapov 0:a1a69d32f310 3626 {
nikapov 0:a1a69d32f310 3627 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 3628 VL53L0X_RangingMeasurementData_t ranging_measurement_data;
nikapov 0:a1a69d32f310 3629
nikapov 0:a1a69d32f310 3630 uint8_t sequence_config = 0;
nikapov 0:a1a69d32f310 3631
nikapov 0:a1a69d32f310 3632 /* store the value of the sequence config,
nikapov 0:a1a69d32f310 3633 * this will be reset before the end of the function
nikapov 0:a1a69d32f310 3634 */
nikapov 0:a1a69d32f310 3635
nikapov 0:a1a69d32f310 3636 sequence_config = PALDevDataGet(dev, SequenceConfig);
nikapov 0:a1a69d32f310 3637
nikapov 0:a1a69d32f310 3638 /*
nikapov 0:a1a69d32f310 3639 * This function performs a reference signal rate measurement.
nikapov 0:a1a69d32f310 3640 */
nikapov 0:a1a69d32f310 3641 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3642 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3643 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
nikapov 0:a1a69d32f310 3644 }
nikapov 0:a1a69d32f310 3645
nikapov 0:a1a69d32f310 3646 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3647 status = VL53L0X_perform_single_ranging_measurement(dev,
nikapov 0:a1a69d32f310 3648 &ranging_measurement_data);
nikapov 0:a1a69d32f310 3649 }
nikapov 0:a1a69d32f310 3650
nikapov 0:a1a69d32f310 3651 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3652 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 3653 }
nikapov 0:a1a69d32f310 3654
nikapov 0:a1a69d32f310 3655 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3656 status = VL53L0X_read_word(dev,
nikapov 0:a1a69d32f310 3657 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
nikapov 0:a1a69d32f310 3658 p_ref_signal_rate);
Davidroid 2:d07edeaff6f1 3659 }
nikapov 0:a1a69d32f310 3660
nikapov 0:a1a69d32f310 3661 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3662 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
Davidroid 2:d07edeaff6f1 3663 }
nikapov 0:a1a69d32f310 3664
nikapov 0:a1a69d32f310 3665 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3666 /* restore the previous Sequence Config */
nikapov 0:a1a69d32f310 3667 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 3668 sequence_config);
nikapov 0:a1a69d32f310 3669 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3670 PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov 0:a1a69d32f310 3671 }
nikapov 0:a1a69d32f310 3672 }
nikapov 0:a1a69d32f310 3673
nikapov 0:a1a69d32f310 3674 return status;
nikapov 0:a1a69d32f310 3675 }
nikapov 0:a1a69d32f310 3676
nikapov 0:a1a69d32f310 3677 VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 3678 uint32_t *ref_spad_count,
nikapov 0:a1a69d32f310 3679 uint8_t *is_aperture_spads)
nikapov 0:a1a69d32f310 3680 {
nikapov 0:a1a69d32f310 3681 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 3682 uint8_t last_spad_array[6];
nikapov 0:a1a69d32f310 3683 uint8_t start_select = 0xB4;
nikapov 0:a1a69d32f310 3684 uint32_t minimum_spad_count = 3;
nikapov 0:a1a69d32f310 3685 uint32_t max_spad_count = 44;
nikapov 0:a1a69d32f310 3686 uint32_t current_spad_index = 0;
nikapov 0:a1a69d32f310 3687 uint32_t last_spad_index = 0;
nikapov 0:a1a69d32f310 3688 int32_t next_good_spad = 0;
nikapov 0:a1a69d32f310 3689 uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */
nikapov 0:a1a69d32f310 3690 uint16_t peak_signal_rate_ref;
nikapov 0:a1a69d32f310 3691 uint32_t need_apt_spads = 0;
nikapov 0:a1a69d32f310 3692 uint32_t index = 0;
nikapov 0:a1a69d32f310 3693 uint32_t spad_array_size = 6;
nikapov 0:a1a69d32f310 3694 uint32_t signal_rate_diff = 0;
nikapov 0:a1a69d32f310 3695 uint32_t last_signal_rate_diff = 0;
nikapov 0:a1a69d32f310 3696 uint8_t complete = 0;
nikapov 0:a1a69d32f310 3697 uint8_t vhv_settings = 0;
nikapov 0:a1a69d32f310 3698 uint8_t phase_cal = 0;
nikapov 0:a1a69d32f310 3699 uint32_t ref_spad_count_int = 0;
nikapov 0:a1a69d32f310 3700 uint8_t is_aperture_spads_int = 0;
nikapov 0:a1a69d32f310 3701
nikapov 0:a1a69d32f310 3702 /*
nikapov 0:a1a69d32f310 3703 * The reference SPAD initialization procedure determines the minimum
nikapov 0:a1a69d32f310 3704 * amount of reference spads to be enables to achieve a target reference
nikapov 0:a1a69d32f310 3705 * signal rate and should be performed once during initialization.
nikapov 0:a1a69d32f310 3706 *
nikapov 0:a1a69d32f310 3707 * Either aperture or non-aperture spads are applied but never both.
nikapov 0:a1a69d32f310 3708 * Firstly non-aperture spads are set, begining with 5 spads, and
nikapov 0:a1a69d32f310 3709 * increased one spad at a time until the closest measurement to the
nikapov 0:a1a69d32f310 3710 * target rate is achieved.
nikapov 0:a1a69d32f310 3711 *
nikapov 0:a1a69d32f310 3712 * If the target rate is exceeded when 5 non-aperture spads are enabled,
nikapov 0:a1a69d32f310 3713 * initialization is performed instead with aperture spads.
nikapov 0:a1a69d32f310 3714 *
nikapov 0:a1a69d32f310 3715 * When setting spads, a 'Good Spad Map' is applied.
nikapov 0:a1a69d32f310 3716 *
nikapov 0:a1a69d32f310 3717 * This procedure operates within a SPAD window of interest of a maximum
nikapov 0:a1a69d32f310 3718 * 44 spads.
nikapov 0:a1a69d32f310 3719 * The start point is currently fixed to 180, which lies towards the end
nikapov 0:a1a69d32f310 3720 * of the non-aperture quadrant and runs in to the adjacent aperture
nikapov 0:a1a69d32f310 3721 * quadrant.
nikapov 0:a1a69d32f310 3722 */
nikapov 0:a1a69d32f310 3723 target_ref_rate = PALDevDataGet(dev, targetRefRate);
nikapov 0:a1a69d32f310 3724
nikapov 0:a1a69d32f310 3725 /*
nikapov 0:a1a69d32f310 3726 * Initialize Spad arrays.
nikapov 0:a1a69d32f310 3727 * Currently the good spad map is initialised to 'All good'.
nikapov 0:a1a69d32f310 3728 * This is a short term implementation. The good spad map will be
nikapov 0:a1a69d32f310 3729 * provided as an input.
nikapov 0:a1a69d32f310 3730 * Note that there are 6 bytes. Only the first 44 bits will be used to
nikapov 0:a1a69d32f310 3731 * represent spads.
nikapov 0:a1a69d32f310 3732 */
Davidroid 3:e9269ff624ed 3733 for (index = 0; index < spad_array_size; index++) {
sepp_nepp 5:b95f6951f7d5 3734 dev->SpadData.RefSpadEnables[index] = 0;
Davidroid 3:e9269ff624ed 3735 }
nikapov 0:a1a69d32f310 3736
nikapov 0:a1a69d32f310 3737
nikapov 0:a1a69d32f310 3738 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 3739
nikapov 0:a1a69d32f310 3740 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3741 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3742 VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
nikapov 0:a1a69d32f310 3743 }
nikapov 0:a1a69d32f310 3744
nikapov 0:a1a69d32f310 3745 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3746 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3747 VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
Davidroid 2:d07edeaff6f1 3748 }
nikapov 0:a1a69d32f310 3749
nikapov 0:a1a69d32f310 3750 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3751 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 3752 }
nikapov 0:a1a69d32f310 3753
nikapov 0:a1a69d32f310 3754 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3755 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3756 VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
nikapov 0:a1a69d32f310 3757 start_select);
Davidroid 2:d07edeaff6f1 3758 }
nikapov 0:a1a69d32f310 3759
nikapov 0:a1a69d32f310 3760 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3761 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3762 VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
Davidroid 2:d07edeaff6f1 3763 }
nikapov 0:a1a69d32f310 3764
nikapov 0:a1a69d32f310 3765 /* Perform ref calibration */
nikapov 0:a1a69d32f310 3766 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3767 status = VL53L0X_perform_ref_calibration(dev, &vhv_settings,
nikapov 0:a1a69d32f310 3768 &phase_cal, 0);
Davidroid 2:d07edeaff6f1 3769 }
nikapov 0:a1a69d32f310 3770
nikapov 0:a1a69d32f310 3771 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3772 /* Enable Minimum NON-APERTURE Spads */
nikapov 0:a1a69d32f310 3773 current_spad_index = 0;
nikapov 0:a1a69d32f310 3774 last_spad_index = current_spad_index;
nikapov 0:a1a69d32f310 3775 need_apt_spads = 0;
nikapov 0:a1a69d32f310 3776 status = enable_ref_spads(dev,
nikapov 0:a1a69d32f310 3777 need_apt_spads,
sepp_nepp 5:b95f6951f7d5 3778 dev->SpadData.RefGoodSpadMap,
sepp_nepp 5:b95f6951f7d5 3779 dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 3780 spad_array_size,
nikapov 0:a1a69d32f310 3781 start_select,
nikapov 0:a1a69d32f310 3782 current_spad_index,
nikapov 0:a1a69d32f310 3783 minimum_spad_count,
nikapov 0:a1a69d32f310 3784 &last_spad_index);
nikapov 0:a1a69d32f310 3785 }
nikapov 0:a1a69d32f310 3786
nikapov 0:a1a69d32f310 3787 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3788 current_spad_index = last_spad_index;
nikapov 0:a1a69d32f310 3789
nikapov 0:a1a69d32f310 3790 status = perform_ref_signal_measurement(dev,
nikapov 0:a1a69d32f310 3791 &peak_signal_rate_ref);
nikapov 0:a1a69d32f310 3792 if ((status == VL53L0X_ERROR_NONE) &&
nikapov 0:a1a69d32f310 3793 (peak_signal_rate_ref > target_ref_rate)) {
nikapov 0:a1a69d32f310 3794 /* Signal rate measurement too high,
nikapov 0:a1a69d32f310 3795 * switch to APERTURE SPADs */
nikapov 0:a1a69d32f310 3796
nikapov 0:a1a69d32f310 3797 for (index = 0; index < spad_array_size; index++) {
sepp_nepp 5:b95f6951f7d5 3798 dev->SpadData.RefSpadEnables[index] = 0;
nikapov 0:a1a69d32f310 3799 }
nikapov 0:a1a69d32f310 3800
nikapov 0:a1a69d32f310 3801
nikapov 0:a1a69d32f310 3802 /* Increment to the first APERTURE spad */
nikapov 0:a1a69d32f310 3803 while ((is_aperture(start_select + current_spad_index)
nikapov 0:a1a69d32f310 3804 == 0) && (current_spad_index < max_spad_count)) {
nikapov 0:a1a69d32f310 3805 current_spad_index++;
nikapov 0:a1a69d32f310 3806 }
nikapov 0:a1a69d32f310 3807
nikapov 0:a1a69d32f310 3808 need_apt_spads = 1;
nikapov 0:a1a69d32f310 3809
nikapov 0:a1a69d32f310 3810 status = enable_ref_spads(dev,
nikapov 0:a1a69d32f310 3811 need_apt_spads,
sepp_nepp 5:b95f6951f7d5 3812 dev->SpadData.RefGoodSpadMap,
sepp_nepp 5:b95f6951f7d5 3813 dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 3814 spad_array_size,
nikapov 0:a1a69d32f310 3815 start_select,
nikapov 0:a1a69d32f310 3816 current_spad_index,
nikapov 0:a1a69d32f310 3817 minimum_spad_count,
nikapov 0:a1a69d32f310 3818 &last_spad_index);
nikapov 0:a1a69d32f310 3819
nikapov 0:a1a69d32f310 3820 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3821 current_spad_index = last_spad_index;
nikapov 0:a1a69d32f310 3822 status = perform_ref_signal_measurement(dev,
nikapov 0:a1a69d32f310 3823 &peak_signal_rate_ref);
nikapov 0:a1a69d32f310 3824
nikapov 0:a1a69d32f310 3825 if ((status == VL53L0X_ERROR_NONE) &&
nikapov 0:a1a69d32f310 3826 (peak_signal_rate_ref > target_ref_rate)) {
nikapov 0:a1a69d32f310 3827 /* Signal rate still too high after
nikapov 0:a1a69d32f310 3828 * setting the minimum number of
nikapov 0:a1a69d32f310 3829 * APERTURE spads. Can do no more
nikapov 0:a1a69d32f310 3830 * therefore set the min number of
nikapov 0:a1a69d32f310 3831 * aperture spads as the result.
nikapov 0:a1a69d32f310 3832 */
nikapov 0:a1a69d32f310 3833 is_aperture_spads_int = 1;
nikapov 0:a1a69d32f310 3834 ref_spad_count_int = minimum_spad_count;
nikapov 0:a1a69d32f310 3835 }
nikapov 0:a1a69d32f310 3836 }
nikapov 0:a1a69d32f310 3837 } else {
nikapov 0:a1a69d32f310 3838 need_apt_spads = 0;
nikapov 0:a1a69d32f310 3839 }
nikapov 0:a1a69d32f310 3840 }
nikapov 0:a1a69d32f310 3841
nikapov 0:a1a69d32f310 3842 if ((status == VL53L0X_ERROR_NONE) &&
nikapov 0:a1a69d32f310 3843 (peak_signal_rate_ref < target_ref_rate)) {
nikapov 0:a1a69d32f310 3844 /* At this point, the minimum number of either aperture
nikapov 0:a1a69d32f310 3845 * or non-aperture spads have been set. Proceed to add
nikapov 0:a1a69d32f310 3846 * spads and perform measurements until the target
nikapov 0:a1a69d32f310 3847 * reference is reached.
nikapov 0:a1a69d32f310 3848 */
nikapov 0:a1a69d32f310 3849 is_aperture_spads_int = need_apt_spads;
nikapov 0:a1a69d32f310 3850 ref_spad_count_int = minimum_spad_count;
nikapov 0:a1a69d32f310 3851
sepp_nepp 5:b95f6951f7d5 3852 memcpy(last_spad_array, dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 3853 spad_array_size);
nikapov 0:a1a69d32f310 3854 last_signal_rate_diff = abs(peak_signal_rate_ref -
nikapov 0:a1a69d32f310 3855 target_ref_rate);
nikapov 0:a1a69d32f310 3856 complete = 0;
nikapov 0:a1a69d32f310 3857
nikapov 0:a1a69d32f310 3858 while (!complete) {
nikapov 0:a1a69d32f310 3859 get_next_good_spad(
sepp_nepp 5:b95f6951f7d5 3860 dev->SpadData.RefGoodSpadMap,
nikapov 0:a1a69d32f310 3861 spad_array_size, current_spad_index,
nikapov 0:a1a69d32f310 3862 &next_good_spad);
nikapov 0:a1a69d32f310 3863
nikapov 0:a1a69d32f310 3864 if (next_good_spad == -1) {
nikapov 0:a1a69d32f310 3865 status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 3866 break;
nikapov 0:a1a69d32f310 3867 }
nikapov 0:a1a69d32f310 3868
nikapov 0:a1a69d32f310 3869 /* Cannot combine Aperture and Non-Aperture spads, so
nikapov 0:a1a69d32f310 3870 * ensure the current spad is of the correct type.
nikapov 0:a1a69d32f310 3871 */
nikapov 0:a1a69d32f310 3872 if (is_aperture((uint32_t)start_select + next_good_spad) !=
nikapov 0:a1a69d32f310 3873 need_apt_spads) {
nikapov 0:a1a69d32f310 3874 /* At this point we have enabled the maximum
nikapov 0:a1a69d32f310 3875 * number of Aperture spads.
nikapov 0:a1a69d32f310 3876 */
nikapov 0:a1a69d32f310 3877 complete = 1;
nikapov 0:a1a69d32f310 3878 break;
nikapov 0:a1a69d32f310 3879 }
nikapov 0:a1a69d32f310 3880
nikapov 0:a1a69d32f310 3881 (ref_spad_count_int)++;
nikapov 0:a1a69d32f310 3882
nikapov 0:a1a69d32f310 3883 current_spad_index = next_good_spad;
nikapov 0:a1a69d32f310 3884 status = enable_spad_bit(
sepp_nepp 5:b95f6951f7d5 3885 dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 3886 spad_array_size, current_spad_index);
nikapov 0:a1a69d32f310 3887
nikapov 0:a1a69d32f310 3888 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3889 current_spad_index++;
nikapov 0:a1a69d32f310 3890 /* Proceed to apply the additional spad and
nikapov 0:a1a69d32f310 3891 * perform measurement. */
nikapov 0:a1a69d32f310 3892 status = set_ref_spad_map(dev,
sepp_nepp 5:b95f6951f7d5 3893 dev->SpadData.RefSpadEnables);
nikapov 0:a1a69d32f310 3894 }
nikapov 0:a1a69d32f310 3895
nikapov 0:a1a69d32f310 3896 if (status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3897 break;
nikapov 0:a1a69d32f310 3898 }
nikapov 0:a1a69d32f310 3899
nikapov 0:a1a69d32f310 3900 status = perform_ref_signal_measurement(dev,
nikapov 0:a1a69d32f310 3901 &peak_signal_rate_ref);
nikapov 0:a1a69d32f310 3902
nikapov 0:a1a69d32f310 3903 if (status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3904 break;
nikapov 0:a1a69d32f310 3905 }
nikapov 0:a1a69d32f310 3906
nikapov 0:a1a69d32f310 3907 signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate);
nikapov 0:a1a69d32f310 3908
nikapov 0:a1a69d32f310 3909 if (peak_signal_rate_ref > target_ref_rate) {
nikapov 0:a1a69d32f310 3910 /* Select the spad map that provides the
nikapov 0:a1a69d32f310 3911 * measurement closest to the target rate,
nikapov 0:a1a69d32f310 3912 * either above or below it.
nikapov 0:a1a69d32f310 3913 */
nikapov 0:a1a69d32f310 3914 if (signal_rate_diff > last_signal_rate_diff) {
nikapov 0:a1a69d32f310 3915 /* Previous spad map produced a closer
nikapov 0:a1a69d32f310 3916 * measurement, so choose this. */
nikapov 0:a1a69d32f310 3917 status = set_ref_spad_map(dev,
nikapov 0:a1a69d32f310 3918 last_spad_array);
nikapov 0:a1a69d32f310 3919 memcpy(
sepp_nepp 5:b95f6951f7d5 3920 dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 3921 last_spad_array, spad_array_size);
nikapov 0:a1a69d32f310 3922
nikapov 0:a1a69d32f310 3923 (ref_spad_count_int)--;
nikapov 0:a1a69d32f310 3924 }
nikapov 0:a1a69d32f310 3925 complete = 1;
nikapov 0:a1a69d32f310 3926 } else {
nikapov 0:a1a69d32f310 3927 /* Continue to add spads */
nikapov 0:a1a69d32f310 3928 last_signal_rate_diff = signal_rate_diff;
nikapov 0:a1a69d32f310 3929 memcpy(last_spad_array,
sepp_nepp 5:b95f6951f7d5 3930 dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 3931 spad_array_size);
nikapov 0:a1a69d32f310 3932 }
nikapov 0:a1a69d32f310 3933
nikapov 0:a1a69d32f310 3934 } /* while */
nikapov 0:a1a69d32f310 3935 }
nikapov 0:a1a69d32f310 3936
nikapov 0:a1a69d32f310 3937 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3938 *ref_spad_count = ref_spad_count_int;
nikapov 0:a1a69d32f310 3939 *is_aperture_spads = is_aperture_spads_int;
nikapov 0:a1a69d32f310 3940
nikapov 0:a1a69d32f310 3941 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1);
nikapov 0:a1a69d32f310 3942 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 3943 ReferenceSpadCount, (uint8_t)(*ref_spad_count));
nikapov 0:a1a69d32f310 3944 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 3945 ReferenceSpadType, *is_aperture_spads);
nikapov 0:a1a69d32f310 3946 }
nikapov 0:a1a69d32f310 3947
nikapov 0:a1a69d32f310 3948 return status;
nikapov 0:a1a69d32f310 3949 }
nikapov 0:a1a69d32f310 3950
nikapov 0:a1a69d32f310 3951 VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 3952 uint32_t count, uint8_t is_aperture_spads)
nikapov 0:a1a69d32f310 3953 {
nikapov 0:a1a69d32f310 3954 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 3955 uint32_t current_spad_index = 0;
nikapov 0:a1a69d32f310 3956 uint8_t start_select = 0xB4;
nikapov 0:a1a69d32f310 3957 uint32_t spad_array_size = 6;
nikapov 0:a1a69d32f310 3958 uint32_t max_spad_count = 44;
nikapov 0:a1a69d32f310 3959 uint32_t last_spad_index;
nikapov 0:a1a69d32f310 3960 uint32_t index;
nikapov 0:a1a69d32f310 3961
nikapov 0:a1a69d32f310 3962 /*
nikapov 0:a1a69d32f310 3963 * This function applies a requested number of reference spads, either
nikapov 0:a1a69d32f310 3964 * aperture or
nikapov 0:a1a69d32f310 3965 * non-aperture, as requested.
nikapov 0:a1a69d32f310 3966 * The good spad map will be applied.
nikapov 0:a1a69d32f310 3967 */
nikapov 0:a1a69d32f310 3968
nikapov 0:a1a69d32f310 3969 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 3970
nikapov 0:a1a69d32f310 3971 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3972 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3973 VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
Davidroid 2:d07edeaff6f1 3974 }
nikapov 0:a1a69d32f310 3975
nikapov 0:a1a69d32f310 3976 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3977 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3978 VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
Davidroid 2:d07edeaff6f1 3979 }
nikapov 0:a1a69d32f310 3980
nikapov 0:a1a69d32f310 3981 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3982 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
Davidroid 2:d07edeaff6f1 3983 }
nikapov 0:a1a69d32f310 3984
nikapov 0:a1a69d32f310 3985 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 3986 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 3987 VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
nikapov 0:a1a69d32f310 3988 start_select);
Davidroid 2:d07edeaff6f1 3989 }
nikapov 0:a1a69d32f310 3990
nikapov 0:a1a69d32f310 3991 for (index = 0; index < spad_array_size; index++) {
sepp_nepp 5:b95f6951f7d5 3992 dev->SpadData.RefSpadEnables[index] = 0;
Davidroid 2:d07edeaff6f1 3993 }
nikapov 0:a1a69d32f310 3994
nikapov 0:a1a69d32f310 3995 if (is_aperture_spads) {
nikapov 0:a1a69d32f310 3996 /* Increment to the first APERTURE spad */
nikapov 0:a1a69d32f310 3997 while ((is_aperture(start_select + current_spad_index) == 0) &&
nikapov 0:a1a69d32f310 3998 (current_spad_index < max_spad_count)) {
nikapov 0:a1a69d32f310 3999 current_spad_index++;
nikapov 0:a1a69d32f310 4000 }
nikapov 0:a1a69d32f310 4001 }
nikapov 0:a1a69d32f310 4002 status = enable_ref_spads(dev,
nikapov 0:a1a69d32f310 4003 is_aperture_spads,
sepp_nepp 5:b95f6951f7d5 4004 dev->SpadData.RefGoodSpadMap,
sepp_nepp 5:b95f6951f7d5 4005 dev->SpadData.RefSpadEnables,
nikapov 0:a1a69d32f310 4006 spad_array_size,
nikapov 0:a1a69d32f310 4007 start_select,
nikapov 0:a1a69d32f310 4008 current_spad_index,
nikapov 0:a1a69d32f310 4009 count,
nikapov 0:a1a69d32f310 4010 &last_spad_index);
nikapov 0:a1a69d32f310 4011
nikapov 0:a1a69d32f310 4012 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4013 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1);
nikapov 0:a1a69d32f310 4014 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 4015 ReferenceSpadCount, (uint8_t)(count));
nikapov 0:a1a69d32f310 4016 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 4017 ReferenceSpadType, is_aperture_spads);
nikapov 0:a1a69d32f310 4018 }
nikapov 0:a1a69d32f310 4019
nikapov 0:a1a69d32f310 4020 return status;
nikapov 0:a1a69d32f310 4021 }
nikapov 0:a1a69d32f310 4022
nikapov 0:a1a69d32f310 4023 VL53L0X_Error VL53L0X::VL53L0X_wait_device_booted(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 4024 {
nikapov 0:a1a69d32f310 4025 VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
sepp_nepp 5:b95f6951f7d5 4026
nikapov 0:a1a69d32f310 4027
nikapov 0:a1a69d32f310 4028 /* not implemented on VL53L0X */
nikapov 0:a1a69d32f310 4029
sepp_nepp 5:b95f6951f7d5 4030
nikapov 0:a1a69d32f310 4031 return status;
nikapov 0:a1a69d32f310 4032 }
nikapov 0:a1a69d32f310 4033
nikapov 0:a1a69d32f310 4034 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV dev, uint8_t *p_vhv_settings,
nikapov 0:a1a69d32f310 4035 uint8_t *p_phase_cal)
nikapov 0:a1a69d32f310 4036 {
nikapov 0:a1a69d32f310 4037 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 4038
nikapov 0:a1a69d32f310 4039
nikapov 0:a1a69d32f310 4040 status = VL53L0X_perform_ref_calibration(dev, p_vhv_settings,
nikapov 0:a1a69d32f310 4041 p_phase_cal, 1);
nikapov 0:a1a69d32f310 4042
sepp_nepp 5:b95f6951f7d5 4043
nikapov 0:a1a69d32f310 4044 return status;
nikapov 0:a1a69d32f310 4045 }
nikapov 0:a1a69d32f310 4046
nikapov 0:a1a69d32f310 4047 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 4048 uint32_t *ref_spad_count, uint8_t *is_aperture_spads)
nikapov 0:a1a69d32f310 4049 {
nikapov 0:a1a69d32f310 4050 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 4051
nikapov 0:a1a69d32f310 4052
nikapov 0:a1a69d32f310 4053 status = wrapped_VL53L0X_perform_ref_spad_management(dev, ref_spad_count,
nikapov 0:a1a69d32f310 4054 is_aperture_spads);
nikapov 0:a1a69d32f310 4055
sepp_nepp 5:b95f6951f7d5 4056
nikapov 0:a1a69d32f310 4057
nikapov 0:a1a69d32f310 4058 return status;
nikapov 0:a1a69d32f310 4059 }
nikapov 0:a1a69d32f310 4060
nikapov 0:a1a69d32f310 4061 /* Group PAL Init Functions */
nikapov 0:a1a69d32f310 4062 VL53L0X_Error VL53L0X::VL53L0X_set_device_address(VL53L0X_DEV dev, uint8_t device_address)
nikapov 0:a1a69d32f310 4063 {
nikapov 0:a1a69d32f310 4064 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 4065
nikapov 0:a1a69d32f310 4066
nikapov 0:a1a69d32f310 4067 status = VL53L0X_write_byte(dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
nikapov 0:a1a69d32f310 4068 device_address / 2);
nikapov 0:a1a69d32f310 4069
sepp_nepp 5:b95f6951f7d5 4070
nikapov 0:a1a69d32f310 4071 return status;
nikapov 0:a1a69d32f310 4072 }
nikapov 0:a1a69d32f310 4073
nikapov 0:a1a69d32f310 4074 VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config(VL53L0X_DEV dev, uint8_t pin,
nikapov 0:a1a69d32f310 4075 VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
nikapov 0:a1a69d32f310 4076 VL53L0X_InterruptPolarity polarity)
nikapov 0:a1a69d32f310 4077 {
nikapov 0:a1a69d32f310 4078 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4079 uint8_t data;
nikapov 0:a1a69d32f310 4080
sepp_nepp 5:b95f6951f7d5 4081
nikapov 0:a1a69d32f310 4082
nikapov 0:a1a69d32f310 4083 if (pin != 0) {
nikapov 0:a1a69d32f310 4084 status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
nikapov 0:a1a69d32f310 4085 } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
nikapov 0:a1a69d32f310 4086 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
nikapov 0:a1a69d32f310 4087 data = 0x10;
nikapov 0:a1a69d32f310 4088 } else {
nikapov 0:a1a69d32f310 4089 data = 1;
nikapov 0:a1a69d32f310 4090 }
nikapov 0:a1a69d32f310 4091
nikapov 0:a1a69d32f310 4092 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 4093 VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
nikapov 0:a1a69d32f310 4094
nikapov 0:a1a69d32f310 4095 } else {
Davidroid 2:d07edeaff6f1 4096 if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) {
Davidroid 2:d07edeaff6f1 4097
Davidroid 2:d07edeaff6f1 4098 status |= VL53L0X_write_byte(dev, 0xff, 0x01);
Davidroid 2:d07edeaff6f1 4099 status |= VL53L0X_write_byte(dev, 0x00, 0x00);
Davidroid 2:d07edeaff6f1 4100
Davidroid 2:d07edeaff6f1 4101 status |= VL53L0X_write_byte(dev, 0xff, 0x00);
Davidroid 2:d07edeaff6f1 4102 status |= VL53L0X_write_byte(dev, 0x80, 0x01);
Davidroid 2:d07edeaff6f1 4103 status |= VL53L0X_write_byte(dev, 0x85, 0x02);
Davidroid 2:d07edeaff6f1 4104
Davidroid 2:d07edeaff6f1 4105 status |= VL53L0X_write_byte(dev, 0xff, 0x04);
Davidroid 2:d07edeaff6f1 4106 status |= VL53L0X_write_byte(dev, 0xcd, 0x00);
Davidroid 2:d07edeaff6f1 4107 status |= VL53L0X_write_byte(dev, 0xcc, 0x11);
Davidroid 2:d07edeaff6f1 4108
Davidroid 2:d07edeaff6f1 4109 status |= VL53L0X_write_byte(dev, 0xff, 0x07);
Davidroid 2:d07edeaff6f1 4110 status |= VL53L0X_write_byte(dev, 0xbe, 0x00);
Davidroid 2:d07edeaff6f1 4111
Davidroid 2:d07edeaff6f1 4112 status |= VL53L0X_write_byte(dev, 0xff, 0x06);
Davidroid 2:d07edeaff6f1 4113 status |= VL53L0X_write_byte(dev, 0xcc, 0x09);
Davidroid 2:d07edeaff6f1 4114
Davidroid 2:d07edeaff6f1 4115 status |= VL53L0X_write_byte(dev, 0xff, 0x00);
Davidroid 2:d07edeaff6f1 4116 status |= VL53L0X_write_byte(dev, 0xff, 0x01);
Davidroid 2:d07edeaff6f1 4117 status |= VL53L0X_write_byte(dev, 0x00, 0x00);
Davidroid 2:d07edeaff6f1 4118
Davidroid 2:d07edeaff6f1 4119 } else {
Davidroid 2:d07edeaff6f1 4120
Davidroid 2:d07edeaff6f1 4121 if (status == VL53L0X_ERROR_NONE) {
Davidroid 2:d07edeaff6f1 4122 switch (functionality) {
Davidroid 2:d07edeaff6f1 4123 case VL53L0X_GPIOFUNCTIONALITY_OFF:
Davidroid 2:d07edeaff6f1 4124 data = 0x00;
Davidroid 2:d07edeaff6f1 4125 break;
Davidroid 2:d07edeaff6f1 4126 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
Davidroid 2:d07edeaff6f1 4127 data = 0x01;
Davidroid 2:d07edeaff6f1 4128 break;
Davidroid 2:d07edeaff6f1 4129 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
Davidroid 2:d07edeaff6f1 4130 data = 0x02;
Davidroid 2:d07edeaff6f1 4131 break;
Davidroid 2:d07edeaff6f1 4132 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
Davidroid 2:d07edeaff6f1 4133 data = 0x03;
Davidroid 2:d07edeaff6f1 4134 break;
Davidroid 2:d07edeaff6f1 4135 case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
Davidroid 2:d07edeaff6f1 4136 data = 0x04;
Davidroid 2:d07edeaff6f1 4137 break;
Davidroid 2:d07edeaff6f1 4138 default:
Davidroid 2:d07edeaff6f1 4139 status =
Davidroid 2:d07edeaff6f1 4140 VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
Davidroid 2:d07edeaff6f1 4141 }
Davidroid 2:d07edeaff6f1 4142 }
Davidroid 2:d07edeaff6f1 4143
Davidroid 2:d07edeaff6f1 4144 if (status == VL53L0X_ERROR_NONE) {
Davidroid 2:d07edeaff6f1 4145 status = VL53L0X_write_byte(dev,
Davidroid 2:d07edeaff6f1 4146 VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
Davidroid 2:d07edeaff6f1 4147 }
Davidroid 2:d07edeaff6f1 4148
Davidroid 2:d07edeaff6f1 4149 if (status == VL53L0X_ERROR_NONE) {
Davidroid 2:d07edeaff6f1 4150 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
Davidroid 2:d07edeaff6f1 4151 data = 0;
Davidroid 2:d07edeaff6f1 4152 } else {
Davidroid 2:d07edeaff6f1 4153 data = (uint8_t)(1 << 4);
Davidroid 2:d07edeaff6f1 4154 }
Davidroid 2:d07edeaff6f1 4155 status = VL53L0X_update_byte(dev,
Davidroid 2:d07edeaff6f1 4156 VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
Davidroid 2:d07edeaff6f1 4157 }
Davidroid 2:d07edeaff6f1 4158
Davidroid 2:d07edeaff6f1 4159 if (status == VL53L0X_ERROR_NONE) {
Davidroid 2:d07edeaff6f1 4160 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
Davidroid 2:d07edeaff6f1 4161 Pin0GpioFunctionality, functionality);
Davidroid 2:d07edeaff6f1 4162 }
Davidroid 2:d07edeaff6f1 4163
Davidroid 2:d07edeaff6f1 4164 if (status == VL53L0X_ERROR_NONE) {
Davidroid 2:d07edeaff6f1 4165 status = VL53L0X_clear_interrupt_mask(dev, 0);
Davidroid 2:d07edeaff6f1 4166 }
Davidroid 2:d07edeaff6f1 4167 }
nikapov 0:a1a69d32f310 4168 }
sepp_nepp 5:b95f6951f7d5 4169
nikapov 0:a1a69d32f310 4170 return status;
nikapov 0:a1a69d32f310 4171 }
nikapov 0:a1a69d32f310 4172
nikapov 0:a1a69d32f310 4173 VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable(VL53L0X_DEV dev, uint8_t *p_enabled)
nikapov 0:a1a69d32f310 4174 {
nikapov 0:a1a69d32f310 4175 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 4176
nikapov 0:a1a69d32f310 4177
nikapov 0:a1a69d32f310 4178 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled);
nikapov 0:a1a69d32f310 4179
nikapov 0:a1a69d32f310 4180 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4181 *p_enabled = (*p_enabled & 1);
nikapov 0:a1a69d32f310 4182 }
nikapov 0:a1a69d32f310 4183
sepp_nepp 5:b95f6951f7d5 4184
nikapov 0:a1a69d32f310 4185 return status;
nikapov 0:a1a69d32f310 4186 }
nikapov 0:a1a69d32f310 4187
nikapov 0:a1a69d32f310 4188 uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
nikapov 0:a1a69d32f310 4189 {
nikapov 0:a1a69d32f310 4190 /*!
nikapov 0:a1a69d32f310 4191 * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
nikapov 0:a1a69d32f310 4192 */
nikapov 0:a1a69d32f310 4193
nikapov 0:a1a69d32f310 4194 uint16_t encoded_timeout = 0;
nikapov 0:a1a69d32f310 4195 uint32_t ls_byte = 0;
nikapov 0:a1a69d32f310 4196 uint16_t ms_byte = 0;
nikapov 0:a1a69d32f310 4197
nikapov 0:a1a69d32f310 4198 if (timeout_macro_clks > 0) {
nikapov 0:a1a69d32f310 4199 ls_byte = timeout_macro_clks - 1;
nikapov 0:a1a69d32f310 4200
nikapov 0:a1a69d32f310 4201 while ((ls_byte & 0xFFFFFF00) > 0) {
nikapov 0:a1a69d32f310 4202 ls_byte = ls_byte >> 1;
nikapov 0:a1a69d32f310 4203 ms_byte++;
nikapov 0:a1a69d32f310 4204 }
nikapov 0:a1a69d32f310 4205
nikapov 0:a1a69d32f310 4206 encoded_timeout = (ms_byte << 8)
nikapov 0:a1a69d32f310 4207 + (uint16_t)(ls_byte & 0x000000FF);
nikapov 0:a1a69d32f310 4208 }
nikapov 0:a1a69d32f310 4209
nikapov 0:a1a69d32f310 4210 return encoded_timeout;
nikapov 0:a1a69d32f310 4211
nikapov 0:a1a69d32f310 4212 }
nikapov 0:a1a69d32f310 4213
nikapov 0:a1a69d32f310 4214 VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 4215 VL53L0X_SequenceStepId sequence_step_id,
nikapov 0:a1a69d32f310 4216 uint32_t timeout_micro_secs)
nikapov 0:a1a69d32f310 4217 {
nikapov 0:a1a69d32f310 4218 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4219 uint8_t current_vcsel_pulse_period_p_clk;
nikapov 0:a1a69d32f310 4220 uint8_t msrc_encoded_time_out;
nikapov 0:a1a69d32f310 4221 uint16_t pre_range_encoded_time_out;
nikapov 0:a1a69d32f310 4222 uint16_t pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 4223 uint16_t msrc_range_time_out_m_clks;
nikapov 0:a1a69d32f310 4224 uint32_t final_range_time_out_m_clks;
nikapov 0:a1a69d32f310 4225 uint16_t final_range_encoded_time_out;
nikapov 0:a1a69d32f310 4226 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov 0:a1a69d32f310 4227
nikapov 0:a1a69d32f310 4228 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
nikapov 0:a1a69d32f310 4229 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
nikapov 0:a1a69d32f310 4230 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
nikapov 0:a1a69d32f310 4231
nikapov 0:a1a69d32f310 4232 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 4233 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 4234 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4235
nikapov 0:a1a69d32f310 4236 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4237 msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(dev,
nikapov 0:a1a69d32f310 4238 timeout_micro_secs,
nikapov 0:a1a69d32f310 4239 (uint8_t)current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4240
nikapov 0:a1a69d32f310 4241 if (msrc_range_time_out_m_clks > 256) {
nikapov 0:a1a69d32f310 4242 msrc_encoded_time_out = 255;
nikapov 0:a1a69d32f310 4243 } else {
nikapov 0:a1a69d32f310 4244 msrc_encoded_time_out =
nikapov 0:a1a69d32f310 4245 (uint8_t)msrc_range_time_out_m_clks - 1;
nikapov 0:a1a69d32f310 4246 }
nikapov 0:a1a69d32f310 4247
nikapov 0:a1a69d32f310 4248 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 4249 LastEncodedTimeout,
nikapov 0:a1a69d32f310 4250 msrc_encoded_time_out);
nikapov 0:a1a69d32f310 4251 }
nikapov 0:a1a69d32f310 4252
nikapov 0:a1a69d32f310 4253 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4254 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 4255 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
nikapov 0:a1a69d32f310 4256 msrc_encoded_time_out);
nikapov 0:a1a69d32f310 4257 }
nikapov 0:a1a69d32f310 4258 } else {
nikapov 0:a1a69d32f310 4259
nikapov 0:a1a69d32f310 4260 if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
nikapov 0:a1a69d32f310 4261
nikapov 0:a1a69d32f310 4262 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4263 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 4264 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 4265 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4266 pre_range_time_out_m_clks =
nikapov 0:a1a69d32f310 4267 VL53L0X_calc_timeout_mclks(dev,
nikapov 0:a1a69d32f310 4268 timeout_micro_secs,
nikapov 0:a1a69d32f310 4269 (uint8_t)current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4270 pre_range_encoded_time_out = VL53L0X_encode_timeout(
nikapov 0:a1a69d32f310 4271 pre_range_time_out_m_clks);
nikapov 0:a1a69d32f310 4272
nikapov 0:a1a69d32f310 4273 VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 4274 LastEncodedTimeout,
nikapov 0:a1a69d32f310 4275 pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 4276 }
nikapov 0:a1a69d32f310 4277
nikapov 0:a1a69d32f310 4278 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4279 status = VL53L0X_write_word(dev,
nikapov 0:a1a69d32f310 4280 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov 0:a1a69d32f310 4281 pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 4282 }
nikapov 0:a1a69d32f310 4283
nikapov 0:a1a69d32f310 4284 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4285 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 4286 dev,
nikapov 0:a1a69d32f310 4287 PreRangeTimeoutMicroSecs,
nikapov 0:a1a69d32f310 4288 timeout_micro_secs);
nikapov 0:a1a69d32f310 4289 }
nikapov 0:a1a69d32f310 4290 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov 0:a1a69d32f310 4291
nikapov 0:a1a69d32f310 4292 /* For the final range timeout, the pre-range timeout
nikapov 0:a1a69d32f310 4293 * must be added. To do this both final and pre-range
nikapov 0:a1a69d32f310 4294 * timeouts must be expressed in macro periods MClks
nikapov 0:a1a69d32f310 4295 * because they have different vcsel periods.
nikapov 0:a1a69d32f310 4296 */
nikapov 0:a1a69d32f310 4297
nikapov 0:a1a69d32f310 4298 VL53L0X_get_sequence_step_enables(dev,
nikapov 0:a1a69d32f310 4299 &scheduler_sequence_steps);
nikapov 0:a1a69d32f310 4300 pre_range_time_out_m_clks = 0;
nikapov 0:a1a69d32f310 4301 if (scheduler_sequence_steps.PreRangeOn) {
nikapov 0:a1a69d32f310 4302
nikapov 0:a1a69d32f310 4303 /* Retrieve PRE-RANGE VCSEL Period */
nikapov 0:a1a69d32f310 4304 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 4305 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 4306 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4307
nikapov 0:a1a69d32f310 4308 /* Retrieve PRE-RANGE Timeout in Macro periods
nikapov 0:a1a69d32f310 4309 * (MCLKS) */
nikapov 0:a1a69d32f310 4310 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4311 status = VL53L0X_read_word(dev, 0x51,
nikapov 0:a1a69d32f310 4312 &pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 4313 pre_range_time_out_m_clks =
nikapov 0:a1a69d32f310 4314 VL53L0X_decode_timeout(
nikapov 0:a1a69d32f310 4315 pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 4316 }
nikapov 0:a1a69d32f310 4317 }
nikapov 0:a1a69d32f310 4318
nikapov 0:a1a69d32f310 4319 /* Calculate FINAL RANGE Timeout in Macro Periods
nikapov 0:a1a69d32f310 4320 * (MCLKS) and add PRE-RANGE value
nikapov 0:a1a69d32f310 4321 */
nikapov 0:a1a69d32f310 4322 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4323 status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov 0:a1a69d32f310 4324 VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov 0:a1a69d32f310 4325 &current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4326 }
nikapov 0:a1a69d32f310 4327 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4328 final_range_time_out_m_clks =
nikapov 0:a1a69d32f310 4329 VL53L0X_calc_timeout_mclks(dev,
nikapov 0:a1a69d32f310 4330 timeout_micro_secs,
nikapov 0:a1a69d32f310 4331 (uint8_t) current_vcsel_pulse_period_p_clk);
nikapov 0:a1a69d32f310 4332
nikapov 0:a1a69d32f310 4333 final_range_time_out_m_clks += pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 4334
nikapov 0:a1a69d32f310 4335 final_range_encoded_time_out =
nikapov 0:a1a69d32f310 4336 VL53L0X_encode_timeout(final_range_time_out_m_clks);
nikapov 0:a1a69d32f310 4337
nikapov 0:a1a69d32f310 4338 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4339 status = VL53L0X_write_word(dev, 0x71,
nikapov 0:a1a69d32f310 4340 final_range_encoded_time_out);
nikapov 0:a1a69d32f310 4341 }
nikapov 0:a1a69d32f310 4342
nikapov 0:a1a69d32f310 4343 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4344 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 4345 dev,
nikapov 0:a1a69d32f310 4346 FinalRangeTimeoutMicroSecs,
nikapov 0:a1a69d32f310 4347 timeout_micro_secs);
nikapov 0:a1a69d32f310 4348 }
nikapov 0:a1a69d32f310 4349 }
nikapov 0:a1a69d32f310 4350 } else {
nikapov 0:a1a69d32f310 4351 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4352 }
nikapov 0:a1a69d32f310 4353
nikapov 0:a1a69d32f310 4354 }
nikapov 0:a1a69d32f310 4355 return status;
nikapov 0:a1a69d32f310 4356 }
nikapov 0:a1a69d32f310 4357
nikapov 0:a1a69d32f310 4358 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 4359 uint32_t measurement_timing_budget_micro_seconds)
nikapov 0:a1a69d32f310 4360 {
nikapov 0:a1a69d32f310 4361 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4362 uint32_t final_range_timing_budget_micro_seconds;
nikapov 0:a1a69d32f310 4363 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov 0:a1a69d32f310 4364 uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
nikapov 0:a1a69d32f310 4365 uint32_t start_overhead_micro_seconds = 1910;
nikapov 0:a1a69d32f310 4366 uint32_t end_overhead_micro_seconds = 960;
nikapov 0:a1a69d32f310 4367 uint32_t msrc_overhead_micro_seconds = 660;
nikapov 0:a1a69d32f310 4368 uint32_t tcc_overhead_micro_seconds = 590;
nikapov 0:a1a69d32f310 4369 uint32_t dss_overhead_micro_seconds = 690;
nikapov 0:a1a69d32f310 4370 uint32_t pre_range_overhead_micro_seconds = 660;
nikapov 0:a1a69d32f310 4371 uint32_t final_range_overhead_micro_seconds = 550;
nikapov 0:a1a69d32f310 4372 uint32_t pre_range_timeout_micro_seconds = 0;
nikapov 0:a1a69d32f310 4373 uint32_t c_min_timing_budget_micro_seconds = 20000;
nikapov 0:a1a69d32f310 4374 uint32_t sub_timeout = 0;
nikapov 0:a1a69d32f310 4375
sepp_nepp 5:b95f6951f7d5 4376
nikapov 0:a1a69d32f310 4377
nikapov 0:a1a69d32f310 4378 if (measurement_timing_budget_micro_seconds
nikapov 0:a1a69d32f310 4379 < c_min_timing_budget_micro_seconds) {
nikapov 0:a1a69d32f310 4380 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4381 return status;
nikapov 0:a1a69d32f310 4382 }
nikapov 0:a1a69d32f310 4383
nikapov 0:a1a69d32f310 4384 final_range_timing_budget_micro_seconds =
nikapov 0:a1a69d32f310 4385 measurement_timing_budget_micro_seconds -
nikapov 0:a1a69d32f310 4386 (start_overhead_micro_seconds + end_overhead_micro_seconds);
nikapov 0:a1a69d32f310 4387
nikapov 0:a1a69d32f310 4388 status = VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps);
nikapov 0:a1a69d32f310 4389
nikapov 0:a1a69d32f310 4390 if (status == VL53L0X_ERROR_NONE &&
nikapov 0:a1a69d32f310 4391 (scheduler_sequence_steps.TccOn ||
nikapov 0:a1a69d32f310 4392 scheduler_sequence_steps.MsrcOn ||
nikapov 0:a1a69d32f310 4393 scheduler_sequence_steps.DssOn)) {
nikapov 0:a1a69d32f310 4394
nikapov 0:a1a69d32f310 4395 /* TCC, MSRC and DSS all share the same timeout */
nikapov 0:a1a69d32f310 4396 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 4397 VL53L0X_SEQUENCESTEP_MSRC,
nikapov 0:a1a69d32f310 4398 &msrc_dcc_tcc_timeout_micro_seconds);
nikapov 0:a1a69d32f310 4399
nikapov 0:a1a69d32f310 4400 /* Subtract the TCC, MSRC and DSS timeouts if they are
nikapov 0:a1a69d32f310 4401 * enabled. */
nikapov 0:a1a69d32f310 4402
nikapov 0:a1a69d32f310 4403 if (status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4404 return status;
nikapov 0:a1a69d32f310 4405 }
nikapov 0:a1a69d32f310 4406
nikapov 0:a1a69d32f310 4407 /* TCC */
nikapov 0:a1a69d32f310 4408 if (scheduler_sequence_steps.TccOn) {
nikapov 0:a1a69d32f310 4409
nikapov 0:a1a69d32f310 4410 sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
nikapov 0:a1a69d32f310 4411 + tcc_overhead_micro_seconds;
nikapov 0:a1a69d32f310 4412
nikapov 0:a1a69d32f310 4413 if (sub_timeout <
nikapov 0:a1a69d32f310 4414 final_range_timing_budget_micro_seconds) {
nikapov 0:a1a69d32f310 4415 final_range_timing_budget_micro_seconds -=
nikapov 0:a1a69d32f310 4416 sub_timeout;
nikapov 0:a1a69d32f310 4417 } else {
nikapov 0:a1a69d32f310 4418 /* Requested timeout too big. */
nikapov 0:a1a69d32f310 4419 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4420 }
nikapov 0:a1a69d32f310 4421 }
nikapov 0:a1a69d32f310 4422
nikapov 0:a1a69d32f310 4423 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 5:b95f6951f7d5 4424
nikapov 0:a1a69d32f310 4425 return status;
nikapov 0:a1a69d32f310 4426 }
nikapov 0:a1a69d32f310 4427
nikapov 0:a1a69d32f310 4428 /* DSS */
nikapov 0:a1a69d32f310 4429 if (scheduler_sequence_steps.DssOn) {
nikapov 0:a1a69d32f310 4430
nikapov 0:a1a69d32f310 4431 sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds +
nikapov 0:a1a69d32f310 4432 dss_overhead_micro_seconds);
nikapov 0:a1a69d32f310 4433
nikapov 0:a1a69d32f310 4434 if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov 0:a1a69d32f310 4435 final_range_timing_budget_micro_seconds
nikapov 0:a1a69d32f310 4436 -= sub_timeout;
nikapov 0:a1a69d32f310 4437 } else {
nikapov 0:a1a69d32f310 4438 /* Requested timeout too big. */
nikapov 0:a1a69d32f310 4439 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4440 }
nikapov 0:a1a69d32f310 4441 } else if (scheduler_sequence_steps.MsrcOn) {
nikapov 0:a1a69d32f310 4442 /* MSRC */
nikapov 0:a1a69d32f310 4443 sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
nikapov 0:a1a69d32f310 4444 msrc_overhead_micro_seconds;
nikapov 0:a1a69d32f310 4445
nikapov 0:a1a69d32f310 4446 if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov 0:a1a69d32f310 4447 final_range_timing_budget_micro_seconds
nikapov 0:a1a69d32f310 4448 -= sub_timeout;
nikapov 0:a1a69d32f310 4449 } else {
nikapov 0:a1a69d32f310 4450 /* Requested timeout too big. */
nikapov 0:a1a69d32f310 4451 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4452 }
nikapov 0:a1a69d32f310 4453 }
nikapov 0:a1a69d32f310 4454
nikapov 0:a1a69d32f310 4455 }
nikapov 0:a1a69d32f310 4456
nikapov 0:a1a69d32f310 4457 if (status != VL53L0X_ERROR_NONE) {
sepp_nepp 5:b95f6951f7d5 4458
nikapov 0:a1a69d32f310 4459 return status;
nikapov 0:a1a69d32f310 4460 }
nikapov 0:a1a69d32f310 4461
nikapov 0:a1a69d32f310 4462 if (scheduler_sequence_steps.PreRangeOn) {
nikapov 0:a1a69d32f310 4463
nikapov 0:a1a69d32f310 4464 /* Subtract the Pre-range timeout if enabled. */
nikapov 0:a1a69d32f310 4465
nikapov 0:a1a69d32f310 4466 status = get_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 4467 VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov 0:a1a69d32f310 4468 &pre_range_timeout_micro_seconds);
nikapov 0:a1a69d32f310 4469
nikapov 0:a1a69d32f310 4470 sub_timeout = pre_range_timeout_micro_seconds +
nikapov 0:a1a69d32f310 4471 pre_range_overhead_micro_seconds;
nikapov 0:a1a69d32f310 4472
nikapov 0:a1a69d32f310 4473 if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov 0:a1a69d32f310 4474 final_range_timing_budget_micro_seconds -= sub_timeout;
nikapov 0:a1a69d32f310 4475 } else {
nikapov 0:a1a69d32f310 4476 /* Requested timeout too big. */
nikapov 0:a1a69d32f310 4477 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4478 }
nikapov 0:a1a69d32f310 4479 }
nikapov 0:a1a69d32f310 4480
nikapov 0:a1a69d32f310 4481
nikapov 0:a1a69d32f310 4482 if (status == VL53L0X_ERROR_NONE &&
nikapov 0:a1a69d32f310 4483 scheduler_sequence_steps.FinalRangeOn) {
nikapov 0:a1a69d32f310 4484
nikapov 0:a1a69d32f310 4485 final_range_timing_budget_micro_seconds -=
nikapov 0:a1a69d32f310 4486 final_range_overhead_micro_seconds;
nikapov 0:a1a69d32f310 4487
nikapov 0:a1a69d32f310 4488 /* Final Range Timeout
nikapov 0:a1a69d32f310 4489 * Note that the final range timeout is determined by the timing
nikapov 0:a1a69d32f310 4490 * budget and the sum of all other timeouts within the sequence.
nikapov 0:a1a69d32f310 4491 * If there is no room for the final range timeout, then an error
nikapov 0:a1a69d32f310 4492 * will be set. Otherwise the remaining time will be applied to
nikapov 0:a1a69d32f310 4493 * the final range.
nikapov 0:a1a69d32f310 4494 */
nikapov 0:a1a69d32f310 4495 status = set_sequence_step_timeout(dev,
nikapov 0:a1a69d32f310 4496 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov 0:a1a69d32f310 4497 final_range_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 4498
nikapov 0:a1a69d32f310 4499 VL53L0X_SETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 4500 MeasurementTimingBudgetMicroSeconds,
nikapov 0:a1a69d32f310 4501 measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 4502 }
nikapov 0:a1a69d32f310 4503
sepp_nepp 5:b95f6951f7d5 4504
nikapov 0:a1a69d32f310 4505
nikapov 0:a1a69d32f310 4506 return status;
nikapov 0:a1a69d32f310 4507 }
nikapov 0:a1a69d32f310 4508
nikapov 0:a1a69d32f310 4509 VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 4510 uint32_t measurement_timing_budget_micro_seconds)
nikapov 0:a1a69d32f310 4511 {
nikapov 0:a1a69d32f310 4512 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 4513
nikapov 0:a1a69d32f310 4514
nikapov 0:a1a69d32f310 4515 status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(dev,
nikapov 0:a1a69d32f310 4516 measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 4517
sepp_nepp 5:b95f6951f7d5 4518
nikapov 0:a1a69d32f310 4519
nikapov 0:a1a69d32f310 4520 return status;
nikapov 0:a1a69d32f310 4521 }
nikapov 0:a1a69d32f310 4522
nikapov 0:a1a69d32f310 4523 VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 4524 VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled)
nikapov 0:a1a69d32f310 4525 {
nikapov 0:a1a69d32f310 4526 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4527 uint8_t sequence_config = 0;
nikapov 0:a1a69d32f310 4528 uint8_t sequence_config_new = 0;
nikapov 0:a1a69d32f310 4529 uint32_t measurement_timing_budget_micro_seconds;
sepp_nepp 5:b95f6951f7d5 4530
nikapov 0:a1a69d32f310 4531
nikapov 0:a1a69d32f310 4532 status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov 0:a1a69d32f310 4533 &sequence_config);
nikapov 0:a1a69d32f310 4534
nikapov 0:a1a69d32f310 4535 sequence_config_new = sequence_config;
nikapov 0:a1a69d32f310 4536
nikapov 0:a1a69d32f310 4537 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4538 if (sequence_step_enabled == 1) {
nikapov 0:a1a69d32f310 4539
nikapov 0:a1a69d32f310 4540 /* Enable requested sequence step
nikapov 0:a1a69d32f310 4541 */
nikapov 0:a1a69d32f310 4542 switch (sequence_step_id) {
nikapov 0:a1a69d32f310 4543 case VL53L0X_SEQUENCESTEP_TCC:
nikapov 0:a1a69d32f310 4544 sequence_config_new |= 0x10;
nikapov 0:a1a69d32f310 4545 break;
nikapov 0:a1a69d32f310 4546 case VL53L0X_SEQUENCESTEP_DSS:
nikapov 0:a1a69d32f310 4547 sequence_config_new |= 0x28;
nikapov 0:a1a69d32f310 4548 break;
nikapov 0:a1a69d32f310 4549 case VL53L0X_SEQUENCESTEP_MSRC:
nikapov 0:a1a69d32f310 4550 sequence_config_new |= 0x04;
nikapov 0:a1a69d32f310 4551 break;
nikapov 0:a1a69d32f310 4552 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov 0:a1a69d32f310 4553 sequence_config_new |= 0x40;
nikapov 0:a1a69d32f310 4554 break;
nikapov 0:a1a69d32f310 4555 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov 0:a1a69d32f310 4556 sequence_config_new |= 0x80;
nikapov 0:a1a69d32f310 4557 break;
nikapov 0:a1a69d32f310 4558 default:
nikapov 0:a1a69d32f310 4559 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4560 }
nikapov 0:a1a69d32f310 4561 } else {
nikapov 0:a1a69d32f310 4562 /* Disable requested sequence step
nikapov 0:a1a69d32f310 4563 */
nikapov 0:a1a69d32f310 4564 switch (sequence_step_id) {
nikapov 0:a1a69d32f310 4565 case VL53L0X_SEQUENCESTEP_TCC:
nikapov 0:a1a69d32f310 4566 sequence_config_new &= 0xef;
nikapov 0:a1a69d32f310 4567 break;
nikapov 0:a1a69d32f310 4568 case VL53L0X_SEQUENCESTEP_DSS:
nikapov 0:a1a69d32f310 4569 sequence_config_new &= 0xd7;
nikapov 0:a1a69d32f310 4570 break;
nikapov 0:a1a69d32f310 4571 case VL53L0X_SEQUENCESTEP_MSRC:
nikapov 0:a1a69d32f310 4572 sequence_config_new &= 0xfb;
nikapov 0:a1a69d32f310 4573 break;
nikapov 0:a1a69d32f310 4574 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov 0:a1a69d32f310 4575 sequence_config_new &= 0xbf;
nikapov 0:a1a69d32f310 4576 break;
nikapov 0:a1a69d32f310 4577 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov 0:a1a69d32f310 4578 sequence_config_new &= 0x7f;
nikapov 0:a1a69d32f310 4579 break;
nikapov 0:a1a69d32f310 4580 default:
nikapov 0:a1a69d32f310 4581 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4582 }
nikapov 0:a1a69d32f310 4583 }
nikapov 0:a1a69d32f310 4584 }
nikapov 0:a1a69d32f310 4585
nikapov 0:a1a69d32f310 4586 if (sequence_config_new != sequence_config) {
nikapov 0:a1a69d32f310 4587 /* Apply New Setting */
nikapov 0:a1a69d32f310 4588 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4589 status = VL53L0X_write_byte(dev,
nikapov 0:a1a69d32f310 4590 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new);
nikapov 0:a1a69d32f310 4591 }
nikapov 0:a1a69d32f310 4592 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4593 PALDevDataSet(dev, SequenceConfig, sequence_config_new);
nikapov 0:a1a69d32f310 4594 }
nikapov 0:a1a69d32f310 4595
nikapov 0:a1a69d32f310 4596
nikapov 0:a1a69d32f310 4597 /* Recalculate timing budget */
nikapov 0:a1a69d32f310 4598 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4599 VL53L0X_GETPARAMETERFIELD(dev,
nikapov 0:a1a69d32f310 4600 MeasurementTimingBudgetMicroSeconds,
nikapov 0:a1a69d32f310 4601 measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 4602
nikapov 0:a1a69d32f310 4603 VL53L0X_set_measurement_timing_budget_micro_seconds(dev,
nikapov 0:a1a69d32f310 4604 measurement_timing_budget_micro_seconds);
nikapov 0:a1a69d32f310 4605 }
nikapov 0:a1a69d32f310 4606 }
nikapov 0:a1a69d32f310 4607
sepp_nepp 5:b95f6951f7d5 4608
nikapov 0:a1a69d32f310 4609
nikapov 0:a1a69d32f310 4610 return status;
nikapov 0:a1a69d32f310 4611 }
nikapov 0:a1a69d32f310 4612
nikapov 0:a1a69d32f310 4613 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id,
nikapov 0:a1a69d32f310 4614 uint8_t limit_check_enable)
nikapov 0:a1a69d32f310 4615 {
nikapov 0:a1a69d32f310 4616 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4617 FixPoint1616_t temp_fix1616 = 0;
nikapov 0:a1a69d32f310 4618 uint8_t limit_check_enable_int = 0;
nikapov 0:a1a69d32f310 4619 uint8_t limit_check_disable = 0;
nikapov 0:a1a69d32f310 4620 uint8_t temp8;
nikapov 0:a1a69d32f310 4621
sepp_nepp 5:b95f6951f7d5 4622
nikapov 0:a1a69d32f310 4623
nikapov 0:a1a69d32f310 4624 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
nikapov 0:a1a69d32f310 4625 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4626 } else {
nikapov 0:a1a69d32f310 4627 if (limit_check_enable == 0) {
nikapov 0:a1a69d32f310 4628 temp_fix1616 = 0;
nikapov 0:a1a69d32f310 4629 limit_check_enable_int = 0;
nikapov 0:a1a69d32f310 4630 limit_check_disable = 1;
nikapov 0:a1a69d32f310 4631
nikapov 0:a1a69d32f310 4632 } else {
nikapov 0:a1a69d32f310 4633 VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov 0:a1a69d32f310 4634 limit_check_id, temp_fix1616);
nikapov 0:a1a69d32f310 4635 limit_check_disable = 0;
nikapov 0:a1a69d32f310 4636 /* this to be sure to have either 0 or 1 */
nikapov 0:a1a69d32f310 4637 limit_check_enable_int = 1;
nikapov 0:a1a69d32f310 4638 }
nikapov 0:a1a69d32f310 4639
nikapov 0:a1a69d32f310 4640 switch (limit_check_id) {
nikapov 0:a1a69d32f310 4641
nikapov 0:a1a69d32f310 4642 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov 0:a1a69d32f310 4643 /* internal computation: */
nikapov 0:a1a69d32f310 4644 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov 0:a1a69d32f310 4645 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov 0:a1a69d32f310 4646 limit_check_enable_int);
nikapov 0:a1a69d32f310 4647
nikapov 0:a1a69d32f310 4648 break;
nikapov 0:a1a69d32f310 4649
nikapov 0:a1a69d32f310 4650 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
nikapov 0:a1a69d32f310 4651
nikapov 0:a1a69d32f310 4652 status = VL53L0X_write_word(dev,
nikapov 0:a1a69d32f310 4653 VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
nikapov 0:a1a69d32f310 4654 VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616));
nikapov 0:a1a69d32f310 4655
nikapov 0:a1a69d32f310 4656 break;
nikapov 0:a1a69d32f310 4657
nikapov 0:a1a69d32f310 4658 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov 0:a1a69d32f310 4659
nikapov 0:a1a69d32f310 4660 /* internal computation: */
nikapov 0:a1a69d32f310 4661 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov 0:a1a69d32f310 4662 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov 0:a1a69d32f310 4663 limit_check_enable_int);
nikapov 0:a1a69d32f310 4664
nikapov 0:a1a69d32f310 4665 break;
nikapov 0:a1a69d32f310 4666
nikapov 0:a1a69d32f310 4667 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov 0:a1a69d32f310 4668
nikapov 0:a1a69d32f310 4669 /* internal computation: */
nikapov 0:a1a69d32f310 4670 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov 0:a1a69d32f310 4671 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov 0:a1a69d32f310 4672 limit_check_enable_int);
nikapov 0:a1a69d32f310 4673
nikapov 0:a1a69d32f310 4674 break;
nikapov 0:a1a69d32f310 4675
nikapov 0:a1a69d32f310 4676 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov 0:a1a69d32f310 4677
nikapov 0:a1a69d32f310 4678 temp8 = (uint8_t)(limit_check_disable << 1);
nikapov 0:a1a69d32f310 4679 status = VL53L0X_update_byte(dev,
nikapov 0:a1a69d32f310 4680 VL53L0X_REG_MSRC_CONFIG_CONTROL,
nikapov 0:a1a69d32f310 4681 0xFE, temp8);
nikapov 0:a1a69d32f310 4682
nikapov 0:a1a69d32f310 4683 break;
nikapov 0:a1a69d32f310 4684
nikapov 0:a1a69d32f310 4685 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
nikapov 0:a1a69d32f310 4686
nikapov 0:a1a69d32f310 4687 temp8 = (uint8_t)(limit_check_disable << 4);
nikapov 0:a1a69d32f310 4688 status = VL53L0X_update_byte(dev,
nikapov 0:a1a69d32f310 4689 VL53L0X_REG_MSRC_CONFIG_CONTROL,
nikapov 0:a1a69d32f310 4690 0xEF, temp8);
nikapov 0:a1a69d32f310 4691
nikapov 0:a1a69d32f310 4692 break;
nikapov 0:a1a69d32f310 4693
nikapov 0:a1a69d32f310 4694
nikapov 0:a1a69d32f310 4695 default:
nikapov 0:a1a69d32f310 4696 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4697
nikapov 0:a1a69d32f310 4698 }
nikapov 0:a1a69d32f310 4699
nikapov 0:a1a69d32f310 4700 }
nikapov 0:a1a69d32f310 4701
nikapov 0:a1a69d32f310 4702 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4703 if (limit_check_enable == 0) {
nikapov 0:a1a69d32f310 4704 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov 0:a1a69d32f310 4705 limit_check_id, 0);
nikapov 0:a1a69d32f310 4706 } else {
nikapov 0:a1a69d32f310 4707 VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov 0:a1a69d32f310 4708 limit_check_id, 1);
nikapov 0:a1a69d32f310 4709 }
nikapov 0:a1a69d32f310 4710 }
nikapov 0:a1a69d32f310 4711
sepp_nepp 5:b95f6951f7d5 4712
nikapov 0:a1a69d32f310 4713 return status;
nikapov 0:a1a69d32f310 4714 }
nikapov 0:a1a69d32f310 4715
nikapov 0:a1a69d32f310 4716 VL53L0X_Error VL53L0X::VL53L0X_static_init(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 4717 {
nikapov 0:a1a69d32f310 4718 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4719 VL53L0X_DeviceParameters_t current_parameters = {0};
nikapov 0:a1a69d32f310 4720 uint8_t *p_tuning_setting_buffer;
nikapov 0:a1a69d32f310 4721 uint16_t tempword = 0;
nikapov 0:a1a69d32f310 4722 uint8_t tempbyte = 0;
nikapov 0:a1a69d32f310 4723 uint8_t use_internal_tuning_settings = 0;
nikapov 0:a1a69d32f310 4724 uint32_t count = 0;
nikapov 0:a1a69d32f310 4725 uint8_t is_aperture_spads = 0;
nikapov 0:a1a69d32f310 4726 uint32_t ref_spad_count = 0;
nikapov 0:a1a69d32f310 4727 uint8_t aperture_spads = 0;
nikapov 0:a1a69d32f310 4728 uint8_t vcsel_pulse_period_pclk;
nikapov 0:a1a69d32f310 4729 uint32_t seq_timeout_micro_secs;
nikapov 0:a1a69d32f310 4730
sepp_nepp 5:b95f6951f7d5 4731
nikapov 0:a1a69d32f310 4732
nikapov 0:a1a69d32f310 4733 status = VL53L0X_get_info_from_device(dev, 1);
nikapov 0:a1a69d32f310 4734
nikapov 0:a1a69d32f310 4735 /* set the ref spad from NVM */
nikapov 0:a1a69d32f310 4736 count = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 4737 ReferenceSpadCount);
nikapov 0:a1a69d32f310 4738 aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov 0:a1a69d32f310 4739 ReferenceSpadType);
nikapov 0:a1a69d32f310 4740
nikapov 0:a1a69d32f310 4741 /* NVM value invalid */
nikapov 0:a1a69d32f310 4742 if ((aperture_spads > 1) ||
nikapov 0:a1a69d32f310 4743 ((aperture_spads == 1) && (count > 32)) ||
nikapov 0:a1a69d32f310 4744 ((aperture_spads == 0) && (count > 12))) {
nikapov 0:a1a69d32f310 4745 status = wrapped_VL53L0X_perform_ref_spad_management(dev, &ref_spad_count,
nikapov 0:a1a69d32f310 4746 &is_aperture_spads);
nikapov 0:a1a69d32f310 4747 } else {
nikapov 0:a1a69d32f310 4748 status = VL53L0X_set_reference_spads(dev, count, aperture_spads);
nikapov 0:a1a69d32f310 4749 }
nikapov 0:a1a69d32f310 4750
nikapov 0:a1a69d32f310 4751
nikapov 0:a1a69d32f310 4752 /* Initialize tuning settings buffer to prevent compiler warning. */
nikapov 0:a1a69d32f310 4753 p_tuning_setting_buffer = DefaultTuningSettings;
nikapov 0:a1a69d32f310 4754
nikapov 0:a1a69d32f310 4755 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4756 use_internal_tuning_settings = PALDevDataGet(dev,
nikapov 0:a1a69d32f310 4757 UseInternalTuningSettings);
nikapov 0:a1a69d32f310 4758
nikapov 0:a1a69d32f310 4759 if (use_internal_tuning_settings == 0) {
nikapov 0:a1a69d32f310 4760 p_tuning_setting_buffer = PALDevDataGet(dev,
nikapov 0:a1a69d32f310 4761 pTuningSettingsPointer);
nikapov 0:a1a69d32f310 4762 } else {
nikapov 0:a1a69d32f310 4763 p_tuning_setting_buffer = DefaultTuningSettings;
nikapov 0:a1a69d32f310 4764 }
nikapov 0:a1a69d32f310 4765
nikapov 0:a1a69d32f310 4766 }
nikapov 0:a1a69d32f310 4767
nikapov 0:a1a69d32f310 4768 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4769 status = VL53L0X_load_tuning_settings(dev, p_tuning_setting_buffer);
nikapov 0:a1a69d32f310 4770 }
nikapov 0:a1a69d32f310 4771
nikapov 0:a1a69d32f310 4772
nikapov 0:a1a69d32f310 4773 /* Set interrupt config to new sample ready */
nikapov 0:a1a69d32f310 4774 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4775 status = VL53L0X_set_gpio_config(dev, 0, 0,
nikapov 0:a1a69d32f310 4776 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov 0:a1a69d32f310 4777 VL53L0X_INTERRUPTPOLARITY_LOW);
nikapov 0:a1a69d32f310 4778 }
nikapov 0:a1a69d32f310 4779
nikapov 0:a1a69d32f310 4780 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4781 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 4782 status |= VL53L0X_read_word(dev, 0x84, &tempword);
nikapov 0:a1a69d32f310 4783 status |= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 4784 }
nikapov 0:a1a69d32f310 4785
nikapov 0:a1a69d32f310 4786 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4787 VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz,
nikapov 0:a1a69d32f310 4788 VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword));
nikapov 0:a1a69d32f310 4789 }
nikapov 0:a1a69d32f310 4790
nikapov 0:a1a69d32f310 4791 /* After static init, some device parameters may be changed,
nikapov 0:a1a69d32f310 4792 * so update them */
nikapov 0:a1a69d32f310 4793 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4794 status = VL53L0X_get_device_parameters(dev, &current_parameters);
nikapov 0:a1a69d32f310 4795 }
nikapov 0:a1a69d32f310 4796
nikapov 0:a1a69d32f310 4797
nikapov 0:a1a69d32f310 4798 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4799 status = VL53L0X_get_fraction_enable(dev, &tempbyte);
Davidroid 3:e9269ff624ed 4800 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4801 PALDevDataSet(dev, RangeFractionalEnable, tempbyte);
Davidroid 3:e9269ff624ed 4802 }
nikapov 0:a1a69d32f310 4803
nikapov 0:a1a69d32f310 4804 }
nikapov 0:a1a69d32f310 4805
nikapov 0:a1a69d32f310 4806 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4807 PALDevDataSet(dev, CurrentParameters, current_parameters);
nikapov 0:a1a69d32f310 4808 }
nikapov 0:a1a69d32f310 4809
nikapov 0:a1a69d32f310 4810
nikapov 0:a1a69d32f310 4811 /* read the sequence config and save it */
nikapov 0:a1a69d32f310 4812 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4813 status = VL53L0X_read_byte(dev,
nikapov 0:a1a69d32f310 4814 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
nikapov 0:a1a69d32f310 4815 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4816 PALDevDataSet(dev, SequenceConfig, tempbyte);
nikapov 0:a1a69d32f310 4817 }
nikapov 0:a1a69d32f310 4818 }
nikapov 0:a1a69d32f310 4819
nikapov 0:a1a69d32f310 4820 /* Disable MSRC and TCC by default */
nikapov 0:a1a69d32f310 4821 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4822 status = VL53L0X_set_sequence_step_enable(dev,
nikapov 0:a1a69d32f310 4823 VL53L0X_SEQUENCESTEP_TCC, 0);
Davidroid 2:d07edeaff6f1 4824 }
nikapov 0:a1a69d32f310 4825
nikapov 0:a1a69d32f310 4826 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4827 status = VL53L0X_set_sequence_step_enable(dev,
nikapov 0:a1a69d32f310 4828 VL53L0X_SEQUENCESTEP_MSRC, 0);
Davidroid 2:d07edeaff6f1 4829 }
nikapov 0:a1a69d32f310 4830
nikapov 0:a1a69d32f310 4831 /* Set PAL State to standby */
nikapov 0:a1a69d32f310 4832 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4833 PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
nikapov 0:a1a69d32f310 4834 }
nikapov 0:a1a69d32f310 4835
nikapov 0:a1a69d32f310 4836 /* Store pre-range vcsel period */
nikapov 0:a1a69d32f310 4837 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4838 status = VL53L0X_get_vcsel_pulse_period(
nikapov 0:a1a69d32f310 4839 dev,
nikapov 0:a1a69d32f310 4840 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov 0:a1a69d32f310 4841 &vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 4842 }
nikapov 0:a1a69d32f310 4843
nikapov 0:a1a69d32f310 4844 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4845 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 4846 dev,
nikapov 0:a1a69d32f310 4847 PreRangeVcselPulsePeriod,
nikapov 0:a1a69d32f310 4848 vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 4849 }
nikapov 0:a1a69d32f310 4850
nikapov 0:a1a69d32f310 4851 /* Store final-range vcsel period */
nikapov 0:a1a69d32f310 4852 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4853 status = VL53L0X_get_vcsel_pulse_period(
nikapov 0:a1a69d32f310 4854 dev,
nikapov 0:a1a69d32f310 4855 VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov 0:a1a69d32f310 4856 &vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 4857 }
nikapov 0:a1a69d32f310 4858
nikapov 0:a1a69d32f310 4859 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4860 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 4861 dev,
nikapov 0:a1a69d32f310 4862 FinalRangeVcselPulsePeriod,
nikapov 0:a1a69d32f310 4863 vcsel_pulse_period_pclk);
nikapov 0:a1a69d32f310 4864 }
nikapov 0:a1a69d32f310 4865
nikapov 0:a1a69d32f310 4866 /* Store pre-range timeout */
nikapov 0:a1a69d32f310 4867 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4868 status = get_sequence_step_timeout(
nikapov 0:a1a69d32f310 4869 dev,
nikapov 0:a1a69d32f310 4870 VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov 0:a1a69d32f310 4871 &seq_timeout_micro_secs);
nikapov 0:a1a69d32f310 4872 }
nikapov 0:a1a69d32f310 4873
nikapov 0:a1a69d32f310 4874 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4875 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 4876 dev,
nikapov 0:a1a69d32f310 4877 PreRangeTimeoutMicroSecs,
nikapov 0:a1a69d32f310 4878 seq_timeout_micro_secs);
nikapov 0:a1a69d32f310 4879 }
nikapov 0:a1a69d32f310 4880
nikapov 0:a1a69d32f310 4881 /* Store final-range timeout */
nikapov 0:a1a69d32f310 4882 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4883 status = get_sequence_step_timeout(
nikapov 0:a1a69d32f310 4884 dev,
nikapov 0:a1a69d32f310 4885 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov 0:a1a69d32f310 4886 &seq_timeout_micro_secs);
nikapov 0:a1a69d32f310 4887 }
nikapov 0:a1a69d32f310 4888
nikapov 0:a1a69d32f310 4889 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4890 VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov 0:a1a69d32f310 4891 dev,
nikapov 0:a1a69d32f310 4892 FinalRangeTimeoutMicroSecs,
nikapov 0:a1a69d32f310 4893 seq_timeout_micro_secs);
nikapov 0:a1a69d32f310 4894 }
nikapov 0:a1a69d32f310 4895
sepp_nepp 5:b95f6951f7d5 4896
nikapov 0:a1a69d32f310 4897 return status;
nikapov 0:a1a69d32f310 4898 }
nikapov 0:a1a69d32f310 4899
nikapov 0:a1a69d32f310 4900
nikapov 0:a1a69d32f310 4901 VL53L0X_Error VL53L0X::VL53L0X_stop_measurement(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 4902 {
nikapov 0:a1a69d32f310 4903 VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp 5:b95f6951f7d5 4904
nikapov 0:a1a69d32f310 4905
nikapov 0:a1a69d32f310 4906 status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START,
nikapov 0:a1a69d32f310 4907 VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
nikapov 0:a1a69d32f310 4908
nikapov 0:a1a69d32f310 4909 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 4910 status = VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov 0:a1a69d32f310 4911 status = VL53L0X_write_byte(dev, 0x91, 0x00);
nikapov 0:a1a69d32f310 4912 status = VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov 0:a1a69d32f310 4913 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 4914
nikapov 0:a1a69d32f310 4915 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4916 /* Set PAL State to Idle */
nikapov 0:a1a69d32f310 4917 PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
nikapov 0:a1a69d32f310 4918 }
nikapov 0:a1a69d32f310 4919
nikapov 0:a1a69d32f310 4920 /* Check if need to apply interrupt settings */
nikapov 0:a1a69d32f310 4921 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4922 status = VL53L0X_check_and_load_interrupt_settings(dev, 0);
nikapov 0:a1a69d32f310 4923 }
nikapov 0:a1a69d32f310 4924
sepp_nepp 5:b95f6951f7d5 4925
nikapov 0:a1a69d32f310 4926 return status;
nikapov 0:a1a69d32f310 4927 }
nikapov 0:a1a69d32f310 4928
nikapov 0:a1a69d32f310 4929 VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(VL53L0X_DEV dev,
nikapov 0:a1a69d32f310 4930 uint32_t *p_stop_status)
nikapov 0:a1a69d32f310 4931 {
nikapov 0:a1a69d32f310 4932 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 4933 uint8_t byte = 0;
sepp_nepp 5:b95f6951f7d5 4934
nikapov 0:a1a69d32f310 4935
nikapov 0:a1a69d32f310 4936 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 4937
nikapov 0:a1a69d32f310 4938 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4939 status = VL53L0X_read_byte(dev, 0x04, &byte);
nikapov 0:a1a69d32f310 4940 }
nikapov 0:a1a69d32f310 4941
nikapov 0:a1a69d32f310 4942 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 4943 status = VL53L0X_write_byte(dev, 0xFF, 0x0);
nikapov 0:a1a69d32f310 4944 }
nikapov 0:a1a69d32f310 4945
nikapov 0:a1a69d32f310 4946 *p_stop_status = byte;
nikapov 0:a1a69d32f310 4947
nikapov 0:a1a69d32f310 4948 if (byte == 0) {
nikapov 0:a1a69d32f310 4949 status = VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov 0:a1a69d32f310 4950 status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov 0:a1a69d32f310 4951 status = VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov 0:a1a69d32f310 4952 status = VL53L0X_write_byte(dev, 0x91,
nikapov 0:a1a69d32f310 4953 PALDevDataGet(dev, StopVariable));
nikapov 0:a1a69d32f310 4954 status = VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov 0:a1a69d32f310 4955 status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov 0:a1a69d32f310 4956 status = VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov 0:a1a69d32f310 4957 }
nikapov 0:a1a69d32f310 4958
sepp_nepp 5:b95f6951f7d5 4959
nikapov 0:a1a69d32f310 4960 return status;
nikapov 0:a1a69d32f310 4961 }
nikapov 0:a1a69d32f310 4962
nikapov 0:a1a69d32f310 4963 /****************** Write and read functions from I2C *************************/
nikapov 0:a1a69d32f310 4964
nikapov 0:a1a69d32f310 4965 VL53L0X_Error VL53L0X::VL53L0X_write_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count)
nikapov 0:a1a69d32f310 4966 {
nikapov 0:a1a69d32f310 4967 int status;
nikapov 0:a1a69d32f310 4968
nikapov 0:a1a69d32f310 4969 status = VL53L0X_i2c_write(dev->I2cDevAddr, index, p_data, (uint16_t)count);
nikapov 0:a1a69d32f310 4970 return status;
nikapov 0:a1a69d32f310 4971 }
nikapov 0:a1a69d32f310 4972
nikapov 0:a1a69d32f310 4973 VL53L0X_Error VL53L0X::VL53L0X_read_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count)
nikapov 0:a1a69d32f310 4974 {
nikapov 0:a1a69d32f310 4975 int status;
nikapov 0:a1a69d32f310 4976
nikapov 0:a1a69d32f310 4977 if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
nikapov 0:a1a69d32f310 4978 status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 4979 }
nikapov 0:a1a69d32f310 4980
nikapov 0:a1a69d32f310 4981 status = VL53L0X_i2c_read(dev->I2cDevAddr, index, p_data, (uint16_t)count);
nikapov 0:a1a69d32f310 4982
nikapov 0:a1a69d32f310 4983 return status;
nikapov 0:a1a69d32f310 4984 }
nikapov 0:a1a69d32f310 4985
nikapov 0:a1a69d32f310 4986
nikapov 0:a1a69d32f310 4987 VL53L0X_Error VL53L0X::VL53L0X_write_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
nikapov 0:a1a69d32f310 4988 {
nikapov 0:a1a69d32f310 4989 int status;
nikapov 0:a1a69d32f310 4990
nikapov 0:a1a69d32f310 4991 status = VL53L0X_i2c_write(Dev->I2cDevAddr, index, &data, 1);
nikapov 0:a1a69d32f310 4992 return status;
nikapov 0:a1a69d32f310 4993 }
nikapov 0:a1a69d32f310 4994
nikapov 0:a1a69d32f310 4995 VL53L0X_Error VL53L0X::VL53L0X_write_word(VL53L0X_DEV dev, uint8_t index, uint16_t data)
nikapov 0:a1a69d32f310 4996 {
nikapov 0:a1a69d32f310 4997 int status;
nikapov 0:a1a69d32f310 4998 uint8_t buffer[2];
nikapov 0:a1a69d32f310 4999
nikapov 0:a1a69d32f310 5000 buffer[0] = data >> 8;
nikapov 0:a1a69d32f310 5001 buffer[1] = data & 0x00FF;
nikapov 0:a1a69d32f310 5002 status = VL53L0X_i2c_write(dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
nikapov 0:a1a69d32f310 5003 return status;
nikapov 0:a1a69d32f310 5004 }
nikapov 0:a1a69d32f310 5005
nikapov 0:a1a69d32f310 5006 VL53L0X_Error VL53L0X::VL53L0X_write_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
nikapov 0:a1a69d32f310 5007 {
nikapov 0:a1a69d32f310 5008 int status;
nikapov 0:a1a69d32f310 5009 uint8_t buffer[4];
nikapov 0:a1a69d32f310 5010
nikapov 0:a1a69d32f310 5011 buffer[0] = (data >> 24) & 0xFF;
nikapov 0:a1a69d32f310 5012 buffer[1] = (data >> 16) & 0xFF;
nikapov 0:a1a69d32f310 5013 buffer[2] = (data >> 8) & 0xFF;
nikapov 0:a1a69d32f310 5014 buffer[3] = (data >> 0) & 0xFF;
nikapov 0:a1a69d32f310 5015 status = VL53L0X_i2c_write(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
nikapov 0:a1a69d32f310 5016 return status;
nikapov 0:a1a69d32f310 5017 }
nikapov 0:a1a69d32f310 5018
nikapov 0:a1a69d32f310 5019
nikapov 0:a1a69d32f310 5020 VL53L0X_Error VL53L0X::VL53L0X_read_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t *p_data)
nikapov 0:a1a69d32f310 5021 {
nikapov 0:a1a69d32f310 5022 int status;
nikapov 0:a1a69d32f310 5023
nikapov 0:a1a69d32f310 5024 status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, p_data, 1);
nikapov 0:a1a69d32f310 5025
Davidroid 3:e9269ff624ed 5026 if (status) {
nikapov 0:a1a69d32f310 5027 return -1;
Davidroid 3:e9269ff624ed 5028 }
nikapov 0:a1a69d32f310 5029
nikapov 0:a1a69d32f310 5030 return 0;
nikapov 0:a1a69d32f310 5031 }
nikapov 0:a1a69d32f310 5032
nikapov 0:a1a69d32f310 5033 VL53L0X_Error VL53L0X::VL53L0X_read_word(VL53L0X_DEV Dev, uint8_t index, uint16_t *p_data)
nikapov 0:a1a69d32f310 5034 {
nikapov 0:a1a69d32f310 5035 int status;
nikapov 0:a1a69d32f310 5036 uint8_t buffer[2] = {0, 0};
nikapov 0:a1a69d32f310 5037
nikapov 0:a1a69d32f310 5038 status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, buffer, 2);
nikapov 0:a1a69d32f310 5039 if (!status) {
nikapov 0:a1a69d32f310 5040 *p_data = (buffer[0] << 8) + buffer[1];
nikapov 0:a1a69d32f310 5041 }
nikapov 0:a1a69d32f310 5042 return status;
nikapov 0:a1a69d32f310 5043
nikapov 0:a1a69d32f310 5044 }
nikapov 0:a1a69d32f310 5045
nikapov 0:a1a69d32f310 5046 VL53L0X_Error VL53L0X::VL53L0X_read_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t *p_data)
nikapov 0:a1a69d32f310 5047 {
nikapov 0:a1a69d32f310 5048 int status;
nikapov 0:a1a69d32f310 5049 uint8_t buffer[4] = {0, 0, 0, 0};
nikapov 0:a1a69d32f310 5050
nikapov 0:a1a69d32f310 5051 status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, buffer, 4);
nikapov 0:a1a69d32f310 5052 if (!status) {
nikapov 0:a1a69d32f310 5053 *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
nikapov 0:a1a69d32f310 5054 }
nikapov 0:a1a69d32f310 5055 return status;
nikapov 0:a1a69d32f310 5056
nikapov 0:a1a69d32f310 5057 }
nikapov 0:a1a69d32f310 5058
nikapov 0:a1a69d32f310 5059 VL53L0X_Error VL53L0X::VL53L0X_update_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t and_data, uint8_t or_data)
nikapov 0:a1a69d32f310 5060 {
nikapov 0:a1a69d32f310 5061 int status;
nikapov 0:a1a69d32f310 5062 uint8_t buffer = 0;
nikapov 0:a1a69d32f310 5063
nikapov 0:a1a69d32f310 5064 /* read data direct onto buffer */
nikapov 0:a1a69d32f310 5065 status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, &buffer, 1);
nikapov 0:a1a69d32f310 5066 if (!status) {
nikapov 0:a1a69d32f310 5067 buffer = (buffer & and_data) | or_data;
nikapov 0:a1a69d32f310 5068 status = VL53L0X_i2c_write(Dev->I2cDevAddr, index, &buffer, (uint8_t)1);
nikapov 0:a1a69d32f310 5069 }
nikapov 0:a1a69d32f310 5070 return status;
nikapov 0:a1a69d32f310 5071 }
nikapov 0:a1a69d32f310 5072
nikapov 0:a1a69d32f310 5073 VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
nikapov 0:a1a69d32f310 5074 uint16_t NumByteToWrite)
nikapov 0:a1a69d32f310 5075 {
nikapov 0:a1a69d32f310 5076 int ret;
nikapov 0:a1a69d32f310 5077
nikapov 0:a1a69d32f310 5078 ret = _dev_i2c->i2c_write(p_data, DeviceAddr, RegisterAddr, NumByteToWrite);
nikapov 0:a1a69d32f310 5079
nikapov 0:a1a69d32f310 5080 if (ret) {
nikapov 0:a1a69d32f310 5081 return -1;
nikapov 0:a1a69d32f310 5082 }
nikapov 0:a1a69d32f310 5083 return 0;
nikapov 0:a1a69d32f310 5084 }
nikapov 0:a1a69d32f310 5085
nikapov 0:a1a69d32f310 5086 VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
nikapov 0:a1a69d32f310 5087 uint16_t NumByteToRead)
nikapov 0:a1a69d32f310 5088 {
nikapov 0:a1a69d32f310 5089 int ret;
nikapov 0:a1a69d32f310 5090
nikapov 0:a1a69d32f310 5091 ret = _dev_i2c->i2c_read(p_data, DeviceAddr, RegisterAddr, NumByteToRead);
nikapov 0:a1a69d32f310 5092
nikapov 0:a1a69d32f310 5093 if (ret) {
nikapov 0:a1a69d32f310 5094 return -1;
nikapov 0:a1a69d32f310 5095 }
nikapov 0:a1a69d32f310 5096 return 0;
nikapov 0:a1a69d32f310 5097 }
nikapov 0:a1a69d32f310 5098
nikapov 0:a1a69d32f310 5099 int VL53L0X::read_id(uint8_t *id)
nikapov 0:a1a69d32f310 5100 {
nikapov 0:a1a69d32f310 5101 int status = 0;
nikapov 0:a1a69d32f310 5102 uint16_t rl_id = 0;
nikapov 0:a1a69d32f310 5103
nikapov 0:a1a69d32f310 5104 status = VL53L0X_read_word(_device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id);
nikapov 0:a1a69d32f310 5105 if (rl_id == 0xEEAA) {
nikapov 0:a1a69d32f310 5106 return status;
nikapov 0:a1a69d32f310 5107 }
nikapov 0:a1a69d32f310 5108
nikapov 0:a1a69d32f310 5109 return -1;
nikapov 0:a1a69d32f310 5110 }
nikapov 0:a1a69d32f310 5111
nikapov 0:a1a69d32f310 5112
nikapov 0:a1a69d32f310 5113 VL53L0X_Error VL53L0X::wait_measurement_data_ready(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 5114 {
nikapov 0:a1a69d32f310 5115 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 5116 uint8_t new_dat_ready = 0;
nikapov 0:a1a69d32f310 5117 uint32_t loop_nb;
nikapov 0:a1a69d32f310 5118
nikapov 0:a1a69d32f310 5119 // Wait until it finished
nikapov 0:a1a69d32f310 5120 // use timeout to avoid deadlock
nikapov 0:a1a69d32f310 5121 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5122 loop_nb = 0;
nikapov 0:a1a69d32f310 5123 do {
nikapov 0:a1a69d32f310 5124 status = VL53L0X_get_measurement_data_ready(dev, &new_dat_ready);
nikapov 0:a1a69d32f310 5125 if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5126 break;
nikapov 0:a1a69d32f310 5127 }
nikapov 0:a1a69d32f310 5128 loop_nb = loop_nb + 1;
nikapov 0:a1a69d32f310 5129 VL53L0X_polling_delay(dev);
nikapov 0:a1a69d32f310 5130 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov 0:a1a69d32f310 5131
nikapov 0:a1a69d32f310 5132 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov 0:a1a69d32f310 5133 status = VL53L0X_ERROR_TIME_OUT;
nikapov 0:a1a69d32f310 5134 }
nikapov 0:a1a69d32f310 5135 }
nikapov 0:a1a69d32f310 5136
nikapov 0:a1a69d32f310 5137 return status;
nikapov 0:a1a69d32f310 5138 }
nikapov 0:a1a69d32f310 5139
nikapov 0:a1a69d32f310 5140 VL53L0X_Error VL53L0X::wait_stop_completed(VL53L0X_DEV dev)
nikapov 0:a1a69d32f310 5141 {
nikapov 0:a1a69d32f310 5142 VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 5143 uint32_t stop_completed = 0;
nikapov 0:a1a69d32f310 5144 uint32_t loop_nb;
nikapov 0:a1a69d32f310 5145
nikapov 0:a1a69d32f310 5146 // Wait until it finished
nikapov 0:a1a69d32f310 5147 // use timeout to avoid deadlock
nikapov 0:a1a69d32f310 5148 if (status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5149 loop_nb = 0;
nikapov 0:a1a69d32f310 5150 do {
nikapov 0:a1a69d32f310 5151 status = VL53L0X_get_stop_completed_status(dev, &stop_completed);
nikapov 0:a1a69d32f310 5152 if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5153 break;
nikapov 0:a1a69d32f310 5154 }
nikapov 0:a1a69d32f310 5155 loop_nb = loop_nb + 1;
nikapov 0:a1a69d32f310 5156 VL53L0X_polling_delay(dev);
nikapov 0:a1a69d32f310 5157 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov 0:a1a69d32f310 5158
nikapov 0:a1a69d32f310 5159 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov 0:a1a69d32f310 5160 status = VL53L0X_ERROR_TIME_OUT;
nikapov 0:a1a69d32f310 5161 }
nikapov 0:a1a69d32f310 5162
nikapov 0:a1a69d32f310 5163 }
nikapov 0:a1a69d32f310 5164
nikapov 0:a1a69d32f310 5165 return status;
nikapov 0:a1a69d32f310 5166 }
nikapov 0:a1a69d32f310 5167
nikapov 0:a1a69d32f310 5168
nikapov 0:a1a69d32f310 5169 int VL53L0X::init_sensor(uint8_t new_addr)
nikapov 0:a1a69d32f310 5170 {
nikapov 0:a1a69d32f310 5171 int status;
nikapov 0:a1a69d32f310 5172
nikapov 0:a1a69d32f310 5173 VL53L0X_off();
nikapov 0:a1a69d32f310 5174 VL53L0X_on();
nikapov 0:a1a69d32f310 5175
nikapov 0:a1a69d32f310 5176 // status=VL53L0X_WaitDeviceBooted(Device);
nikapov 0:a1a69d32f310 5177 // if(status)
nikapov 0:a1a69d32f310 5178 // printf("WaitDeviceBooted fail\n\r");
nikapov 0:a1a69d32f310 5179 status = is_present();
nikapov 0:a1a69d32f310 5180 if (!status) {
nikapov 0:a1a69d32f310 5181 status = init(&_my_device);
nikapov 0:a1a69d32f310 5182 if (status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5183 printf("Failed to init VL53L0X sensor!\n\r");
nikapov 0:a1a69d32f310 5184 return status;
nikapov 0:a1a69d32f310 5185 }
nikapov 0:a1a69d32f310 5186
nikapov 0:a1a69d32f310 5187 // deduce silicon version
nikapov 0:a1a69d32f310 5188 status = VL53L0X_get_device_info(&_my_device, &_device_info);
nikapov 0:a1a69d32f310 5189
nikapov 0:a1a69d32f310 5190 status = prepare();
nikapov 0:a1a69d32f310 5191 if (status != VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5192 printf("Failed to prepare VL53L0X!\n\r");
nikapov 0:a1a69d32f310 5193 return status;
nikapov 0:a1a69d32f310 5194 }
nikapov 0:a1a69d32f310 5195
nikapov 1:834986cdde0a 5196 if (new_addr != VL53L0X_DEFAULT_ADDRESS) {
nikapov 0:a1a69d32f310 5197 status = set_device_address(new_addr);
nikapov 0:a1a69d32f310 5198 if (status) {
nikapov 0:a1a69d32f310 5199 printf("Failed to change I2C address!\n\r");
nikapov 0:a1a69d32f310 5200 return status;
nikapov 0:a1a69d32f310 5201 }
nikapov 0:a1a69d32f310 5202 } else {
nikapov 0:a1a69d32f310 5203 printf("Invalid new address!\n\r");
nikapov 0:a1a69d32f310 5204 return VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 5205 }
nikapov 0:a1a69d32f310 5206 }
nikapov 0:a1a69d32f310 5207 return status;
nikapov 0:a1a69d32f310 5208 }
nikapov 0:a1a69d32f310 5209
nikapov 0:a1a69d32f310 5210 int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void))
nikapov 0:a1a69d32f310 5211 {
sepp_nepp 5:b95f6951f7d5 5212 int status;
nikapov 0:a1a69d32f310 5213
nikapov 0:a1a69d32f310 5214 status = VL53L0X_stop_measurement(_device); // it is safer to do this while sensor is stopped
nikapov 0:a1a69d32f310 5215
nikapov 0:a1a69d32f310 5216 // status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
nikapov 0:a1a69d32f310 5217
nikapov 0:a1a69d32f310 5218 status = VL53L0X_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov 0:a1a69d32f310 5219 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov 0:a1a69d32f310 5220 VL53L0X_INTERRUPTPOLARITY_HIGH);
nikapov 0:a1a69d32f310 5221
nikapov 0:a1a69d32f310 5222 if (!status) {
nikapov 0:a1a69d32f310 5223 attach_interrupt_measure_detection_irq(fptr);
nikapov 0:a1a69d32f310 5224 enable_interrupt_measure_detection_irq();
nikapov 0:a1a69d32f310 5225 }
nikapov 0:a1a69d32f310 5226
sepp_nepp 5:b95f6951f7d5 5227 clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS);
sepp_nepp 5:b95f6951f7d5 5228 // NB: return value was previously only passed to logging macro, but did not get passed back
nikapov 0:a1a69d32f310 5229
nikapov 0:a1a69d32f310 5230 if (!status) {
nikapov 0:a1a69d32f310 5231 status = range_start_continuous_mode();
nikapov 0:a1a69d32f310 5232 }
nikapov 0:a1a69d32f310 5233 return status;
nikapov 0:a1a69d32f310 5234 }
nikapov 0:a1a69d32f310 5235
nikapov 0:a1a69d32f310 5236
nikapov 0:a1a69d32f310 5237 int VL53L0X::start_measurement(OperatingMode operating_mode, void (*fptr)(void))
nikapov 0:a1a69d32f310 5238 {
nikapov 0:a1a69d32f310 5239 int Status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 5240
nikapov 0:a1a69d32f310 5241 uint8_t VhvSettings;
nikapov 0:a1a69d32f310 5242 uint8_t PhaseCal;
nikapov 0:a1a69d32f310 5243 // *** from mass market cube expansion v1.1, ranging with satellites.
nikapov 0:a1a69d32f310 5244 // default settings, for normal range.
nikapov 0:a1a69d32f310 5245 FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536);
nikapov 0:a1a69d32f310 5246 FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536);
nikapov 0:a1a69d32f310 5247 uint32_t timingBudget = 33000;
nikapov 0:a1a69d32f310 5248 uint8_t preRangeVcselPeriod = 14;
nikapov 0:a1a69d32f310 5249 uint8_t finalRangeVcselPeriod = 10;
nikapov 0:a1a69d32f310 5250
nikapov 0:a1a69d32f310 5251 if (operating_mode == range_continuous_interrupt) {
nikapov 0:a1a69d32f310 5252 if (_gpio1Int == NULL) {
nikapov 0:a1a69d32f310 5253 printf("GPIO1 Error\r\n");
nikapov 0:a1a69d32f310 5254 return 1;
nikapov 0:a1a69d32f310 5255 }
nikapov 0:a1a69d32f310 5256
nikapov 0:a1a69d32f310 5257 Status = VL53L0X_stop_measurement(_device); // it is safer to do this while sensor is stopped
nikapov 0:a1a69d32f310 5258
nikapov 0:a1a69d32f310 5259 // Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
nikapov 0:a1a69d32f310 5260
nikapov 0:a1a69d32f310 5261 Status = VL53L0X_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov 0:a1a69d32f310 5262 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov 0:a1a69d32f310 5263 VL53L0X_INTERRUPTPOLARITY_HIGH);
nikapov 0:a1a69d32f310 5264
nikapov 0:a1a69d32f310 5265 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5266 attach_interrupt_measure_detection_irq(fptr);
nikapov 0:a1a69d32f310 5267 enable_interrupt_measure_detection_irq();
nikapov 0:a1a69d32f310 5268 }
nikapov 0:a1a69d32f310 5269
sepp_nepp 5:b95f6951f7d5 5270 clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS);
sepp_nepp 5:b95f6951f7d5 5271 // NB: return value was previously only passed to logging macro, but did not get passed back
nikapov 0:a1a69d32f310 5272
nikapov 0:a1a69d32f310 5273 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5274 Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
nikapov 0:a1a69d32f310 5275 }
nikapov 0:a1a69d32f310 5276
nikapov 0:a1a69d32f310 5277 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5278 Status = VL53L0X_start_measurement(_device);
nikapov 0:a1a69d32f310 5279 }
nikapov 0:a1a69d32f310 5280 }
nikapov 0:a1a69d32f310 5281
nikapov 0:a1a69d32f310 5282 if (operating_mode == range_single_shot_polling) {
nikapov 0:a1a69d32f310 5283 // singelshot, polled ranging
nikapov 0:a1a69d32f310 5284 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5285 // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
nikapov 0:a1a69d32f310 5286 Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
nikapov 0:a1a69d32f310 5287 }
nikapov 0:a1a69d32f310 5288
nikapov 0:a1a69d32f310 5289 // Enable/Disable Sigma and Signal check
nikapov 0:a1a69d32f310 5290 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5291 Status = VL53L0X_set_limit_check_enable(_device,
nikapov 0:a1a69d32f310 5292 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
nikapov 0:a1a69d32f310 5293 }
nikapov 0:a1a69d32f310 5294 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5295 Status = VL53L0X_set_limit_check_enable(_device,
nikapov 0:a1a69d32f310 5296 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
nikapov 0:a1a69d32f310 5297 }
nikapov 0:a1a69d32f310 5298
nikapov 0:a1a69d32f310 5299 // *** from mass market cube expansion v1.1, ranging with satellites.
nikapov 0:a1a69d32f310 5300 /* Ranging configuration */
nikapov 0:a1a69d32f310 5301 //*
nikapov 0:a1a69d32f310 5302 // switch(rangingConfig) {
nikapov 0:a1a69d32f310 5303 // case LONG_RANGE:
nikapov 0:a1a69d32f310 5304 signalLimit = (FixPoint1616_t)(0.1 * 65536);
nikapov 0:a1a69d32f310 5305 sigmaLimit = (FixPoint1616_t)(60 * 65536);
nikapov 0:a1a69d32f310 5306 timingBudget = 33000;
nikapov 0:a1a69d32f310 5307 preRangeVcselPeriod = 18;
nikapov 0:a1a69d32f310 5308 finalRangeVcselPeriod = 14;
nikapov 0:a1a69d32f310 5309 /* break;
nikapov 0:a1a69d32f310 5310 case HIGH_ACCURACY:
nikapov 0:a1a69d32f310 5311 signalLimit = (FixPoint1616_t)(0.25*65536);
nikapov 0:a1a69d32f310 5312 sigmaLimit = (FixPoint1616_t)(18*65536);
nikapov 0:a1a69d32f310 5313 timingBudget = 200000;
nikapov 0:a1a69d32f310 5314 preRangeVcselPeriod = 14;
nikapov 0:a1a69d32f310 5315 finalRangeVcselPeriod = 10;
nikapov 0:a1a69d32f310 5316 break;
nikapov 0:a1a69d32f310 5317 case HIGH_SPEED:
nikapov 0:a1a69d32f310 5318 signalLimit = (FixPoint1616_t)(0.25*65536);
nikapov 0:a1a69d32f310 5319 sigmaLimit = (FixPoint1616_t)(32*65536);
nikapov 0:a1a69d32f310 5320 timingBudget = 20000;
nikapov 0:a1a69d32f310 5321 preRangeVcselPeriod = 14;
nikapov 0:a1a69d32f310 5322 finalRangeVcselPeriod = 10;
nikapov 0:a1a69d32f310 5323 break;
nikapov 0:a1a69d32f310 5324 default:
nikapov 0:a1a69d32f310 5325 debug_printf("Not Supported");
nikapov 0:a1a69d32f310 5326 }
nikapov 0:a1a69d32f310 5327 */
nikapov 0:a1a69d32f310 5328
nikapov 0:a1a69d32f310 5329 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5330 Status = VL53L0X_set_limit_check_value(_device,
nikapov 0:a1a69d32f310 5331 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
nikapov 0:a1a69d32f310 5332 }
nikapov 0:a1a69d32f310 5333
nikapov 0:a1a69d32f310 5334 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5335 Status = VL53L0X_set_limit_check_value(_device,
nikapov 0:a1a69d32f310 5336 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
nikapov 0:a1a69d32f310 5337 }
nikapov 0:a1a69d32f310 5338
nikapov 0:a1a69d32f310 5339 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5340 Status = VL53L0X_set_measurement_timing_budget_micro_seconds(_device, timingBudget);
nikapov 0:a1a69d32f310 5341 }
nikapov 0:a1a69d32f310 5342
nikapov 0:a1a69d32f310 5343 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5344 Status = VL53L0X_set_vcsel_pulse_period(_device,
nikapov 0:a1a69d32f310 5345 VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
nikapov 0:a1a69d32f310 5346 }
nikapov 0:a1a69d32f310 5347
nikapov 0:a1a69d32f310 5348 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5349 Status = VL53L0X_set_vcsel_pulse_period(_device,
nikapov 0:a1a69d32f310 5350 VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
nikapov 0:a1a69d32f310 5351 }
nikapov 0:a1a69d32f310 5352
nikapov 0:a1a69d32f310 5353 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5354 Status = VL53L0X_perform_ref_calibration(_device, &VhvSettings, &PhaseCal);
nikapov 0:a1a69d32f310 5355 }
nikapov 0:a1a69d32f310 5356
nikapov 0:a1a69d32f310 5357 }
nikapov 0:a1a69d32f310 5358
nikapov 0:a1a69d32f310 5359 if (operating_mode == range_continuous_polling) {
nikapov 0:a1a69d32f310 5360 if (Status == VL53L0X_ERROR_NONE) {
nikapov 1:834986cdde0a 5361 //printf("Call of VL53L0X_SetDeviceMode\n");
nikapov 0:a1a69d32f310 5362 Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
nikapov 0:a1a69d32f310 5363 }
nikapov 0:a1a69d32f310 5364
nikapov 0:a1a69d32f310 5365 if (Status == VL53L0X_ERROR_NONE) {
nikapov 1:834986cdde0a 5366 //printf("Call of VL53L0X_StartMeasurement\n");
nikapov 0:a1a69d32f310 5367 Status = VL53L0X_start_measurement(_device);
nikapov 0:a1a69d32f310 5368 }
nikapov 0:a1a69d32f310 5369 }
nikapov 0:a1a69d32f310 5370
nikapov 0:a1a69d32f310 5371 return Status;
nikapov 0:a1a69d32f310 5372 }
nikapov 0:a1a69d32f310 5373
nikapov 0:a1a69d32f310 5374
nikapov 0:a1a69d32f310 5375 int VL53L0X::get_measurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data)
nikapov 0:a1a69d32f310 5376 {
nikapov 0:a1a69d32f310 5377 int Status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 5378
nikapov 0:a1a69d32f310 5379 if (operating_mode == range_single_shot_polling) {
nikapov 0:a1a69d32f310 5380 Status = VL53L0X_perform_single_ranging_measurement(_device, p_data);
nikapov 0:a1a69d32f310 5381 }
nikapov 0:a1a69d32f310 5382
nikapov 0:a1a69d32f310 5383 if (operating_mode == range_continuous_polling) {
Davidroid 3:e9269ff624ed 5384 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5385 Status = VL53L0X_measurement_poll_for_completion(_device);
Davidroid 3:e9269ff624ed 5386 }
nikapov 0:a1a69d32f310 5387
nikapov 0:a1a69d32f310 5388 if (Status == VL53L0X_ERROR_NONE) {
nikapov 0:a1a69d32f310 5389 Status = VL53L0X_get_ranging_measurement_data(_device, p_data);
nikapov 0:a1a69d32f310 5390
nikapov 0:a1a69d32f310 5391 // Clear the interrupt
nikapov 0:a1a69d32f310 5392 VL53L0X_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
nikapov 0:a1a69d32f310 5393 VL53L0X_polling_delay(_device);
nikapov 0:a1a69d32f310 5394 }
nikapov 0:a1a69d32f310 5395 }
nikapov 0:a1a69d32f310 5396
nikapov 0:a1a69d32f310 5397 if (operating_mode == range_continuous_interrupt) {
nikapov 0:a1a69d32f310 5398 Status = VL53L0X_get_ranging_measurement_data(_device, p_data);
nikapov 0:a1a69d32f310 5399 VL53L0X_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS);
nikapov 0:a1a69d32f310 5400 }
nikapov 0:a1a69d32f310 5401
nikapov 0:a1a69d32f310 5402 return Status;
nikapov 0:a1a69d32f310 5403 }
nikapov 0:a1a69d32f310 5404
nikapov 0:a1a69d32f310 5405
nikapov 0:a1a69d32f310 5406 int VL53L0X::stop_measurement(OperatingMode operating_mode)
nikapov 0:a1a69d32f310 5407 {
nikapov 0:a1a69d32f310 5408 int status = VL53L0X_ERROR_NONE;
nikapov 0:a1a69d32f310 5409
nikapov 0:a1a69d32f310 5410
nikapov 0:a1a69d32f310 5411 // don't need to stop for a singleshot range!
nikapov 0:a1a69d32f310 5412 if (operating_mode == range_single_shot_polling) {
nikapov 0:a1a69d32f310 5413 }
nikapov 0:a1a69d32f310 5414
nikapov 0:a1a69d32f310 5415 if (operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling) {
nikapov 0:a1a69d32f310 5416 // continuous mode
nikapov 0:a1a69d32f310 5417 if (status == VL53L0X_ERROR_NONE) {
nikapov 1:834986cdde0a 5418 //printf("Call of VL53L0X_StopMeasurement\n");
nikapov 0:a1a69d32f310 5419 status = VL53L0X_stop_measurement(_device);
nikapov 0:a1a69d32f310 5420 }
nikapov 0:a1a69d32f310 5421
nikapov 0:a1a69d32f310 5422 if (status == VL53L0X_ERROR_NONE) {
nikapov 1:834986cdde0a 5423 //printf("Wait Stop to be competed\n");
nikapov 0:a1a69d32f310 5424 status = wait_stop_completed(_device);
nikapov 0:a1a69d32f310 5425 }
nikapov 0:a1a69d32f310 5426
nikapov 0:a1a69d32f310 5427 if (status == VL53L0X_ERROR_NONE)
nikapov 0:a1a69d32f310 5428 status = VL53L0X_clear_interrupt_mask(_device,
nikapov 0:a1a69d32f310 5429 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
nikapov 0:a1a69d32f310 5430 }
nikapov 0:a1a69d32f310 5431
nikapov 0:a1a69d32f310 5432 return status;
nikapov 0:a1a69d32f310 5433 }
nikapov 0:a1a69d32f310 5434
nikapov 0:a1a69d32f310 5435
nikapov 0:a1a69d32f310 5436 int VL53L0X::handle_irq(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data)
nikapov 0:a1a69d32f310 5437 {
nikapov 0:a1a69d32f310 5438 int status;
nikapov 0:a1a69d32f310 5439 status = get_measurement(operating_mode, data);
nikapov 0:a1a69d32f310 5440 enable_interrupt_measure_detection_irq();
nikapov 0:a1a69d32f310 5441 return status;
nikapov 0:a1a69d32f310 5442 }
nikapov 0:a1a69d32f310 5443
nikapov 0:a1a69d32f310 5444
nikapov 0:a1a69d32f310 5445 /******************************************************************************/