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@5:89031b2f5316, 2021-07-14 (annotated)

Committer:
Charles MacNeill
Date:
Wed Jul 14 12:45:49 2021 +0100
Revision:
5:89031b2f5316
The class files now just are wrappers for the files in the "MODULES" directory.The porting directory includes the mbed interface

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