ST Expansion SW Team / VL53L3CX_mbed

Rename library

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging

modules/vl53lx_api.c@7:7f1bbf370283, 2021-07-21 (annotated)

Committer:
charlesmn
Date:
Wed Jul 21 14:07:59 2021 +0000
Revision:
7:7f1bbf370283
Parent:
5:89031b2f5316 
Moved vl53l3cx_class.cpp and .h to 53l3a2_RangingClass

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 5:89031b2f5316 1
Charles MacNeill 5:89031b2f5316 2 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
Charles MacNeill 5:89031b2f5316 3 /******************************************************************************
Charles MacNeill 5:89031b2f5316 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 5:89031b2f5316 5
Charles MacNeill 5:89031b2f5316 6 This file is part of VL53LX and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either GPL-2.0+
Charles MacNeill 5:89031b2f5316 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 5:89031b2f5316 9 ******************************************************************************
Charles MacNeill 5:89031b2f5316 10 */
Charles MacNeill 5:89031b2f5316 11
Charles MacNeill 5:89031b2f5316 12
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14
Charles MacNeill 5:89031b2f5316 15
Charles MacNeill 5:89031b2f5316 16
Charles MacNeill 5:89031b2f5316 17 #include "vl53lx_api.h"
Charles MacNeill 5:89031b2f5316 18 #include "vl53lx_register_settings.h"
Charles MacNeill 5:89031b2f5316 19 #include "vl53lx_register_funcs.h"
Charles MacNeill 5:89031b2f5316 20 #include "vl53lx_core.h"
Charles MacNeill 5:89031b2f5316 21 #include "vl53lx_api_calibration.h"
Charles MacNeill 5:89031b2f5316 22 #include "vl53lx_wait.h"
Charles MacNeill 5:89031b2f5316 23 #include "vl53lx_preset_setup.h"
Charles MacNeill 5:89031b2f5316 24 #include "vl53lx_api_debug.h"
Charles MacNeill 5:89031b2f5316 25 #include "vl53lx_api_core.h"
Charles MacNeill 5:89031b2f5316 26 #include "vl53lx_nvm.h"
Charles MacNeill 5:89031b2f5316 27 #include "spi_interface.h"
Charles MacNeill 5:89031b2f5316 28
Charles MacNeill 5:89031b2f5316 29
Charles MacNeill 5:89031b2f5316 30
Charles MacNeill 5:89031b2f5316 31 #define ZONE_CHECK 5
Charles MacNeill 5:89031b2f5316 32
Charles MacNeill 5:89031b2f5316 33 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 34 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_API, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 35 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 36 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_API, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 37 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 38 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_API, status, \
Charles MacNeill 5:89031b2f5316 39 fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 40
Charles MacNeill 5:89031b2f5316 41 #ifdef VL53LX_LOG_ENABLE
Charles MacNeill 5:89031b2f5316 42 #define trace_print(level, ...) trace_print_module_function(\
Charles MacNeill 5:89031b2f5316 43 VL53LX_TRACE_MODULE_API, level, VL53LX_TRACE_FUNCTION_NONE, \
Charles MacNeill 5:89031b2f5316 44 ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 45 #endif
Charles MacNeill 5:89031b2f5316 46
Charles MacNeill 5:89031b2f5316 47 #ifndef MIN
Charles MacNeill 5:89031b2f5316 48 #define MIN(v1, v2) ((v1) < (v2) ? (v1) : (v2))
Charles MacNeill 5:89031b2f5316 49 #endif
Charles MacNeill 5:89031b2f5316 50 #ifndef MAX
Charles MacNeill 5:89031b2f5316 51 #define MAX(v1, v2) ((v1) < (v2) ? (v2) : (v1))
Charles MacNeill 5:89031b2f5316 52 #endif
Charles MacNeill 5:89031b2f5316 53
Charles MacNeill 5:89031b2f5316 54 #define DMAX_REFLECTANCE_IDX 2
Charles MacNeill 5:89031b2f5316 55
Charles MacNeill 5:89031b2f5316 56
Charles MacNeill 5:89031b2f5316 57
Charles MacNeill 5:89031b2f5316 58 #define LOWPOWER_AUTO_VHV_LOOP_DURATION_US 245
Charles MacNeill 5:89031b2f5316 59 #define LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING 1448
Charles MacNeill 5:89031b2f5316 60 #define LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING 2100
Charles MacNeill 5:89031b2f5316 61
Charles MacNeill 5:89031b2f5316 62 #define FDA_MAX_TIMING_BUDGET_US 550000
Charles MacNeill 5:89031b2f5316 63 #define L4_FDA_MAX_TIMING_BUDGET_US 200000
Charles MacNeill 5:89031b2f5316 64
Charles MacNeill 5:89031b2f5316 65
Charles MacNeill 5:89031b2f5316 66
Charles MacNeill 5:89031b2f5316 67
Charles MacNeill 5:89031b2f5316 68 static int32_t BDTable[VL53LX_TUNING_MAX_TUNABLE_KEY] = {
Charles MacNeill 5:89031b2f5316 69 TUNING_VERSION,
Charles MacNeill 5:89031b2f5316 70 TUNING_PROXY_MIN,
Charles MacNeill 5:89031b2f5316 71 TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM,
Charles MacNeill 5:89031b2f5316 72 TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER,
Charles MacNeill 5:89031b2f5316 73 TUNING_MIN_AMBIENT_DMAX_VALID,
Charles MacNeill 5:89031b2f5316 74 TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER,
Charles MacNeill 5:89031b2f5316 75 TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM,
Charles MacNeill 5:89031b2f5316 76 TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT,
Charles MacNeill 5:89031b2f5316 77 TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN,
Charles MacNeill 5:89031b2f5316 78 TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET,
Charles MacNeill 5:89031b2f5316 79 TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR_DEFAULT
Charles MacNeill 5:89031b2f5316 80 };
Charles MacNeill 5:89031b2f5316 81
Charles MacNeill 5:89031b2f5316 82 static VL53LX_Error SetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 83 uint32_t InterMeasurementPeriodMilliSeconds);
Charles MacNeill 5:89031b2f5316 84
Charles MacNeill 5:89031b2f5316 85 static VL53LX_Error GetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 86 uint32_t *pInterMeasurementPeriodMilliSeconds);
Charles MacNeill 5:89031b2f5316 87
Charles MacNeill 5:89031b2f5316 88 static VL53LX_Error ComputeDevicePresetMode(
Charles MacNeill 5:89031b2f5316 89 VL53LX_DistanceModes DistanceMode,
Charles MacNeill 5:89031b2f5316 90 VL53LX_DevicePresetModes *pDevicePresetMode);
Charles MacNeill 5:89031b2f5316 91
Charles MacNeill 5:89031b2f5316 92 static VL53LX_Error SetPresetModeL3CX(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 93 VL53LX_DistanceModes DistanceMode,
Charles MacNeill 5:89031b2f5316 94 uint32_t inter_measurement_period_ms);
Charles MacNeill 5:89031b2f5316 95
Charles MacNeill 5:89031b2f5316 96 static int IsL4(VL53LX_DEV Dev);
Charles MacNeill 5:89031b2f5316 97
Charles MacNeill 5:89031b2f5316 98 static VL53LX_Error CheckValidRectRoi(VL53LX_UserRoi_t ROI);
Charles MacNeill 5:89031b2f5316 99
Charles MacNeill 5:89031b2f5316 100
Charles MacNeill 5:89031b2f5316 101 VL53LX_Error VL53LX_GetVersion(VL53LX_Version_t *pVersion)
Charles MacNeill 5:89031b2f5316 102 {
Charles MacNeill 5:89031b2f5316 103 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 104
Charles MacNeill 5:89031b2f5316 105 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 106
Charles MacNeill 5:89031b2f5316 107 pVersion->major = VL53LX_IMPLEMENTATION_VER_MAJOR;
Charles MacNeill 5:89031b2f5316 108 pVersion->minor = VL53LX_IMPLEMENTATION_VER_MINOR;
Charles MacNeill 5:89031b2f5316 109 pVersion->build = VL53LX_IMPLEMENTATION_VER_SUB;
Charles MacNeill 5:89031b2f5316 110
Charles MacNeill 5:89031b2f5316 111 pVersion->revision = VL53LX_IMPLEMENTATION_VER_REVISION;
Charles MacNeill 5:89031b2f5316 112
Charles MacNeill 5:89031b2f5316 113 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 114 return Status;
Charles MacNeill 5:89031b2f5316 115 }
Charles MacNeill 5:89031b2f5316 116
Charles MacNeill 5:89031b2f5316 117 VL53LX_Error VL53LX_GetProductRevision(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 118 uint8_t *pProductRevisionMajor, uint8_t *pProductRevisionMinor)
Charles MacNeill 5:89031b2f5316 119 {
Charles MacNeill 5:89031b2f5316 120 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 121 uint8_t revision_id;
Charles MacNeill 5:89031b2f5316 122 VL53LX_LLDriverData_t *pLLData;
Charles MacNeill 5:89031b2f5316 123
Charles MacNeill 5:89031b2f5316 124 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 125
Charles MacNeill 5:89031b2f5316 126 pLLData = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 127 revision_id = pLLData->nvm_copy_data.identification__revision_id;
Charles MacNeill 5:89031b2f5316 128 *pProductRevisionMajor = 1;
Charles MacNeill 5:89031b2f5316 129 *pProductRevisionMinor = (revision_id & 0xF0) >> 4;
Charles MacNeill 5:89031b2f5316 130
Charles MacNeill 5:89031b2f5316 131 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 132 return Status;
Charles MacNeill 5:89031b2f5316 133
Charles MacNeill 5:89031b2f5316 134 }
Charles MacNeill 5:89031b2f5316 135
Charles MacNeill 5:89031b2f5316 136 VL53LX_Error VL53LX_GetDeviceInfo(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 137 VL53LX_DeviceInfo_t *pVL53LX_DeviceInfo)
Charles MacNeill 5:89031b2f5316 138 {
Charles MacNeill 5:89031b2f5316 139 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 140 uint8_t revision_id;
Charles MacNeill 5:89031b2f5316 141 VL53LX_LLDriverData_t *pLLData;
Charles MacNeill 5:89031b2f5316 142
Charles MacNeill 5:89031b2f5316 143 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 144
Charles MacNeill 5:89031b2f5316 145 pLLData = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147 pVL53LX_DeviceInfo->ProductType =
Charles MacNeill 5:89031b2f5316 148 pLLData->nvm_copy_data.identification__module_type;
Charles MacNeill 5:89031b2f5316 149
Charles MacNeill 5:89031b2f5316 150 revision_id = pLLData->nvm_copy_data.identification__revision_id;
Charles MacNeill 5:89031b2f5316 151 pVL53LX_DeviceInfo->ProductRevisionMajor = 1;
Charles MacNeill 5:89031b2f5316 152 pVL53LX_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4;
Charles MacNeill 5:89031b2f5316 153
Charles MacNeill 5:89031b2f5316 154 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 155 return Status;
Charles MacNeill 5:89031b2f5316 156 }
Charles MacNeill 5:89031b2f5316 157
Charles MacNeill 5:89031b2f5316 158 VL53LX_Error VL53LX_GetUID(VL53LX_DEV Dev, uint64_t *pUid)
Charles MacNeill 5:89031b2f5316 159 {
Charles MacNeill 5:89031b2f5316 160 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 161 uint8_t fmtdata[8];
Charles MacNeill 5:89031b2f5316 162
Charles MacNeill 5:89031b2f5316 163 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 164
Charles MacNeill 5:89031b2f5316 165 Status = VL53LX_read_nvm_raw_data(Dev,
Charles MacNeill 5:89031b2f5316 166 (uint8_t)(0x1F8 >> 2),
Charles MacNeill 5:89031b2f5316 167 (uint8_t)(8 >> 2),
Charles MacNeill 5:89031b2f5316 168 fmtdata);
Charles MacNeill 5:89031b2f5316 169 memcpy(pUid, fmtdata, sizeof(uint64_t));
Charles MacNeill 5:89031b2f5316 170
Charles MacNeill 5:89031b2f5316 171 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 172 return Status;
Charles MacNeill 5:89031b2f5316 173 }
Charles MacNeill 5:89031b2f5316 174
Charles MacNeill 5:89031b2f5316 175
Charles MacNeill 5:89031b2f5316 176
Charles MacNeill 5:89031b2f5316 177 VL53LX_Error VL53LX_SetDeviceAddress(VL53LX_DEV Dev, uint8_t DeviceAddress)
Charles MacNeill 5:89031b2f5316 178 {
Charles MacNeill 5:89031b2f5316 179
Charles MacNeill 5:89031b2f5316 180 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 181 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 182 VL53LX_static_nvm_managed_t *pdata = &(pdev->stat_nvm);
Charles MacNeill 5:89031b2f5316 183
Charles MacNeill 5:89031b2f5316 184 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 185 printf("VL53LX_SetDeviceAddress VL53LX_WrByte from %d to %d\n",Dev->IO.Address,DeviceAddress);
Charles MacNeill 5:89031b2f5316 186
Charles MacNeill 5:89031b2f5316 187 Status = VL53LX_WrByte(Dev, VL53LX_I2C_SLAVE__DEVICE_ADDRESS,
Charles MacNeill 5:89031b2f5316 188 DeviceAddress / 2);
Charles MacNeill 5:89031b2f5316 189 printf("VL53LX_SetDeviceAddress VL53LX_WrByte status %d\n",Status);
Charles MacNeill 5:89031b2f5316 190 pdata->i2c_slave__device_address = (DeviceAddress / 2) & 0x7F;
Charles MacNeill 5:89031b2f5316 191 Dev->IO.Address = DeviceAddress;
Charles MacNeill 5:89031b2f5316 192
Charles MacNeill 5:89031b2f5316 193 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 194
Charles MacNeill 5:89031b2f5316 195 return Status;
Charles MacNeill 5:89031b2f5316 196 }
Charles MacNeill 5:89031b2f5316 197
Charles MacNeill 5:89031b2f5316 198
Charles MacNeill 5:89031b2f5316 199 VL53LX_Error VL53LX_DataInit(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 200 {
Charles MacNeill 5:89031b2f5316 201 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 202 VL53LX_LLDriverData_t *pdev;
Charles MacNeill 5:89031b2f5316 203 uint8_t measurement_mode;
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207
Charles MacNeill 5:89031b2f5316 208 #ifdef USE_I2C_2V8
Charles MacNeill 5:89031b2f5316 209 Status = VL53LX_RdByte(Dev, VL53LX_PAD_I2C_HV__EXTSUP_CONFIG, &i);
Charles MacNeill 5:89031b2f5316 210 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 211 i = (i & 0xfe) | 0x01;
Charles MacNeill 5:89031b2f5316 212 Status = VL53LX_WrByte(Dev, VL53LX_PAD_I2C_HV__EXTSUP_CONFIG,
Charles MacNeill 5:89031b2f5316 213 i);
Charles MacNeill 5:89031b2f5316 214 }
Charles MacNeill 5:89031b2f5316 215 #endif
Charles MacNeill 5:89031b2f5316 216
Charles MacNeill 5:89031b2f5316 217 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 218 Status = VL53LX_data_init(Dev, 1);
Charles MacNeill 5:89031b2f5316 219
Charles MacNeill 5:89031b2f5316 220 Status = SetPresetModeL3CX(Dev,
Charles MacNeill 5:89031b2f5316 221 VL53LX_DISTANCEMODE_MEDIUM,
Charles MacNeill 5:89031b2f5316 222 1000);
Charles MacNeill 5:89031b2f5316 223
Charles MacNeill 5:89031b2f5316 224
Charles MacNeill 5:89031b2f5316 225 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 226 Status = VL53LX_SetMeasurementTimingBudgetMicroSeconds(Dev,
Charles MacNeill 5:89031b2f5316 227 33333);
Charles MacNeill 5:89031b2f5316 228
Charles MacNeill 5:89031b2f5316 229 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 230 Status = SetInterMeasurementPeriodMilliSeconds(Dev, 1000);
Charles MacNeill 5:89031b2f5316 231
Charles MacNeill 5:89031b2f5316 232 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 233 pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 234 memset(&pdev->per_vcsel_cal_data, 0,
Charles MacNeill 5:89031b2f5316 235 sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 5:89031b2f5316 236 }
Charles MacNeill 5:89031b2f5316 237
Charles MacNeill 5:89031b2f5316 238 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 239 Status = VL53LX_set_dmax_mode(Dev,
Charles MacNeill 5:89031b2f5316 240 VL53LX_DEVICEDMAXMODE__CUST_CAL_DATA);
Charles MacNeill 5:89031b2f5316 241 }
Charles MacNeill 5:89031b2f5316 242
Charles MacNeill 5:89031b2f5316 243
Charles MacNeill 5:89031b2f5316 244 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 245 Status = VL53LX_SmudgeCorrectionEnable(Dev,
Charles MacNeill 5:89031b2f5316 246 VL53LX_SMUDGE_CORRECTION_NONE);
Charles MacNeill 5:89031b2f5316 247
Charles MacNeill 5:89031b2f5316 248 measurement_mode = VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK;
Charles MacNeill 5:89031b2f5316 249 VL53LXDevDataSet(Dev, LLData.measurement_mode, measurement_mode);
Charles MacNeill 5:89031b2f5316 250
Charles MacNeill 5:89031b2f5316 251 VL53LXDevDataSet(Dev, CurrentParameters.DistanceMode,
Charles MacNeill 5:89031b2f5316 252 VL53LX_DISTANCEMODE_MEDIUM);
Charles MacNeill 5:89031b2f5316 253
Charles MacNeill 5:89031b2f5316 254 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 255 return Status;
Charles MacNeill 5:89031b2f5316 256 }
Charles MacNeill 5:89031b2f5316 257
Charles MacNeill 5:89031b2f5316 258
Charles MacNeill 5:89031b2f5316 259 VL53LX_Error VL53LX_WaitDeviceBooted(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 260 {
Charles MacNeill 5:89031b2f5316 261 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 262
Charles MacNeill 5:89031b2f5316 263 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 264
Charles MacNeill 5:89031b2f5316 265 Status = VL53LX_poll_for_boot_completion(Dev,
Charles MacNeill 5:89031b2f5316 266 VL53LX_BOOT_COMPLETION_POLLING_TIMEOUT_MS);
Charles MacNeill 5:89031b2f5316 267
Charles MacNeill 5:89031b2f5316 268 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 269 return Status;
Charles MacNeill 5:89031b2f5316 270 }
Charles MacNeill 5:89031b2f5316 271
Charles MacNeill 5:89031b2f5316 272
Charles MacNeill 5:89031b2f5316 273
Charles MacNeill 5:89031b2f5316 274
Charles MacNeill 5:89031b2f5316 275 static VL53LX_Error ComputeDevicePresetMode(
Charles MacNeill 5:89031b2f5316 276 VL53LX_DistanceModes DistanceMode,
Charles MacNeill 5:89031b2f5316 277 VL53LX_DevicePresetModes *pDevicePresetMode)
Charles MacNeill 5:89031b2f5316 278 {
Charles MacNeill 5:89031b2f5316 279 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 280
Charles MacNeill 5:89031b2f5316 281 uint8_t DistIdx;
Charles MacNeill 5:89031b2f5316 282 VL53LX_DevicePresetModes RangingModes[3] = {
Charles MacNeill 5:89031b2f5316 283 VL53LX_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE,
Charles MacNeill 5:89031b2f5316 284 VL53LX_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE,
Charles MacNeill 5:89031b2f5316 285 VL53LX_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE};
Charles MacNeill 5:89031b2f5316 286
Charles MacNeill 5:89031b2f5316 287 switch (DistanceMode) {
Charles MacNeill 5:89031b2f5316 288 case VL53LX_DISTANCEMODE_SHORT:
Charles MacNeill 5:89031b2f5316 289 DistIdx = 0;
Charles MacNeill 5:89031b2f5316 290 break;
Charles MacNeill 5:89031b2f5316 291 case VL53LX_DISTANCEMODE_MEDIUM:
Charles MacNeill 5:89031b2f5316 292 DistIdx = 1;
Charles MacNeill 5:89031b2f5316 293 break;
Charles MacNeill 5:89031b2f5316 294 default:
Charles MacNeill 5:89031b2f5316 295 DistIdx = 2;
Charles MacNeill 5:89031b2f5316 296 }
Charles MacNeill 5:89031b2f5316 297
Charles MacNeill 5:89031b2f5316 298 *pDevicePresetMode = RangingModes[DistIdx];
Charles MacNeill 5:89031b2f5316 299
Charles MacNeill 5:89031b2f5316 300 return Status;
Charles MacNeill 5:89031b2f5316 301 }
Charles MacNeill 5:89031b2f5316 302
Charles MacNeill 5:89031b2f5316 303 static VL53LX_Error SetPresetModeL3CX(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 304 VL53LX_DistanceModes DistanceMode,
Charles MacNeill 5:89031b2f5316 305 uint32_t inter_measurement_period_ms)
Charles MacNeill 5:89031b2f5316 306 {
Charles MacNeill 5:89031b2f5316 307 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 308 VL53LX_DevicePresetModes device_preset_mode;
Charles MacNeill 5:89031b2f5316 309 uint8_t measurement_mode;
Charles MacNeill 5:89031b2f5316 310 uint16_t dss_config__target_total_rate_mcps;
Charles MacNeill 5:89031b2f5316 311 uint32_t phasecal_config_timeout_us;
Charles MacNeill 5:89031b2f5316 312 uint32_t mm_config_timeout_us;
Charles MacNeill 5:89031b2f5316 313 uint32_t lld_range_config_timeout_us;
Charles MacNeill 5:89031b2f5316 314
Charles MacNeill 5:89031b2f5316 315 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 316
Charles MacNeill 5:89031b2f5316 317 measurement_mode = VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK;
Charles MacNeill 5:89031b2f5316 318
Charles MacNeill 5:89031b2f5316 319 Status = ComputeDevicePresetMode(DistanceMode,
Charles MacNeill 5:89031b2f5316 320 &device_preset_mode);
Charles MacNeill 5:89031b2f5316 321
Charles MacNeill 5:89031b2f5316 322 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 323 Status = VL53LX_get_preset_mode_timing_cfg(Dev,
Charles MacNeill 5:89031b2f5316 324 device_preset_mode,
Charles MacNeill 5:89031b2f5316 325 &dss_config__target_total_rate_mcps,
Charles MacNeill 5:89031b2f5316 326 &phasecal_config_timeout_us,
Charles MacNeill 5:89031b2f5316 327 &mm_config_timeout_us,
Charles MacNeill 5:89031b2f5316 328 &lld_range_config_timeout_us);
Charles MacNeill 5:89031b2f5316 329
Charles MacNeill 5:89031b2f5316 330 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 331 Status = VL53LX_set_preset_mode(
Charles MacNeill 5:89031b2f5316 332 Dev,
Charles MacNeill 5:89031b2f5316 333 device_preset_mode,
Charles MacNeill 5:89031b2f5316 334 dss_config__target_total_rate_mcps,
Charles MacNeill 5:89031b2f5316 335 phasecal_config_timeout_us,
Charles MacNeill 5:89031b2f5316 336 mm_config_timeout_us,
Charles MacNeill 5:89031b2f5316 337 lld_range_config_timeout_us,
Charles MacNeill 5:89031b2f5316 338 inter_measurement_period_ms);
Charles MacNeill 5:89031b2f5316 339
Charles MacNeill 5:89031b2f5316 340 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 341 VL53LXDevDataSet(Dev, LLData.measurement_mode,
Charles MacNeill 5:89031b2f5316 342 measurement_mode);
Charles MacNeill 5:89031b2f5316 343
Charles MacNeill 5:89031b2f5316 344 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 345 return Status;
Charles MacNeill 5:89031b2f5316 346 }
Charles MacNeill 5:89031b2f5316 347
Charles MacNeill 5:89031b2f5316 348 static int IsL4(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 349 {
Charles MacNeill 5:89031b2f5316 350 int devL4 = 0;
Charles MacNeill 5:89031b2f5316 351 VL53LX_LLDriverData_t *pDev;
Charles MacNeill 5:89031b2f5316 352 pDev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 353
Charles MacNeill 5:89031b2f5316 354 if ((pDev->nvm_copy_data.identification__module_type == 0xAA) &&
Charles MacNeill 5:89031b2f5316 355 (pDev->nvm_copy_data.identification__model_id == 0xEB))
Charles MacNeill 5:89031b2f5316 356 devL4 = 1;
Charles MacNeill 5:89031b2f5316 357 return devL4;
Charles MacNeill 5:89031b2f5316 358 }
Charles MacNeill 5:89031b2f5316 359
Charles MacNeill 5:89031b2f5316 360 static VL53LX_Error CheckValidRectRoi(VL53LX_UserRoi_t ROI)
Charles MacNeill 5:89031b2f5316 361 {
Charles MacNeill 5:89031b2f5316 362 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 363
Charles MacNeill 5:89031b2f5316 364 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 365
Charles MacNeill 5:89031b2f5316 366
Charles MacNeill 5:89031b2f5316 367 if ((ROI.TopLeftX > 15) || (ROI.TopLeftY > 15) ||
Charles MacNeill 5:89031b2f5316 368 (ROI.BotRightX > 15) || (ROI.BotRightY > 15))
Charles MacNeill 5:89031b2f5316 369 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 370
Charles MacNeill 5:89031b2f5316 371 if ((ROI.TopLeftX > ROI.BotRightX) || (ROI.TopLeftY < ROI.BotRightY))
Charles MacNeill 5:89031b2f5316 372 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 373
Charles MacNeill 5:89031b2f5316 374 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 375 return Status;
Charles MacNeill 5:89031b2f5316 376 }
Charles MacNeill 5:89031b2f5316 377
Charles MacNeill 5:89031b2f5316 378
Charles MacNeill 5:89031b2f5316 379 VL53LX_Error VL53LX_SetDistanceMode(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 380 VL53LX_DistanceModes DistanceMode)
Charles MacNeill 5:89031b2f5316 381 {
Charles MacNeill 5:89031b2f5316 382 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 383 uint32_t inter_measurement_period_ms;
Charles MacNeill 5:89031b2f5316 384 uint32_t TimingBudget;
Charles MacNeill 5:89031b2f5316 385 uint32_t MmTimeoutUs;
Charles MacNeill 5:89031b2f5316 386 uint32_t PhaseCalTimeoutUs;
Charles MacNeill 5:89031b2f5316 387
Charles MacNeill 5:89031b2f5316 388 LOG_FUNCTION_START("%d", (int)DistanceMode);
Charles MacNeill 5:89031b2f5316 389
Charles MacNeill 5:89031b2f5316 390
Charles MacNeill 5:89031b2f5316 391
Charles MacNeill 5:89031b2f5316 392 if ((DistanceMode != VL53LX_DISTANCEMODE_SHORT) &&
Charles MacNeill 5:89031b2f5316 393 (DistanceMode != VL53LX_DISTANCEMODE_MEDIUM) &&
Charles MacNeill 5:89031b2f5316 394 (DistanceMode != VL53LX_DISTANCEMODE_LONG))
Charles MacNeill 5:89031b2f5316 395 return VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 396
Charles MacNeill 5:89031b2f5316 397 if (IsL4(Dev) && (DistanceMode == VL53LX_DISTANCEMODE_SHORT))
Charles MacNeill 5:89031b2f5316 398 return VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 399
Charles MacNeill 5:89031b2f5316 400 inter_measurement_period_ms = VL53LXDevDataGet(Dev,
Charles MacNeill 5:89031b2f5316 401 LLData.inter_measurement_period_ms);
Charles MacNeill 5:89031b2f5316 402
Charles MacNeill 5:89031b2f5316 403 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 404 Status = VL53LX_get_timeouts_us(Dev, &PhaseCalTimeoutUs,
Charles MacNeill 5:89031b2f5316 405 &MmTimeoutUs, &TimingBudget);
Charles MacNeill 5:89031b2f5316 406
Charles MacNeill 5:89031b2f5316 407 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 408 Status = SetPresetModeL3CX(Dev,
Charles MacNeill 5:89031b2f5316 409 DistanceMode,
Charles MacNeill 5:89031b2f5316 410 inter_measurement_period_ms);
Charles MacNeill 5:89031b2f5316 411
Charles MacNeill 5:89031b2f5316 412 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 413 VL53LXDevDataSet(Dev, CurrentParameters.DistanceMode,
Charles MacNeill 5:89031b2f5316 414 DistanceMode);
Charles MacNeill 5:89031b2f5316 415 }
Charles MacNeill 5:89031b2f5316 416
Charles MacNeill 5:89031b2f5316 417 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 418 Status = VL53LX_set_timeouts_us(Dev, PhaseCalTimeoutUs,
Charles MacNeill 5:89031b2f5316 419 MmTimeoutUs, TimingBudget);
Charles MacNeill 5:89031b2f5316 420
Charles MacNeill 5:89031b2f5316 421 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 422 VL53LXDevDataSet(Dev, LLData.range_config_timeout_us,
Charles MacNeill 5:89031b2f5316 423 TimingBudget);
Charles MacNeill 5:89031b2f5316 424 }
Charles MacNeill 5:89031b2f5316 425
Charles MacNeill 5:89031b2f5316 426 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 427 return Status;
Charles MacNeill 5:89031b2f5316 428 }
Charles MacNeill 5:89031b2f5316 429
Charles MacNeill 5:89031b2f5316 430 VL53LX_Error VL53LX_GetDistanceMode(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 431 VL53LX_DistanceModes *pDistanceMode)
Charles MacNeill 5:89031b2f5316 432 {
Charles MacNeill 5:89031b2f5316 433 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 434
Charles MacNeill 5:89031b2f5316 435 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 436
Charles MacNeill 5:89031b2f5316 437 *pDistanceMode = VL53LXDevDataGet(Dev, CurrentParameters.DistanceMode);
Charles MacNeill 5:89031b2f5316 438
Charles MacNeill 5:89031b2f5316 439 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 440 return Status;
Charles MacNeill 5:89031b2f5316 441 }
Charles MacNeill 5:89031b2f5316 442
Charles MacNeill 5:89031b2f5316 443
Charles MacNeill 5:89031b2f5316 444 VL53LX_Error VL53LX_SetMeasurementTimingBudgetMicroSeconds(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 445 uint32_t MeasurementTimingBudgetMicroSeconds)
Charles MacNeill 5:89031b2f5316 446 {
Charles MacNeill 5:89031b2f5316 447 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 448 uint32_t TimingGuard;
Charles MacNeill 5:89031b2f5316 449 uint32_t divisor;
Charles MacNeill 5:89031b2f5316 450 uint32_t TimingBudget;
Charles MacNeill 5:89031b2f5316 451 uint32_t MmTimeoutUs;
Charles MacNeill 5:89031b2f5316 452 uint32_t PhaseCalTimeoutUs;
Charles MacNeill 5:89031b2f5316 453 uint32_t FDAMaxTimingBudgetUs = FDA_MAX_TIMING_BUDGET_US;
Charles MacNeill 5:89031b2f5316 454
Charles MacNeill 5:89031b2f5316 455 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 456
Charles MacNeill 5:89031b2f5316 457
Charles MacNeill 5:89031b2f5316 458 if (MeasurementTimingBudgetMicroSeconds > 10000000)
Charles MacNeill 5:89031b2f5316 459 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 460
Charles MacNeill 5:89031b2f5316 461 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 462 Status = VL53LX_get_timeouts_us(Dev,
Charles MacNeill 5:89031b2f5316 463 &PhaseCalTimeoutUs,
Charles MacNeill 5:89031b2f5316 464 &MmTimeoutUs,
Charles MacNeill 5:89031b2f5316 465 &TimingBudget);
Charles MacNeill 5:89031b2f5316 466
Charles MacNeill 5:89031b2f5316 467 TimingGuard = 1700;
Charles MacNeill 5:89031b2f5316 468 divisor = 6;
Charles MacNeill 5:89031b2f5316 469
Charles MacNeill 5:89031b2f5316 470 if (IsL4(Dev))
Charles MacNeill 5:89031b2f5316 471 FDAMaxTimingBudgetUs = L4_FDA_MAX_TIMING_BUDGET_US;
Charles MacNeill 5:89031b2f5316 472
Charles MacNeill 5:89031b2f5316 473 if (MeasurementTimingBudgetMicroSeconds <= TimingGuard)
Charles MacNeill 5:89031b2f5316 474 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 475 else {
Charles MacNeill 5:89031b2f5316 476 TimingBudget = (MeasurementTimingBudgetMicroSeconds
Charles MacNeill 5:89031b2f5316 477 - TimingGuard);
Charles MacNeill 5:89031b2f5316 478 }
Charles MacNeill 5:89031b2f5316 479
Charles MacNeill 5:89031b2f5316 480 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 481 if (TimingBudget > FDAMaxTimingBudgetUs)
Charles MacNeill 5:89031b2f5316 482 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 483 else {
Charles MacNeill 5:89031b2f5316 484 TimingBudget /= divisor;
Charles MacNeill 5:89031b2f5316 485 Status = VL53LX_set_timeouts_us(
Charles MacNeill 5:89031b2f5316 486 Dev,
Charles MacNeill 5:89031b2f5316 487 PhaseCalTimeoutUs,
Charles MacNeill 5:89031b2f5316 488 MmTimeoutUs,
Charles MacNeill 5:89031b2f5316 489 TimingBudget);
Charles MacNeill 5:89031b2f5316 490 }
Charles MacNeill 5:89031b2f5316 491
Charles MacNeill 5:89031b2f5316 492 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 493 VL53LXDevDataSet(Dev,
Charles MacNeill 5:89031b2f5316 494 LLData.range_config_timeout_us,
Charles MacNeill 5:89031b2f5316 495 TimingBudget);
Charles MacNeill 5:89031b2f5316 496 }
Charles MacNeill 5:89031b2f5316 497
Charles MacNeill 5:89031b2f5316 498 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 499 VL53LXDevDataSet(Dev,
Charles MacNeill 5:89031b2f5316 500 CurrentParameters.MeasurementTimingBudgetMicroSeconds,
Charles MacNeill 5:89031b2f5316 501 MeasurementTimingBudgetMicroSeconds);
Charles MacNeill 5:89031b2f5316 502 }
Charles MacNeill 5:89031b2f5316 503
Charles MacNeill 5:89031b2f5316 504 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 505 return Status;
Charles MacNeill 5:89031b2f5316 506 }
Charles MacNeill 5:89031b2f5316 507
Charles MacNeill 5:89031b2f5316 508
Charles MacNeill 5:89031b2f5316 509 VL53LX_Error VL53LX_GetMeasurementTimingBudgetMicroSeconds(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 510 uint32_t *pMeasurementTimingBudgetMicroSeconds)
Charles MacNeill 5:89031b2f5316 511 {
Charles MacNeill 5:89031b2f5316 512 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 513 uint32_t MmTimeoutUs = 0;
Charles MacNeill 5:89031b2f5316 514 uint32_t RangeTimeoutUs = 0;
Charles MacNeill 5:89031b2f5316 515 uint32_t PhaseCalTimeoutUs = 0;
Charles MacNeill 5:89031b2f5316 516
Charles MacNeill 5:89031b2f5316 517 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 518
Charles MacNeill 5:89031b2f5316 519 *pMeasurementTimingBudgetMicroSeconds = 0;
Charles MacNeill 5:89031b2f5316 520
Charles MacNeill 5:89031b2f5316 521 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 522 Status = VL53LX_get_timeouts_us(Dev,
Charles MacNeill 5:89031b2f5316 523 &PhaseCalTimeoutUs,
Charles MacNeill 5:89031b2f5316 524 &MmTimeoutUs,
Charles MacNeill 5:89031b2f5316 525 &RangeTimeoutUs);
Charles MacNeill 5:89031b2f5316 526
Charles MacNeill 5:89031b2f5316 527 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 528 *pMeasurementTimingBudgetMicroSeconds = (6 * RangeTimeoutUs) +
Charles MacNeill 5:89031b2f5316 529 1700;
Charles MacNeill 5:89031b2f5316 530
Charles MacNeill 5:89031b2f5316 531 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 532 return Status;
Charles MacNeill 5:89031b2f5316 533 }
Charles MacNeill 5:89031b2f5316 534
Charles MacNeill 5:89031b2f5316 535
Charles MacNeill 5:89031b2f5316 536
Charles MacNeill 5:89031b2f5316 537 static VL53LX_Error SetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 538 uint32_t InterMeasurementPeriodMilliSeconds)
Charles MacNeill 5:89031b2f5316 539 {
Charles MacNeill 5:89031b2f5316 540 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 541 uint32_t adjustedIMP;
Charles MacNeill 5:89031b2f5316 542
Charles MacNeill 5:89031b2f5316 543 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 544
Charles MacNeill 5:89031b2f5316 545
Charles MacNeill 5:89031b2f5316 546 adjustedIMP = InterMeasurementPeriodMilliSeconds;
Charles MacNeill 5:89031b2f5316 547 adjustedIMP += (adjustedIMP * 64) / 1000;
Charles MacNeill 5:89031b2f5316 548
Charles MacNeill 5:89031b2f5316 549 Status = VL53LX_set_inter_measurement_period_ms(Dev,
Charles MacNeill 5:89031b2f5316 550 adjustedIMP);
Charles MacNeill 5:89031b2f5316 551
Charles MacNeill 5:89031b2f5316 552 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 553 return Status;
Charles MacNeill 5:89031b2f5316 554 }
Charles MacNeill 5:89031b2f5316 555
Charles MacNeill 5:89031b2f5316 556 static VL53LX_Error GetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 557 uint32_t *pInterMeasurementPeriodMilliSeconds)
Charles MacNeill 5:89031b2f5316 558 {
Charles MacNeill 5:89031b2f5316 559 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 560 uint32_t adjustedIMP;
Charles MacNeill 5:89031b2f5316 561
Charles MacNeill 5:89031b2f5316 562 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 563
Charles MacNeill 5:89031b2f5316 564 Status = VL53LX_get_inter_measurement_period_ms(Dev, &adjustedIMP);
Charles MacNeill 5:89031b2f5316 565
Charles MacNeill 5:89031b2f5316 566 adjustedIMP -= (adjustedIMP * 64) / 1000;
Charles MacNeill 5:89031b2f5316 567 *pInterMeasurementPeriodMilliSeconds = adjustedIMP;
Charles MacNeill 5:89031b2f5316 568
Charles MacNeill 5:89031b2f5316 569
Charles MacNeill 5:89031b2f5316 570 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 571 return Status;
Charles MacNeill 5:89031b2f5316 572 }
Charles MacNeill 5:89031b2f5316 573
Charles MacNeill 5:89031b2f5316 574
Charles MacNeill 5:89031b2f5316 575
Charles MacNeill 5:89031b2f5316 576
Charles MacNeill 5:89031b2f5316 577
Charles MacNeill 5:89031b2f5316 578 VL53LX_Error VL53LX_SetUserROI(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 579 VL53LX_UserRoi_t *pRoi)
Charles MacNeill 5:89031b2f5316 580 {
Charles MacNeill 5:89031b2f5316 581 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 582 VL53LX_zone_config_t zone_cfg;
Charles MacNeill 5:89031b2f5316 583 uint8_t x_centre, y_centre, width, height;
Charles MacNeill 5:89031b2f5316 584
Charles MacNeill 5:89031b2f5316 585 Status = CheckValidRectRoi(*pRoi);
Charles MacNeill 5:89031b2f5316 586 if (Status != VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 587 return VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 588
Charles MacNeill 5:89031b2f5316 589 x_centre = (pRoi->BotRightX + pRoi->TopLeftX + 1) / 2;
Charles MacNeill 5:89031b2f5316 590 y_centre = (pRoi->TopLeftY + pRoi->BotRightY + 1) / 2;
Charles MacNeill 5:89031b2f5316 591 width = (pRoi->BotRightX - pRoi->TopLeftX);
Charles MacNeill 5:89031b2f5316 592 height = (pRoi->TopLeftY - pRoi->BotRightY);
Charles MacNeill 5:89031b2f5316 593 zone_cfg.max_zones = 1;
Charles MacNeill 5:89031b2f5316 594 zone_cfg.active_zones = 0;
Charles MacNeill 5:89031b2f5316 595 zone_cfg.user_zones[0].x_centre = x_centre;
Charles MacNeill 5:89031b2f5316 596 zone_cfg.user_zones[0].y_centre = y_centre;
Charles MacNeill 5:89031b2f5316 597 zone_cfg.user_zones[0].width = width;
Charles MacNeill 5:89031b2f5316 598 zone_cfg.user_zones[0].height = height;
Charles MacNeill 5:89031b2f5316 599 if ((width < 3) || (height < 3))
Charles MacNeill 5:89031b2f5316 600 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 601 else
Charles MacNeill 5:89031b2f5316 602 Status = VL53LX_set_zone_config(Dev, &zone_cfg);
Charles MacNeill 5:89031b2f5316 603
Charles MacNeill 5:89031b2f5316 604 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 605 return Status;
Charles MacNeill 5:89031b2f5316 606 }
Charles MacNeill 5:89031b2f5316 607
Charles MacNeill 5:89031b2f5316 608 VL53LX_Error VL53LX_GetUserROI(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 609 VL53LX_UserRoi_t *pRoi)
Charles MacNeill 5:89031b2f5316 610 {
Charles MacNeill 5:89031b2f5316 611 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 612 VL53LX_zone_config_t zone_cfg;
Charles MacNeill 5:89031b2f5316 613 uint8_t TopLeftX;
Charles MacNeill 5:89031b2f5316 614 uint8_t TopLeftY;
Charles MacNeill 5:89031b2f5316 615 uint8_t BotRightX;
Charles MacNeill 5:89031b2f5316 616 uint8_t BotRightY;
Charles MacNeill 5:89031b2f5316 617
Charles MacNeill 5:89031b2f5316 618 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 619
Charles MacNeill 5:89031b2f5316 620 VL53LX_get_zone_config(Dev, &zone_cfg);
Charles MacNeill 5:89031b2f5316 621
Charles MacNeill 5:89031b2f5316 622 TopLeftX = (2 * zone_cfg.user_zones[0].x_centre -
Charles MacNeill 5:89031b2f5316 623 zone_cfg.user_zones[0].width) >> 1;
Charles MacNeill 5:89031b2f5316 624 TopLeftY = (2 * zone_cfg.user_zones[0].y_centre +
Charles MacNeill 5:89031b2f5316 625 zone_cfg.user_zones[0].height) >> 1;
Charles MacNeill 5:89031b2f5316 626 BotRightX = (2 * zone_cfg.user_zones[0].x_centre +
Charles MacNeill 5:89031b2f5316 627 zone_cfg.user_zones[0].width) >> 1;
Charles MacNeill 5:89031b2f5316 628 BotRightY = (2 * zone_cfg.user_zones[0].y_centre -
Charles MacNeill 5:89031b2f5316 629 zone_cfg.user_zones[0].height) >> 1;
Charles MacNeill 5:89031b2f5316 630 pRoi->TopLeftX = TopLeftX;
Charles MacNeill 5:89031b2f5316 631 pRoi->TopLeftY = TopLeftY;
Charles MacNeill 5:89031b2f5316 632 pRoi->BotRightX = BotRightX;
Charles MacNeill 5:89031b2f5316 633 pRoi->BotRightY = BotRightY;
Charles MacNeill 5:89031b2f5316 634
Charles MacNeill 5:89031b2f5316 635 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 636 return Status;
Charles MacNeill 5:89031b2f5316 637 }
Charles MacNeill 5:89031b2f5316 638
Charles MacNeill 5:89031b2f5316 639
Charles MacNeill 5:89031b2f5316 640
Charles MacNeill 5:89031b2f5316 641
Charles MacNeill 5:89031b2f5316 642
Charles MacNeill 5:89031b2f5316 643 VL53LX_Error VL53LX_StartMeasurement(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 644 {
Charles MacNeill 5:89031b2f5316 645 #define TIMED_MODE_TIMING_GUARD_MILLISECONDS 4
Charles MacNeill 5:89031b2f5316 646 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 647 uint8_t DeviceMeasurementMode;
Charles MacNeill 5:89031b2f5316 648 VL53LX_Error lStatus;
Charles MacNeill 5:89031b2f5316 649 uint32_t MTBus, IMPms;
Charles MacNeill 5:89031b2f5316 650 uint8_t i;
Charles MacNeill 5:89031b2f5316 651 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 652
Charles MacNeill 5:89031b2f5316 653 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 654
Charles MacNeill 5:89031b2f5316 655 VL53LX_load_patch(Dev);
Charles MacNeill 5:89031b2f5316 656 for (i = 0; i < VL53LX_MAX_RANGE_RESULTS; i++) {
Charles MacNeill 5:89031b2f5316 657 pdev->PreviousRangeMilliMeter[i] = 0;
Charles MacNeill 5:89031b2f5316 658 pdev->PreviousRangeStatus[i] = 255;
Charles MacNeill 5:89031b2f5316 659 pdev->PreviousExtendedRange[i] = 0;
Charles MacNeill 5:89031b2f5316 660 }
Charles MacNeill 5:89031b2f5316 661 pdev->PreviousStreamCount = 0;
Charles MacNeill 5:89031b2f5316 662
Charles MacNeill 5:89031b2f5316 663 DeviceMeasurementMode = VL53LXDevDataGet(Dev, LLData.measurement_mode);
Charles MacNeill 5:89031b2f5316 664
Charles MacNeill 5:89031b2f5316 665
Charles MacNeill 5:89031b2f5316 666 if ((Status == VL53LX_ERROR_NONE) &&
Charles MacNeill 5:89031b2f5316 667 (DeviceMeasurementMode == VL53LX_DEVICEMEASUREMENTMODE_TIMED)) {
Charles MacNeill 5:89031b2f5316 668 lStatus = VL53LX_GetMeasurementTimingBudgetMicroSeconds(Dev,
Charles MacNeill 5:89031b2f5316 669 &MTBus);
Charles MacNeill 5:89031b2f5316 670
Charles MacNeill 5:89031b2f5316 671 MTBus /= 1000;
Charles MacNeill 5:89031b2f5316 672 lStatus = GetInterMeasurementPeriodMilliSeconds(Dev,
Charles MacNeill 5:89031b2f5316 673 &IMPms);
Charles MacNeill 5:89031b2f5316 674
Charles MacNeill 5:89031b2f5316 675 SUPPRESS_UNUSED_WARNING(lStatus);
Charles MacNeill 5:89031b2f5316 676 if (IMPms < MTBus + TIMED_MODE_TIMING_GUARD_MILLISECONDS)
Charles MacNeill 5:89031b2f5316 677 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 678 }
Charles MacNeill 5:89031b2f5316 679
Charles MacNeill 5:89031b2f5316 680 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 681 Status = VL53LX_init_and_start_range(
Charles MacNeill 5:89031b2f5316 682 Dev,
Charles MacNeill 5:89031b2f5316 683 DeviceMeasurementMode,
Charles MacNeill 5:89031b2f5316 684 VL53LX_DEVICECONFIGLEVEL_FULL);
Charles MacNeill 5:89031b2f5316 685
Charles MacNeill 5:89031b2f5316 686 if ( Dev->EnableInterrupt == 1)
Charles MacNeill 5:89031b2f5316 687 {
Charles MacNeill 5:89031b2f5316 688 enable_interrupt_measure_detection_irq();
Charles MacNeill 5:89031b2f5316 689 attach_interrupt_measure_detection_irq(Dev->Interrupt_Func );
Charles MacNeill 5:89031b2f5316 690 }
Charles MacNeill 5:89031b2f5316 691
Charles MacNeill 5:89031b2f5316 692 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 693 return Status;
Charles MacNeill 5:89031b2f5316 694 }
Charles MacNeill 5:89031b2f5316 695
Charles MacNeill 5:89031b2f5316 696 VL53LX_Error VL53LX_StopMeasurement(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 697 {
Charles MacNeill 5:89031b2f5316 698 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 699
Charles MacNeill 5:89031b2f5316 700 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 701
Charles MacNeill 5:89031b2f5316 702 if ( Dev->EnableInterrupt == 1)
Charles MacNeill 5:89031b2f5316 703 {
Charles MacNeill 5:89031b2f5316 704 disable_interrupt_measure_detection_irq();
Charles MacNeill 5:89031b2f5316 705 }
Charles MacNeill 5:89031b2f5316 706
Charles MacNeill 5:89031b2f5316 707 Status = VL53LX_stop_range(Dev);
Charles MacNeill 5:89031b2f5316 708
Charles MacNeill 5:89031b2f5316 709 VL53LX_unload_patch(Dev);
Charles MacNeill 5:89031b2f5316 710
Charles MacNeill 5:89031b2f5316 711 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 712 return Status;
Charles MacNeill 5:89031b2f5316 713 }
Charles MacNeill 5:89031b2f5316 714
Charles MacNeill 5:89031b2f5316 715
Charles MacNeill 5:89031b2f5316 716 VL53LX_Error VL53LX_ClearInterruptAndStartMeasurement(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 717 {
Charles MacNeill 5:89031b2f5316 718 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 719 uint8_t DeviceMeasurementMode;
Charles MacNeill 5:89031b2f5316 720
Charles MacNeill 5:89031b2f5316 721 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 722
Charles MacNeill 5:89031b2f5316 723 DeviceMeasurementMode = VL53LXDevDataGet(Dev, LLData.measurement_mode);
Charles MacNeill 5:89031b2f5316 724
Charles MacNeill 5:89031b2f5316 725 Status = VL53LX_clear_interrupt_and_enable_next_range(Dev,
Charles MacNeill 5:89031b2f5316 726 DeviceMeasurementMode);
Charles MacNeill 5:89031b2f5316 727
Charles MacNeill 5:89031b2f5316 728 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 729 return Status;
Charles MacNeill 5:89031b2f5316 730 }
Charles MacNeill 5:89031b2f5316 731
Charles MacNeill 5:89031b2f5316 732
Charles MacNeill 5:89031b2f5316 733 VL53LX_Error VL53LX_GetMeasurementDataReady(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 734 uint8_t *pMeasurementDataReady)
Charles MacNeill 5:89031b2f5316 735 {
Charles MacNeill 5:89031b2f5316 736 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 737
Charles MacNeill 5:89031b2f5316 738 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 739
Charles MacNeill 5:89031b2f5316 740 Status = VL53LX_is_new_data_ready(Dev, pMeasurementDataReady);
Charles MacNeill 5:89031b2f5316 741
Charles MacNeill 5:89031b2f5316 742 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 743 return Status;
Charles MacNeill 5:89031b2f5316 744 }
Charles MacNeill 5:89031b2f5316 745
Charles MacNeill 5:89031b2f5316 746 VL53LX_Error VL53LX_WaitMeasurementDataReady(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 747 {
Charles MacNeill 5:89031b2f5316 748 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 749
Charles MacNeill 5:89031b2f5316 750 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 751
Charles MacNeill 5:89031b2f5316 752
Charles MacNeill 5:89031b2f5316 753
Charles MacNeill 5:89031b2f5316 754 Status = VL53LX_poll_for_range_completion(Dev,
Charles MacNeill 5:89031b2f5316 755 VL53LX_RANGE_COMPLETION_POLLING_TIMEOUT_MS);
Charles MacNeill 5:89031b2f5316 756
Charles MacNeill 5:89031b2f5316 757 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 758 return Status;
Charles MacNeill 5:89031b2f5316 759 }
Charles MacNeill 5:89031b2f5316 760
Charles MacNeill 5:89031b2f5316 761 static uint8_t ConvertStatusHisto(uint8_t FilteredRangeStatus)
Charles MacNeill 5:89031b2f5316 762 {
Charles MacNeill 5:89031b2f5316 763 uint8_t RangeStatus;
Charles MacNeill 5:89031b2f5316 764
Charles MacNeill 5:89031b2f5316 765 switch (FilteredRangeStatus) {
Charles MacNeill 5:89031b2f5316 766 case VL53LX_DEVICEERROR_RANGEPHASECHECK:
Charles MacNeill 5:89031b2f5316 767 RangeStatus = VL53LX_RANGESTATUS_OUTOFBOUNDS_FAIL;
Charles MacNeill 5:89031b2f5316 768 break;
Charles MacNeill 5:89031b2f5316 769 case VL53LX_DEVICEERROR_SIGMATHRESHOLDCHECK:
Charles MacNeill 5:89031b2f5316 770 RangeStatus = VL53LX_RANGESTATUS_SIGMA_FAIL;
Charles MacNeill 5:89031b2f5316 771 break;
Charles MacNeill 5:89031b2f5316 772 case VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
Charles MacNeill 5:89031b2f5316 773 RangeStatus =
Charles MacNeill 5:89031b2f5316 774 VL53LX_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
Charles MacNeill 5:89031b2f5316 775 break;
Charles MacNeill 5:89031b2f5316 776 case VL53LX_DEVICEERROR_PHASECONSISTENCY:
Charles MacNeill 5:89031b2f5316 777 RangeStatus = VL53LX_RANGESTATUS_WRAP_TARGET_FAIL;
Charles MacNeill 5:89031b2f5316 778 break;
Charles MacNeill 5:89031b2f5316 779 case VL53LX_DEVICEERROR_PREV_RANGE_NO_TARGETS:
Charles MacNeill 5:89031b2f5316 780 RangeStatus = VL53LX_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL;
Charles MacNeill 5:89031b2f5316 781 break;
Charles MacNeill 5:89031b2f5316 782 case VL53LX_DEVICEERROR_EVENTCONSISTENCY:
Charles MacNeill 5:89031b2f5316 783 RangeStatus = VL53LX_RANGESTATUS_WRAP_TARGET_FAIL;
Charles MacNeill 5:89031b2f5316 784 break;
Charles MacNeill 5:89031b2f5316 785 case VL53LX_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE:
Charles MacNeill 5:89031b2f5316 786 RangeStatus = VL53LX_RANGESTATUS_RANGE_VALID_MERGED_PULSE;
Charles MacNeill 5:89031b2f5316 787 break;
Charles MacNeill 5:89031b2f5316 788 case VL53LX_DEVICEERROR_RANGECOMPLETE:
Charles MacNeill 5:89031b2f5316 789 RangeStatus = VL53LX_RANGESTATUS_RANGE_VALID;
Charles MacNeill 5:89031b2f5316 790 break;
Charles MacNeill 5:89031b2f5316 791 default:
Charles MacNeill 5:89031b2f5316 792 RangeStatus = VL53LX_RANGESTATUS_NONE;
Charles MacNeill 5:89031b2f5316 793 }
Charles MacNeill 5:89031b2f5316 794
Charles MacNeill 5:89031b2f5316 795 return RangeStatus;
Charles MacNeill 5:89031b2f5316 796 }
Charles MacNeill 5:89031b2f5316 797
Charles MacNeill 5:89031b2f5316 798 static VL53LX_Error SetTargetData(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 799 uint8_t active_results, uint8_t streamcount, uint8_t iteration,
Charles MacNeill 5:89031b2f5316 800 uint8_t device_status, VL53LX_range_data_t *presults_data,
Charles MacNeill 5:89031b2f5316 801 VL53LX_TargetRangeData_t *pRangeData)
Charles MacNeill 5:89031b2f5316 802 {
Charles MacNeill 5:89031b2f5316 803 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 804 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 805 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 806 VL53LX_tuning_parm_storage_t *tp =
Charles MacNeill 5:89031b2f5316 807 &(pdev->tuning_parms);
Charles MacNeill 5:89031b2f5316 808 uint8_t sequency;
Charles MacNeill 5:89031b2f5316 809 uint8_t FilteredRangeStatus;
Charles MacNeill 5:89031b2f5316 810 FixPoint1616_t AmbientRate;
Charles MacNeill 5:89031b2f5316 811 FixPoint1616_t SignalRate;
Charles MacNeill 5:89031b2f5316 812 FixPoint1616_t TempFix1616;
Charles MacNeill 5:89031b2f5316 813 int16_t Range, RangeDiff, RangeMillimeterInit;
Charles MacNeill 5:89031b2f5316 814 int32_t ExtendedRangeEnabled = 0;
Charles MacNeill 5:89031b2f5316 815 uint8_t uwr_status;
Charles MacNeill 5:89031b2f5316 816 int16_t AddOffset;
Charles MacNeill 5:89031b2f5316 817
Charles MacNeill 5:89031b2f5316 818 SUPPRESS_UNUSED_WARNING(Dev);
Charles MacNeill 5:89031b2f5316 819
Charles MacNeill 5:89031b2f5316 820 FilteredRangeStatus = presults_data->range_status & 0x1F;
Charles MacNeill 5:89031b2f5316 821
Charles MacNeill 5:89031b2f5316 822 SignalRate = VL53LX_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 5:89031b2f5316 823 presults_data->peak_signal_count_rate_mcps);
Charles MacNeill 5:89031b2f5316 824 pRangeData->SignalRateRtnMegaCps
Charles MacNeill 5:89031b2f5316 825 = SignalRate;
Charles MacNeill 5:89031b2f5316 826
Charles MacNeill 5:89031b2f5316 827 AmbientRate = VL53LX_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 5:89031b2f5316 828 presults_data->ambient_count_rate_mcps);
Charles MacNeill 5:89031b2f5316 829 pRangeData->AmbientRateRtnMegaCps = AmbientRate;
Charles MacNeill 5:89031b2f5316 830
Charles MacNeill 5:89031b2f5316 831 TempFix1616 = VL53LX_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 5:89031b2f5316 832 presults_data->VL53LX_p_002);
Charles MacNeill 5:89031b2f5316 833
Charles MacNeill 5:89031b2f5316 834 pRangeData->SigmaMilliMeter = TempFix1616;
Charles MacNeill 5:89031b2f5316 835
Charles MacNeill 5:89031b2f5316 836 pRangeData->RangeMilliMeter = presults_data->median_range_mm;
Charles MacNeill 5:89031b2f5316 837 pRangeData->RangeMaxMilliMeter = presults_data->max_range_mm;
Charles MacNeill 5:89031b2f5316 838 pRangeData->RangeMinMilliMeter = presults_data->min_range_mm;
Charles MacNeill 5:89031b2f5316 839
Charles MacNeill 5:89031b2f5316 840
Charles MacNeill 5:89031b2f5316 841 switch (device_status) {
Charles MacNeill 5:89031b2f5316 842 case VL53LX_DEVICEERROR_MULTCLIPFAIL:
Charles MacNeill 5:89031b2f5316 843 case VL53LX_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
Charles MacNeill 5:89031b2f5316 844 case VL53LX_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
Charles MacNeill 5:89031b2f5316 845 case VL53LX_DEVICEERROR_NOVHVVALUEFOUND:
Charles MacNeill 5:89031b2f5316 846 pRangeData->RangeStatus = VL53LX_RANGESTATUS_HARDWARE_FAIL;
Charles MacNeill 5:89031b2f5316 847 break;
Charles MacNeill 5:89031b2f5316 848 case VL53LX_DEVICEERROR_USERROICLIP:
Charles MacNeill 5:89031b2f5316 849 pRangeData->RangeStatus = VL53LX_RANGESTATUS_MIN_RANGE_FAIL;
Charles MacNeill 5:89031b2f5316 850 break;
Charles MacNeill 5:89031b2f5316 851 default:
Charles MacNeill 5:89031b2f5316 852 pRangeData->RangeStatus = VL53LX_RANGESTATUS_RANGE_VALID;
Charles MacNeill 5:89031b2f5316 853 }
Charles MacNeill 5:89031b2f5316 854
Charles MacNeill 5:89031b2f5316 855
Charles MacNeill 5:89031b2f5316 856 if ((pRangeData->RangeStatus == VL53LX_RANGESTATUS_RANGE_VALID) &&
Charles MacNeill 5:89031b2f5316 857 (active_results == 0)) {
Charles MacNeill 5:89031b2f5316 858 pRangeData->RangeStatus = VL53LX_RANGESTATUS_NONE;
Charles MacNeill 5:89031b2f5316 859 pRangeData->SignalRateRtnMegaCps = 0;
Charles MacNeill 5:89031b2f5316 860 pRangeData->SigmaMilliMeter = 0;
Charles MacNeill 5:89031b2f5316 861 pRangeData->RangeMilliMeter = 8191;
Charles MacNeill 5:89031b2f5316 862 pRangeData->RangeMaxMilliMeter = 8191;
Charles MacNeill 5:89031b2f5316 863 pRangeData->RangeMinMilliMeter = 8191;
Charles MacNeill 5:89031b2f5316 864 }
Charles MacNeill 5:89031b2f5316 865
Charles MacNeill 5:89031b2f5316 866
Charles MacNeill 5:89031b2f5316 867 if (pRangeData->RangeStatus == VL53LX_RANGESTATUS_RANGE_VALID)
Charles MacNeill 5:89031b2f5316 868 pRangeData->RangeStatus =
Charles MacNeill 5:89031b2f5316 869 ConvertStatusHisto(FilteredRangeStatus);
Charles MacNeill 5:89031b2f5316 870
Charles MacNeill 5:89031b2f5316 871
Charles MacNeill 5:89031b2f5316 872
Charles MacNeill 5:89031b2f5316 873 VL53LX_get_tuning_parm(Dev, VL53LX_TUNINGPARM_UWR_ENABLE,
Charles MacNeill 5:89031b2f5316 874 &ExtendedRangeEnabled);
Charles MacNeill 5:89031b2f5316 875
Charles MacNeill 5:89031b2f5316 876 sequency = streamcount % 2;
Charles MacNeill 5:89031b2f5316 877 uwr_status = 1;
Charles MacNeill 5:89031b2f5316 878 RangeMillimeterInit = pRangeData->RangeMilliMeter;
Charles MacNeill 5:89031b2f5316 879 AddOffset = 0;
Charles MacNeill 5:89031b2f5316 880
Charles MacNeill 5:89031b2f5316 881 pRangeData->ExtendedRange = 0;
Charles MacNeill 5:89031b2f5316 882
Charles MacNeill 5:89031b2f5316 883 if (ExtendedRangeEnabled &&
Charles MacNeill 5:89031b2f5316 884 (pRangeData->RangeStatus ==
Charles MacNeill 5:89031b2f5316 885 VL53LX_RANGESTATUS_WRAP_TARGET_FAIL ||
Charles MacNeill 5:89031b2f5316 886 pRangeData->RangeStatus ==
Charles MacNeill 5:89031b2f5316 887 VL53LX_RANGESTATUS_OUTOFBOUNDS_FAIL)
Charles MacNeill 5:89031b2f5316 888 && (pdev->PreviousRangeStatus[iteration] ==
Charles MacNeill 5:89031b2f5316 889 VL53LX_RANGESTATUS_WRAP_TARGET_FAIL ||
Charles MacNeill 5:89031b2f5316 890 pdev->PreviousRangeStatus[iteration] ==
Charles MacNeill 5:89031b2f5316 891 VL53LX_RANGESTATUS_OUTOFBOUNDS_FAIL ||
Charles MacNeill 5:89031b2f5316 892 (pdev->PreviousRangeStatus[iteration] ==
Charles MacNeill 5:89031b2f5316 893 VL53LX_RANGESTATUS_RANGE_VALID &&
Charles MacNeill 5:89031b2f5316 894 pdev->PreviousExtendedRange[iteration] == 1)))
Charles MacNeill 5:89031b2f5316 895 {
Charles MacNeill 5:89031b2f5316 896 if (((pdev->PreviousStreamCount) ==
Charles MacNeill 5:89031b2f5316 897 (pdev->hist_data.result__stream_count - 1 ))
Charles MacNeill 5:89031b2f5316 898 || ((pdev->PreviousStreamCount) ==
Charles MacNeill 5:89031b2f5316 899 (pdev->hist_data.result__stream_count + 127)))
Charles MacNeill 5:89031b2f5316 900 {
Charles MacNeill 5:89031b2f5316 901 RangeDiff = pRangeData->RangeMilliMeter -
Charles MacNeill 5:89031b2f5316 902 pdev->PreviousRangeMilliMeter[iteration];
Charles MacNeill 5:89031b2f5316 903
Charles MacNeill 5:89031b2f5316 904 switch (pdev->preset_mode) {
Charles MacNeill 5:89031b2f5316 905 case VL53LX_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE:
Charles MacNeill 5:89031b2f5316 906
Charles MacNeill 5:89031b2f5316 907 uwr_status = 0;
Charles MacNeill 5:89031b2f5316 908 break;
Charles MacNeill 5:89031b2f5316 909
Charles MacNeill 5:89031b2f5316 910 case VL53LX_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE:
Charles MacNeill 5:89031b2f5316 911 if (RangeDiff > tp->tp_uwr_med_z_1_min &&
Charles MacNeill 5:89031b2f5316 912 RangeDiff < tp->tp_uwr_med_z_1_max &&
Charles MacNeill 5:89031b2f5316 913 sequency == 1) {
Charles MacNeill 5:89031b2f5316 914 AddOffset =
Charles MacNeill 5:89031b2f5316 915 tp->tp_uwr_med_corr_z_1_rangeb;
Charles MacNeill 5:89031b2f5316 916 }
Charles MacNeill 5:89031b2f5316 917 else
Charles MacNeill 5:89031b2f5316 918 if (RangeDiff < -tp->tp_uwr_med_z_1_min &&
Charles MacNeill 5:89031b2f5316 919 RangeDiff > -tp->tp_uwr_med_z_1_max &&
Charles MacNeill 5:89031b2f5316 920 sequency == 0) {
Charles MacNeill 5:89031b2f5316 921 AddOffset =
Charles MacNeill 5:89031b2f5316 922 tp->tp_uwr_med_corr_z_1_rangea;
Charles MacNeill 5:89031b2f5316 923 }
Charles MacNeill 5:89031b2f5316 924 else
Charles MacNeill 5:89031b2f5316 925 if (RangeDiff > tp->tp_uwr_med_z_2_min &&
Charles MacNeill 5:89031b2f5316 926 RangeDiff < tp->tp_uwr_med_z_2_max &&
Charles MacNeill 5:89031b2f5316 927 sequency == 0) {
Charles MacNeill 5:89031b2f5316 928 AddOffset =
Charles MacNeill 5:89031b2f5316 929 tp->tp_uwr_med_corr_z_2_rangea;
Charles MacNeill 5:89031b2f5316 930 }
Charles MacNeill 5:89031b2f5316 931 else
Charles MacNeill 5:89031b2f5316 932 if (RangeDiff < -tp->tp_uwr_med_z_2_min &&
Charles MacNeill 5:89031b2f5316 933 RangeDiff > -tp->tp_uwr_med_z_2_max &&
Charles MacNeill 5:89031b2f5316 934 sequency == 1) {
Charles MacNeill 5:89031b2f5316 935 AddOffset =
Charles MacNeill 5:89031b2f5316 936 tp->tp_uwr_med_corr_z_2_rangeb;
Charles MacNeill 5:89031b2f5316 937 }
Charles MacNeill 5:89031b2f5316 938 else
Charles MacNeill 5:89031b2f5316 939 if (RangeDiff > tp->tp_uwr_med_z_3_min &&
Charles MacNeill 5:89031b2f5316 940 RangeDiff < tp->tp_uwr_med_z_3_max &&
Charles MacNeill 5:89031b2f5316 941 sequency == 1) {
Charles MacNeill 5:89031b2f5316 942 AddOffset =
Charles MacNeill 5:89031b2f5316 943 tp->tp_uwr_med_corr_z_3_rangeb;
Charles MacNeill 5:89031b2f5316 944 }
Charles MacNeill 5:89031b2f5316 945 else
Charles MacNeill 5:89031b2f5316 946 if (RangeDiff < -tp->tp_uwr_med_z_3_min &&
Charles MacNeill 5:89031b2f5316 947 RangeDiff > -tp->tp_uwr_med_z_3_max &&
Charles MacNeill 5:89031b2f5316 948 sequency == 0) {
Charles MacNeill 5:89031b2f5316 949 AddOffset =
Charles MacNeill 5:89031b2f5316 950 tp->tp_uwr_med_corr_z_3_rangea;
Charles MacNeill 5:89031b2f5316 951 }
Charles MacNeill 5:89031b2f5316 952 else
Charles MacNeill 5:89031b2f5316 953 if (RangeDiff > tp->tp_uwr_med_z_4_min &&
Charles MacNeill 5:89031b2f5316 954 RangeDiff < tp->tp_uwr_med_z_4_max &&
Charles MacNeill 5:89031b2f5316 955 sequency == 0) {
Charles MacNeill 5:89031b2f5316 956 AddOffset =
Charles MacNeill 5:89031b2f5316 957 tp->tp_uwr_med_corr_z_4_rangea;
Charles MacNeill 5:89031b2f5316 958 }
Charles MacNeill 5:89031b2f5316 959 else
Charles MacNeill 5:89031b2f5316 960 if (RangeDiff < -tp->tp_uwr_med_z_4_min &&
Charles MacNeill 5:89031b2f5316 961 RangeDiff > -tp->tp_uwr_med_z_4_max &&
Charles MacNeill 5:89031b2f5316 962 sequency == 1) {
Charles MacNeill 5:89031b2f5316 963 AddOffset =
Charles MacNeill 5:89031b2f5316 964 tp->tp_uwr_med_corr_z_4_rangeb;
Charles MacNeill 5:89031b2f5316 965 }
Charles MacNeill 5:89031b2f5316 966 else
Charles MacNeill 5:89031b2f5316 967 if (RangeDiff < tp->tp_uwr_med_z_5_max &&
Charles MacNeill 5:89031b2f5316 968 RangeDiff > tp->tp_uwr_med_z_5_min) {
Charles MacNeill 5:89031b2f5316 969 AddOffset =
Charles MacNeill 5:89031b2f5316 970 tp->tp_uwr_med_corr_z_5_rangea;
Charles MacNeill 5:89031b2f5316 971 } else
Charles MacNeill 5:89031b2f5316 972 uwr_status = 0;
Charles MacNeill 5:89031b2f5316 973 break;
Charles MacNeill 5:89031b2f5316 974
Charles MacNeill 5:89031b2f5316 975 case VL53LX_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE:
Charles MacNeill 5:89031b2f5316 976 if (RangeDiff > tp->tp_uwr_lng_z_1_min &&
Charles MacNeill 5:89031b2f5316 977 RangeDiff < tp->tp_uwr_lng_z_1_max &&
Charles MacNeill 5:89031b2f5316 978 sequency == 0) {
Charles MacNeill 5:89031b2f5316 979 AddOffset =
Charles MacNeill 5:89031b2f5316 980 tp->tp_uwr_lng_corr_z_1_rangea;
Charles MacNeill 5:89031b2f5316 981 }
Charles MacNeill 5:89031b2f5316 982 else
Charles MacNeill 5:89031b2f5316 983 if (RangeDiff < -tp->tp_uwr_lng_z_1_min &&
Charles MacNeill 5:89031b2f5316 984 RangeDiff > -tp->tp_uwr_lng_z_1_max &&
Charles MacNeill 5:89031b2f5316 985 sequency == 1) {
Charles MacNeill 5:89031b2f5316 986 AddOffset =
Charles MacNeill 5:89031b2f5316 987 tp->tp_uwr_lng_corr_z_1_rangeb;
Charles MacNeill 5:89031b2f5316 988 }
Charles MacNeill 5:89031b2f5316 989 else
Charles MacNeill 5:89031b2f5316 990 if (RangeDiff > tp->tp_uwr_lng_z_2_min &&
Charles MacNeill 5:89031b2f5316 991 RangeDiff < tp->tp_uwr_lng_z_2_max &&
Charles MacNeill 5:89031b2f5316 992 sequency == 1) {
Charles MacNeill 5:89031b2f5316 993 AddOffset =
Charles MacNeill 5:89031b2f5316 994 tp->tp_uwr_lng_corr_z_2_rangeb;
Charles MacNeill 5:89031b2f5316 995 }
Charles MacNeill 5:89031b2f5316 996 else
Charles MacNeill 5:89031b2f5316 997 if (RangeDiff < -tp->tp_uwr_lng_z_2_min &&
Charles MacNeill 5:89031b2f5316 998 RangeDiff > -tp->tp_uwr_lng_z_2_max &&
Charles MacNeill 5:89031b2f5316 999 sequency == 0) {
Charles MacNeill 5:89031b2f5316 1000 AddOffset =
Charles MacNeill 5:89031b2f5316 1001 tp->tp_uwr_lng_corr_z_2_rangea;
Charles MacNeill 5:89031b2f5316 1002 }
Charles MacNeill 5:89031b2f5316 1003 else
Charles MacNeill 5:89031b2f5316 1004 if (RangeDiff < tp->tp_uwr_lng_z_3_max &&
Charles MacNeill 5:89031b2f5316 1005 RangeDiff > tp->tp_uwr_lng_z_3_min) {
Charles MacNeill 5:89031b2f5316 1006 AddOffset =
Charles MacNeill 5:89031b2f5316 1007 tp->tp_uwr_lng_corr_z_3_rangea;
Charles MacNeill 5:89031b2f5316 1008 }
Charles MacNeill 5:89031b2f5316 1009 else
Charles MacNeill 5:89031b2f5316 1010 uwr_status = 0;
Charles MacNeill 5:89031b2f5316 1011 break;
Charles MacNeill 5:89031b2f5316 1012
Charles MacNeill 5:89031b2f5316 1013 default:
Charles MacNeill 5:89031b2f5316 1014 uwr_status = 0;
Charles MacNeill 5:89031b2f5316 1015 break;
Charles MacNeill 5:89031b2f5316 1016 }
Charles MacNeill 5:89031b2f5316 1017 }
Charles MacNeill 5:89031b2f5316 1018
Charles MacNeill 5:89031b2f5316 1019 if (uwr_status) {
Charles MacNeill 5:89031b2f5316 1020 pRangeData->RangeMilliMeter += AddOffset;
Charles MacNeill 5:89031b2f5316 1021 pRangeData->RangeMinMilliMeter += AddOffset;
Charles MacNeill 5:89031b2f5316 1022 pRangeData->RangeMaxMilliMeter += AddOffset;
Charles MacNeill 5:89031b2f5316 1023 pRangeData->ExtendedRange = 1;
Charles MacNeill 5:89031b2f5316 1024 pRangeData->RangeStatus = 0;
Charles MacNeill 5:89031b2f5316 1025 }
Charles MacNeill 5:89031b2f5316 1026
Charles MacNeill 5:89031b2f5316 1027 }
Charles MacNeill 5:89031b2f5316 1028
Charles MacNeill 5:89031b2f5316 1029 pdev->PreviousRangeMilliMeter[iteration] = RangeMillimeterInit;
Charles MacNeill 5:89031b2f5316 1030 pdev->PreviousRangeStatus[iteration] = pRangeData->RangeStatus;
Charles MacNeill 5:89031b2f5316 1031 pdev->PreviousExtendedRange[iteration] = pRangeData->ExtendedRange;
Charles MacNeill 5:89031b2f5316 1032 pdev->PreviousStreamCount = pdev->hist_data.result__stream_count;
Charles MacNeill 5:89031b2f5316 1033
Charles MacNeill 5:89031b2f5316 1034 Range = pRangeData->RangeMilliMeter;
Charles MacNeill 5:89031b2f5316 1035 if ((pRangeData->RangeStatus == VL53LX_RANGESTATUS_RANGE_VALID) &&
Charles MacNeill 5:89031b2f5316 1036 (Range < 0)) {
Charles MacNeill 5:89031b2f5316 1037 if (Range < BDTable[VL53LX_TUNING_PROXY_MIN])
Charles MacNeill 5:89031b2f5316 1038 pRangeData->RangeStatus =
Charles MacNeill 5:89031b2f5316 1039 VL53LX_RANGESTATUS_RANGE_INVALID;
Charles MacNeill 5:89031b2f5316 1040 else
Charles MacNeill 5:89031b2f5316 1041 pRangeData->RangeMilliMeter = 0;
Charles MacNeill 5:89031b2f5316 1042 }
Charles MacNeill 5:89031b2f5316 1043
Charles MacNeill 5:89031b2f5316 1044 return Status;
Charles MacNeill 5:89031b2f5316 1045 }
Charles MacNeill 5:89031b2f5316 1046
Charles MacNeill 5:89031b2f5316 1047
Charles MacNeill 5:89031b2f5316 1048 static VL53LX_Error SetMeasurementData(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1049 VL53LX_range_results_t *presults,
Charles MacNeill 5:89031b2f5316 1050 VL53LX_MultiRangingData_t *pMultiRangingData)
Charles MacNeill 5:89031b2f5316 1051 {
Charles MacNeill 5:89031b2f5316 1052 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1053 uint8_t i;
Charles MacNeill 5:89031b2f5316 1054 uint8_t iteration;
Charles MacNeill 5:89031b2f5316 1055 VL53LX_TargetRangeData_t *pRangeData;
Charles MacNeill 5:89031b2f5316 1056 VL53LX_range_data_t *presults_data;
Charles MacNeill 5:89031b2f5316 1057 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1058 uint8_t ActiveResults;
Charles MacNeill 5:89031b2f5316 1059
Charles MacNeill 5:89031b2f5316 1060 pMultiRangingData->NumberOfObjectsFound = presults->active_results;
Charles MacNeill 5:89031b2f5316 1061 pMultiRangingData->HasXtalkValueChanged =
Charles MacNeill 5:89031b2f5316 1062 presults->smudge_corrector_data.new_xtalk_applied_flag;
Charles MacNeill 5:89031b2f5316 1063
Charles MacNeill 5:89031b2f5316 1064
Charles MacNeill 5:89031b2f5316 1065 pMultiRangingData->TimeStamp = 0;
Charles MacNeill 5:89031b2f5316 1066
Charles MacNeill 5:89031b2f5316 1067 pMultiRangingData->StreamCount = presults->stream_count;
Charles MacNeill 5:89031b2f5316 1068
Charles MacNeill 5:89031b2f5316 1069 ActiveResults = presults->active_results;
Charles MacNeill 5:89031b2f5316 1070 if (ActiveResults < 1)
Charles MacNeill 5:89031b2f5316 1071
Charles MacNeill 5:89031b2f5316 1072 iteration = 1;
Charles MacNeill 5:89031b2f5316 1073 else
Charles MacNeill 5:89031b2f5316 1074 iteration = ActiveResults;
Charles MacNeill 5:89031b2f5316 1075 for (i = 0; i < iteration; i++) {
Charles MacNeill 5:89031b2f5316 1076 pRangeData = &(pMultiRangingData->RangeData[i]);
Charles MacNeill 5:89031b2f5316 1077
Charles MacNeill 5:89031b2f5316 1078 presults_data = &(presults->VL53LX_p_003[i]);
Charles MacNeill 5:89031b2f5316 1079 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1080 Status = SetTargetData(Dev, ActiveResults,
Charles MacNeill 5:89031b2f5316 1081 pMultiRangingData->StreamCount,
Charles MacNeill 5:89031b2f5316 1082 i,
Charles MacNeill 5:89031b2f5316 1083 presults->device_status,
Charles MacNeill 5:89031b2f5316 1084 presults_data,
Charles MacNeill 5:89031b2f5316 1085 pRangeData);
Charles MacNeill 5:89031b2f5316 1086
Charles MacNeill 5:89031b2f5316 1087 pMultiRangingData->EffectiveSpadRtnCount =
Charles MacNeill 5:89031b2f5316 1088 presults_data->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 1089
Charles MacNeill 5:89031b2f5316 1090 }
Charles MacNeill 5:89031b2f5316 1091
Charles MacNeill 5:89031b2f5316 1092 for (i = iteration; i < VL53LX_MAX_RANGE_RESULTS; i++) {
Charles MacNeill 5:89031b2f5316 1093 pdev->PreviousRangeMilliMeter[i] = 0;
Charles MacNeill 5:89031b2f5316 1094 pdev->PreviousRangeStatus[i] = 255;
Charles MacNeill 5:89031b2f5316 1095 pdev->PreviousExtendedRange[i] = 0;
Charles MacNeill 5:89031b2f5316 1096 }
Charles MacNeill 5:89031b2f5316 1097
Charles MacNeill 5:89031b2f5316 1098 return Status;
Charles MacNeill 5:89031b2f5316 1099 }
Charles MacNeill 5:89031b2f5316 1100
Charles MacNeill 5:89031b2f5316 1101
Charles MacNeill 5:89031b2f5316 1102 VL53LX_Error VL53LX_GetMultiRangingData(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1103 VL53LX_MultiRangingData_t *pMultiRangingData)
Charles MacNeill 5:89031b2f5316 1104 {
Charles MacNeill 5:89031b2f5316 1105 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1106 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 1107 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1108 VL53LX_range_results_t *presults =
Charles MacNeill 5:89031b2f5316 1109 (VL53LX_range_results_t *) pdev->wArea1;
Charles MacNeill 5:89031b2f5316 1110
Charles MacNeill 5:89031b2f5316 1111 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1112
Charles MacNeill 5:89031b2f5316 1113
Charles MacNeill 5:89031b2f5316 1114 memset(pMultiRangingData, 0xFF,
Charles MacNeill 5:89031b2f5316 1115 sizeof(VL53LX_MultiRangingData_t));
Charles MacNeill 5:89031b2f5316 1116
Charles MacNeill 5:89031b2f5316 1117
Charles MacNeill 5:89031b2f5316 1118 Status = VL53LX_get_device_results(
Charles MacNeill 5:89031b2f5316 1119 Dev,
Charles MacNeill 5:89031b2f5316 1120 VL53LX_DEVICERESULTSLEVEL_FULL,
Charles MacNeill 5:89031b2f5316 1121 presults);
Charles MacNeill 5:89031b2f5316 1122
Charles MacNeill 5:89031b2f5316 1123 Status = SetMeasurementData(Dev,
Charles MacNeill 5:89031b2f5316 1124 presults,
Charles MacNeill 5:89031b2f5316 1125 pMultiRangingData);
Charles MacNeill 5:89031b2f5316 1126
Charles MacNeill 5:89031b2f5316 1127 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1128 return Status;
Charles MacNeill 5:89031b2f5316 1129 }
Charles MacNeill 5:89031b2f5316 1130
Charles MacNeill 5:89031b2f5316 1131 VL53LX_Error VL53LX_GetAdditionalData(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1132 VL53LX_AdditionalData_t *pAdditionalData)
Charles MacNeill 5:89031b2f5316 1133 {
Charles MacNeill 5:89031b2f5316 1134 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1135
Charles MacNeill 5:89031b2f5316 1136 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1137
Charles MacNeill 5:89031b2f5316 1138 Status = VL53LX_get_additional_data(Dev, pAdditionalData);
Charles MacNeill 5:89031b2f5316 1139
Charles MacNeill 5:89031b2f5316 1140 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1141 return Status;
Charles MacNeill 5:89031b2f5316 1142 }
Charles MacNeill 5:89031b2f5316 1143
Charles MacNeill 5:89031b2f5316 1144
Charles MacNeill 5:89031b2f5316 1145
Charles MacNeill 5:89031b2f5316 1146
Charles MacNeill 5:89031b2f5316 1147
Charles MacNeill 5:89031b2f5316 1148
Charles MacNeill 5:89031b2f5316 1149 VL53LX_Error VL53LX_SetTuningParameter(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1150 uint16_t TuningParameterId, int32_t TuningParameterValue)
Charles MacNeill 5:89031b2f5316 1151 {
Charles MacNeill 5:89031b2f5316 1152 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1153
Charles MacNeill 5:89031b2f5316 1154 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1155
Charles MacNeill 5:89031b2f5316 1156 if (TuningParameterId ==
Charles MacNeill 5:89031b2f5316 1157 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS)
Charles MacNeill 5:89031b2f5316 1158 return VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 1159
Charles MacNeill 5:89031b2f5316 1160 if (TuningParameterId >= 32768)
Charles MacNeill 5:89031b2f5316 1161 Status = VL53LX_set_tuning_parm(Dev,
Charles MacNeill 5:89031b2f5316 1162 TuningParameterId,
Charles MacNeill 5:89031b2f5316 1163 TuningParameterValue);
Charles MacNeill 5:89031b2f5316 1164 else {
Charles MacNeill 5:89031b2f5316 1165 if (TuningParameterId < VL53LX_TUNING_MAX_TUNABLE_KEY)
Charles MacNeill 5:89031b2f5316 1166 BDTable[TuningParameterId] = TuningParameterValue;
Charles MacNeill 5:89031b2f5316 1167 else
Charles MacNeill 5:89031b2f5316 1168 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 1169 }
Charles MacNeill 5:89031b2f5316 1170
Charles MacNeill 5:89031b2f5316 1171 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1172 return Status;
Charles MacNeill 5:89031b2f5316 1173 }
Charles MacNeill 5:89031b2f5316 1174
Charles MacNeill 5:89031b2f5316 1175 VL53LX_Error VL53LX_GetTuningParameter(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1176 uint16_t TuningParameterId, int32_t *pTuningParameterValue)
Charles MacNeill 5:89031b2f5316 1177 {
Charles MacNeill 5:89031b2f5316 1178 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1179
Charles MacNeill 5:89031b2f5316 1180 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1181
Charles MacNeill 5:89031b2f5316 1182 if (TuningParameterId >= 32768)
Charles MacNeill 5:89031b2f5316 1183 Status = VL53LX_get_tuning_parm(Dev,
Charles MacNeill 5:89031b2f5316 1184 TuningParameterId,
Charles MacNeill 5:89031b2f5316 1185 pTuningParameterValue);
Charles MacNeill 5:89031b2f5316 1186 else {
Charles MacNeill 5:89031b2f5316 1187 if (TuningParameterId < VL53LX_TUNING_MAX_TUNABLE_KEY)
Charles MacNeill 5:89031b2f5316 1188 *pTuningParameterValue = BDTable[TuningParameterId];
Charles MacNeill 5:89031b2f5316 1189 else
Charles MacNeill 5:89031b2f5316 1190 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 1191 }
Charles MacNeill 5:89031b2f5316 1192
Charles MacNeill 5:89031b2f5316 1193 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1194 return Status;
Charles MacNeill 5:89031b2f5316 1195 }
Charles MacNeill 5:89031b2f5316 1196
Charles MacNeill 5:89031b2f5316 1197
Charles MacNeill 5:89031b2f5316 1198 VL53LX_Error VL53LX_PerformRefSpadManagement(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1199 {
Charles MacNeill 5:89031b2f5316 1200 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1201 VL53LX_Error RawStatus;
Charles MacNeill 5:89031b2f5316 1202 uint8_t dcrbuffer[24];
Charles MacNeill 5:89031b2f5316 1203 uint8_t *commbuf;
Charles MacNeill 5:89031b2f5316 1204 uint8_t numloc[2] = {5, 3};
Charles MacNeill 5:89031b2f5316 1205 VL53LX_LLDriverData_t *pdev;
Charles MacNeill 5:89031b2f5316 1206 VL53LX_customer_nvm_managed_t *pc;
Charles MacNeill 5:89031b2f5316 1207 VL53LX_DistanceModes DistanceMode;
Charles MacNeill 5:89031b2f5316 1208
Charles MacNeill 5:89031b2f5316 1209 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1210
Charles MacNeill 5:89031b2f5316 1211 pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1212 pc = &pdev->customer;
Charles MacNeill 5:89031b2f5316 1213
Charles MacNeill 5:89031b2f5316 1214 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1215 DistanceMode = VL53LXDevDataGet(Dev,
Charles MacNeill 5:89031b2f5316 1216 CurrentParameters.DistanceMode);
Charles MacNeill 5:89031b2f5316 1217 Status = VL53LX_run_ref_spad_char(Dev, &RawStatus);
Charles MacNeill 5:89031b2f5316 1218
Charles MacNeill 5:89031b2f5316 1219 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1220 Status = VL53LX_SetDistanceMode(Dev, DistanceMode);
Charles MacNeill 5:89031b2f5316 1221 }
Charles MacNeill 5:89031b2f5316 1222
Charles MacNeill 5:89031b2f5316 1223 if (Status == VL53LX_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH) {
Charles MacNeill 5:89031b2f5316 1224
Charles MacNeill 5:89031b2f5316 1225 Status = VL53LX_read_nvm_raw_data(Dev,
Charles MacNeill 5:89031b2f5316 1226 (uint8_t)(0xA0 >> 2),
Charles MacNeill 5:89031b2f5316 1227 (uint8_t)(24 >> 2),
Charles MacNeill 5:89031b2f5316 1228 dcrbuffer);
Charles MacNeill 5:89031b2f5316 1229
Charles MacNeill 5:89031b2f5316 1230 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1231 Status = VL53LX_WriteMulti(Dev,
Charles MacNeill 5:89031b2f5316 1232 VL53LX_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
Charles MacNeill 5:89031b2f5316 1233 numloc, 2);
Charles MacNeill 5:89031b2f5316 1234
Charles MacNeill 5:89031b2f5316 1235 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1236 pc->ref_spad_man__num_requested_ref_spads = numloc[0];
Charles MacNeill 5:89031b2f5316 1237 pc->ref_spad_man__ref_location = numloc[1];
Charles MacNeill 5:89031b2f5316 1238 }
Charles MacNeill 5:89031b2f5316 1239
Charles MacNeill 5:89031b2f5316 1240 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1241 commbuf = &dcrbuffer[16];
Charles MacNeill 5:89031b2f5316 1242
Charles MacNeill 5:89031b2f5316 1243
Charles MacNeill 5:89031b2f5316 1244
Charles MacNeill 5:89031b2f5316 1245 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1246 Status = VL53LX_WriteMulti(Dev,
Charles MacNeill 5:89031b2f5316 1247 VL53LX_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
Charles MacNeill 5:89031b2f5316 1248 commbuf, 6);
Charles MacNeill 5:89031b2f5316 1249
Charles MacNeill 5:89031b2f5316 1250 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1251 pc->global_config__spad_enables_ref_0 = commbuf[0];
Charles MacNeill 5:89031b2f5316 1252 pc->global_config__spad_enables_ref_1 = commbuf[1];
Charles MacNeill 5:89031b2f5316 1253 pc->global_config__spad_enables_ref_2 = commbuf[2];
Charles MacNeill 5:89031b2f5316 1254 pc->global_config__spad_enables_ref_3 = commbuf[3];
Charles MacNeill 5:89031b2f5316 1255 pc->global_config__spad_enables_ref_4 = commbuf[4];
Charles MacNeill 5:89031b2f5316 1256 pc->global_config__spad_enables_ref_5 = commbuf[5];
Charles MacNeill 5:89031b2f5316 1257 }
Charles MacNeill 5:89031b2f5316 1258
Charles MacNeill 5:89031b2f5316 1259 }
Charles MacNeill 5:89031b2f5316 1260
Charles MacNeill 5:89031b2f5316 1261 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1262 return Status;
Charles MacNeill 5:89031b2f5316 1263 }
Charles MacNeill 5:89031b2f5316 1264
Charles MacNeill 5:89031b2f5316 1265
Charles MacNeill 5:89031b2f5316 1266 VL53LX_Error VL53LX_SmudgeCorrectionEnable(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1267 VL53LX_SmudgeCorrectionModes Mode)
Charles MacNeill 5:89031b2f5316 1268 {
Charles MacNeill 5:89031b2f5316 1269 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1270 VL53LX_Error s1 = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1271 VL53LX_Error s2 = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1272 VL53LX_Error s3 = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1273
Charles MacNeill 5:89031b2f5316 1274 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1275
Charles MacNeill 5:89031b2f5316 1276 switch (Mode) {
Charles MacNeill 5:89031b2f5316 1277 case VL53LX_SMUDGE_CORRECTION_NONE:
Charles MacNeill 5:89031b2f5316 1278 s1 = VL53LX_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 5:89031b2f5316 1279 s2 = VL53LX_dynamic_xtalk_correction_apply_disable(Dev);
Charles MacNeill 5:89031b2f5316 1280 s3 = VL53LX_dynamic_xtalk_correction_single_apply_disable(Dev);
Charles MacNeill 5:89031b2f5316 1281 break;
Charles MacNeill 5:89031b2f5316 1282 case VL53LX_SMUDGE_CORRECTION_CONTINUOUS:
Charles MacNeill 5:89031b2f5316 1283 s1 = VL53LX_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 5:89031b2f5316 1284 s2 = VL53LX_dynamic_xtalk_correction_apply_enable(Dev);
Charles MacNeill 5:89031b2f5316 1285 s3 = VL53LX_dynamic_xtalk_correction_single_apply_disable(Dev);
Charles MacNeill 5:89031b2f5316 1286 break;
Charles MacNeill 5:89031b2f5316 1287 case VL53LX_SMUDGE_CORRECTION_SINGLE:
Charles MacNeill 5:89031b2f5316 1288 s1 = VL53LX_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 5:89031b2f5316 1289 s2 = VL53LX_dynamic_xtalk_correction_apply_enable(Dev);
Charles MacNeill 5:89031b2f5316 1290 s3 = VL53LX_dynamic_xtalk_correction_single_apply_enable(Dev);
Charles MacNeill 5:89031b2f5316 1291 break;
Charles MacNeill 5:89031b2f5316 1292 case VL53LX_SMUDGE_CORRECTION_DEBUG:
Charles MacNeill 5:89031b2f5316 1293 s1 = VL53LX_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 5:89031b2f5316 1294 s2 = VL53LX_dynamic_xtalk_correction_apply_disable(Dev);
Charles MacNeill 5:89031b2f5316 1295 s3 = VL53LX_dynamic_xtalk_correction_single_apply_disable(Dev);
Charles MacNeill 5:89031b2f5316 1296 break;
Charles MacNeill 5:89031b2f5316 1297 default:
Charles MacNeill 5:89031b2f5316 1298 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 1299 break;
Charles MacNeill 5:89031b2f5316 1300 }
Charles MacNeill 5:89031b2f5316 1301
Charles MacNeill 5:89031b2f5316 1302 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1303 Status = s1;
Charles MacNeill 5:89031b2f5316 1304 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1305 Status = s2;
Charles MacNeill 5:89031b2f5316 1306 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1307 Status = s3;
Charles MacNeill 5:89031b2f5316 1308 }
Charles MacNeill 5:89031b2f5316 1309
Charles MacNeill 5:89031b2f5316 1310 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1311 return Status;
Charles MacNeill 5:89031b2f5316 1312 }
Charles MacNeill 5:89031b2f5316 1313
Charles MacNeill 5:89031b2f5316 1314
Charles MacNeill 5:89031b2f5316 1315 VL53LX_Error VL53LX_SetXTalkCompensationEnable(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1316 uint8_t XTalkCompensationEnable)
Charles MacNeill 5:89031b2f5316 1317 {
Charles MacNeill 5:89031b2f5316 1318 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1319
Charles MacNeill 5:89031b2f5316 1320 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1321
Charles MacNeill 5:89031b2f5316 1322 if (XTalkCompensationEnable == 0)
Charles MacNeill 5:89031b2f5316 1323 Status = VL53LX_disable_xtalk_compensation(Dev);
Charles MacNeill 5:89031b2f5316 1324 else
Charles MacNeill 5:89031b2f5316 1325 Status = VL53LX_enable_xtalk_compensation(Dev);
Charles MacNeill 5:89031b2f5316 1326
Charles MacNeill 5:89031b2f5316 1327 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1328 return Status;
Charles MacNeill 5:89031b2f5316 1329 }
Charles MacNeill 5:89031b2f5316 1330
Charles MacNeill 5:89031b2f5316 1331
Charles MacNeill 5:89031b2f5316 1332 VL53LX_Error VL53LX_GetXTalkCompensationEnable(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1333 uint8_t *pXTalkCompensationEnable)
Charles MacNeill 5:89031b2f5316 1334 {
Charles MacNeill 5:89031b2f5316 1335 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1336
Charles MacNeill 5:89031b2f5316 1337 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1338
Charles MacNeill 5:89031b2f5316 1339 VL53LX_get_xtalk_compensation_enable(
Charles MacNeill 5:89031b2f5316 1340 Dev,
Charles MacNeill 5:89031b2f5316 1341 pXTalkCompensationEnable);
Charles MacNeill 5:89031b2f5316 1342
Charles MacNeill 5:89031b2f5316 1343 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1344 return Status;
Charles MacNeill 5:89031b2f5316 1345 }
Charles MacNeill 5:89031b2f5316 1346
Charles MacNeill 5:89031b2f5316 1347 VL53LX_Error VL53LX_PerformXTalkCalibration(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1348 {
Charles MacNeill 5:89031b2f5316 1349 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1350 VL53LX_Error UStatus;
Charles MacNeill 5:89031b2f5316 1351 int16_t CalDistanceMm;
Charles MacNeill 5:89031b2f5316 1352 VL53LX_xtalk_calibration_results_t xtalk;
Charles MacNeill 5:89031b2f5316 1353
Charles MacNeill 5:89031b2f5316 1354 VL53LX_CalibrationData_t caldata;
Charles MacNeill 5:89031b2f5316 1355 VL53LX_LLDriverData_t *pLLData;
Charles MacNeill 5:89031b2f5316 1356 int i;
Charles MacNeill 5:89031b2f5316 1357 uint32_t *pPlaneOffsetKcps;
Charles MacNeill 5:89031b2f5316 1358 uint32_t Margin =
Charles MacNeill 5:89031b2f5316 1359 BDTable[VL53LX_TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN];
Charles MacNeill 5:89031b2f5316 1360 uint32_t DefaultOffset =
Charles MacNeill 5:89031b2f5316 1361 BDTable[VL53LX_TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET];
Charles MacNeill 5:89031b2f5316 1362 uint32_t *pLLDataPlaneOffsetKcps;
Charles MacNeill 5:89031b2f5316 1363 uint32_t sum = 0;
Charles MacNeill 5:89031b2f5316 1364 uint8_t binok = 0;
Charles MacNeill 5:89031b2f5316 1365
Charles MacNeill 5:89031b2f5316 1366 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1367
Charles MacNeill 5:89031b2f5316 1368 pPlaneOffsetKcps =
Charles MacNeill 5:89031b2f5316 1369 &caldata.customer.algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 1370 pLLData = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1371 pLLDataPlaneOffsetKcps =
Charles MacNeill 5:89031b2f5316 1372 &pLLData->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 1373
Charles MacNeill 5:89031b2f5316 1374 CalDistanceMm = (int16_t)
Charles MacNeill 5:89031b2f5316 1375 BDTable[VL53LX_TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM];
Charles MacNeill 5:89031b2f5316 1376 Status = VL53LX_run_hist_xtalk_extraction(Dev, CalDistanceMm,
Charles MacNeill 5:89031b2f5316 1377 &UStatus);
Charles MacNeill 5:89031b2f5316 1378
Charles MacNeill 5:89031b2f5316 1379 VL53LX_GetCalibrationData(Dev, &caldata);
Charles MacNeill 5:89031b2f5316 1380 for (i = 0; i < VL53LX_XTALK_HISTO_BINS; i++) {
Charles MacNeill 5:89031b2f5316 1381 sum += caldata.xtalkhisto.xtalk_shape.bin_data[i];
Charles MacNeill 5:89031b2f5316 1382 if (caldata.xtalkhisto.xtalk_shape.bin_data[i] > 0)
Charles MacNeill 5:89031b2f5316 1383 binok++;
Charles MacNeill 5:89031b2f5316 1384 }
Charles MacNeill 5:89031b2f5316 1385 if ((UStatus ==
Charles MacNeill 5:89031b2f5316 1386 VL53LX_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL) ||
Charles MacNeill 5:89031b2f5316 1387 (sum > (1024 + Margin)) || (sum < (1024 - Margin)) ||
Charles MacNeill 5:89031b2f5316 1388 (binok < 3)) {
Charles MacNeill 5:89031b2f5316 1389 *pPlaneOffsetKcps = DefaultOffset;
Charles MacNeill 5:89031b2f5316 1390 *pLLDataPlaneOffsetKcps = DefaultOffset;
Charles MacNeill 5:89031b2f5316 1391 caldata.xtalkhisto.xtalk_shape.bin_data[0] = 307;
Charles MacNeill 5:89031b2f5316 1392 caldata.xtalkhisto.xtalk_shape.bin_data[1] = 410;
Charles MacNeill 5:89031b2f5316 1393 caldata.xtalkhisto.xtalk_shape.bin_data[2] = 410;
Charles MacNeill 5:89031b2f5316 1394 caldata.xtalkhisto.xtalk_shape.bin_data[3] = 307;
Charles MacNeill 5:89031b2f5316 1395 for (i = 4; i < VL53LX_XTALK_HISTO_BINS; i++)
Charles MacNeill 5:89031b2f5316 1396 caldata.xtalkhisto.xtalk_shape.bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 1397 for (i = 0; i < VL53LX_BIN_REC_SIZE; i++)
Charles MacNeill 5:89031b2f5316 1398 caldata.algo__xtalk_cpo_HistoMerge_kcps[i] =
Charles MacNeill 5:89031b2f5316 1399 DefaultOffset + DefaultOffset * i;
Charles MacNeill 5:89031b2f5316 1400 VL53LX_SetCalibrationData(Dev, &caldata);
Charles MacNeill 5:89031b2f5316 1401 }
Charles MacNeill 5:89031b2f5316 1402
Charles MacNeill 5:89031b2f5316 1403 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1404 Status = VL53LX_get_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 5:89031b2f5316 1405 Status = VL53LX_set_tuning_parm(Dev,
Charles MacNeill 5:89031b2f5316 1406 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS,
Charles MacNeill 5:89031b2f5316 1407 xtalk.algo__crosstalk_compensation_plane_offset_kcps);
Charles MacNeill 5:89031b2f5316 1408 }
Charles MacNeill 5:89031b2f5316 1409
Charles MacNeill 5:89031b2f5316 1410 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1411 return Status;
Charles MacNeill 5:89031b2f5316 1412 }
Charles MacNeill 5:89031b2f5316 1413
Charles MacNeill 5:89031b2f5316 1414
Charles MacNeill 5:89031b2f5316 1415 VL53LX_Error VL53LX_SetOffsetCorrectionMode(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1416 VL53LX_OffsetCorrectionModes OffsetCorrectionMode)
Charles MacNeill 5:89031b2f5316 1417 {
Charles MacNeill 5:89031b2f5316 1418 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1419 VL53LX_OffsetCorrectionMode offset_cor_mode;
Charles MacNeill 5:89031b2f5316 1420
Charles MacNeill 5:89031b2f5316 1421 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1422
Charles MacNeill 5:89031b2f5316 1423 if (OffsetCorrectionMode == VL53LX_OFFSETCORRECTIONMODE_STANDARD) {
Charles MacNeill 5:89031b2f5316 1424 offset_cor_mode =
Charles MacNeill 5:89031b2f5316 1425 VL53LX_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS;
Charles MacNeill 5:89031b2f5316 1426 } else if (OffsetCorrectionMode ==
Charles MacNeill 5:89031b2f5316 1427 VL53LX_OFFSETCORRECTIONMODE_PERVCSEL) {
Charles MacNeill 5:89031b2f5316 1428 offset_cor_mode =
Charles MacNeill 5:89031b2f5316 1429 VL53LX_OFFSETCORRECTIONMODE__PER_VCSEL_OFFSETS;
Charles MacNeill 5:89031b2f5316 1430 } else {
Charles MacNeill 5:89031b2f5316 1431 Status = VL53LX_ERROR_INVALID_PARAMS;
Charles MacNeill 5:89031b2f5316 1432 }
Charles MacNeill 5:89031b2f5316 1433
Charles MacNeill 5:89031b2f5316 1434 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1435 Status = VL53LX_set_offset_correction_mode(Dev,
Charles MacNeill 5:89031b2f5316 1436 offset_cor_mode);
Charles MacNeill 5:89031b2f5316 1437
Charles MacNeill 5:89031b2f5316 1438 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1439 return Status;
Charles MacNeill 5:89031b2f5316 1440 }
Charles MacNeill 5:89031b2f5316 1441
Charles MacNeill 5:89031b2f5316 1442
Charles MacNeill 5:89031b2f5316 1443 VL53LX_Error VL53LX_PerformOffsetSimpleCalibration(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1444 int32_t CalDistanceMilliMeter)
Charles MacNeill 5:89031b2f5316 1445 {
Charles MacNeill 5:89031b2f5316 1446 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1447 int32_t sum_ranging;
Charles MacNeill 5:89031b2f5316 1448 uint8_t offset_meas;
Charles MacNeill 5:89031b2f5316 1449 int16_t Max, UnderMax, OverMax, Repeat;
Charles MacNeill 5:89031b2f5316 1450 int32_t total_count, inloopcount;
Charles MacNeill 5:89031b2f5316 1451 int32_t IncRounding;
Charles MacNeill 5:89031b2f5316 1452 int16_t meanDistance_mm;
Charles MacNeill 5:89031b2f5316 1453 int16_t offset;
Charles MacNeill 5:89031b2f5316 1454 VL53LX_MultiRangingData_t RangingMeasurementData;
Charles MacNeill 5:89031b2f5316 1455 VL53LX_LLDriverData_t *pdev;
Charles MacNeill 5:89031b2f5316 1456 uint8_t goodmeas;
Charles MacNeill 5:89031b2f5316 1457 VL53LX_Error SmudgeStatus = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1458 uint8_t smudge_corr_en;
Charles MacNeill 5:89031b2f5316 1459 VL53LX_TargetRangeData_t *pRange;
Charles MacNeill 5:89031b2f5316 1460
Charles MacNeill 5:89031b2f5316 1461 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1462
Charles MacNeill 5:89031b2f5316 1463 pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1464
Charles MacNeill 5:89031b2f5316 1465 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
Charles MacNeill 5:89031b2f5316 1466 SmudgeStatus = VL53LX_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 5:89031b2f5316 1467
Charles MacNeill 5:89031b2f5316 1468 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1469 pdev->customer.mm_config__inner_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1470 pdev->customer.mm_config__outer_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1471 memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 5:89031b2f5316 1472 Repeat = BDTable[VL53LX_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
Charles MacNeill 5:89031b2f5316 1473 Max = BDTable[
Charles MacNeill 5:89031b2f5316 1474 VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
Charles MacNeill 5:89031b2f5316 1475 UnderMax = 1 + (Max / 2);
Charles MacNeill 5:89031b2f5316 1476 OverMax = Max + (Max / 2);
Charles MacNeill 5:89031b2f5316 1477 sum_ranging = 0;
Charles MacNeill 5:89031b2f5316 1478 total_count = 0;
Charles MacNeill 5:89031b2f5316 1479
Charles MacNeill 5:89031b2f5316 1480 while ((Repeat > 0) && (Status == VL53LX_ERROR_NONE)) {
Charles MacNeill 5:89031b2f5316 1481 Status = VL53LX_StartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1482
Charles MacNeill 5:89031b2f5316 1483 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1484 VL53LX_WaitMeasurementDataReady(Dev);
Charles MacNeill 5:89031b2f5316 1485 VL53LX_GetMultiRangingData(Dev,
Charles MacNeill 5:89031b2f5316 1486 &RangingMeasurementData);
Charles MacNeill 5:89031b2f5316 1487 VL53LX_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1488 }
Charles MacNeill 5:89031b2f5316 1489
Charles MacNeill 5:89031b2f5316 1490 inloopcount = 0;
Charles MacNeill 5:89031b2f5316 1491 offset_meas = 0;
Charles MacNeill 5:89031b2f5316 1492 while ((Status == VL53LX_ERROR_NONE) && (inloopcount < Max) &&
Charles MacNeill 5:89031b2f5316 1493 (offset_meas < OverMax)) {
Charles MacNeill 5:89031b2f5316 1494 Status = VL53LX_WaitMeasurementDataReady(Dev);
Charles MacNeill 5:89031b2f5316 1495 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1496 Status = VL53LX_GetMultiRangingData(Dev,
Charles MacNeill 5:89031b2f5316 1497 &RangingMeasurementData);
Charles MacNeill 5:89031b2f5316 1498 pRange = &(RangingMeasurementData.RangeData[0]);
Charles MacNeill 5:89031b2f5316 1499 goodmeas = (pRange->RangeStatus ==
Charles MacNeill 5:89031b2f5316 1500 VL53LX_RANGESTATUS_RANGE_VALID);
Charles MacNeill 5:89031b2f5316 1501 if ((Status == VL53LX_ERROR_NONE) && goodmeas) {
Charles MacNeill 5:89031b2f5316 1502 sum_ranging += pRange->RangeMilliMeter;
Charles MacNeill 5:89031b2f5316 1503 inloopcount++;
Charles MacNeill 5:89031b2f5316 1504 }
Charles MacNeill 5:89031b2f5316 1505 Status = VL53LX_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1506 offset_meas++;
Charles MacNeill 5:89031b2f5316 1507 }
Charles MacNeill 5:89031b2f5316 1508 total_count += inloopcount;
Charles MacNeill 5:89031b2f5316 1509
Charles MacNeill 5:89031b2f5316 1510
Charles MacNeill 5:89031b2f5316 1511 if (inloopcount < UnderMax)
Charles MacNeill 5:89031b2f5316 1512 Status = VL53LX_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
Charles MacNeill 5:89031b2f5316 1513
Charles MacNeill 5:89031b2f5316 1514 VL53LX_StopMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1515
Charles MacNeill 5:89031b2f5316 1516 Repeat--;
Charles MacNeill 5:89031b2f5316 1517
Charles MacNeill 5:89031b2f5316 1518 }
Charles MacNeill 5:89031b2f5316 1519
Charles MacNeill 5:89031b2f5316 1520 if ((SmudgeStatus == VL53LX_ERROR_NONE) && (smudge_corr_en == 1))
Charles MacNeill 5:89031b2f5316 1521 SmudgeStatus = VL53LX_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 5:89031b2f5316 1522
Charles MacNeill 5:89031b2f5316 1523 if ((sum_ranging < 0) ||
Charles MacNeill 5:89031b2f5316 1524 (sum_ranging > ((int32_t) total_count * 0xffff)))
Charles MacNeill 5:89031b2f5316 1525 Status = VL53LX_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
Charles MacNeill 5:89031b2f5316 1526
Charles MacNeill 5:89031b2f5316 1527 if ((Status == VL53LX_ERROR_NONE) && (total_count > 0)) {
Charles MacNeill 5:89031b2f5316 1528 IncRounding = total_count / 2;
Charles MacNeill 5:89031b2f5316 1529 meanDistance_mm = (int16_t)((sum_ranging + IncRounding)
Charles MacNeill 5:89031b2f5316 1530 / total_count);
Charles MacNeill 5:89031b2f5316 1531 offset = (int16_t)CalDistanceMilliMeter - meanDistance_mm;
Charles MacNeill 5:89031b2f5316 1532 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1533 pdev->customer.mm_config__inner_offset_mm = offset;
Charles MacNeill 5:89031b2f5316 1534 pdev->customer.mm_config__outer_offset_mm = offset;
Charles MacNeill 5:89031b2f5316 1535
Charles MacNeill 5:89031b2f5316 1536 Status = VL53LX_set_customer_nvm_managed(Dev,
Charles MacNeill 5:89031b2f5316 1537 &(pdev->customer));
Charles MacNeill 5:89031b2f5316 1538 }
Charles MacNeill 5:89031b2f5316 1539
Charles MacNeill 5:89031b2f5316 1540 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1541 return Status;
Charles MacNeill 5:89031b2f5316 1542 }
Charles MacNeill 5:89031b2f5316 1543
Charles MacNeill 5:89031b2f5316 1544 VL53LX_Error VL53LX_PerformOffsetZeroDistanceCalibration(VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1545 {
Charles MacNeill 5:89031b2f5316 1546 #define START_OFFSET 50
Charles MacNeill 5:89031b2f5316 1547 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1548 int32_t sum_ranging;
Charles MacNeill 5:89031b2f5316 1549 uint8_t offset_meas;
Charles MacNeill 5:89031b2f5316 1550 int16_t Max, UnderMax, OverMax, Repeat;
Charles MacNeill 5:89031b2f5316 1551 int32_t total_count, inloopcount;
Charles MacNeill 5:89031b2f5316 1552 int32_t IncRounding;
Charles MacNeill 5:89031b2f5316 1553 int16_t meanDistance_mm;
Charles MacNeill 5:89031b2f5316 1554 int16_t offset, ZeroDistanceOffset;
Charles MacNeill 5:89031b2f5316 1555 VL53LX_MultiRangingData_t RangingMeasurementData;
Charles MacNeill 5:89031b2f5316 1556 VL53LX_LLDriverData_t *pdev;
Charles MacNeill 5:89031b2f5316 1557 uint8_t goodmeas;
Charles MacNeill 5:89031b2f5316 1558 VL53LX_Error SmudgeStatus = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1559 uint8_t smudge_corr_en;
Charles MacNeill 5:89031b2f5316 1560 VL53LX_TargetRangeData_t *pRange;
Charles MacNeill 5:89031b2f5316 1561
Charles MacNeill 5:89031b2f5316 1562 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1563
Charles MacNeill 5:89031b2f5316 1564 pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1565 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
Charles MacNeill 5:89031b2f5316 1566 SmudgeStatus = VL53LX_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 5:89031b2f5316 1567 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1568 pdev->customer.mm_config__inner_offset_mm = START_OFFSET;
Charles MacNeill 5:89031b2f5316 1569 pdev->customer.mm_config__outer_offset_mm = START_OFFSET;
Charles MacNeill 5:89031b2f5316 1570 memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 5:89031b2f5316 1571 ZeroDistanceOffset = BDTable[
Charles MacNeill 5:89031b2f5316 1572 VL53LX_TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR];
Charles MacNeill 5:89031b2f5316 1573 Repeat = BDTable[VL53LX_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
Charles MacNeill 5:89031b2f5316 1574 Max =
Charles MacNeill 5:89031b2f5316 1575 BDTable[VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
Charles MacNeill 5:89031b2f5316 1576 UnderMax = 1 + (Max / 2);
Charles MacNeill 5:89031b2f5316 1577 OverMax = Max + (Max / 2);
Charles MacNeill 5:89031b2f5316 1578 sum_ranging = 0;
Charles MacNeill 5:89031b2f5316 1579 total_count = 0;
Charles MacNeill 5:89031b2f5316 1580
Charles MacNeill 5:89031b2f5316 1581 while ((Repeat > 0) && (Status == VL53LX_ERROR_NONE)) {
Charles MacNeill 5:89031b2f5316 1582 Status = VL53LX_StartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1583 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1584 VL53LX_WaitMeasurementDataReady(Dev);
Charles MacNeill 5:89031b2f5316 1585 VL53LX_GetMultiRangingData(Dev,
Charles MacNeill 5:89031b2f5316 1586 &RangingMeasurementData);
Charles MacNeill 5:89031b2f5316 1587 VL53LX_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1588 }
Charles MacNeill 5:89031b2f5316 1589 inloopcount = 0;
Charles MacNeill 5:89031b2f5316 1590 offset_meas = 0;
Charles MacNeill 5:89031b2f5316 1591 while ((Status == VL53LX_ERROR_NONE) && (inloopcount < Max) &&
Charles MacNeill 5:89031b2f5316 1592 (offset_meas < OverMax)) {
Charles MacNeill 5:89031b2f5316 1593 Status = VL53LX_WaitMeasurementDataReady(Dev);
Charles MacNeill 5:89031b2f5316 1594 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1595 Status = VL53LX_GetMultiRangingData(Dev,
Charles MacNeill 5:89031b2f5316 1596 &RangingMeasurementData);
Charles MacNeill 5:89031b2f5316 1597 pRange = &(RangingMeasurementData.RangeData[0]);
Charles MacNeill 5:89031b2f5316 1598 goodmeas = (pRange->RangeStatus ==
Charles MacNeill 5:89031b2f5316 1599 VL53LX_RANGESTATUS_RANGE_VALID);
Charles MacNeill 5:89031b2f5316 1600 if ((Status == VL53LX_ERROR_NONE) && goodmeas) {
Charles MacNeill 5:89031b2f5316 1601 sum_ranging = sum_ranging +
Charles MacNeill 5:89031b2f5316 1602 pRange->RangeMilliMeter;
Charles MacNeill 5:89031b2f5316 1603 inloopcount++;
Charles MacNeill 5:89031b2f5316 1604 }
Charles MacNeill 5:89031b2f5316 1605 Status = VL53LX_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1606 offset_meas++;
Charles MacNeill 5:89031b2f5316 1607 }
Charles MacNeill 5:89031b2f5316 1608 total_count += inloopcount;
Charles MacNeill 5:89031b2f5316 1609 if (inloopcount < UnderMax)
Charles MacNeill 5:89031b2f5316 1610 Status = VL53LX_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
Charles MacNeill 5:89031b2f5316 1611 VL53LX_StopMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1612 Repeat--;
Charles MacNeill 5:89031b2f5316 1613 }
Charles MacNeill 5:89031b2f5316 1614 if ((SmudgeStatus == VL53LX_ERROR_NONE) && (smudge_corr_en == 1))
Charles MacNeill 5:89031b2f5316 1615 SmudgeStatus = VL53LX_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 5:89031b2f5316 1616 if ((sum_ranging < 0) ||
Charles MacNeill 5:89031b2f5316 1617 (sum_ranging > ((int32_t) total_count * 0xffff)))
Charles MacNeill 5:89031b2f5316 1618 Status = VL53LX_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
Charles MacNeill 5:89031b2f5316 1619
Charles MacNeill 5:89031b2f5316 1620 if ((Status == VL53LX_ERROR_NONE) && (total_count > 0)) {
Charles MacNeill 5:89031b2f5316 1621 IncRounding = total_count / 2;
Charles MacNeill 5:89031b2f5316 1622 meanDistance_mm = (int16_t)
Charles MacNeill 5:89031b2f5316 1623 ((sum_ranging + IncRounding) / total_count);
Charles MacNeill 5:89031b2f5316 1624 offset = START_OFFSET - meanDistance_mm + ZeroDistanceOffset;
Charles MacNeill 5:89031b2f5316 1625 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1626 pdev->customer.mm_config__inner_offset_mm = offset;
Charles MacNeill 5:89031b2f5316 1627 pdev->customer.mm_config__outer_offset_mm = offset;
Charles MacNeill 5:89031b2f5316 1628 Status = VL53LX_set_customer_nvm_managed(Dev,
Charles MacNeill 5:89031b2f5316 1629 &(pdev->customer));
Charles MacNeill 5:89031b2f5316 1630 }
Charles MacNeill 5:89031b2f5316 1631
Charles MacNeill 5:89031b2f5316 1632 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1633 return Status;
Charles MacNeill 5:89031b2f5316 1634 }
Charles MacNeill 5:89031b2f5316 1635
Charles MacNeill 5:89031b2f5316 1636 VL53LX_Error VL53LX_SetCalibrationData(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1637 VL53LX_CalibrationData_t *pCalibrationData)
Charles MacNeill 5:89031b2f5316 1638 {
Charles MacNeill 5:89031b2f5316 1639 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1640 VL53LX_CustomerNvmManaged_t *pC;
Charles MacNeill 5:89031b2f5316 1641 VL53LX_calibration_data_t cal_data;
Charles MacNeill 5:89031b2f5316 1642 uint32_t x;
Charles MacNeill 5:89031b2f5316 1643 VL53LX_xtalk_calibration_results_t xtalk;
Charles MacNeill 5:89031b2f5316 1644
Charles MacNeill 5:89031b2f5316 1645 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1646
Charles MacNeill 5:89031b2f5316 1647 cal_data.struct_version = pCalibrationData->struct_version -
Charles MacNeill 5:89031b2f5316 1648 VL53LX_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
Charles MacNeill 5:89031b2f5316 1649
Charles MacNeill 5:89031b2f5316 1650
Charles MacNeill 5:89031b2f5316 1651 memcpy(
Charles MacNeill 5:89031b2f5316 1652 &(cal_data.add_off_cal_data),
Charles MacNeill 5:89031b2f5316 1653 &(pCalibrationData->add_off_cal_data),
Charles MacNeill 5:89031b2f5316 1654 sizeof(VL53LX_additional_offset_cal_data_t));
Charles MacNeill 5:89031b2f5316 1655
Charles MacNeill 5:89031b2f5316 1656
Charles MacNeill 5:89031b2f5316 1657 memcpy(
Charles MacNeill 5:89031b2f5316 1658 &(cal_data.optical_centre),
Charles MacNeill 5:89031b2f5316 1659 &(pCalibrationData->optical_centre),
Charles MacNeill 5:89031b2f5316 1660 sizeof(VL53LX_optical_centre_t));
Charles MacNeill 5:89031b2f5316 1661
Charles MacNeill 5:89031b2f5316 1662
Charles MacNeill 5:89031b2f5316 1663 memcpy(
Charles MacNeill 5:89031b2f5316 1664 &(cal_data.xtalkhisto),
Charles MacNeill 5:89031b2f5316 1665 &(pCalibrationData->xtalkhisto),
Charles MacNeill 5:89031b2f5316 1666 sizeof(VL53LX_xtalk_histogram_data_t));
Charles MacNeill 5:89031b2f5316 1667
Charles MacNeill 5:89031b2f5316 1668
Charles MacNeill 5:89031b2f5316 1669 memcpy(
Charles MacNeill 5:89031b2f5316 1670 &(cal_data.gain_cal),
Charles MacNeill 5:89031b2f5316 1671 &(pCalibrationData->gain_cal),
Charles MacNeill 5:89031b2f5316 1672 sizeof(VL53LX_gain_calibration_data_t));
Charles MacNeill 5:89031b2f5316 1673
Charles MacNeill 5:89031b2f5316 1674
Charles MacNeill 5:89031b2f5316 1675 memcpy(
Charles MacNeill 5:89031b2f5316 1676 &(cal_data.cal_peak_rate_map),
Charles MacNeill 5:89031b2f5316 1677 &(pCalibrationData->cal_peak_rate_map),
Charles MacNeill 5:89031b2f5316 1678 sizeof(VL53LX_cal_peak_rate_map_t));
Charles MacNeill 5:89031b2f5316 1679
Charles MacNeill 5:89031b2f5316 1680
Charles MacNeill 5:89031b2f5316 1681 memcpy(
Charles MacNeill 5:89031b2f5316 1682 &(cal_data.per_vcsel_cal_data),
Charles MacNeill 5:89031b2f5316 1683 &(pCalibrationData->per_vcsel_cal_data),
Charles MacNeill 5:89031b2f5316 1684 sizeof(VL53LX_per_vcsel_period_offset_cal_data_t));
Charles MacNeill 5:89031b2f5316 1685
Charles MacNeill 5:89031b2f5316 1686 pC = &pCalibrationData->customer;
Charles MacNeill 5:89031b2f5316 1687 x = pC->algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 1688 cal_data.customer.algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 5:89031b2f5316 1689 (uint16_t)(x&0x0000FFFF);
Charles MacNeill 5:89031b2f5316 1690
Charles MacNeill 5:89031b2f5316 1691 cal_data.customer.global_config__spad_enables_ref_0 =
Charles MacNeill 5:89031b2f5316 1692 pC->global_config__spad_enables_ref_0;
Charles MacNeill 5:89031b2f5316 1693 cal_data.customer.global_config__spad_enables_ref_1 =
Charles MacNeill 5:89031b2f5316 1694 pC->global_config__spad_enables_ref_1;
Charles MacNeill 5:89031b2f5316 1695 cal_data.customer.global_config__spad_enables_ref_2 =
Charles MacNeill 5:89031b2f5316 1696 pC->global_config__spad_enables_ref_2;
Charles MacNeill 5:89031b2f5316 1697 cal_data.customer.global_config__spad_enables_ref_3 =
Charles MacNeill 5:89031b2f5316 1698 pC->global_config__spad_enables_ref_3;
Charles MacNeill 5:89031b2f5316 1699 cal_data.customer.global_config__spad_enables_ref_4 =
Charles MacNeill 5:89031b2f5316 1700 pC->global_config__spad_enables_ref_4;
Charles MacNeill 5:89031b2f5316 1701 cal_data.customer.global_config__spad_enables_ref_5 =
Charles MacNeill 5:89031b2f5316 1702 pC->global_config__spad_enables_ref_5;
Charles MacNeill 5:89031b2f5316 1703 cal_data.customer.global_config__ref_en_start_select =
Charles MacNeill 5:89031b2f5316 1704 pC->global_config__ref_en_start_select;
Charles MacNeill 5:89031b2f5316 1705 cal_data.customer.ref_spad_man__num_requested_ref_spads =
Charles MacNeill 5:89031b2f5316 1706 pC->ref_spad_man__num_requested_ref_spads;
Charles MacNeill 5:89031b2f5316 1707 cal_data.customer.ref_spad_man__ref_location =
Charles MacNeill 5:89031b2f5316 1708 pC->ref_spad_man__ref_location;
Charles MacNeill 5:89031b2f5316 1709 cal_data.customer.algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 1710 pC->algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 1711 cal_data.customer.algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 1712 pC->algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 1713 cal_data.customer.ref_spad_char__total_rate_target_mcps =
Charles MacNeill 5:89031b2f5316 1714 pC->ref_spad_char__total_rate_target_mcps;
Charles MacNeill 5:89031b2f5316 1715 cal_data.customer.algo__part_to_part_range_offset_mm =
Charles MacNeill 5:89031b2f5316 1716 pC->algo__part_to_part_range_offset_mm;
Charles MacNeill 5:89031b2f5316 1717 cal_data.customer.mm_config__inner_offset_mm =
Charles MacNeill 5:89031b2f5316 1718 pC->mm_config__inner_offset_mm;
Charles MacNeill 5:89031b2f5316 1719 cal_data.customer.mm_config__outer_offset_mm =
Charles MacNeill 5:89031b2f5316 1720 pC->mm_config__outer_offset_mm;
Charles MacNeill 5:89031b2f5316 1721
Charles MacNeill 5:89031b2f5316 1722 Status = VL53LX_set_part_to_part_data(Dev, &cal_data);
Charles MacNeill 5:89031b2f5316 1723
Charles MacNeill 5:89031b2f5316 1724 if (Status != VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1725 goto ENDFUNC;
Charles MacNeill 5:89031b2f5316 1726
Charles MacNeill 5:89031b2f5316 1727 Status = VL53LX_get_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 5:89031b2f5316 1728
Charles MacNeill 5:89031b2f5316 1729 if (Status != VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1730 goto ENDFUNC;
Charles MacNeill 5:89031b2f5316 1731
Charles MacNeill 5:89031b2f5316 1732 xtalk.algo__crosstalk_compensation_plane_offset_kcps = x;
Charles MacNeill 5:89031b2f5316 1733
Charles MacNeill 5:89031b2f5316 1734 Status = VL53LX_set_tuning_parm(Dev,
Charles MacNeill 5:89031b2f5316 1735 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS,
Charles MacNeill 5:89031b2f5316 1736 x);
Charles MacNeill 5:89031b2f5316 1737
Charles MacNeill 5:89031b2f5316 1738
Charles MacNeill 5:89031b2f5316 1739 memcpy(
Charles MacNeill 5:89031b2f5316 1740 &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 5:89031b2f5316 1741 &(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 5:89031b2f5316 1742 sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
Charles MacNeill 5:89031b2f5316 1743
Charles MacNeill 5:89031b2f5316 1744 Status = VL53LX_set_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 5:89031b2f5316 1745
Charles MacNeill 5:89031b2f5316 1746 ENDFUNC:
Charles MacNeill 5:89031b2f5316 1747 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1748 return Status;
Charles MacNeill 5:89031b2f5316 1749
Charles MacNeill 5:89031b2f5316 1750 }
Charles MacNeill 5:89031b2f5316 1751
Charles MacNeill 5:89031b2f5316 1752 VL53LX_Error VL53LX_GetCalibrationData(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1753 VL53LX_CalibrationData_t *pCalibrationData)
Charles MacNeill 5:89031b2f5316 1754 {
Charles MacNeill 5:89031b2f5316 1755 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1756 VL53LX_calibration_data_t cal_data;
Charles MacNeill 5:89031b2f5316 1757 VL53LX_CustomerNvmManaged_t *pC;
Charles MacNeill 5:89031b2f5316 1758 VL53LX_customer_nvm_managed_t *pC2;
Charles MacNeill 5:89031b2f5316 1759 VL53LX_xtalk_calibration_results_t xtalk;
Charles MacNeill 5:89031b2f5316 1760 uint32_t tmp;
Charles MacNeill 5:89031b2f5316 1761
Charles MacNeill 5:89031b2f5316 1762 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1763
Charles MacNeill 5:89031b2f5316 1764
Charles MacNeill 5:89031b2f5316 1765 Status = VL53LX_get_part_to_part_data(Dev, &cal_data);
Charles MacNeill 5:89031b2f5316 1766
Charles MacNeill 5:89031b2f5316 1767 pCalibrationData->struct_version = cal_data.struct_version +
Charles MacNeill 5:89031b2f5316 1768 VL53LX_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
Charles MacNeill 5:89031b2f5316 1769
Charles MacNeill 5:89031b2f5316 1770
Charles MacNeill 5:89031b2f5316 1771 memcpy(
Charles MacNeill 5:89031b2f5316 1772 &(pCalibrationData->add_off_cal_data),
Charles MacNeill 5:89031b2f5316 1773 &(cal_data.add_off_cal_data),
Charles MacNeill 5:89031b2f5316 1774 sizeof(VL53LX_additional_offset_cal_data_t));
Charles MacNeill 5:89031b2f5316 1775
Charles MacNeill 5:89031b2f5316 1776
Charles MacNeill 5:89031b2f5316 1777 memcpy(
Charles MacNeill 5:89031b2f5316 1778 &(pCalibrationData->optical_centre),
Charles MacNeill 5:89031b2f5316 1779 &(cal_data.optical_centre),
Charles MacNeill 5:89031b2f5316 1780 sizeof(VL53LX_optical_centre_t));
Charles MacNeill 5:89031b2f5316 1781
Charles MacNeill 5:89031b2f5316 1782
Charles MacNeill 5:89031b2f5316 1783 memcpy(
Charles MacNeill 5:89031b2f5316 1784 &(pCalibrationData->xtalkhisto),
Charles MacNeill 5:89031b2f5316 1785 &(cal_data.xtalkhisto),
Charles MacNeill 5:89031b2f5316 1786 sizeof(VL53LX_xtalk_histogram_data_t));
Charles MacNeill 5:89031b2f5316 1787
Charles MacNeill 5:89031b2f5316 1788 memcpy(
Charles MacNeill 5:89031b2f5316 1789 &(pCalibrationData->gain_cal),
Charles MacNeill 5:89031b2f5316 1790 &(cal_data.gain_cal),
Charles MacNeill 5:89031b2f5316 1791 sizeof(VL53LX_gain_calibration_data_t));
Charles MacNeill 5:89031b2f5316 1792
Charles MacNeill 5:89031b2f5316 1793
Charles MacNeill 5:89031b2f5316 1794 memcpy(
Charles MacNeill 5:89031b2f5316 1795 &(pCalibrationData->cal_peak_rate_map),
Charles MacNeill 5:89031b2f5316 1796 &(cal_data.cal_peak_rate_map),
Charles MacNeill 5:89031b2f5316 1797 sizeof(VL53LX_cal_peak_rate_map_t));
Charles MacNeill 5:89031b2f5316 1798
Charles MacNeill 5:89031b2f5316 1799
Charles MacNeill 5:89031b2f5316 1800 memcpy(
Charles MacNeill 5:89031b2f5316 1801 &(pCalibrationData->per_vcsel_cal_data),
Charles MacNeill 5:89031b2f5316 1802 &(cal_data.per_vcsel_cal_data),
Charles MacNeill 5:89031b2f5316 1803 sizeof(VL53LX_per_vcsel_period_offset_cal_data_t));
Charles MacNeill 5:89031b2f5316 1804
Charles MacNeill 5:89031b2f5316 1805 pC = &pCalibrationData->customer;
Charles MacNeill 5:89031b2f5316 1806 pC2 = &cal_data.customer;
Charles MacNeill 5:89031b2f5316 1807 pC->global_config__spad_enables_ref_0 =
Charles MacNeill 5:89031b2f5316 1808 pC2->global_config__spad_enables_ref_0;
Charles MacNeill 5:89031b2f5316 1809 pC->global_config__spad_enables_ref_1 =
Charles MacNeill 5:89031b2f5316 1810 pC2->global_config__spad_enables_ref_1;
Charles MacNeill 5:89031b2f5316 1811 pC->global_config__spad_enables_ref_2 =
Charles MacNeill 5:89031b2f5316 1812 pC2->global_config__spad_enables_ref_2;
Charles MacNeill 5:89031b2f5316 1813 pC->global_config__spad_enables_ref_3 =
Charles MacNeill 5:89031b2f5316 1814 pC2->global_config__spad_enables_ref_3;
Charles MacNeill 5:89031b2f5316 1815 pC->global_config__spad_enables_ref_4 =
Charles MacNeill 5:89031b2f5316 1816 pC2->global_config__spad_enables_ref_4;
Charles MacNeill 5:89031b2f5316 1817 pC->global_config__spad_enables_ref_5 =
Charles MacNeill 5:89031b2f5316 1818 pC2->global_config__spad_enables_ref_5;
Charles MacNeill 5:89031b2f5316 1819 pC->global_config__ref_en_start_select =
Charles MacNeill 5:89031b2f5316 1820 pC2->global_config__ref_en_start_select;
Charles MacNeill 5:89031b2f5316 1821 pC->ref_spad_man__num_requested_ref_spads =
Charles MacNeill 5:89031b2f5316 1822 pC2->ref_spad_man__num_requested_ref_spads;
Charles MacNeill 5:89031b2f5316 1823 pC->ref_spad_man__ref_location =
Charles MacNeill 5:89031b2f5316 1824 pC2->ref_spad_man__ref_location;
Charles MacNeill 5:89031b2f5316 1825 pC->algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 1826 pC2->algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 1827 pC->algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 1828 pC2->algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 1829 pC->ref_spad_char__total_rate_target_mcps =
Charles MacNeill 5:89031b2f5316 1830 pC2->ref_spad_char__total_rate_target_mcps;
Charles MacNeill 5:89031b2f5316 1831 pC->algo__part_to_part_range_offset_mm =
Charles MacNeill 5:89031b2f5316 1832 pC2->algo__part_to_part_range_offset_mm;
Charles MacNeill 5:89031b2f5316 1833 pC->mm_config__inner_offset_mm =
Charles MacNeill 5:89031b2f5316 1834 pC2->mm_config__inner_offset_mm;
Charles MacNeill 5:89031b2f5316 1835 pC->mm_config__outer_offset_mm =
Charles MacNeill 5:89031b2f5316 1836 pC2->mm_config__outer_offset_mm;
Charles MacNeill 5:89031b2f5316 1837
Charles MacNeill 5:89031b2f5316 1838 pC->algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 5:89031b2f5316 1839 (uint32_t)(
Charles MacNeill 5:89031b2f5316 1840 pC2->algo__crosstalk_compensation_plane_offset_kcps);
Charles MacNeill 5:89031b2f5316 1841
Charles MacNeill 5:89031b2f5316 1842 Status = VL53LX_get_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 5:89031b2f5316 1843
Charles MacNeill 5:89031b2f5316 1844 if (Status != VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1845 goto ENDFUNC;
Charles MacNeill 5:89031b2f5316 1846
Charles MacNeill 5:89031b2f5316 1847 tmp = xtalk.algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 1848 pC->algo__crosstalk_compensation_plane_offset_kcps = tmp;
Charles MacNeill 5:89031b2f5316 1849 tmp = xtalk.algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 1850 pC->algo__crosstalk_compensation_x_plane_gradient_kcps = tmp;
Charles MacNeill 5:89031b2f5316 1851 tmp = xtalk.algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 1852 pC->algo__crosstalk_compensation_y_plane_gradient_kcps = tmp;
Charles MacNeill 5:89031b2f5316 1853
Charles MacNeill 5:89031b2f5316 1854 memcpy(&(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 5:89031b2f5316 1855 &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 5:89031b2f5316 1856 sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
Charles MacNeill 5:89031b2f5316 1857 ENDFUNC:
Charles MacNeill 5:89031b2f5316 1858 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 1859 return Status;
Charles MacNeill 5:89031b2f5316 1860 }
Charles MacNeill 5:89031b2f5316 1861
Charles MacNeill 5:89031b2f5316 1862
Charles MacNeill 5:89031b2f5316 1863
Charles MacNeill 5:89031b2f5316 1864 VL53LX_Error VL53LX_PerformOffsetPerVcselCalibration(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1865 int32_t CalDistanceMilliMeter)
Charles MacNeill 5:89031b2f5316 1866 {
Charles MacNeill 5:89031b2f5316 1867 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1868 int32_t sum_ranging_range_A, sum_ranging_range_B;
Charles MacNeill 5:89031b2f5316 1869 uint8_t offset_meas_range_A, offset_meas_range_B;
Charles MacNeill 5:89031b2f5316 1870 int16_t Max, UnderMax, OverMax, Repeat;
Charles MacNeill 5:89031b2f5316 1871 int32_t inloopcount;
Charles MacNeill 5:89031b2f5316 1872 int32_t IncRounding;
Charles MacNeill 5:89031b2f5316 1873 int16_t meanDistance_mm;
Charles MacNeill 5:89031b2f5316 1874 VL53LX_MultiRangingData_t RangingMeasurementData;
Charles MacNeill 5:89031b2f5316 1875 VL53LX_LLDriverData_t *pdev;
Charles MacNeill 5:89031b2f5316 1876 uint8_t goodmeas;
Charles MacNeill 5:89031b2f5316 1877 VL53LX_DistanceModes currentDist;
Charles MacNeill 5:89031b2f5316 1878 VL53LX_DistanceModes DistMode[3] = {VL53LX_DISTANCEMODE_SHORT,
Charles MacNeill 5:89031b2f5316 1879 VL53LX_DISTANCEMODE_MEDIUM, VL53LX_DISTANCEMODE_LONG};
Charles MacNeill 5:89031b2f5316 1880 int16_t offsetA[3] = {0, 0, 0};
Charles MacNeill 5:89031b2f5316 1881 int16_t offsetB[3] = {0, 0, 0};
Charles MacNeill 5:89031b2f5316 1882
Charles MacNeill 5:89031b2f5316 1883 VL53LX_Error SmudgeStatus = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1884 uint8_t smudge_corr_en, ics;
Charles MacNeill 5:89031b2f5316 1885 VL53LX_TargetRangeData_t *pRange;
Charles MacNeill 5:89031b2f5316 1886
Charles MacNeill 5:89031b2f5316 1887 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1888
Charles MacNeill 5:89031b2f5316 1889 pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1890
Charles MacNeill 5:89031b2f5316 1891 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
Charles MacNeill 5:89031b2f5316 1892 SmudgeStatus = VL53LX_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 5:89031b2f5316 1893
Charles MacNeill 5:89031b2f5316 1894 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1895 pdev->customer.mm_config__inner_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1896 pdev->customer.mm_config__outer_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1897 pdev->customer.mm_config__outer_offset_mm = 0;
Charles MacNeill 5:89031b2f5316 1898 memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 5:89031b2f5316 1899
Charles MacNeill 5:89031b2f5316 1900 Repeat = 0;
Charles MacNeill 5:89031b2f5316 1901 if (IsL4(Dev))
Charles MacNeill 5:89031b2f5316 1902 Repeat = 1;
Charles MacNeill 5:89031b2f5316 1903 Max = 2 * BDTable[
Charles MacNeill 5:89031b2f5316 1904 VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
Charles MacNeill 5:89031b2f5316 1905 UnderMax = 1 + (Max / 2);
Charles MacNeill 5:89031b2f5316 1906 OverMax = Max + (Max / 2);
Charles MacNeill 5:89031b2f5316 1907
Charles MacNeill 5:89031b2f5316 1908 Status = VL53LX_GetDistanceMode(Dev, &currentDist);
Charles MacNeill 5:89031b2f5316 1909
Charles MacNeill 5:89031b2f5316 1910 while ((Repeat < 3) && (Status == VL53LX_ERROR_NONE)) {
Charles MacNeill 5:89031b2f5316 1911 Status = VL53LX_SetDistanceMode(Dev, DistMode[Repeat]);
Charles MacNeill 5:89031b2f5316 1912 Status = VL53LX_StartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1913
Charles MacNeill 5:89031b2f5316 1914 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1915 VL53LX_WaitMeasurementDataReady(Dev);
Charles MacNeill 5:89031b2f5316 1916 VL53LX_GetMultiRangingData(Dev,
Charles MacNeill 5:89031b2f5316 1917 &RangingMeasurementData);
Charles MacNeill 5:89031b2f5316 1918 VL53LX_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1919 }
Charles MacNeill 5:89031b2f5316 1920
Charles MacNeill 5:89031b2f5316 1921 inloopcount = 0;
Charles MacNeill 5:89031b2f5316 1922 offset_meas_range_A = 0;
Charles MacNeill 5:89031b2f5316 1923 sum_ranging_range_A = 0;
Charles MacNeill 5:89031b2f5316 1924 offset_meas_range_B = 0;
Charles MacNeill 5:89031b2f5316 1925 sum_ranging_range_B = 0;
Charles MacNeill 5:89031b2f5316 1926 while ((Status == VL53LX_ERROR_NONE) && (inloopcount < Max) &&
Charles MacNeill 5:89031b2f5316 1927 (inloopcount < OverMax)) {
Charles MacNeill 5:89031b2f5316 1928 Status = VL53LX_WaitMeasurementDataReady(Dev);
Charles MacNeill 5:89031b2f5316 1929 if (Status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1930 Status = VL53LX_GetMultiRangingData(Dev,
Charles MacNeill 5:89031b2f5316 1931 &RangingMeasurementData);
Charles MacNeill 5:89031b2f5316 1932 pRange = &(RangingMeasurementData.RangeData[0]);
Charles MacNeill 5:89031b2f5316 1933 goodmeas = (pRange->RangeStatus ==
Charles MacNeill 5:89031b2f5316 1934 VL53LX_RANGESTATUS_RANGE_VALID);
Charles MacNeill 5:89031b2f5316 1935 ics = pdev->ll_state.cfg_internal_stream_count;
Charles MacNeill 5:89031b2f5316 1936 if ((Status == VL53LX_ERROR_NONE) && goodmeas) {
Charles MacNeill 5:89031b2f5316 1937 if (ics & 0x01) {
Charles MacNeill 5:89031b2f5316 1938 sum_ranging_range_A +=
Charles MacNeill 5:89031b2f5316 1939 pRange->RangeMilliMeter;
Charles MacNeill 5:89031b2f5316 1940 offset_meas_range_A++;
Charles MacNeill 5:89031b2f5316 1941 } else {
Charles MacNeill 5:89031b2f5316 1942 sum_ranging_range_B +=
Charles MacNeill 5:89031b2f5316 1943 pRange->RangeMilliMeter;
Charles MacNeill 5:89031b2f5316 1944 offset_meas_range_B++;
Charles MacNeill 5:89031b2f5316 1945 }
Charles MacNeill 5:89031b2f5316 1946 inloopcount = offset_meas_range_A +
Charles MacNeill 5:89031b2f5316 1947 offset_meas_range_B;
Charles MacNeill 5:89031b2f5316 1948 }
Charles MacNeill 5:89031b2f5316 1949 Status = VL53LX_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1950 }
Charles MacNeill 5:89031b2f5316 1951
Charles MacNeill 5:89031b2f5316 1952
Charles MacNeill 5:89031b2f5316 1953 if (inloopcount < UnderMax)
Charles MacNeill 5:89031b2f5316 1954 Status = VL53LX_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
Charles MacNeill 5:89031b2f5316 1955
Charles MacNeill 5:89031b2f5316 1956 VL53LX_StopMeasurement(Dev);
Charles MacNeill 5:89031b2f5316 1957
Charles MacNeill 5:89031b2f5316 1958
Charles MacNeill 5:89031b2f5316 1959 if ((sum_ranging_range_A < 0) ||
Charles MacNeill 5:89031b2f5316 1960 (sum_ranging_range_B < 0) ||
Charles MacNeill 5:89031b2f5316 1961 (sum_ranging_range_A >
Charles MacNeill 5:89031b2f5316 1962 ((int32_t) offset_meas_range_A * 0xffff)) ||
Charles MacNeill 5:89031b2f5316 1963 (sum_ranging_range_B >
Charles MacNeill 5:89031b2f5316 1964 ((int32_t) offset_meas_range_B * 0xffff))) {
Charles MacNeill 5:89031b2f5316 1965 Status = VL53LX_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
Charles MacNeill 5:89031b2f5316 1966 }
Charles MacNeill 5:89031b2f5316 1967
Charles MacNeill 5:89031b2f5316 1968 if ((Status == VL53LX_ERROR_NONE) &&
Charles MacNeill 5:89031b2f5316 1969 (offset_meas_range_A > 0)) {
Charles MacNeill 5:89031b2f5316 1970 IncRounding = offset_meas_range_A / 2;
Charles MacNeill 5:89031b2f5316 1971 meanDistance_mm = (int16_t)
Charles MacNeill 5:89031b2f5316 1972 ((sum_ranging_range_A + IncRounding)
Charles MacNeill 5:89031b2f5316 1973 / offset_meas_range_A);
Charles MacNeill 5:89031b2f5316 1974 offsetA[Repeat] = (int16_t)
Charles MacNeill 5:89031b2f5316 1975 CalDistanceMilliMeter - meanDistance_mm;
Charles MacNeill 5:89031b2f5316 1976 }
Charles MacNeill 5:89031b2f5316 1977
Charles MacNeill 5:89031b2f5316 1978 if ((Status == VL53LX_ERROR_NONE) &&
Charles MacNeill 5:89031b2f5316 1979 (offset_meas_range_B > 0)) {
Charles MacNeill 5:89031b2f5316 1980 IncRounding = offset_meas_range_B / 2;
Charles MacNeill 5:89031b2f5316 1981 meanDistance_mm = (int16_t)
Charles MacNeill 5:89031b2f5316 1982 ((sum_ranging_range_B + IncRounding)
Charles MacNeill 5:89031b2f5316 1983 / offset_meas_range_B);
Charles MacNeill 5:89031b2f5316 1984 offsetB[Repeat] = (int16_t)
Charles MacNeill 5:89031b2f5316 1985 CalDistanceMilliMeter - meanDistance_mm;
Charles MacNeill 5:89031b2f5316 1986 }
Charles MacNeill 5:89031b2f5316 1987 Repeat++;
Charles MacNeill 5:89031b2f5316 1988 }
Charles MacNeill 5:89031b2f5316 1989
Charles MacNeill 5:89031b2f5316 1990 if ((SmudgeStatus == VL53LX_ERROR_NONE) && (smudge_corr_en == 1))
Charles MacNeill 5:89031b2f5316 1991 SmudgeStatus = VL53LX_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 5:89031b2f5316 1992
Charles MacNeill 5:89031b2f5316 1993 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1994 pdev->per_vcsel_cal_data.short_a_offset_mm = offsetA[0];
Charles MacNeill 5:89031b2f5316 1995 pdev->per_vcsel_cal_data.short_b_offset_mm = offsetB[0];
Charles MacNeill 5:89031b2f5316 1996 pdev->per_vcsel_cal_data.medium_a_offset_mm = offsetA[1];
Charles MacNeill 5:89031b2f5316 1997 pdev->per_vcsel_cal_data.medium_b_offset_mm = offsetB[1];
Charles MacNeill 5:89031b2f5316 1998 pdev->per_vcsel_cal_data.long_a_offset_mm = offsetA[2];
Charles MacNeill 5:89031b2f5316 1999 pdev->per_vcsel_cal_data.long_b_offset_mm = offsetB[2];
Charles MacNeill 5:89031b2f5316 2000 }
Charles MacNeill 5:89031b2f5316 2001
Charles MacNeill 5:89031b2f5316 2002 VL53LX_SetDistanceMode(Dev, currentDist);
Charles MacNeill 5:89031b2f5316 2003
Charles MacNeill 5:89031b2f5316 2004 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 2005 return Status;
Charles MacNeill 5:89031b2f5316 2006 }
Charles MacNeill 5:89031b2f5316 2007
Charles MacNeill 5:89031b2f5316 2008 VL53LX_Error VL53LX_GetOpticalCenter(VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 2009 FixPoint1616_t *pOpticalCenterX,
Charles MacNeill 5:89031b2f5316 2010 FixPoint1616_t *pOpticalCenterY)
Charles MacNeill 5:89031b2f5316 2011 {
Charles MacNeill 5:89031b2f5316 2012 VL53LX_Error Status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2013 VL53LX_calibration_data_t CalibrationData;
Charles MacNeill 5:89031b2f5316 2014
Charles MacNeill 5:89031b2f5316 2015 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2016
Charles MacNeill 5:89031b2f5316 2017 *pOpticalCenterX = 0;
Charles MacNeill 5:89031b2f5316 2018 *pOpticalCenterY = 0;
Charles MacNeill 5:89031b2f5316 2019 Status = VL53LX_get_part_to_part_data(Dev, &CalibrationData);
Charles MacNeill 5:89031b2f5316 2020 if (Status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 2021 *pOpticalCenterX = VL53LX_FIXPOINT44TOFIXPOINT1616(
Charles MacNeill 5:89031b2f5316 2022 CalibrationData.optical_centre.x_centre);
Charles MacNeill 5:89031b2f5316 2023 *pOpticalCenterY = VL53LX_FIXPOINT44TOFIXPOINT1616(
Charles MacNeill 5:89031b2f5316 2024 CalibrationData.optical_centre.y_centre);
Charles MacNeill 5:89031b2f5316 2025 }
Charles MacNeill 5:89031b2f5316 2026
Charles MacNeill 5:89031b2f5316 2027 LOG_FUNCTION_END(Status);
Charles MacNeill 5:89031b2f5316 2028 return Status;
Charles MacNeill 5:89031b2f5316 2029 }
Charles MacNeill 5:89031b2f5316 2030
Charles MacNeill 5:89031b2f5316 2031