Tarek Lule / VL53L0X_Condensed

Condensed Version of Public VL53L0X

Dependents:   ToF-Only-Tryout

VL53L0X.cpp@10:cd251e0fc2fd, 2019-03-24 (annotated)

Committer:
sepp_nepp
Date:
Sun Mar 24 22:24:16 2019 +0000
Revision:
10:cd251e0fc2fd
Parent:
8:abea9638127a
Child:
11:c6f95a42d4d7
Now condensed and compiles

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