Florian Fillol / Laser

ds

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Fork of VL53L0X by ST

VL53L0X.cpp@6:7bb5d564af90, 2018-05-05 (annotated)

Committer:
PeaceBearer
Date:
Sat May 05 15:26:12 2018 +0000
Revision:
6:7bb5d564af90
Parent:
5:0750bf6ea43b 
code;

Who changed what in which revision?

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