Garage Door Monitor and Opener

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Introduction

This system implements a simple garage door opener and environmental monitor. The hardware connects to the internet using Wi-Fi then on to the Pelion Device Management Platform which provides device monitoring and secure firmware updates over the air (FOTA). Pelion Device Management provides a flexible set of REST APIs which we will use to communicate to a web application running on an EC-2 instance in AWS. The web application will serve a web page where we can monitor and control our garage..

This project is intended to work on the DISCO-L475VG-IOT01A from ST Microelectronics It implements a simple actuator to drive a relay to simulate pushing the "open" button on older style garage doors which do not use a rolling code interface.

The system is designed to be mounted over the door so that the on board time of flight sensor can be used to detect if the door is open or closed.

The system also monitors temperature, humidity and barometric pressure.

https://os.mbed.com/media/uploads/JimCarver/garageopener.jpg

Hardware Requirements:

DISCO-L475G-IOT01A https://os.mbed.com/platforms/ST-Discovery-L475E-IOT01A/

Seeed Studio Grove Relay module https://www.seeedstudio.com/Grove-Relay.html

Seeed Studio Grove cable, I used this one: https://www.seeedstudio.com/Grove-4-pin-Male-Jumper-to-Grove-4-pin-Conversion-Cable-5-PCs-per-Pack.html

Connect to the PMOD connector like this:

https://os.mbed.com/media/uploads/JimCarver/opener.jpg

This shows how I installed so that the time of flight sensor can detect when the door is open

https://os.mbed.com/media/uploads/JimCarver/opener1.jpg https://os.mbed.com/media/uploads/JimCarver/opener2.jpg

To use the project:

You will also need a Pelion developers account.

I suggest you first use the Pelion quick state to become familiar with Pelion Device Management. https://os.mbed.com/guides/connect-device-to-pelion/1/?board=ST-Discovery-L475E-IOT01A

Web Interface

For my web interface I am running node-red under Ubuntu in an EC2 instance on AWS. This can run for 12 month within the constraints of their free tier. Here is a tutorial: https://nodered.org/docs/getting-started/aws

You will also need to install several node-red add ons:

sudo npm install -g node-red-dashboard

sudo npm install -g node-red-contrib-mbed-cloud

sudo npm istall -g node-red-contrib-moment

After starting node-red import the contents of GarageFlow.txt from the project, pin the flow into the page.

To enable your web app to access your Pelion account you need an API key.

First you will neet to use your Pelion account to create an API key.

https://os.mbed.com/media/uploads/JimCarver/api_portal.jpg

Now we need to apply that API key to your Node-Red flow.

https://os.mbed.com/media/uploads/JimCarver/api_node-red.jpg

Committer:
JimCarver
Date:
Thu Dec 05 19:03:48 2019 +0000
Revision:
37:ec1124e5ec1f
Parent:
18:a15bfe7aaebd
Bug fix

Who changed what in which revision?

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