Florian Fillol / Laser

ds

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Fork of VL53L0X by ST

VL53L0X.cpp@5:0750bf6ea43b, 2018-04-11 (annotated)

Committer:
PeaceBearer
Date:
Wed Apr 11 14:36:06 2018 +0000
Revision:
5:0750bf6ea43b
Parent:
3:e9269ff624ed
Child:
6:7bb5d564af90
a

Who changed what in which revision?

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