Jim Carver / Mbed OS pelion-workshop

Workshop example

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

sensors/VL53L0X/VL53L0X.cpp@35:42b3fba640b1, 2019-05-21 (annotated)

Committer:
JimCarver
Date:
Tue May 21 21:16:24 2019 +0000
Revision:
35:42b3fba640b1
Parent:
18:a15bfe7aaebd 
Simple version for workshop

Who changed what in which revision?

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