Tarek Lule
Condensed Version of Public VL53L0X
VL53L0X.cpp
- Committer:
- sepp_nepp
- Date:
- 2019-03-24
- Revision:
- 7:41cbc431e1f4
- Parent:
- 6:1976f4afed97
- Child:
- 8:abea9638127a
File content as of revision 7:41cbc431e1f4:
/**
******************************************************************************
* @file VL53L0X_class.cpp
* @author IMG
* @version V0.0.1
* @date 28-June-2016
* @brief Implementation file for the VL53L0X driver class
******************************************************************************
* @attention
*
* <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
*
* Redistribution and use in source and binary forms,with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL
* DAMAGES (INCLUDING,BUT NOT LIMITED TO,PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE,DATA,OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
// Some example regex that were used to replace useless macros
// \QVL53L0X_SETDEVICESPECIFICPARAMETER(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
// _device->DeviceSpecificParameters.\1 = \3;
// \QVL53L0X_GETDEVICESPECIFICPARAMETER(\E([A-Z\d]+)\Q);\E
// _device->DeviceSpecificParameters.\1;
// \QVL53L0X_SETPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
// _device->CurrentParameters.\1 = \3;
// \QVL53L0X_GETPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
// \3 = _device->CurrentParameters.\1
// \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E
// _device->CurrentParameters.\1[\3] = \5;
// is used to replace following macro for ex:
// #define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value) \
//_device->CurrentParameters.field[index] = value
// \QVL53L0X_GETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E
// \5 = _device->CurrentParameters.\1[\3]
// is used to replace following macro for ex:
// #define VL53L0X_GETARRAYPARAMETERFIELD(field, index, variable) \
// variable = _device-> CurrentParameters.field[index]
// \QPALDevDataSet(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
// _device->\1 = \3;
// is used to replace:
// #define PALDevDataSet(field, data) (Dev->field)=(data)
/* Includes */
#include <stdlib.h>
#include "VL53L0X.h"
VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t strobe;
uint32_t loop_nb;
status |= VL53L0X_write_byte(0x83,0x00);
/* polling
* use timeout to avoid deadlock*/
if (status == VL53L0X_ERROR_NONE) {
loop_nb = 0;
do {
status = VL53L0X_read_byte(0x83,&strobe);
if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE) {
break;
}
loop_nb = loop_nb + 1;
} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
status = VL53L0X_ERROR_TIME_OUT;
}
}
status |= VL53L0X_write_byte(0x83,0x01);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(uint8_t option)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t byte;
uint32_t tmp_dword;
uint8_t module_id;
uint8_t revision;
uint8_t reference_spad_count = 0;
uint8_t reference_spad_type = 0;
uint32_t part_uid_upper = 0;
uint32_t part_uid_lower = 0;
uint32_t offset_fixed1104_mm = 0;
int16_t offset_micro_meters = 0;
uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
uint32_t dist_meas_fixed1104_400_mm = 0;
uint32_t signal_rate_meas_fixed1104_400_mm = 0;
char product_id[19];
char *product_id_tmp;
uint8_t read_data_from_device_done;
FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0;
uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE];
int i;
read_data_from_device_done = _device->DeviceSpecificParameters.ReadDataFromDeviceDone;
read_data_from_device_done = _device->DeviceSpecificParameters.ReadDataFromDeviceDone;
/* This access is done only once after that a GetDeviceInfo or
* datainit is done*/
if (read_data_from_device_done != 7) {
status |= VL53L0X_write_byte(0x80,0x01);
status |= VL53L0X_write_byte(0xFF,0x01);
status |= VL53L0X_write_byte(0x00,0x00);
status |= VL53L0X_write_byte(0xFF,0x06);
status |= VL53L0X_read_byte(0x83,&byte);
status |= VL53L0X_write_byte(0x83,byte | 4);
status |= VL53L0X_write_byte(0xFF,0x07);
status |= VL53L0X_write_byte(0x81,0x01);
status |= VL53L0X_polling_delay();
status |= VL53L0X_write_byte(0x80,0x01);
if (((option & 1) == 1) &&
((read_data_from_device_done & 1) == 0)) {
status |= VL53L0X_write_byte(0x94,0x6b);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f);
reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01);
status |= VL53L0X_write_byte(0x94,0x24);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24)
& 0xff);
nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16)
& 0xff);
nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8)
& 0xff);
nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff);
status |= VL53L0X_write_byte(0x94,0x25);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24)
& 0xff);
nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16)
& 0xff);
}
if (((option & 2) == 2) &&
((read_data_from_device_done & 2) == 0)) {
status |= VL53L0X_write_byte(0x94,0x02);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_byte(0x90,&module_id);
status |= VL53L0X_write_byte(0x94,0x7B);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_byte(0x90,&revision);
status |= VL53L0X_write_byte(0x94,0x77);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
product_id[0] = (char)((tmp_dword >> 25) & 0x07f);
product_id[1] = (char)((tmp_dword >> 18) & 0x07f);
product_id[2] = (char)((tmp_dword >> 11) & 0x07f);
product_id[3] = (char)((tmp_dword >> 4) & 0x07f);
byte = (uint8_t)((tmp_dword & 0x00f) << 3);
status |= VL53L0X_write_byte(0x94,0x78);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
product_id[4] = (char)(byte +
((tmp_dword >> 29) & 0x07f));
product_id[5] = (char)((tmp_dword >> 22) & 0x07f);
product_id[6] = (char)((tmp_dword >> 15) & 0x07f);
product_id[7] = (char)((tmp_dword >> 8) & 0x07f);
product_id[8] = (char)((tmp_dword >> 1) & 0x07f);
byte = (uint8_t)((tmp_dword & 0x001) << 6);
status |= VL53L0X_write_byte(0x94,0x79);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
product_id[9] = (char)(byte +
((tmp_dword >> 26) & 0x07f));
product_id[10] = (char)((tmp_dword >> 19) & 0x07f);
product_id[11] = (char)((tmp_dword >> 12) & 0x07f);
product_id[12] = (char)((tmp_dword >> 5) & 0x07f);
byte = (uint8_t)((tmp_dword & 0x01f) << 2);
status |= VL53L0X_write_byte(0x94,0x7A);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
product_id[13] = (char)(byte +
((tmp_dword >> 30) & 0x07f));
product_id[14] = (char)((tmp_dword >> 23) & 0x07f);
product_id[15] = (char)((tmp_dword >> 16) & 0x07f);
product_id[16] = (char)((tmp_dword >> 9) & 0x07f);
product_id[17] = (char)((tmp_dword >> 2) & 0x07f);
product_id[18] = '\0';
}
if (((option & 4) == 4) &&
((read_data_from_device_done & 4) == 0)) {
status |= VL53L0X_write_byte(0x94,0x7B);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&part_uid_upper);
status |= VL53L0X_write_byte(0x94,0x7C);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&part_uid_lower);
status |= VL53L0X_write_byte(0x94,0x73);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
signal_rate_meas_fixed1104_400_mm = (tmp_dword &
0x0000000ff) << 8;
status |= VL53L0X_write_byte(0x94,0x74);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
signal_rate_meas_fixed1104_400_mm |= ((tmp_dword &
0xff000000) >> 24);
status |= VL53L0X_write_byte(0x94,0x75);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)
<< 8;
status |= VL53L0X_write_byte(0x94,0x76);
status |= VL53L0X_device_read_strobe();
status |= VL53L0X_read_dword(0x90,&tmp_dword);
dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000)
>> 24);
}
status |= VL53L0X_write_byte(0x81,0x00);
status |= VL53L0X_write_byte(0xFF,0x06);
status |= VL53L0X_read_byte(0x83,&byte);
status |= VL53L0X_write_byte(0x83,byte & 0xfb);
status |= VL53L0X_write_byte(0xFF,0x01);
status |= VL53L0X_write_byte(0x00,0x01);
status |= VL53L0X_write_byte(0xFF,0x00);
status |= VL53L0X_write_byte(0x80,0x00);
}
if ((status == VL53L0X_ERROR_NONE) &&
(read_data_from_device_done != 7)) {
/* Assign to variable if status is ok */
if (((option & 1) == 1) &&
((read_data_from_device_done & 1) == 0)) {
_device->DeviceSpecificParameters.ReferenceSpadCount=reference_spad_count;
_device->DeviceSpecificParameters.ReferenceSpadType=reference_spad_type;
for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
_device->SpadData.RefGoodSpadMap[i] =
nvm_ref_good_spad_map[i];
}
}
if (((option & 2) == 2) &&
((read_data_from_device_done & 2) == 0)) {
_device->DeviceSpecificParameters.ModuleId = module_id;
_device->DeviceSpecificParameters.Revision = revision;
product_id_tmp = _device->DeviceSpecificParameters.ProductId;
strcpy(product_id_tmp,product_id);
}
if (((option & 4) == 4) &&
((read_data_from_device_done & 4) == 0)) {
_device->DeviceSpecificParameters.PartUIDUpper = part_uid_upper;
_device->DeviceSpecificParameters.PartUIDLower = part_uid_lower;
signal_rate_meas_fixed400_mm_fix =
VL53L0X_FIXPOINT97TOFIXPOINT1616(signal_rate_meas_fixed1104_400_mm);
_device->DeviceSpecificParameters.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
_device->DeviceSpecificParameters.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
offset_micro_meters = 0;
if (dist_meas_fixed1104_400_mm != 0) {
offset_fixed1104_mm =
dist_meas_fixed1104_400_mm -
dist_meas_tgt_fixed1104_mm;
offset_micro_meters = (offset_fixed1104_mm
* 1000) >> 4;
offset_micro_meters *= -1;
}
_device->Part2PartOffsetAdjustmentNVMMicroMeter = offset_micro_meters;
}
byte = (uint8_t)(read_data_from_device_done | option);
_device->DeviceSpecificParameters.ReadDataFromDeviceDone = byte;
}
return status;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t range_offset_register;
int16_t c_max_offset = 2047;
int16_t c_offset_range = 4096;
/* Note that offset has 10.2 format */
status = VL53L0X_read_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
&range_offset_register);
if (status == VL53L0X_ERROR_NONE) {
range_offset_register = (range_offset_register & 0x0fff);
/* Apply 12 bit 2's compliment conversion */
if (range_offset_register > c_max_offset) {
*p_offset_calibration_data_micro_meter =
(int16_t)(range_offset_register - c_offset_range)
* 250;
} else {
*p_offset_calibration_data_micro_meter =
(int16_t)range_offset_register * 250;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_get_offset_calibration_data_micro_meter(p_offset_calibration_data_micro_meter);
return status;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int32_t c_max_offset_micro_meter = 511000;
int32_t c_min_offset_micro_meter = -512000;
int16_t c_offset_range = 4096;
uint32_t encoded_offset_val;
if (offset_calibration_data_micro_meter > c_max_offset_micro_meter) {
offset_calibration_data_micro_meter = c_max_offset_micro_meter;
} else {
if (offset_calibration_data_micro_meter < c_min_offset_micro_meter) {
offset_calibration_data_micro_meter = c_min_offset_micro_meter;
}
}
/* The offset register is 10.2 format and units are mm
* therefore conversion is applied by a division of
* 250.
*/
if (offset_calibration_data_micro_meter >= 0) {
encoded_offset_val =
offset_calibration_data_micro_meter / 250;
} else {
encoded_offset_val =
c_offset_range +
offset_calibration_data_micro_meter / 250;
}
status = VL53L0X_write_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
encoded_offset_val);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_set_offset_calibration_data_micro_meter(offset_calibration_data_micro_meter);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int32_t corrected_offset_micro_meters;
int32_t current_offset_micro_meters;
/* if we run on this function we can read all the NVM info
* used by the API */
status = VL53L0X_get_info_from_device(7);
/* Read back current device offset */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_offset_calibration_data_micro_meter(¤t_offset_micro_meters);
}
/* Apply Offset Adjustment derived from 400mm measurements */
if (status == VL53L0X_ERROR_NONE) {
/* Store initial device offset */
_device->Part2PartOffsetNVMMicroMeter = current_offset_micro_meters;
corrected_offset_micro_meters = current_offset_micro_meters +
(int32_t) _device->Part2PartOffsetAdjustmentNVMMicroMeter;
status = VL53L0X_set_offset_calibration_data_micro_meter(corrected_offset_micro_meters);
/* store current,adjusted offset */
if (status == VL53L0X_ERROR_NONE) {
_device->CurrentParameters.RangeOffsetMicroMeters = corrected_offset_micro_meters;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_device_mode(VL53L0X_DeviceModes *p_device_mode)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
*p_device_mode = _device->CurrentParameters.DeviceMode;
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_milli_seconds(uint32_t *p_inter_measurement_period_milli_seconds)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t osc_calibrate_val;
uint32_t im_period_milli_seconds;
status = VL53L0X_read_word(VL53L0X_REG_OSC_CALIBRATE_VAL,
&osc_calibrate_val);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_dword(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
&im_period_milli_seconds);
}
if (status == VL53L0X_ERROR_NONE) {
if (osc_calibrate_val != 0) {
*p_inter_measurement_period_milli_seconds =
im_period_milli_seconds / osc_calibrate_val;
}
_device->CurrentParameters.InterMeasurementPeriodMilliSeconds=
*p_inter_measurement_period_milli_seconds;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_mega_cps(FixPoint1616_t *p_xtalk_compensation_rate_mega_cps)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t value;
FixPoint1616_t temp_fix1616;
status = VL53L0X_read_word(VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS,(uint16_t *)&value);
if (status == VL53L0X_ERROR_NONE) {
if (value == 0) {
/* the Xtalk is disabled return value from memory */
temp_fix1616 = _device->CurrentParameters.XTalkCompensationRateMegaCps;
*p_xtalk_compensation_rate_mega_cps = temp_fix1616;
_device->CurrentParameters.XTalkCompensationEnable = 0;
} else {
temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value);
*p_xtalk_compensation_rate_mega_cps = temp_fix1616;
_device->CurrentParameters.XTalkCompensationRateMegaCps = temp_fix1616;
_device->CurrentParameters.XTalkCompensationEnable = 1;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value(uint16_t limit_check_id,
FixPoint1616_t *p_limit_check_value)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t enable_zero_value = 0;
uint16_t temp16;
FixPoint1616_t temp_fix1616;
switch (limit_check_id) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
temp_fix1616 = _device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE];
enable_zero_value = 0;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
&temp16);
if (status == VL53L0X_ERROR_NONE) {
temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
}
enable_zero_value = 1;
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
temp_fix1616 = _device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP];
enable_zero_value = 0;
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
temp_fix1616 = _device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD];
enable_zero_value = 0;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
&temp16);
if (status == VL53L0X_ERROR_NONE) {
temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
}
enable_zero_value = 0;
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
if (status == VL53L0X_ERROR_NONE) {
if (enable_zero_value == 1) {
if (temp_fix1616 == 0) {
/* disabled: return value from memory */
temp_fix1616 = _device->CurrentParameters.LimitChecksValue[limit_check_id];
*p_limit_check_value = temp_fix1616;
_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
} else {
*p_limit_check_value = temp_fix1616;
_device->CurrentParameters.LimitChecksValue[limit_check_id] = temp_fix1616;
_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 1;
}
} else {
*p_limit_check_value = temp_fix1616;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable(uint16_t limit_check_id,
uint8_t *p_limit_check_enable)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t temp8;
if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
status = VL53L0X_ERROR_INVALID_PARAMS;
*p_limit_check_enable = 0;
} else {
temp8 = _device->CurrentParameters.LimitChecksEnable[limit_check_id];
*p_limit_check_enable = temp8;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(uint8_t *p_wrap_around_check_enable)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t data;
status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,&data);
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = data;
if (data & (0x01 << 7)) {
*p_wrap_around_check_enable = 0x01;
} else {
*p_wrap_around_check_enable = 0x00;
}
}
if (status == VL53L0X_ERROR_NONE) {
_device->CurrentParameters.WrapAroundCheckEnable=
*p_wrap_around_check_enable;
}
return status;
}
VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_SequenceStepId sequence_step_id,uint8_t sequence_config,
uint8_t *p_sequence_step_enabled)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
*p_sequence_step_enabled = 0;
switch (sequence_step_id) {
case VL53L0X_SEQUENCESTEP_TCC:
*p_sequence_step_enabled = (sequence_config & 0x10) >> 4;
break;
case VL53L0X_SEQUENCESTEP_DSS:
*p_sequence_step_enabled = (sequence_config & 0x08) >> 3;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
*p_sequence_step_enabled = (sequence_config & 0x04) >> 2;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
*p_sequence_step_enabled = (sequence_config & 0x40) >> 6;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
*p_sequence_step_enabled = (sequence_config & 0x80) >> 7;
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
&sequence_config);
if (status == VL53L0X_ERROR_NONE) {
status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_TCC,sequence_config,
&p_scheduler_sequence_steps->TccOn);
}
if (status == VL53L0X_ERROR_NONE) {
status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_DSS,sequence_config,
&p_scheduler_sequence_steps->DssOn);
}
if (status == VL53L0X_ERROR_NONE) {
status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_MSRC,sequence_config,
&p_scheduler_sequence_steps->MsrcOn);
}
if (status == VL53L0X_ERROR_NONE) {
status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_PRE_RANGE,sequence_config,
&p_scheduler_sequence_steps->PreRangeOn);
}
if (status == VL53L0X_ERROR_NONE) {
status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_FINAL_RANGE,sequence_config,
&p_scheduler_sequence_steps->FinalRangeOn);
}
return status;
}
uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
{
/*!
* Converts the encoded VCSEL period register value into the real
* period in PLL clocks
*/
uint8_t vcsel_period_pclks = 0;
vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
return vcsel_period_pclks;
}
uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks)
{
/*!
* Converts the encoded VCSEL period register value into the real period
* in PLL clocks
*/
uint8_t vcsel_period_reg = 0;
vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
return vcsel_period_reg;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t vcsel_pulse_period_pclk)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t vcsel_period_reg;
uint8_t min_pre_vcsel_period_pclk = 12;
uint8_t max_pre_vcsel_period_pclk = 18;
uint8_t min_final_vcsel_period_pclk = 8;
uint8_t max_final_vcsel_period_pclk = 14;
uint32_t measurement_timing_budget_micro_seconds;
uint32_t final_range_timeout_micro_seconds;
uint32_t pre_range_timeout_micro_seconds;
uint32_t msrc_timeout_micro_seconds;
uint8_t phase_cal_int = 0;
/* Check if valid clock period requested */
if ((vcsel_pulse_period_pclk % 2) != 0) {
/* Value must be an even number */
status = VL53L0X_ERROR_INVALID_PARAMS;
} else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
(vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk ||
vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) {
status = VL53L0X_ERROR_INVALID_PARAMS;
} else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
(vcsel_pulse_period_pclk < min_final_vcsel_period_pclk ||
vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
/* Apply specific settings for the requested clock period */
if (status != VL53L0X_ERROR_NONE) {
return status;
}
if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
/* Set phase check limits */
if (vcsel_pulse_period_pclk == 12) {
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x18);
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
} else if (vcsel_pulse_period_pclk == 14) {
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x30);
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
} else if (vcsel_pulse_period_pclk == 16) {
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x40);
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
} else if (vcsel_pulse_period_pclk == 18) {
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x50);
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
}
} else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
if (vcsel_pulse_period_pclk == 8) {
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x10);
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x02);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C);
status |= VL53L0X_write_byte(0xff,0x01);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
0x30);
status |= VL53L0X_write_byte(0xff,0x00);
} else if (vcsel_pulse_period_pclk == 10) {
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x28);
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09);
status |= VL53L0X_write_byte(0xff,0x01);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
0x20);
status |= VL53L0X_write_byte(0xff,0x00);
} else if (vcsel_pulse_period_pclk == 12) {
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x38);
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08);
status |= VL53L0X_write_byte(0xff,0x01);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
0x20);
status |= VL53L0X_write_byte(0xff,0x00);
} else if (vcsel_pulse_period_pclk == 14) {
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x048);
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07);
status |= VL53L0X_write_byte(0xff,0x01);
status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
0x20);
status |= VL53L0X_write_byte(0xff,0x00);
}
}
/* Re-calculate and apply timeouts,in macro periods */
if (status == VL53L0X_ERROR_NONE) {
vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t)
vcsel_pulse_period_pclk);
/* When the VCSEL period for the pre or final range is changed,
* the corresponding timeout must be read from the device using
* the current VCSEL period,then the new VCSEL period can be
* applied. The timeout then must be written back to the device
* using the new VCSEL period.
*
* For the MSRC timeout,the same applies - this timeout being
* dependant on the pre-range vcsel period.
*/
switch (vcsel_period_type) {
case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
&pre_range_timeout_micro_seconds);
if (status == VL53L0X_ERROR_NONE)
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
&msrc_timeout_micro_seconds);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
vcsel_period_reg);
if (status == VL53L0X_ERROR_NONE)
status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
pre_range_timeout_micro_seconds);
if (status == VL53L0X_ERROR_NONE)
status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
msrc_timeout_micro_seconds);
_device->DeviceSpecificParameters.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
break;
case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&final_range_timeout_micro_seconds);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
vcsel_period_reg);
if (status == VL53L0X_ERROR_NONE)
status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
final_range_timeout_micro_seconds);
_device->DeviceSpecificParameters.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
/* Finally,the timing budget must be re-applied */
if (status == VL53L0X_ERROR_NONE) {
measurement_timing_budget_micro_seconds = _device->CurrentParameters.MeasurementTimingBudgetMicroSeconds;
status = VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
}
/* Perform the phase calibration. This is needed after changing on
* vcsel period.
* get_data_enable = 0,restore_config = 1 */
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_perform_phase_calibration(&phase_cal_int,0,1);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t vcsel_pulse_period)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_set_vcsel_pulse_period(vcsel_period_type,
vcsel_pulse_period);
return status;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t *p_vcsel_pulse_period_pclk)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t vcsel_period_reg;
switch (vcsel_period_type) {
case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
status = VL53L0X_read_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
&vcsel_period_reg);
break;
case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
status = VL53L0X_read_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
&vcsel_period_reg);
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
if (status == VL53L0X_ERROR_NONE)
*p_vcsel_pulse_period_pclk =
VL53L0X_decode_vcsel_period(vcsel_period_reg);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t *p_vcsel_pulse_period_pclk)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_get_vcsel_pulse_period(vcsel_period_type,
p_vcsel_pulse_period_pclk);
return status;
}
uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout)
{
/*!
* Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
*/
uint32_t timeout_macro_clks = 0;
timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF)
<< (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
return timeout_macro_clks;
}
uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(uint8_t vcsel_period_pclks)
{
uint64_t pll_period_ps;
uint32_t macro_period_vclks;
uint32_t macro_period_ps;
/* The above calculation will produce rounding errors,
therefore set fixed value
*/
pll_period_ps = 1655;
macro_period_vclks = 2304;
macro_period_ps = (uint32_t)(macro_period_vclks
* vcsel_period_pclks * pll_period_ps);
return macro_period_ps;
}
/* To convert register value into us */
uint32_t VL53L0X::VL53L0X_calc_timeout_us(uint16_t timeout_period_mclks,
uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ps;
uint32_t macro_period_ns;
uint32_t actual_timeout_period_us = 0;
macro_period_ps = VL53L0X_calc_macro_period_ps(vcsel_period_pclks);
macro_period_ns = (macro_period_ps + 500) / 1000;
actual_timeout_period_us =
((timeout_period_mclks * macro_period_ns) + 500) / 1000;
return actual_timeout_period_us;
}
VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id,
uint32_t *p_time_out_micro_secs)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t current_vcsel_pulse_period_p_clk;
uint8_t encoded_time_out_byte = 0;
uint32_t timeout_micro_seconds = 0;
uint16_t pre_range_encoded_time_out = 0;
uint16_t msrc_time_out_m_clks;
uint16_t pre_range_time_out_m_clks;
uint16_t final_range_time_out_m_clks = 0;
uint16_t final_range_encoded_time_out;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
&encoded_time_out_byte);
}
msrc_time_out_m_clks = VL53L0X_decode_timeout(encoded_time_out_byte);
timeout_micro_seconds = VL53L0X_calc_timeout_us(msrc_time_out_m_clks,
current_vcsel_pulse_period_p_clk);
} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
/* Retrieve PRE-RANGE VCSEL Period */
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
if (status == VL53L0X_ERROR_NONE) {
/* Retrieve PRE-RANGE VCSEL Period */
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
&pre_range_encoded_time_out);
}
pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out);
timeout_micro_seconds = VL53L0X_calc_timeout_us(pre_range_time_out_m_clks,
current_vcsel_pulse_period_p_clk);
}
} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
pre_range_time_out_m_clks = 0;
if (scheduler_sequence_steps.PreRangeOn) {
/* Retrieve PRE-RANGE VCSEL Period */
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
/* Retrieve PRE-RANGE Timeout in Macro periods
* (MCLKS) */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
&pre_range_encoded_time_out);
pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out);
}
}
if (status == VL53L0X_ERROR_NONE) {
/* Retrieve FINAL-RANGE VCSEL Period */
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
¤t_vcsel_pulse_period_p_clk);
}
/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
&final_range_encoded_time_out);
final_range_time_out_m_clks = VL53L0X_decode_timeout(final_range_encoded_time_out);
}
final_range_time_out_m_clks -= pre_range_time_out_m_clks;
timeout_micro_seconds = VL53L0X_calc_timeout_us(final_range_time_out_m_clks,
current_vcsel_pulse_period_p_clk);
}
*p_time_out_micro_secs = timeout_micro_seconds;
return status;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(uint32_t *p_measurement_timing_budget_micro_seconds)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
uint32_t final_range_timeout_micro_seconds;
uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
uint32_t start_overhead_micro_seconds = 1910;
uint32_t end_overhead_micro_seconds = 960;
uint32_t msrc_overhead_micro_seconds = 660;
uint32_t tcc_overhead_micro_seconds = 590;
uint32_t dss_overhead_micro_seconds = 690;
uint32_t pre_range_overhead_micro_seconds = 660;
uint32_t final_range_overhead_micro_seconds = 550;
uint32_t pre_range_timeout_micro_seconds = 0;
/* Start and end overhead times always present */
*p_measurement_timing_budget_micro_seconds
= start_overhead_micro_seconds + end_overhead_micro_seconds;
status = VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
if (scheduler_sequence_steps.TccOn ||
scheduler_sequence_steps.MsrcOn ||
scheduler_sequence_steps.DssOn) {
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
&msrc_dcc_tcc_timeout_micro_seconds);
if (status == VL53L0X_ERROR_NONE) {
if (scheduler_sequence_steps.TccOn) {
*p_measurement_timing_budget_micro_seconds +=
msrc_dcc_tcc_timeout_micro_seconds +
tcc_overhead_micro_seconds;
}
if (scheduler_sequence_steps.DssOn) {
*p_measurement_timing_budget_micro_seconds +=
2 * (msrc_dcc_tcc_timeout_micro_seconds +
dss_overhead_micro_seconds);
} else if (scheduler_sequence_steps.MsrcOn) {
*p_measurement_timing_budget_micro_seconds +=
msrc_dcc_tcc_timeout_micro_seconds +
msrc_overhead_micro_seconds;
}
}
}
if (status == VL53L0X_ERROR_NONE) {
if (scheduler_sequence_steps.PreRangeOn) {
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
&pre_range_timeout_micro_seconds);
*p_measurement_timing_budget_micro_seconds +=
pre_range_timeout_micro_seconds +
pre_range_overhead_micro_seconds;
}
}
if (status == VL53L0X_ERROR_NONE) {
if (scheduler_sequence_steps.FinalRangeOn) {
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&final_range_timeout_micro_seconds);
*p_measurement_timing_budget_micro_seconds +=
(final_range_timeout_micro_seconds +
final_range_overhead_micro_seconds);
}
}
if (status == VL53L0X_ERROR_NONE) {
_device->CurrentParameters.MeasurementTimingBudgetMicroSeconds =
*p_measurement_timing_budget_micro_seconds;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(uint32_t *p_measurement_timing_budget_micro_seconds)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(p_measurement_timing_budget_micro_seconds);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters(VL53L0X_DeviceParameters_t *p_device_parameters)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int i;
status = VL53L0X_get_device_mode(&(p_device_parameters->DeviceMode));
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_get_inter_measurement_period_milli_seconds(&(p_device_parameters->InterMeasurementPeriodMilliSeconds));
if (status == VL53L0X_ERROR_NONE) {
p_device_parameters->XTalkCompensationEnable = 0;
}
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_get_x_talk_compensation_rate_mega_cps(&(p_device_parameters->XTalkCompensationRateMegaCps));
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_get_offset_calibration_data_micro_meter(&(p_device_parameters->RangeOffsetMicroMeters));
if (status == VL53L0X_ERROR_NONE) {
for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
/* get first the values,then the enables.
* VL53L0X_GetLimitCheckValue will modify the enable
* flags
*/
if (status == VL53L0X_ERROR_NONE) {
status |= VL53L0X_get_limit_check_value(i,
&(p_device_parameters->LimitChecksValue[i]));
} else {
break;
}
if (status == VL53L0X_ERROR_NONE) {
status |= VL53L0X_get_limit_check_enable(i,
&(p_device_parameters->LimitChecksEnable[i]));
} else {
break;
}
}
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_wrap_around_check_enable(&(p_device_parameters->WrapAroundCheckEnable));
}
/* Need to be done at the end as it uses VCSELPulsePeriod */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_measurement_timing_budget_micro_seconds(&(p_device_parameters->MeasurementTimingBudgetMicroSeconds));
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value(uint16_t limit_check_id,
FixPoint1616_t limit_check_value)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t temp8;
temp8 = _device->CurrentParameters.LimitChecksEnable[limit_check_id];
if (temp8 == 0) { /* disabled write only internal value */
_device->CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value;
} else {
switch (limit_check_id) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
_device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_value;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value));
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
_device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_value;
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
_device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_value;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value));
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
if (status == VL53L0X_ERROR_NONE) {
_device->CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_data_init()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceParameters_t CurrentParameters;
int i;
uint8_t StopVariable;
/* by default the I2C is running at 1V8 if you want to change it you
* need to include this define at compilation level. */
#ifdef USE_I2C_2V8
Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
0xFE,
0x01);
#endif
/* Set I2C standard mode */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0x88,0x00);
}
_device->DeviceSpecificParameters.ReadDataFromDeviceDone = 0;
_device->DeviceSpecificParameters.ReadDataFromDeviceDone = 0;
#ifdef USE_IQC_STATION
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_apply_offset_adjustment();
}
#endif
/* Default value is 1000 for Linearity Corrective Gain */
_device->LinearityCorrectiveGain = 1000;
/* Dmax default Parameter */
_device->DmaxCalRangeMilliMeter = 400;
_device->DmaxCalSignalRateRtnMegaCps = (FixPoint1616_t)((0x00016B85)); /* 1.42 No Cover Glass*/
/* Set Default static parameters
*set first temporary values 9.44MHz * 65536 = 618660 */
_device->DeviceSpecificParameters.OscFrequencyMHz = 618660;
/* Set Default XTalkCompensationRateMegaCps to 0 */
_device->CurrentParameters.XTalkCompensationRateMegaCps = 0;
/* Get default parameters */
status = VL53L0X_get_device_parameters(&CurrentParameters);
if (status == VL53L0X_ERROR_NONE) {
/* initialize PAL values */
CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
_device->CurrentParameters = CurrentParameters;
}
/* Sigma estimator variable */
_device->SigmaEstRefArray = 100;
_device->SigmaEstEffPulseWidth = 900;
_device->SigmaEstEffAmbWidth = 500;
_device->targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
/* Use internal default settings */
_device->UseInternalTuningSettings = 1;
status |= VL53L0X_write_byte(0x80,0x01);
status |= VL53L0X_write_byte(0xFF,0x01);
status |= VL53L0X_write_byte(0x00,0x00);
status |= VL53L0X_read_byte(0x91,&StopVariable);
_device->StopVariable = StopVariable;
status |= VL53L0X_write_byte(0x00,0x01);
status |= VL53L0X_write_byte(0xFF,0x00);
status |= VL53L0X_write_byte(0x80,0x00);
/* Enable all check */
for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
if (status == VL53L0X_ERROR_NONE) {
status |= VL53L0X_set_limit_check_enable(i,1);
} else {
break;
}
}
/* Disable the following checks */
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,0);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,0);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC,0);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE,0);
/* Limit default values */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
(FixPoint1616_t)(18 * 65536));
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
(FixPoint1616_t)(25 * 65536 / 100));
/* 0.25 * 65536 */
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
(FixPoint1616_t)(35 * 65536));
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
(FixPoint1616_t)(0 * 65536));
}
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = 0xFF;
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
0xFF);
/* Set PAL state to tell that we are waiting for call to
* VL53L0X_StaticInit */
_device->PalState = VL53L0X_STATE_WAIT_STATICINIT;
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.RefSpadsInitialised = 0;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_check_part_used(uint8_t *revision,
VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t module_id_int;
char *product_id_tmp;
status = VL53L0X_get_info_from_device(2);
if (status == VL53L0X_ERROR_NONE) {
module_id_int = _device->DeviceSpecificParameters.ModuleId;
if (module_id_int == 0) {
*revision = 0;
strcpy(p_VL53L0X_device_info->ProductId,"");
} else {
*revision = _device->DeviceSpecificParameters.Revision;
product_id_tmp = _device->DeviceSpecificParameters.ProductId;
strcpy(p_VL53L0X_device_info->ProductId,product_id_tmp);
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t revision_id;
uint8_t revision;
status = VL53L0X_check_part_used(&revision,p_VL53L0X_device_info);
if (status == VL53L0X_ERROR_NONE) {
if (revision == 0) {
strcpy(p_VL53L0X_device_info->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
} else if ((revision <= 34) && (revision != 32)) {
strcpy(p_VL53L0X_device_info->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
} else if (revision < 39) {
strcpy(p_VL53L0X_device_info->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
} else {
strcpy(p_VL53L0X_device_info->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
}
strcpy(p_VL53L0X_device_info->Type,VL53L0X_STRING_DEVICE_INFO_TYPE);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_MODEL_ID,
&p_VL53L0X_device_info->ProductType);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_REVISION_ID,
&revision_id);
p_VL53L0X_device_info->ProductRevisionMajor = 1;
p_VL53L0X_device_info->ProductRevisionMinor =
(revision_id & 0xF0) >> 4;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t byte;
status = VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS,&byte);
*p_interrupt_mask_status = byte & 0x07;
if (byte & 0x18) {
status = VL53L0X_ERROR_RANGE_ERROR;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(uint8_t *p_measurement_data_ready)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sys_range_status_register;
uint8_t interrupt_config;
uint32_t interrupt_mask;
interrupt_config = _device->DeviceSpecificParameters.Pin0GpioFunctionality;
if (interrupt_config ==
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
status = VL53L0X_get_interrupt_mask_status(&interrupt_mask);
if (interrupt_mask ==
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
*p_measurement_data_ready = 1;
} else {
*p_measurement_data_ready = 0;
}
} else {
status = VL53L0X_read_byte(VL53L0X_REG_RESULT_RANGE_STATUS,
&sys_range_status_register);
if (status == VL53L0X_ERROR_NONE) {
if (sys_range_status_register & 0x01) {
*p_measurement_data_ready = 1;
} else {
*p_measurement_data_ready = 0;
}
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_polling_delay()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
// do nothing VL53L0X_OsDelay();
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t new_data_ready = 0;
uint32_t loop_nb;
loop_nb = 0;
status = VL53L0X_get_measurement_data_ready(&new_data_ready);
while ((status==0) && (new_data_ready != 1) &&
(loop_nb < VL53L0X_DEFAULT_MAX_LOOP) )
{
VL53L0X_polling_delay();
status = VL53L0X_get_measurement_data_ready(&new_data_ready);
loop_nb++;
} // while ;
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) status = VL53L0X_ERROR_TIME_OUT;
return status;
}
/* Group PAL Interrupt Functions */
VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask(uint32_t interrupt_mask)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t loop_count;
uint8_t byte;
/* clear bit 0 range interrupt,bit 1 error interrupt */
loop_count = 0;
do {
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR,0x01);
status |= VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR,0x00);
status |= VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS,&byte);
loop_count++;
} while (((byte & 0x07) != 0x00)
&& (loop_count < 3)
&& (status == VL53L0X_ERROR_NONE));
if (loop_count >= 3) {
status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(uint8_t vhv_init_byte)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_START_STOP |
vhv_init_byte);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_measurement_poll_for_completion();
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_clear_interrupt_mask(0);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,0x00);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(uint8_t read_not_write,
uint8_t vhv_settings,uint8_t phase_cal,
uint8_t *p_vhv_settings,uint8_t *p_phase_cal,
const uint8_t vhv_enable,const uint8_t phase_enable)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t phase_calint = 0;
/* Read VHV from device */
status |= VL53L0X_write_byte(0xFF,0x01);
status |= VL53L0X_write_byte(0x00,0x00);
status |= VL53L0X_write_byte(0xFF,0x00);
if (read_not_write) {
if (vhv_enable) {
status |= VL53L0X_read_byte(0xCB,p_vhv_settings);
}
if (phase_enable) {
status |= VL53L0X_read_byte(0xEE,&phase_calint);
}
} else {
if (vhv_enable) {
status |= VL53L0X_write_byte(0xCB,vhv_settings);
}
if (phase_enable) {
status |= VL53L0X_update_byte(0xEE,0x80,phase_cal);
}
}
status |= VL53L0X_write_byte(0xFF,0x01);
status |= VL53L0X_write_byte(0x00,0x01);
status |= VL53L0X_write_byte(0xFF,0x00);
*p_phase_cal = (uint8_t)(phase_calint & 0xEF);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(uint8_t *p_vhv_settings,const uint8_t get_data_enable,
const uint8_t restore_config)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
uint8_t vhv_settings = 0;
uint8_t phase_cal = 0;
uint8_t phase_cal_int = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
if (restore_config) {sequence_config = _device->SequenceConfig;}
/* Run VHV */
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0x01);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_perform_single_ref_calibration(0x40);
}
/* Read VHV from device */
if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
status = VL53L0X_ref_calibration_io(1,
vhv_settings,phase_cal,/* Not used here */
p_vhv_settings,&phase_cal_int,
1,0);
} else {
*p_vhv_settings = 0;
}
if ((status == VL53L0X_ERROR_NONE) && restore_config) {
/* restore the previous Sequence Config */
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sequence_config);
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = sequence_config;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable,
const uint8_t restore_config)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
uint8_t vhv_settings = 0;
uint8_t phase_cal = 0;
uint8_t vhv_settingsint;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
if (restore_config) {
sequence_config = _device-> SequenceConfig;
}
/* Run PhaseCal */
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0x02);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_perform_single_ref_calibration(0x0);
}
/* Read PhaseCal from device */
if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
status = VL53L0X_ref_calibration_io(1,
vhv_settings,phase_cal,/* Not used here */
&vhv_settingsint,p_phase_cal,
0,1);
} else {
*p_phase_cal = 0;
}
if ((status == VL53L0X_ERROR_NONE) && restore_config) {
/* restore the previous Sequence Config */
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sequence_config);
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = sequence_config;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t *p_vhv_settings,uint8_t *p_phase_cal,uint8_t get_data_enable)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
sequence_config = _device-> SequenceConfig;
/* In the following function we don't save the config to optimize
* writes on device. Config is saved and restored only once. */
status = VL53L0X_perform_vhv_calibration(p_vhv_settings,get_data_enable,0);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_perform_phase_calibration(p_phase_cal,get_data_enable,0);
}
if (status == VL53L0X_ERROR_NONE) {
/* restore the previous Sequence Config */
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sequence_config);
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = sequence_config;
}
}
return status;
}
void VL53L0X::get_next_good_spad(uint8_t good_spad_array[],uint32_t size,
uint32_t curr,int32_t *p_next)
{
uint32_t start_index;
uint32_t fine_offset;
uint32_t c_spads_per_byte = 8;
uint32_t coarse_index;
uint32_t fine_index;
uint8_t data_byte;
uint8_t success = 0;
/*
* Starting with the current good spad,loop through the array to find
* the next. i.e. the next bit set in the sequence.
*
* The coarse index is the byte index of the array and the fine index is
* the index of the bit within each byte.
*/
*p_next = -1;
start_index = curr / c_spads_per_byte;
fine_offset = curr % c_spads_per_byte;
for (coarse_index = start_index; ((coarse_index < size) && !success);
coarse_index++) {
fine_index = 0;
data_byte = good_spad_array[coarse_index];
if (coarse_index == start_index) {
/* locate the bit position of the provided current
* spad bit before iterating */
data_byte >>= fine_offset;
fine_index = fine_offset;
}
while (fine_index < c_spads_per_byte) {
if ((data_byte & 0x1) == 1) {
success = 1;
*p_next = coarse_index * c_spads_per_byte + fine_index;
break;
}
data_byte >>= 1;
fine_index++;
}
}
}
uint8_t VL53L0X::is_aperture(uint32_t spad_index)
{
/*
* This function reports if a given spad index is an aperture SPAD by
* deriving the quadrant.
*/
uint32_t quadrant;
uint8_t is_aperture = 1;
quadrant = spad_index >> 6;
if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) {
is_aperture = 0;
}
return is_aperture;
}
VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[],uint32_t size,
uint32_t spad_index)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t c_spads_per_byte = 8;
uint32_t coarse_index;
uint32_t fine_index;
coarse_index = spad_index / c_spads_per_byte;
fine_index = spad_index % c_spads_per_byte;
if (coarse_index >= size) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
} else {
spad_array[coarse_index] |= (1 << fine_index);
}
return status;
}
VL53L0X_Error VL53L0X::set_ref_spad_map(uint8_t *p_ref_spad_array)
{
VL53L0X_Error status = VL53L0X_write_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
p_ref_spad_array,6);
return status;
}
VL53L0X_Error VL53L0X::get_ref_spad_map(uint8_t *p_ref_spad_array)
{
VL53L0X_Error status = VL53L0X_read_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
p_ref_spad_array,
6);
// VL53L0X_Error status = VL53L0X_ERROR_NONE;
// uint8_t count=0;
// for (count = 0; count < 6; count++)
// status = VL53L0X_RdByte((VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count),&refSpadArray[count]);
return status;
}
VL53L0X_Error VL53L0X::enable_ref_spads(uint8_t aperture_spads,
uint8_t good_spad_array[],
uint8_t spad_array[],
uint32_t size,
uint32_t start,
uint32_t offset,
uint32_t spad_count,
uint32_t *p_last_spad)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t index;
uint32_t i;
int32_t next_good_spad = offset;
uint32_t current_spad;
uint8_t check_spad_array[6];
/*
* This function takes in a spad array which may or may not have SPADS
* already enabled and appends from a given offset a requested number
* of new SPAD enables. The 'good spad map' is applied to
* determine the next SPADs to enable.
*
* This function applies to only aperture or only non-aperture spads.
* Checks are performed to ensure this.
*/
current_spad = offset;
for (index = 0; index < spad_count; index++) {
get_next_good_spad(good_spad_array,size,current_spad,
&next_good_spad);
if (next_good_spad == -1) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
/* Confirm that the next good SPAD is non-aperture */
if (is_aperture(start + next_good_spad) != aperture_spads) {
/* if we can't get the required number of good aperture
* spads from the current quadrant then this is an error
*/
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
current_spad = (uint32_t)next_good_spad;
enable_spad_bit(spad_array,size,current_spad);
current_spad++;
}
*p_last_spad = current_spad;
if (status == VL53L0X_ERROR_NONE) {
status = set_ref_spad_map(spad_array);
}
if (status == VL53L0X_ERROR_NONE) {
status = get_ref_spad_map(check_spad_array);
i = 0;
/* Compare spad maps. If not equal report error. */
while (i < size) {
if (spad_array[i] != check_spad_array[i]) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
i++;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_device_mode(VL53L0X_DeviceModes device_mode)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
switch (device_mode) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
case VL53L0X_DEVICEMODE_GPIO_DRIVE:
case VL53L0X_DEVICEMODE_GPIO_OSC:
/* Supported modes */
_device->CurrentParameters.DeviceMode = device_mode;
break;
default:
/* Unsupported mode */
status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,FixPoint1616_t threshold_low,
FixPoint1616_t threshold_high)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t threshold16;
/* no dependency on DeviceMode for FlightSense */
/* Need to divide by 2 because the FW will apply a x2 */
threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
status = VL53L0X_write_word(VL53L0X_REG_SYSTEM_THRESH_LOW,threshold16);
if (status == VL53L0X_ERROR_NONE) {
/* Need to divide by 2 because the FW will apply a x2 */
threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
status = VL53L0X_write_word(VL53L0X_REG_SYSTEM_THRESH_HIGH,
threshold16);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,FixPoint1616_t *p_threshold_low,
FixPoint1616_t *p_threshold_high)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t threshold16;
/* no dependency on DeviceMode for FlightSense */
status = VL53L0X_read_word(VL53L0X_REG_SYSTEM_THRESH_LOW,&threshold16);
/* Need to multiply by 2 because the FW will apply a x2 */
*p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_SYSTEM_THRESH_HIGH,
&threshold16);
/* Need to multiply by 2 because the FW will apply a x2 */
*p_threshold_high =
(FixPoint1616_t)((0x00fff & threshold16) << 17);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(uint8_t *p_tuning_setting_buffer)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int i;
int index;
uint8_t msb;
uint8_t lsb;
uint8_t select_param;
uint8_t number_of_writes;
uint8_t address;
uint8_t local_buffer[4]; /* max */
uint16_t temp16;
index = 0;
while ((*(p_tuning_setting_buffer + index) != 0) &&
(status == VL53L0X_ERROR_NONE)) {
number_of_writes = *(p_tuning_setting_buffer + index);
index++;
if (number_of_writes == 0xFF) {
/* internal parameters */
select_param = *(p_tuning_setting_buffer + index);
index++;
switch (select_param) {
case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
_device->SigmaEstRefArray = temp16;
break;
case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
_device->SigmaEstEffPulseWidth = temp16;
break;
case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
_device->SigmaEstEffAmbWidth = temp16;
break;
case 3: /* uint16_t targetRefRate -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
_device->targetRefRate = temp16;
break;
default: /* invalid parameter */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
} else if (number_of_writes <= 4) {
address = *(p_tuning_setting_buffer + index);
index++;
for (i = 0; i < number_of_writes; i++) {
local_buffer[i] = *(p_tuning_setting_buffer +
index);
index++;
}
status = VL53L0X_write_multi(address,local_buffer,
number_of_writes);
} else {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(uint8_t start_not_stopflag)
{
uint8_t interrupt_config;
FixPoint1616_t threshold_low;
FixPoint1616_t threshold_high;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
interrupt_config = _device->DeviceSpecificParameters.Pin0GpioFunctionality;
if ((interrupt_config ==
VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) ||
(interrupt_config ==
VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) ||
(interrupt_config ==
VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
status = VL53L0X_get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
&threshold_low,&threshold_high);
if (((threshold_low > 255 * 65536) ||
(threshold_high > 255 * 65536)) &&
(status == VL53L0X_ERROR_NONE)) {
if (start_not_stopflag != 0) {
status = VL53L0X_load_tuning_settings(InterruptThresholdSettings);
} else {
status |= VL53L0X_write_byte(0xFF,0x04);
status |= VL53L0X_write_byte(0x70,0x00);
status |= VL53L0X_write_byte(0xFF,0x00);
status |= VL53L0X_write_byte(0x80,0x00);
}
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_start_measurement()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceModes device_mode;
uint8_t byte;
uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
uint32_t loop_nb;
/* Get Current DeviceMode */
VL53L0X_get_device_mode(&device_mode);
status = VL53L0X_write_byte(0x80,0x01);
status = VL53L0X_write_byte(0xFF,0x01);
status = VL53L0X_write_byte(0x00,0x00);
status = VL53L0X_write_byte(0x91,_device-> StopVariable);
status = VL53L0X_write_byte(0x00,0x01);
status = VL53L0X_write_byte(0xFF,0x00);
status = VL53L0X_write_byte(0x80,0x00);
switch (device_mode) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,0x01);
byte = start_stop_byte;
if (status == VL53L0X_ERROR_NONE) {
/* Wait until start bit has been cleared */
loop_nb = 0;
do {
if (loop_nb > 0)
status = VL53L0X_read_byte(VL53L0X_REG_SYSRANGE_START,&byte);
loop_nb = loop_nb + 1;
} while (((byte & start_stop_byte) == start_stop_byte)
&& (status == VL53L0X_ERROR_NONE)
&& (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
status = VL53L0X_ERROR_TIME_OUT;
}
}
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
/* Back-to-back mode */
/* Check if need to apply interrupt settings */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_check_and_load_interrupt_settings(1);
}
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
if (status == VL53L0X_ERROR_NONE) {
/* Set PAL State to Running */
_device->PalState = VL53L0X_STATE_RUNNING;
}
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
/* Continuous mode */
/* Check if need to apply interrupt settings */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_check_and_load_interrupt_settings(1);
}
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_TIMED);
if (status == VL53L0X_ERROR_NONE) {
/* Set PAL State to Running */
_device->PalState = VL53L0X_STATE_RUNNING;
}
break;
default:
/* Selected mode not supported */
status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
return status;
}
/* Group PAL Measurement Functions */
VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceModes device_mode;
/* Get Current DeviceMode */
status = VL53L0X_get_device_mode(&device_mode);
/* Start immediately to run a single ranging measurement in case of
* single ranging or single histogram */
if (status == VL53L0X_ERROR_NONE
&& device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
status = VL53L0X_start_measurement();
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_measurement_poll_for_completion();
}
/* Change PAL State in case of single ranging or single histogram */
if (status == VL53L0X_ERROR_NONE
&& device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
_device->PalState = VL53L0X_STATE_IDLE;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(uint8_t *p_x_talk_compensation_enable)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t temp8;
temp8 = _device->CurrentParameters.XTalkCompensationEnable;
*p_x_talk_compensation_enable = temp8;
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
FixPoint1616_t *p_total_xtalk_rate_mcps)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t xtalk_comp_enable;
FixPoint1616_t total_xtalk_mega_cps;
FixPoint1616_t xtalk_per_spad_mega_cps;
*p_total_xtalk_rate_mcps = 0;
status = VL53L0X_get_x_talk_compensation_enable(&xtalk_comp_enable);
if (status == VL53L0X_ERROR_NONE) {
if (xtalk_comp_enable) {
xtalk_per_spad_mega_cps = _device->CurrentParameters.XTalkCompensationRateMegaCps;
/* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
total_xtalk_mega_cps =
p_ranging_measurement_data->EffectiveSpadRtnCount *
xtalk_per_spad_mega_cps;
/* FixPoint0824 >> 8 = FixPoint1616 */
*p_total_xtalk_rate_mcps =
(total_xtalk_mega_cps + 0x80) >> 8;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
FixPoint1616_t *p_total_signal_rate_mcps)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
FixPoint1616_t total_xtalk_mega_cps;
*p_total_signal_rate_mcps =
p_ranging_measurement_data->SignalRateRtnMegaCps;
status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data,&total_xtalk_mega_cps);
if (status == VL53L0X_ERROR_NONE) {
*p_total_signal_rate_mcps += total_xtalk_mega_cps;
}
return status;
}
/* To convert ms into register value */
uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(uint32_t timeout_period_us,
uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ps;
uint32_t macro_period_ns;
uint32_t timeout_period_mclks = 0;
macro_period_ps = VL53L0X_calc_macro_period_ps(vcsel_period_pclks);
macro_period_ns = (macro_period_ps + 500) / 1000;
timeout_period_mclks =
(uint32_t)(((timeout_period_us * 1000)
+ (macro_period_ns / 2)) / macro_period_ns);
return timeout_period_mclks;
}
uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
{
/*
* Implements an integer square root
*
* From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
*/
uint32_t res = 0;
uint32_t bit = 1 << 30;
/* The second-to-top bit is set:
* 1 << 14 for 16-bits,1 << 30 for 32 bits */
/* "bit" starts at the highest power of four <= the argument. */
while (bit > num) {
bit >>= 2;
}
while (bit != 0) {
if (num >= res + bit) {
num -= res + bit;
res = (res >> 1) + bit;
} else {
res >>= 1;
}
bit >>= 2;
}
return res;
}
VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(FixPoint1616_t total_signal_rate_mcps,
FixPoint1616_t total_corr_signal_rate_mcps,
FixPoint1616_t pw_mult,
uint32_t sigma_estimate_p1,
FixPoint1616_t sigma_estimate_p2,
uint32_t peak_vcsel_duration_us,
uint32_t *pd_max_mm)
{
const uint32_t c_sigma_limit = 18;
const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */
const uint32_t c_amb_eff_width_sigma_est_ns = 6;
const uint32_t c_amb_eff_width_d_max_ns = 7;
uint32_t dmax_cal_range_mm;
FixPoint1616_t dmax_cal_signal_rate_rtn_mcps;
FixPoint1616_t min_signal_needed;
FixPoint1616_t min_signal_needed_p1;
FixPoint1616_t min_signal_needed_p2;
FixPoint1616_t min_signal_needed_p3;
FixPoint1616_t min_signal_needed_p4;
FixPoint1616_t sigma_limit_tmp;
FixPoint1616_t sigma_est_sq_tmp;
FixPoint1616_t signal_limit_tmp;
FixPoint1616_t signal_at0_mm;
FixPoint1616_t dmax_dark;
FixPoint1616_t dmax_ambient;
FixPoint1616_t dmax_dark_tmp;
FixPoint1616_t sigma_est_p2_tmp;
uint32_t signal_rate_temp_mcps;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
dmax_cal_range_mm =
_device-> DmaxCalRangeMilliMeter;
dmax_cal_signal_rate_rtn_mcps =
_device-> DmaxCalSignalRateRtnMegaCps;
/* uint32 * FixPoint1616 = FixPoint1616 */
signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
/* FixPoint1616 >> 8 = FixPoint2408 */
signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
signal_at0_mm *= dmax_cal_range_mm;
min_signal_needed_p1 = 0;
if (total_corr_signal_rate_mcps > 0) {
/* Shift by 10 bits to increase resolution prior to the
* division */
signal_rate_temp_mcps = total_signal_rate_mcps << 10;
/* Add rounding value prior to division */
min_signal_needed_p1 = signal_rate_temp_mcps +
(total_corr_signal_rate_mcps / 2);
/* FixPoint0626/FixPoint1616 = FixPoint2210 */
min_signal_needed_p1 /= total_corr_signal_rate_mcps;
/* Apply a factored version of the speed of light.
Correction to be applied at the end */
min_signal_needed_p1 *= 3;
/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
min_signal_needed_p1 *= min_signal_needed_p1;
/* FixPoint1220 >> 16 = FixPoint2804 */
min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
}
min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
/* FixPoint1616 >> 16 = uint32 */
min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16;
/* uint32 * uint32 = uint32 */
min_signal_needed_p2 *= min_signal_needed_p2;
/* Check sigmaEstimateP2
* If this value is too high there is not enough signal rate
* to calculate dmax value so set a suitable value to ensure
* a very small dmax.
*/
sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
c_amb_eff_width_sigma_est_ns;
sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
if (sigma_est_p2_tmp > 0xffff) {
min_signal_needed_p3 = 0xfff00000;
} else {
/* DMAX uses a different ambient width from sigma,so apply
* correction.
* Perform division before multiplication to prevent overflow.
*/
sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
c_amb_eff_width_sigma_est_ns;
sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
/* FixPoint1616 >> 16 = uint32 */
min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
min_signal_needed_p3 *= min_signal_needed_p3;
}
/* FixPoint1814 / uint32 = FixPoint1814 */
sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
sigma_limit_tmp *= sigma_limit_tmp;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
/* FixPoint3232 >> 4 = FixPoint0428 */
sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
sigma_limit_tmp -= sigma_est_sq_tmp;
/* uint32_t * FixPoint0428 = FixPoint0428 */
min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
/* FixPoint0428 >> 14 = FixPoint1814 */
min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14;
/* uint32 + uint32 = uint32 */
min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3);
/* uint32 / uint32 = uint32 */
min_signal_needed += (peak_vcsel_duration_us / 2);
min_signal_needed /= peak_vcsel_duration_us;
/* uint32 << 14 = FixPoint1814 */
min_signal_needed <<= 14;
/* FixPoint1814 / FixPoint1814 = uint32 */
min_signal_needed += (min_signal_needed_p4 / 2);
min_signal_needed /= min_signal_needed_p4;
/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
min_signal_needed *= min_signal_needed_p1;
/* Apply correction by dividing by 1000000.
* This assumes 10E16 on the numerator of the equation
* and 10E-22 on the denominator.
* We do this because 32bit fix point calculation can't
* handle the larger and smaller elements of this equation,
* i.e. speed of light and pulse widths.
*/
min_signal_needed = (min_signal_needed + 500) / 1000;
min_signal_needed <<= 4;
min_signal_needed = (min_signal_needed + 500) / 1000;
/* FixPoint1616 >> 8 = FixPoint2408 */
signal_limit_tmp = (c_signal_limit + 0x80) >> 8;
/* FixPoint2408/FixPoint2408 = uint32 */
if (signal_limit_tmp != 0) {
dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
/ signal_limit_tmp;
} else {
dmax_dark_tmp = 0;
}
dmax_dark = VL53L0X_isqrt(dmax_dark_tmp);
/* FixPoint2408/FixPoint2408 = uint32 */
if (min_signal_needed != 0) {
dmax_ambient = (signal_at0_mm + min_signal_needed / 2)
/ min_signal_needed;
} else {
dmax_ambient = 0;
}
dmax_ambient = VL53L0X_isqrt(dmax_ambient);
*pd_max_mm = dmax_dark;
if (dmax_dark > dmax_ambient) {
*pd_max_mm = dmax_ambient;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
FixPoint1616_t *p_sigma_estimate,
uint32_t *p_dmax_mm)
{
/* Expressed in 100ths of a ns,i.e. centi-ns */
const uint32_t c_pulse_effective_width_centi_ns = 800;
/* Expressed in 100ths of a ns,i.e. centi-ns */
const uint32_t c_ambient_effective_width_centi_ns = 600;
const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
const FixPoint1616_t c_sigma_est_max = 0x028F87AE;
const FixPoint1616_t c_sigma_est_rtn_max = 0xF000;
const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
c_ambient_effective_width_centi_ns;
/* Time Of Flight per mm (6.6 pico secs) */
const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A;
const uint32_t c_16bit_rounding_param = 0x00008000;
const FixPoint1616_t c_max_x_talk_kcps = 0x00320000;
const uint32_t c_pll_period_ps = 1655;
uint32_t vcsel_total_events_rtn;
uint32_t final_range_timeout_micro_secs;
uint32_t pre_range_timeout_micro_secs;
uint32_t final_range_integration_time_milli_secs;
FixPoint1616_t sigma_estimate_p1;
FixPoint1616_t sigma_estimate_p2;
FixPoint1616_t sigma_estimate_p3;
FixPoint1616_t delta_t_ps;
FixPoint1616_t pw_mult;
FixPoint1616_t sigma_est_rtn;
FixPoint1616_t sigma_estimate;
FixPoint1616_t x_talk_correction;
FixPoint1616_t ambient_rate_kcps;
FixPoint1616_t peak_signal_rate_kcps;
FixPoint1616_t x_talk_comp_rate_mcps;
uint32_t x_talk_comp_rate_kcps;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
FixPoint1616_t diff1_mcps;
FixPoint1616_t diff2_mcps;
FixPoint1616_t sqr1;
FixPoint1616_t sqr2;
FixPoint1616_t sqr_sum;
FixPoint1616_t sqrt_result_centi_ns;
FixPoint1616_t sqrt_result;
FixPoint1616_t total_signal_rate_mcps;
FixPoint1616_t corrected_signal_rate_mcps;
FixPoint1616_t sigma_est_ref;
uint32_t vcsel_width;
uint32_t final_range_macro_pclks;
uint32_t pre_range_macro_pclks;
uint32_t peak_vcsel_duration_us;
uint8_t final_range_vcsel_pclks;
uint8_t pre_range_vcsel_pclks;
/*! \addtogroup calc_sigma_estimate
* @{
*
* Estimates the range sigma
*/
x_talk_comp_rate_mcps = _device->CurrentParameters.XTalkCompensationRateMegaCps;
/*
* We work in kcps rather than mcps as this helps keep within the
* confines of the 32 Fix1616 type.
*/
ambient_rate_kcps =
(p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16;
corrected_signal_rate_mcps =
p_ranging_measurement_data->SignalRateRtnMegaCps;
status = VL53L0X_get_total_signal_rate(p_ranging_measurement_data,&total_signal_rate_mcps);
status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data,&x_talk_comp_rate_mcps);
/* Signal rate measurement provided by device is the
* peak signal rate,not average.
*/
peak_signal_rate_kcps = (total_signal_rate_mcps * 1000);
peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16;
x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) {
x_talk_comp_rate_kcps = c_max_x_talk_kcps;
}
if (status == VL53L0X_ERROR_NONE) {
/* Calculate final range macro periods */
final_range_timeout_micro_secs = _device->DeviceSpecificParameters.FinalRangeTimeoutMicroSecs;
final_range_vcsel_pclks = _device->DeviceSpecificParameters.FinalRangeVcselPulsePeriod;
final_range_macro_pclks = VL53L0X_calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks);
/* Calculate pre-range macro periods */
pre_range_timeout_micro_secs = _device->DeviceSpecificParameters.PreRangeTimeoutMicroSecs;
pre_range_vcsel_pclks = _device->DeviceSpecificParameters.PreRangeVcselPulsePeriod;
pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks);
vcsel_width = 3;
if (final_range_vcsel_pclks == 8) {
vcsel_width = 2;
}
peak_vcsel_duration_us = vcsel_width * 2048 *
(pre_range_macro_pclks + final_range_macro_pclks);
peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
peak_vcsel_duration_us *= c_pll_period_ps;
peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
/* Fix1616 >> 8 = Fix2408 */
total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8;
/* Fix2408 * uint32 = Fix2408 */
vcsel_total_events_rtn = total_signal_rate_mcps *
peak_vcsel_duration_us;
/* Fix2408 >> 8 = uint32 */
vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
/* Fix2408 << 8 = Fix1616 = */
total_signal_rate_mcps <<= 8;
}
if (status != VL53L0X_ERROR_NONE) {
return status;
}
if (peak_signal_rate_kcps == 0) {
*p_sigma_estimate = c_sigma_est_max;
_device->SigmaEstimate = c_sigma_est_max;
*p_dmax_mm = 0;
} else {
if (vcsel_total_events_rtn < 1) {
vcsel_total_events_rtn = 1;
}
sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps;
if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigma_estimate_p2 = c_amb_to_signal_ratio_max;
}
sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12);
/* uint32 * FixPoint1616 = FixPoint1616 */
delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
c_tof_per_mm_ps;
/*
* vcselRate - xtalkCompRate
* (uint32 << 16) - FixPoint1616 = FixPoint1616.
* Divide result by 1000 to convert to mcps.
* 500 is added to ensure rounding when integer division
* truncates.
*/
diff1_mcps = (((peak_signal_rate_kcps << 16) -
2 * x_talk_comp_rate_kcps) + 500) / 1000;
/* vcselRate + xtalkCompRate */
diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000;
/* Shift by 8 bits to increase resolution prior to the
* division */
diff1_mcps <<= 8;
/* FixPoint0824/FixPoint1616 = FixPoint2408 */
// xTalkCorrection = abs(diff1_mcps/diff2_mcps);
// abs is causing compiler overloading isue in C++,but unsigned types. So,redundant call anyway!
x_talk_correction = diff1_mcps / diff2_mcps;
/* FixPoint2408 << 8 = FixPoint1616 */
x_talk_correction <<= 8;
if (p_ranging_measurement_data->RangeStatus != 0) {
pw_mult = 1 << 16;
} else {
/* FixPoint1616/uint32 = FixPoint1616 */
pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
/*
* FixPoint1616 * FixPoint1616 = FixPoint3232,however both
* values are small enough such that32 bits will not be
* exceeded.
*/
pw_mult *= ((1 << 16) - x_talk_correction);
/* (FixPoint3232 >> 16) = FixPoint1616 */
pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
pw_mult += (1 << 16);
/*
* At this point the value will be 1.xx,therefore if we square
* the value this will exceed 32 bits. To address this perform
* a single shift to the right before the multiplication.
*/
pw_mult >>= 1;
/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
pw_mult = pw_mult * pw_mult;
/* (FixPoint3430 >> 14) = Fix1616 */
pw_mult >>= 14;
}
/* FixPoint1616 * uint32 = FixPoint1616 */
sqr1 = pw_mult * sigma_estimate_p1;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr1 = (sqr1 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr1 *= sqr1;
sqr2 = sigma_estimate_p2;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr2 = (sqr2 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr2 *= sqr2;
/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
sqr_sum = sqr1 + sqr2;
/* SQRT(FixPoin6400) = FixPoint3200 */
sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum);
/* (FixPoint3200 << 16) = FixPoint1616 */
sqrt_result_centi_ns <<= 16;
/*
* Note that the Speed Of Light is expressed in um per 1E-10
* seconds (2997) Therefore to get mm/ns we have to divide by
* 10000
*/
sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
sigma_estimate_p3);
sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
/* Add 5000 before dividing by 10000 to ensure rounding. */
sigma_est_rtn += 5000;
sigma_est_rtn /= 10000;
if (sigma_est_rtn > c_sigma_est_rtn_max) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigma_est_rtn = c_sigma_est_rtn_max;
}
final_range_integration_time_milli_secs =
(final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
* sqrt(FixPoint1616/int) = FixPoint2408)
*/
sigma_est_ref =
VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs +
final_range_integration_time_milli_secs / 2) /
final_range_integration_time_milli_secs);
/* FixPoint2408 << 8 = FixPoint1616 */
sigma_est_ref <<= 8;
sigma_est_ref = (sigma_est_ref + 500) / 1000;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr1 = sigma_est_rtn * sigma_est_rtn;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr2 = sigma_est_ref * sigma_est_ref;
/* sqrt(FixPoint3232) = FixPoint1616 */
sqrt_result = VL53L0X_isqrt((sqr1 + sqr2));
/*
* Note that the Shift by 4 bits increases resolution prior to
* the sqrt,therefore the result must be shifted by 2 bits to
* the right to revert back to the FixPoint1616 format.
*/
sigma_estimate = 1000 * sqrt_result;
if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
(sigma_estimate > c_sigma_est_max)) {
sigma_estimate = c_sigma_est_max;
}
*p_sigma_estimate = (uint32_t)(sigma_estimate);
_device->SigmaEstimate = *p_sigma_estimate;
status = VL53L0X_calc_dmax(total_signal_rate_mcps,
corrected_signal_rate_mcps,
pw_mult,
sigma_estimate_p1,
sigma_estimate_p2,
peak_vcsel_duration_us,
p_dmax_mm);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(uint8_t device_range_status,
FixPoint1616_t signal_rate,
uint16_t effective_spad_rtn_count,
VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
uint8_t *p_pal_range_status)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t none_flag;
uint8_t sigma_limitflag = 0;
uint8_t signal_ref_clipflag = 0;
uint8_t range_ignore_thresholdflag = 0;
uint8_t sigma_limit_check_enable = 0;
uint8_t signal_rate_final_range_limit_check_enable = 0;
uint8_t signal_ref_clip_limit_check_enable = 0;
uint8_t range_ignore_threshold_limit_check_enable = 0;
FixPoint1616_t sigma_estimate;
FixPoint1616_t sigma_limit_value;
FixPoint1616_t signal_ref_clip_value;
FixPoint1616_t range_ignore_threshold_value;
FixPoint1616_t signal_rate_per_spad;
uint8_t device_range_status_internal = 0;
uint16_t tmp_word = 0;
uint8_t temp8;
uint32_t dmax_mm = 0;
FixPoint1616_t last_signal_ref_mcps;
/*
* VL53L0X has a good ranging when the value of the
* DeviceRangeStatus = 11. This function will replace the value 0 with
* the value 11 in the DeviceRangeStatus.
* In addition,the SigmaEstimator is not included in the VL53L0X
* DeviceRangeStatus,this will be added in the PalRangeStatus.
*/
device_range_status_internal = ((device_range_status & 0x78) >> 3);
if ( device_range_status_internal == 0 ||
device_range_status_internal == 5 ||
device_range_status_internal == 7 ||
device_range_status_internal == 12 ||
device_range_status_internal == 13 ||
device_range_status_internal == 14 ||
device_range_status_internal == 15
) {
none_flag = 1;
} else {
none_flag = 0;
}
/*
* Check if Sigma limit is enabled,if yes then do comparison with limit
* value and put the result back into pPalRangeStatus.
*/
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
&sigma_limit_check_enable);
}
if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
/*
* compute the Sigma and check with limit
*/
status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data,
&sigma_estimate,
&dmax_mm);
if (status == VL53L0X_ERROR_NONE) {
p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
&sigma_limit_value);
if ((sigma_limit_value > 0) &&
(sigma_estimate > sigma_limit_value)) {
/* Limit Fail */
sigma_limitflag = 1;
}
}
}
/*
* Check if Signal ref clip limit is enabled,if yes then do comparison
* with limit value and put the result back into pPalRangeStatus.
*/
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
&signal_ref_clip_limit_check_enable);
}
if ((signal_ref_clip_limit_check_enable != 0) &&
(status == VL53L0X_ERROR_NONE)) {
status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
&signal_ref_clip_value);
/* Read LastSignalRefMcps from device */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x01);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
&tmp_word);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x00);
}
last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word);
_device->LastSignalRefMcps = last_signal_ref_mcps;
if ((signal_ref_clip_value > 0) &&
(last_signal_ref_mcps > signal_ref_clip_value)) {
/* Limit Fail */
signal_ref_clipflag = 1;
}
}
/*
* Check if Signal ref clip limit is enabled,if yes then do comparison
* with limit value and put the result back into pPalRangeStatus.
* EffectiveSpadRtnCount has a format 8.8
* If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
*/
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
&range_ignore_threshold_limit_check_enable);
}
if ((range_ignore_threshold_limit_check_enable != 0) &&
(status == VL53L0X_ERROR_NONE)) {
/* Compute the signal rate per spad */
if (effective_spad_rtn_count == 0) {
signal_rate_per_spad = 0;
} else {
signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate)
/ effective_spad_rtn_count);
}
status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
&range_ignore_threshold_value);
if ((range_ignore_threshold_value > 0) &&
(signal_rate_per_spad < range_ignore_threshold_value)) {
/* Limit Fail add 2^6 to range status */
range_ignore_thresholdflag = 1;
}
}
if (status == VL53L0X_ERROR_NONE) {
if (none_flag == 1) {
*p_pal_range_status = 255; /* NONE */
} else if (device_range_status_internal == 1 ||
device_range_status_internal == 2 ||
device_range_status_internal == 3) {
*p_pal_range_status = 5; /* HW fail */
} else if (device_range_status_internal == 6 ||
device_range_status_internal == 9) {
*p_pal_range_status = 4; /* Phase fail */
} else if (device_range_status_internal == 8 ||
device_range_status_internal == 10 ||
signal_ref_clipflag == 1) {
*p_pal_range_status = 3; /* Min range */
} else if (device_range_status_internal == 4 ||
range_ignore_thresholdflag == 1) {
*p_pal_range_status = 2; /* Signal Fail */
} else if (sigma_limitflag == 1) {
*p_pal_range_status = 1; /* Sigma Fail */
} else {
*p_pal_range_status = 0; /* Range Valid */
}
}
/* DMAX only relevant during range error */
if (*p_pal_range_status == 0) {
p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
}
/* fill the Limit Check Status */
status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&signal_rate_final_range_limit_check_enable);
if (status == VL53L0X_ERROR_NONE) {
if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1)) {
temp8 = 1;
} else {
temp8 = 0;
}
_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = temp8;
if ((device_range_status_internal == 4) ||
(signal_rate_final_range_limit_check_enable == 0)) {
temp8 = 1;
} else {
temp8 = 0;
}
_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = temp8;
if ((signal_ref_clip_limit_check_enable == 0) ||
(signal_ref_clipflag == 1)) {
temp8 = 1;
} else {
temp8 = 0;
}
_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = temp8;
if ((range_ignore_threshold_limit_check_enable == 0) ||
(range_ignore_thresholdflag == 1)) {
temp8 = 1;
} else {
temp8 = 0;
}
_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = temp8;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t device_range_status;
uint8_t range_fractional_enable;
uint8_t pal_range_status;
uint8_t x_talk_compensation_enable;
uint16_t ambient_rate;
FixPoint1616_t signal_rate;
uint16_t x_talk_compensation_rate_mega_cps;
uint16_t effective_spad_rtn_count;
uint16_t tmpuint16;
uint16_t xtalk_range_milli_meter;
uint16_t linearity_corrective_gain;
uint8_t localBuffer[12];
VL53L0X_RangingMeasurementData_t last_range_data_buffer;
/*
* use multi read even if some registers are not useful,result will
* be more efficient
* start reading at 0x14 dec20
* end reading at 0x21 dec33 total 14 bytes to read
*/
status = VL53L0X_read_multi(0x14,localBuffer,12);
if (status == VL53L0X_ERROR_NONE) {
p_ranging_measurement_data->ZoneId = 0; /* Only one zone */
p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */
tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]);
/* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
*(format 11.2) else no fractional
*/
p_ranging_measurement_data->MeasurementTimeUsec = 0;
signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6]));
/* peak_signal_count_rate_rtn_mcps */
p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]);
p_ranging_measurement_data->AmbientRateRtnMegaCps =
VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate);
effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3],
localBuffer[2]);
/* EffectiveSpadRtnCount is 8.8 format */
p_ranging_measurement_data->EffectiveSpadRtnCount =
effective_spad_rtn_count;
device_range_status = localBuffer[0];
/* Get Linearity Corrective Gain */
linearity_corrective_gain = _device->
LinearityCorrectiveGain;
/* Get ranging configuration */
range_fractional_enable = _device->
RangeFractionalEnable;
if (linearity_corrective_gain != 1000) {
tmpuint16 = (uint16_t)((linearity_corrective_gain
* tmpuint16 + 500) / 1000);
/* Implement Xtalk */
x_talk_compensation_rate_mega_cps = _device->CurrentParameters.XTalkCompensationRateMegaCps;
x_talk_compensation_enable = _device->CurrentParameters.XTalkCompensationEnable;
if (x_talk_compensation_enable) {
if ((signal_rate
- ((x_talk_compensation_rate_mega_cps
* effective_spad_rtn_count) >> 8))
<= 0) {
if (range_fractional_enable) {
xtalk_range_milli_meter = 8888;
} else {
xtalk_range_milli_meter = 8888 << 2;
}
} else {
xtalk_range_milli_meter =
(tmpuint16 * signal_rate)
/ (signal_rate
- ((x_talk_compensation_rate_mega_cps
* effective_spad_rtn_count)
>> 8));
}
tmpuint16 = xtalk_range_milli_meter;
}
}
if (range_fractional_enable) {
p_ranging_measurement_data->RangeMilliMeter =
(uint16_t)((tmpuint16) >> 2);
p_ranging_measurement_data->RangeFractionalPart =
(uint8_t)((tmpuint16 & 0x03) << 6);
} else {
p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
p_ranging_measurement_data->RangeFractionalPart = 0;
}
/*
* For a standard definition of RangeStatus,this should
* return 0 in case of good result after a ranging
* The range status depends on the device so call a device
* specific function to obtain the right Status.
*/
status |= VL53L0X_get_pal_range_status(device_range_status,
signal_rate,effective_spad_rtn_count,
p_ranging_measurement_data,&pal_range_status);
if (status == VL53L0X_ERROR_NONE) {
p_ranging_measurement_data->RangeStatus = pal_range_status;
}
}
if (status == VL53L0X_ERROR_NONE) {
/* Copy last read data into Dev buffer */
last_range_data_buffer = _device-> LastRangeMeasure;
last_range_data_buffer.RangeMilliMeter =
p_ranging_measurement_data->RangeMilliMeter;
last_range_data_buffer.RangeFractionalPart =
p_ranging_measurement_data->RangeFractionalPart;
last_range_data_buffer.RangeDMaxMilliMeter =
p_ranging_measurement_data->RangeDMaxMilliMeter;
last_range_data_buffer.MeasurementTimeUsec =
p_ranging_measurement_data->MeasurementTimeUsec;
last_range_data_buffer.SignalRateRtnMegaCps =
p_ranging_measurement_data->SignalRateRtnMegaCps;
last_range_data_buffer.AmbientRateRtnMegaCps =
p_ranging_measurement_data->AmbientRateRtnMegaCps;
last_range_data_buffer.EffectiveSpadRtnCount =
p_ranging_measurement_data->EffectiveSpadRtnCount;
last_range_data_buffer.RangeStatus =
p_ranging_measurement_data->RangeStatus;
_device->LastRangeMeasure = last_range_data_buffer;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
/* This function will do a complete single ranging
* Here we fix the mode! */
status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_perform_single_measurement();
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_clear_interrupt_mask(0);
}
return status;
}
VL53L0X_Error VL53L0X::perform_ref_signal_measurement(uint16_t *p_ref_signal_rate)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_RangingMeasurementData_t ranging_measurement_data;
uint8_t sequence_config = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
sequence_config = _device-> SequenceConfig;
/*
* This function performs a reference signal rate measurement.
*/
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0xC0);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x01);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
p_ref_signal_rate);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x00);
}
if (status == VL53L0X_ERROR_NONE) {
/* restore the previous Sequence Config */
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
sequence_config);
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = sequence_config;
}
}
return status;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count,
uint8_t *is_aperture_spads)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t last_spad_array[6];
uint8_t start_select = 0xB4;
uint32_t minimum_spad_count = 3;
uint32_t max_spad_count = 44;
uint32_t current_spad_index = 0;
uint32_t last_spad_index = 0;
int32_t next_good_spad = 0;
uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */
uint16_t peak_signal_rate_ref;
uint32_t need_apt_spads = 0;
uint32_t index = 0;
uint32_t spad_array_size = 6;
uint32_t signal_rate_diff = 0;
uint32_t last_signal_rate_diff = 0;
uint8_t complete = 0;
uint8_t vhv_settings = 0;
uint8_t phase_cal = 0;
uint32_t ref_spad_count_int = 0;
uint8_t is_aperture_spads_int = 0;
/*
* The reference SPAD initialization procedure determines the minimum
* amount of reference spads to be enables to achieve a target reference
* signal rate and should be performed once during initialization.
*
* Either aperture or non-aperture spads are applied but never both.
* Firstly non-aperture spads are set,begining with 5 spads,and
* increased one spad at a time until the closest measurement to the
* target rate is achieved.
*
* If the target rate is exceeded when 5 non-aperture spads are enabled,
* initialization is performed instead with aperture spads.
*
* When setting spads,a 'Good Spad Map' is applied.
*
* This procedure operates within a SPAD window of interest of a maximum
* 44 spads.
* The start point is currently fixed to 180,which lies towards the end
* of the non-aperture quadrant and runs in to the adjacent aperture
* quadrant.
*/
target_ref_rate = _device-> targetRefRate;
/*
* Initialize Spad arrays.
* Currently the good spad map is initialised to 'All good'.
* This is a short term implementation. The good spad map will be
* provided as an input.
* Note that there are 6 bytes. Only the first 44 bits will be used to
* represent spads.
*/
for (index = 0; index < spad_array_size; index++) {
_device->SpadData.RefSpadEnables[index] = 0;
}
status = VL53L0X_write_byte(0xFF,0x01);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x00);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
start_select);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0);
}
/* Perform ref calibration */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_perform_ref_calibration(&vhv_settings,
&phase_cal,0);
}
if (status == VL53L0X_ERROR_NONE) {
/* Enable Minimum NON-APERTURE Spads */
current_spad_index = 0;
last_spad_index = current_spad_index;
need_apt_spads = 0;
status = enable_ref_spads(need_apt_spads,
_device->SpadData.RefGoodSpadMap,
_device->SpadData.RefSpadEnables,
spad_array_size,
start_select,
current_spad_index,
minimum_spad_count,
&last_spad_index);
}
if (status == VL53L0X_ERROR_NONE) {
current_spad_index = last_spad_index;
status = perform_ref_signal_measurement(&peak_signal_rate_ref);
if ((status == VL53L0X_ERROR_NONE) &&
(peak_signal_rate_ref > target_ref_rate)) {
/* Signal rate measurement too high,
* switch to APERTURE SPADs */
for (index = 0; index < spad_array_size; index++) {
_device->SpadData.RefSpadEnables[index] = 0;
}
/* Increment to the first APERTURE spad */
while ((is_aperture(start_select + current_spad_index)
== 0) && (current_spad_index < max_spad_count)) {
current_spad_index++;
}
need_apt_spads = 1;
status = enable_ref_spads(need_apt_spads,
_device->SpadData.RefGoodSpadMap,
_device->SpadData.RefSpadEnables,
spad_array_size,
start_select,
current_spad_index,
minimum_spad_count,
&last_spad_index);
if (status == VL53L0X_ERROR_NONE) {
current_spad_index = last_spad_index;
status = perform_ref_signal_measurement(&peak_signal_rate_ref);
if ((status == VL53L0X_ERROR_NONE) &&
(peak_signal_rate_ref > target_ref_rate)) {
/* Signal rate still too high after
* setting the minimum number of
* APERTURE spads. Can do no more
* therefore set the min number of
* aperture spads as the result.
*/
is_aperture_spads_int = 1;
ref_spad_count_int = minimum_spad_count;
}
}
} else {
need_apt_spads = 0;
}
}
if ((status == VL53L0X_ERROR_NONE) &&
(peak_signal_rate_ref < target_ref_rate)) {
/* At this point,the minimum number of either aperture
* or non-aperture spads have been set. Proceed to add
* spads and perform measurements until the target
* reference is reached.
*/
is_aperture_spads_int = need_apt_spads;
ref_spad_count_int = minimum_spad_count;
memcpy(last_spad_array,_device->SpadData.RefSpadEnables,
spad_array_size);
last_signal_rate_diff = abs(peak_signal_rate_ref -
target_ref_rate);
complete = 0;
while (!complete) {
get_next_good_spad(_device->SpadData.RefGoodSpadMap,
spad_array_size,current_spad_index,
&next_good_spad);
if (next_good_spad == -1) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
/* Cannot combine Aperture and Non-Aperture spads,so
* ensure the current spad is of the correct type.
*/
if (is_aperture((uint32_t)start_select + next_good_spad) !=
need_apt_spads) {
/* At this point we have enabled the maximum
* number of Aperture spads.
*/
complete = 1;
break;
}
(ref_spad_count_int)++;
current_spad_index = next_good_spad;
status = enable_spad_bit(_device->SpadData.RefSpadEnables,
spad_array_size,current_spad_index);
if (status == VL53L0X_ERROR_NONE) {
current_spad_index++;
/* Proceed to apply the additional spad and
* perform measurement. */
status = set_ref_spad_map(_device->SpadData.RefSpadEnables);
}
if (status != VL53L0X_ERROR_NONE) {
break;
}
status = perform_ref_signal_measurement(&peak_signal_rate_ref);
if (status != VL53L0X_ERROR_NONE) {
break;
}
signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate);
if (peak_signal_rate_ref > target_ref_rate) {
/* Select the spad map that provides the
* measurement closest to the target rate,
* either above or below it.
*/
if (signal_rate_diff > last_signal_rate_diff) {
/* Previous spad map produced a closer
* measurement,so choose this. */
status = set_ref_spad_map(last_spad_array);
memcpy(_device->SpadData.RefSpadEnables,
last_spad_array,spad_array_size);
(ref_spad_count_int)--;
}
complete = 1;
} else {
/* Continue to add spads */
last_signal_rate_diff = signal_rate_diff;
memcpy(last_spad_array,
_device->SpadData.RefSpadEnables,
spad_array_size);
}
} /* while */
}
if (status == VL53L0X_ERROR_NONE) {
*ref_spad_count = ref_spad_count_int;
*is_aperture_spads = is_aperture_spads_int;
_device->DeviceSpecificParameters.RefSpadsInitialised = 1;
_device->DeviceSpecificParameters.ReferenceSpadCount = (uint8_t)(*ref_spad_count);
_device->DeviceSpecificParameters.ReferenceSpadType = *is_aperture_spads;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(uint32_t count,uint8_t is_aperture_spads)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t current_spad_index = 0;
uint8_t start_select = 0xB4;
uint32_t spad_array_size = 6;
uint32_t max_spad_count = 44;
uint32_t last_spad_index;
uint32_t index;
/*
* This function applies a requested number of reference spads,either
* aperture or
* non-aperture,as requested.
* The good spad map will be applied.
*/
status = VL53L0X_write_byte(0xFF,0x01);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x00);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
start_select);
}
for (index = 0; index < spad_array_size; index++) {
_device->SpadData.RefSpadEnables[index] = 0;
}
if (is_aperture_spads) {
/* Increment to the first APERTURE spad */
while ((is_aperture(start_select + current_spad_index) == 0) &&
(current_spad_index < max_spad_count)) {
current_spad_index++;
}
}
status = enable_ref_spads(is_aperture_spads,
_device->SpadData.RefGoodSpadMap,
_device->SpadData.RefSpadEnables,
spad_array_size,
start_select,
current_spad_index,
count,
&last_spad_index);
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.RefSpadsInitialised = 1;
_device->DeviceSpecificParameters.ReferenceSpadCount = (uint8_t)(count);
_device->DeviceSpecificParameters.ReferenceSpadType = is_aperture_spads;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_wait_device_booted()
{
VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
/* not implemented on VL53L0X */
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t *p_vhv_settings,
uint8_t *p_phase_cal)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = VL53L0X_perform_ref_calibration(p_vhv_settings,
p_phase_cal,1);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count,uint8_t *is_aperture_spads)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_perform_ref_spad_management(ref_spad_count,
is_aperture_spads);
return status;
}
/* Group PAL Init Functions */
VL53L0X_Error VL53L0X::VL53L0X_set_device_address(uint8_t device_address)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = VL53L0X_write_byte(VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
device_address / 2);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config(uint8_t pin,
VL53L0X_DeviceModes device_mode,VL53L0X_GpioFunctionality functionality,
VL53L0X_InterruptPolarity polarity)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t data;
if (pin != 0) {
status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
} else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
data = 0x10;
} else {
data = 1;
}
status = VL53L0X_write_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,data);
} else {
if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) {
status |= VL53L0X_write_byte(0xff,0x01);
status |= VL53L0X_write_byte(0x00,0x00);
status |= VL53L0X_write_byte(0xff,0x00);
status |= VL53L0X_write_byte(0x80,0x01);
status |= VL53L0X_write_byte(0x85,0x02);
status |= VL53L0X_write_byte(0xff,0x04);
status |= VL53L0X_write_byte(0xcd,0x00);
status |= VL53L0X_write_byte(0xcc,0x11);
status |= VL53L0X_write_byte(0xff,0x07);
status |= VL53L0X_write_byte(0xbe,0x00);
status |= VL53L0X_write_byte(0xff,0x06);
status |= VL53L0X_write_byte(0xcc,0x09);
status |= VL53L0X_write_byte(0xff,0x00);
status |= VL53L0X_write_byte(0xff,0x01);
status |= VL53L0X_write_byte(0x00,0x00);
} else {
if (status == VL53L0X_ERROR_NONE) {
switch (functionality) {
case VL53L0X_GPIOFUNCTIONALITY_OFF:
data = 0x00;
break;
case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
data = 0x01;
break;
case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
data = 0x02;
break;
case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
data = 0x03;
break;
case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
data = 0x04;
break;
default:
status =
VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
}
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO,data);
}
if (status == VL53L0X_ERROR_NONE) {
if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
data = 0;
} else {
data = (uint8_t)(1 << 4);
}
status = VL53L0X_update_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,data);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.Pin0GpioFunctionality = functionality;
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_clear_interrupt_mask(0);
}
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable(uint8_t *p_enabled)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_RANGE_CONFIG,p_enabled);
if (status == VL53L0X_ERROR_NONE) {
*p_enabled = (*p_enabled & 1);
}
return status;
}
uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
{
/*!
* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
*/
uint16_t encoded_timeout = 0;
uint32_t ls_byte = 0;
uint16_t ms_byte = 0;
if (timeout_macro_clks > 0) {
ls_byte = timeout_macro_clks - 1;
while ((ls_byte & 0xFFFFFF00) > 0) {
ls_byte = ls_byte >> 1;
ms_byte++;
}
encoded_timeout = (ms_byte << 8)
+ (uint16_t)(ls_byte & 0x000000FF);
}
return encoded_timeout;
}
VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id,
uint32_t timeout_micro_secs)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t current_vcsel_pulse_period_p_clk;
uint8_t msrc_encoded_time_out;
uint16_t pre_range_encoded_time_out;
uint16_t pre_range_time_out_m_clks;
uint16_t msrc_range_time_out_m_clks;
uint32_t final_range_time_out_m_clks;
uint16_t final_range_encoded_time_out;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
if (status == VL53L0X_ERROR_NONE) {
msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(timeout_micro_secs,
(uint8_t)current_vcsel_pulse_period_p_clk);
if (msrc_range_time_out_m_clks > 256) {
msrc_encoded_time_out = 255;
} else {
msrc_encoded_time_out =
(uint8_t)msrc_range_time_out_m_clks - 1;
}
_device->DeviceSpecificParameters.LastEncodedTimeout = msrc_encoded_time_out;
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
msrc_encoded_time_out);
}
} else {
if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
pre_range_time_out_m_clks =
VL53L0X_calc_timeout_mclks(timeout_micro_secs,
(uint8_t)current_vcsel_pulse_period_p_clk);
pre_range_encoded_time_out = VL53L0X_encode_timeout(pre_range_time_out_m_clks);
_device->DeviceSpecificParameters.LastEncodedTimeout = pre_range_encoded_time_out;
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
pre_range_encoded_time_out);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.PreRangeTimeoutMicroSecs=timeout_micro_secs;
}
} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
/* For the final range timeout,the pre-range timeout
* must be added. To do this both final and pre-range
* timeouts must be expressed in macro periods MClks
* because they have different vcsel periods.
*/
VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
pre_range_time_out_m_clks = 0;
if (scheduler_sequence_steps.PreRangeOn) {
/* Retrieve PRE-RANGE VCSEL Period */
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
¤t_vcsel_pulse_period_p_clk);
/* Retrieve PRE-RANGE Timeout in Macro periods
* (MCLKS) */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(0x51,
&pre_range_encoded_time_out);
pre_range_time_out_m_clks =
VL53L0X_decode_timeout(pre_range_encoded_time_out);
}
}
/* Calculate FINAL RANGE Timeout in Macro Periods
* (MCLKS) and add PRE-RANGE value
*/
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
¤t_vcsel_pulse_period_p_clk);
}
if (status == VL53L0X_ERROR_NONE) {
final_range_time_out_m_clks =
VL53L0X_calc_timeout_mclks(timeout_micro_secs,
(uint8_t) current_vcsel_pulse_period_p_clk);
final_range_time_out_m_clks += pre_range_time_out_m_clks;
final_range_encoded_time_out =
VL53L0X_encode_timeout(final_range_time_out_m_clks);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_word(0x71,
final_range_encoded_time_out);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.FinalRangeTimeoutMicroSecs = timeout_micro_secs;
}
}
} else {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
return status;
}
VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t final_range_timing_budget_micro_seconds;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
uint32_t start_overhead_micro_seconds = 1910;
uint32_t end_overhead_micro_seconds = 960;
uint32_t msrc_overhead_micro_seconds = 660;
uint32_t tcc_overhead_micro_seconds = 590;
uint32_t dss_overhead_micro_seconds = 690;
uint32_t pre_range_overhead_micro_seconds = 660;
uint32_t final_range_overhead_micro_seconds = 550;
uint32_t pre_range_timeout_micro_seconds = 0;
uint32_t c_min_timing_budget_micro_seconds = 20000;
uint32_t sub_timeout = 0;
if (measurement_timing_budget_micro_seconds
< c_min_timing_budget_micro_seconds) {
status = VL53L0X_ERROR_INVALID_PARAMS;
return status;
}
final_range_timing_budget_micro_seconds =
measurement_timing_budget_micro_seconds -
(start_overhead_micro_seconds + end_overhead_micro_seconds);
status = VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
if (status == VL53L0X_ERROR_NONE &&
(scheduler_sequence_steps.TccOn ||
scheduler_sequence_steps.MsrcOn ||
scheduler_sequence_steps.DssOn)) {
/* TCC,MSRC and DSS all share the same timeout */
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
&msrc_dcc_tcc_timeout_micro_seconds);
/* Subtract the TCC,MSRC and DSS timeouts if they are
* enabled. */
if (status != VL53L0X_ERROR_NONE) {
return status;
}
/* TCC */
if (scheduler_sequence_steps.TccOn) {
sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
+ tcc_overhead_micro_seconds;
if (sub_timeout <
final_range_timing_budget_micro_seconds) {
final_range_timing_budget_micro_seconds -=
sub_timeout;
} else {
/* Requested timeout too big. */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (status != VL53L0X_ERROR_NONE) {
return status;
}
/* DSS */
if (scheduler_sequence_steps.DssOn) {
sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds +
dss_overhead_micro_seconds);
if (sub_timeout < final_range_timing_budget_micro_seconds) {
final_range_timing_budget_micro_seconds
-= sub_timeout;
} else {
/* Requested timeout too big. */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
} else if (scheduler_sequence_steps.MsrcOn) {
/* MSRC */
sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
msrc_overhead_micro_seconds;
if (sub_timeout < final_range_timing_budget_micro_seconds) {
final_range_timing_budget_micro_seconds
-= sub_timeout;
} else {
/* Requested timeout too big. */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
}
if (status != VL53L0X_ERROR_NONE) {
return status;
}
if (scheduler_sequence_steps.PreRangeOn) {
/* Subtract the Pre-range timeout if enabled. */
status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
&pre_range_timeout_micro_seconds);
sub_timeout = pre_range_timeout_micro_seconds +
pre_range_overhead_micro_seconds;
if (sub_timeout < final_range_timing_budget_micro_seconds) {
final_range_timing_budget_micro_seconds -= sub_timeout;
} else {
/* Requested timeout too big. */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (status == VL53L0X_ERROR_NONE &&
scheduler_sequence_steps.FinalRangeOn) {
final_range_timing_budget_micro_seconds -=
final_range_overhead_micro_seconds;
/* Final Range Timeout
* Note that the final range timeout is determined by the timing
* budget and the sum of all other timeouts within the sequence.
* If there is no room for the final range timeout,then an error
* will be set. Otherwise the remaining time will be applied to
* the final range.
*/
status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
final_range_timing_budget_micro_seconds);
_device->CurrentParameters.MeasurementTimingBudgetMicroSeconds = measurement_timing_budget_micro_seconds;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id,uint8_t sequence_step_enabled)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
uint8_t sequence_config_new = 0;
uint32_t measurement_timing_budget_micro_seconds;
status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
&sequence_config);
sequence_config_new = sequence_config;
if (status == VL53L0X_ERROR_NONE) {
if (sequence_step_enabled == 1) {
/* Enable requested sequence step
*/
switch (sequence_step_id) {
case VL53L0X_SEQUENCESTEP_TCC:
sequence_config_new |= 0x10;
break;
case VL53L0X_SEQUENCESTEP_DSS:
sequence_config_new |= 0x28;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
sequence_config_new |= 0x04;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sequence_config_new |= 0x40;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sequence_config_new |= 0x80;
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
} else {
/* Disable requested sequence step
*/
switch (sequence_step_id) {
case VL53L0X_SEQUENCESTEP_TCC:
sequence_config_new &= 0xef;
break;
case VL53L0X_SEQUENCESTEP_DSS:
sequence_config_new &= 0xd7;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
sequence_config_new &= 0xfb;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sequence_config_new &= 0xbf;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sequence_config_new &= 0x7f;
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
}
if (sequence_config_new != sequence_config) {
/* Apply New Setting */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,sequence_config_new);
}
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = sequence_config_new;
}
/* Recalculate timing budget */
if (status == VL53L0X_ERROR_NONE) {
measurement_timing_budget_micro_seconds = _device->CurrentParameters.MeasurementTimingBudgetMicroSeconds;
VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable(uint16_t limit_check_id,
uint8_t limit_check_enable)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
FixPoint1616_t temp_fix1616 = 0;
uint8_t limit_check_enable_int = 0;
uint8_t limit_check_disable = 0;
uint8_t temp8;
if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
status = VL53L0X_ERROR_INVALID_PARAMS;
} else {
if (limit_check_enable == 0) {
temp_fix1616 = 0;
limit_check_enable_int = 0;
limit_check_disable = 1;
} else {
temp_fix1616 = _device->CurrentParameters.LimitChecksValue[limit_check_id];
limit_check_disable = 0;
/* this to be sure to have either 0 or 1 */
limit_check_enable_int = 1;
}
switch (limit_check_id) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
_device->CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_enable_int;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616));
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
_device->CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_enable_int;
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
_device->CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_enable_int;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
temp8 = (uint8_t)(limit_check_disable << 1);
status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL,
0xFE,temp8);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
temp8 = (uint8_t)(limit_check_disable << 4);
status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL,
0xEF,temp8);
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (status == VL53L0X_ERROR_NONE) {
if (limit_check_enable == 0) {
_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
} else {
_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 1;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_static_init()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceParameters_t current_parameters = {0};
uint8_t *p_tuning_setting_buffer;
uint16_t tempword = 0;
uint8_t tempbyte = 0;
uint8_t use_internal_tuning_settings = 0;
uint32_t count = 0;
uint8_t is_aperture_spads = 0;
uint32_t ref_spad_count = 0;
uint8_t aperture_spads = 0;
uint8_t vcsel_pulse_period_pclk;
uint32_t seq_timeout_micro_secs;
status = VL53L0X_get_info_from_device(1);
/* set the ref spad from NVM */
count = (uint32_t)_device->DeviceSpecificParameters.ReferenceSpadCount;
aperture_spads = _device->DeviceSpecificParameters.ReferenceSpadType;
/* NVM value invalid */
if ((aperture_spads > 1) ||
((aperture_spads == 1) && (count > 32)) ||
((aperture_spads == 0) && (count > 12))) {
status = wrapped_VL53L0X_perform_ref_spad_management(&ref_spad_count,
&is_aperture_spads);
} else {
status = VL53L0X_set_reference_spads(count,aperture_spads);
}
/* Initialize tuning settings buffer to prevent compiler warning. */
p_tuning_setting_buffer = DefaultTuningSettings;
if (status == VL53L0X_ERROR_NONE) {
use_internal_tuning_settings = _device->UseInternalTuningSettings;
if (use_internal_tuning_settings == 0) {
p_tuning_setting_buffer = _device->pTuningSettingsPointer;
} else {
p_tuning_setting_buffer = DefaultTuningSettings;
}
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_load_tuning_settings(p_tuning_setting_buffer);
}
/* Set interrupt config to new sample ready */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_gpio_config(0,0,
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
VL53L0X_INTERRUPTPOLARITY_LOW);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x01);
status |= VL53L0X_read_word(0x84,&tempword);
status |= VL53L0X_write_byte(0xFF,0x00);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.OscFrequencyMHz=
VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword);
}
/* After static init,some device parameters may be changed,
* so update them */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_device_parameters(¤t_parameters);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_fraction_enable(&tempbyte);
if (status == VL53L0X_ERROR_NONE) {
_device->RangeFractionalEnable = tempbyte;
}
}
if (status == VL53L0X_ERROR_NONE) {
_device->CurrentParameters = current_parameters;
}
/* read the sequence config and save it */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,&tempbyte);
if (status == VL53L0X_ERROR_NONE) {
_device->SequenceConfig = tempbyte;
}
}
/* Disable MSRC and TCC by default */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0);
}
/* Set PAL State to standby */
if (status == VL53L0X_ERROR_NONE) {
_device->PalState = VL53L0X_STATE_IDLE;
}
/* Store pre-range vcsel period */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_vcsel_pulse_period(
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&vcsel_pulse_period_pclk);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
}
/* Store final-range vcsel period */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_get_vcsel_pulse_period(
VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
&vcsel_pulse_period_pclk);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
}
/* Store pre-range timeout */
if (status == VL53L0X_ERROR_NONE) {
status = get_sequence_step_timeout(
VL53L0X_SEQUENCESTEP_PRE_RANGE,
&seq_timeout_micro_secs);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.PreRangeTimeoutMicroSecs = seq_timeout_micro_secs;
}
/* Store final-range timeout */
if (status == VL53L0X_ERROR_NONE) {
status = get_sequence_step_timeout(
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&seq_timeout_micro_secs);
}
if (status == VL53L0X_ERROR_NONE) {
_device->DeviceSpecificParameters.FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs;
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_stop_measurement()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
status = VL53L0X_write_byte(0xFF,0x01);
status = VL53L0X_write_byte(0x00,0x00);
status = VL53L0X_write_byte(0x91,0x00);
status = VL53L0X_write_byte(0x00,0x01);
status = VL53L0X_write_byte(0xFF,0x00);
if (status == VL53L0X_ERROR_NONE) {
/* Set PAL State to Idle */
_device->PalState = VL53L0X_STATE_IDLE;
}
/* Check if need to apply interrupt settings */
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_check_and_load_interrupt_settings(0);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(uint32_t *p_stop_status)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t byte = 0;
status = VL53L0X_write_byte(0xFF,0x01);
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_byte(0x04,&byte);
}
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_write_byte(0xFF,0x0);
}
*p_stop_status = byte;
if (byte == 0) {
status = VL53L0X_write_byte(0x80,0x01);
status = VL53L0X_write_byte(0xFF,0x01);
status = VL53L0X_write_byte(0x00,0x00);
status = VL53L0X_write_byte(0x91,_device->StopVariable);
status = VL53L0X_write_byte(0x00,0x01);
status = VL53L0X_write_byte(0xFF,0x00);
status = VL53L0X_write_byte(0x80,0x00);
}
return status;
}
/****************** Write and read functions from I2C *************************/
VL53L0X_Error VL53L0X::VL53L0X_write_multi(uint8_t index,uint8_t *p_data,uint32_t count)
{
int status;
status = VL53L0X_i2c_write(index,p_data,(uint16_t)count);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_read_multi(uint8_t index,uint8_t *p_data,uint32_t count)
{
int status;
if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
status = VL53L0X_i2c_read(index,p_data,(uint16_t)count);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_write_byte(uint8_t index,uint8_t data)
{
int status;
status = VL53L0X_i2c_write(index,&data,1);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_write_word(uint8_t index,uint16_t data)
{
int status;
uint8_t buffer[2];
buffer[0] = data >> 8;
buffer[1] = data & 0x00FF;
status = VL53L0X_i2c_write(index,(uint8_t *)buffer,2);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_write_dword(uint8_t index,uint32_t data)
{
int status;
uint8_t buffer[4];
buffer[0] = (data >> 24) & 0xFF;
buffer[1] = (data >> 16) & 0xFF;
buffer[2] = (data >> 8) & 0xFF;
buffer[3] = (data >> 0) & 0xFF;
status = VL53L0X_i2c_write(index,(uint8_t *)buffer,4);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_read_byte(uint8_t index,uint8_t *p_data)
{
int status;
status = VL53L0X_i2c_read(index,p_data,1);
if (status) {
return -1;
}
return 0;
}
VL53L0X_Error VL53L0X::VL53L0X_read_word(uint8_t index,uint16_t *p_data)
{
int status;
uint8_t buffer[2] = {0,0};
status = VL53L0X_i2c_read(index,buffer,2);
if (!status) {
*p_data = (buffer[0] << 8) + buffer[1];
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_read_dword(uint8_t index,uint32_t *p_data)
{
int status;
uint8_t buffer[4] = {0,0,0,0};
status = VL53L0X_i2c_read(index,buffer,4);
if (!status) {
*p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_update_byte(uint8_t index,uint8_t and_data,uint8_t or_data)
{
int status;
uint8_t buffer = 0;
/* read data direct onto buffer */
status = VL53L0X_i2c_read(index,&buffer,1);
if (!status) {
buffer = (buffer & and_data) | or_data;
status = VL53L0X_i2c_write(index,&buffer,(uint8_t)1);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t RegisterAddr,uint8_t *p_data,
uint16_t NumByteToWrite)
{
int ret;
ret = _dev_i2c->i2c_write(p_data,_device->I2cDevAddr,RegisterAddr,NumByteToWrite);
if (ret) {
return -1;
}
return 0;
}
VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t RegisterAddr,uint8_t *p_data,
uint16_t NumByteToRead)
{
int ret;
ret = _dev_i2c->i2c_read(p_data,_device->I2cDevAddr,RegisterAddr,NumByteToRead);
if (ret) {
return -1;
}
return 0;
}
int VL53L0X::read_id(uint8_t *id)
{
int status = 0;
uint16_t rl_id = 0;
status = VL53L0X_read_word(VL53L0X_REG_IDENTIFICATION_MODEL_ID,&rl_id);
if (rl_id == 0xEEAA) {
return status;
}
return -1;
}
VL53L0X_Error VL53L0X::wait_measurement_data_ready()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t new_dat_ready = 0;
uint32_t loop_nb;
// Wait until it finished
// use timeout to avoid deadlock
if (status == VL53L0X_ERROR_NONE) {
loop_nb = 0;
do {
status = VL53L0X_get_measurement_data_ready(&new_dat_ready);
if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) {
break;
}
loop_nb = loop_nb + 1;
VL53L0X_polling_delay();
} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
status = VL53L0X_ERROR_TIME_OUT;
}
}
return status;
}
VL53L0X_Error VL53L0X::wait_stop_completed()
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t stop_completed = 0;
uint32_t loop_nb;
// Wait until it finished
// use timeout to avoid deadlock
if (status == VL53L0X_ERROR_NONE) {
loop_nb = 0;
do {
status = VL53L0X_get_stop_completed_status(&stop_completed);
if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) {
break;
}
loop_nb = loop_nb + 1;
VL53L0X_polling_delay();
} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
status = VL53L0X_ERROR_TIME_OUT;
}
}
return status;
}
int VL53L0X::init_sensor(uint8_t new_addr)
{
int status;
VL53L0X_off();
VL53L0X_on();
// status=VL53L0X_WaitDeviceBooted(ice);
// if(status)
// printf("WaitDeviceBooted fail\n\r");
status = is_present();
if (!status) {
status = init(&_my_device);
if (status != VL53L0X_ERROR_NONE) {
printf("Failed to init VL53L0X sensor!\n\r");
return status;
}
// deduce silicon version
status = VL53L0X_get_device_info(&_device_info);
status = prepare();
if (status != VL53L0X_ERROR_NONE) {
printf("Failed to prepare VL53L0X!\n\r");
return status;
}
if (new_addr != VL53L0X_DEFAULT_ADDRESS) {
status = set_device_address(new_addr);
if (status) {
printf("Failed to change I2C address!\n\r");
return status;
}
} else {
printf("Invalid new address!\n\r");
return VL53L0X_ERROR_INVALID_PARAMS;
}
}
return status;
}
int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void))
{
int status;
status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped
status = VL53L0X_set_gpio_config(0,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
VL53L0X_INTERRUPTPOLARITY_HIGH);
if (!status) {
attach_interrupt_measure_detection_irq(fptr);
enable_interrupt_measure_detection_irq();
}
clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS);
// NB: return value was previously only passed to logging macro,but did not get passed back
if (!status) {
status = range_start_continuous_mode();
}
return status;
}
int VL53L0X::start_measurement(OperatingMode operating_mode,void (*fptr)(void))
{
int Status = VL53L0X_ERROR_NONE;
uint8_t VhvSettings;
uint8_t PhaseCal;
// *** from mass market cube expansion v1.1,ranging with satellites.
// default settings,for normal range.
FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536);
FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536);
uint32_t timingBudget = 33000;
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
if (operating_mode == range_continuous_interrupt) {
if (_gpio1Int == NULL) {
printf("GPIO1 Error\r\n");
return 1;
}
Status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped
Status = VL53L0X_set_gpio_config(0,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
VL53L0X_INTERRUPTPOLARITY_HIGH);
if (Status == VL53L0X_ERROR_NONE) {
attach_interrupt_measure_detection_irq(fptr);
enable_interrupt_measure_detection_irq();
}
clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS);
// NB: return value was previously only passed to logging macro,but did not get passed back
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_start_measurement();
}
}
if (operating_mode == range_single_shot_polling) {
// singelshot,polled ranging
if (Status == VL53L0X_ERROR_NONE) {
// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
}
// Enable/Disable Sigma and Signal check
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_limit_check_enable(
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,1);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_limit_check_enable(
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,1);
}
// *** from mass market cube expansion v1.1,ranging with satellites.
/* Ranging configuration */
//*
// switch(rangingConfig) {
// case LONG_RANGE:
signalLimit = (FixPoint1616_t)(0.1 * 65536);
sigmaLimit = (FixPoint1616_t)(60 * 65536);
timingBudget = 33000;
preRangeVcselPeriod = 18;
finalRangeVcselPeriod = 14;
/* break;
case HIGH_ACCURACY:
signalLimit = (FixPoint1616_t)(0.25*65536);
sigmaLimit = (FixPoint1616_t)(18*65536);
timingBudget = 200000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
case HIGH_SPEED:
signalLimit = (FixPoint1616_t)(0.25*65536);
sigmaLimit = (FixPoint1616_t)(32*65536);
timingBudget = 20000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
default:
debug_printf("Not Supported");
}
*/
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_limit_check_value(
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,signalLimit);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_limit_check_value(
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,sigmaLimit);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_measurement_timing_budget_micro_seconds(timingBudget);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_vcsel_pulse_period(
VL53L0X_VCSEL_PERIOD_PRE_RANGE,preRangeVcselPeriod);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_set_vcsel_pulse_period(
VL53L0X_VCSEL_PERIOD_FINAL_RANGE,finalRangeVcselPeriod);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_perform_ref_calibration(&VhvSettings,&PhaseCal);
}
}
if (operating_mode == range_continuous_polling) {
if (Status == VL53L0X_ERROR_NONE) {
//printf("Call of VL53L0X_SetDeviceMode\n");
Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
}
if (Status == VL53L0X_ERROR_NONE) {
//printf("Call of VL53L0X_StartMeasurement\n");
Status = VL53L0X_start_measurement();
}
}
return Status;
}
int VL53L0X::get_measurement(OperatingMode operating_mode,VL53L0X_RangingMeasurementData_t *p_data)
{
int Status = VL53L0X_ERROR_NONE;
if (operating_mode == range_single_shot_polling) {
Status = VL53L0X_perform_single_ranging_measurement(p_data);
}
if (operating_mode == range_continuous_polling) {
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_measurement_poll_for_completion();
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_get_ranging_measurement_data(p_data);
// Clear the interrupt
VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
VL53L0X_polling_delay();
}
}
if (operating_mode == range_continuous_interrupt) {
Status = VL53L0X_get_ranging_measurement_data(p_data);
VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS);
}
return Status;
}
int VL53L0X::stop_measurement(OperatingMode operating_mode)
{
int status = VL53L0X_ERROR_NONE;
// don't need to stop for a singleshot range!
if (operating_mode == range_single_shot_polling) {
}
if (operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling) {
// continuous mode
if (status == VL53L0X_ERROR_NONE) {
//printf("Call of VL53L0X_StopMeasurement\n");
status = VL53L0X_stop_measurement();
}
if (status == VL53L0X_ERROR_NONE) {
//printf("Wait Stop to be competed\n");
status = wait_stop_completed();
}
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_clear_interrupt_mask(
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
}
return status;
}
int VL53L0X::handle_irq(OperatingMode operating_mode,VL53L0X_RangingMeasurementData_t *data)
{
int status;
status = get_measurement(operating_mode,data);
enable_interrupt_measure_detection_irq();
return status;
}
/******************************************************************************/