ST Expansion SW Team / VL53L1CB

The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.

src/vl53l1_api.c@18:0696efe39d08, 2021-07-21 (annotated)

Committer:
lugandc
Date:
Wed Jul 21 17:06:38 2021 +0200
Revision:
18:0696efe39d08
Parent:
7:1add29d51e72 
Cleanup i2c functions, removed all bad references to L1X

Cleanup VL53L1CB class:

 - i2c device object is passed in a consistent way in MyDevice structure

 - removed useless functions

Updated VL53L1CB component driver with bare driver release 6.6.7 content

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 7:1add29d51e72 1
Charles MacNeill 7:1add29d51e72 2 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
Charles MacNeill 7:1add29d51e72 3 /******************************************************************************
Charles MacNeill 7:1add29d51e72 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 7:1add29d51e72 5
Charles MacNeill 7:1add29d51e72 6 This file is part of VL53L1 and is dual licensed,
Charles MacNeill 7:1add29d51e72 7 either GPL-2.0+
Charles MacNeill 7:1add29d51e72 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 7:1add29d51e72 9 ******************************************************************************
Charles MacNeill 7:1add29d51e72 10 */
Charles MacNeill 7:1add29d51e72 11
Charles MacNeill 7:1add29d51e72 12
Charles MacNeill 7:1add29d51e72 13
Charles MacNeill 7:1add29d51e72 14
Charles MacNeill 7:1add29d51e72 15 #include "vl53l1_api.h"
Charles MacNeill 7:1add29d51e72 16 #include "vl53l1_api_strings.h"
Charles MacNeill 7:1add29d51e72 17 #include "vl53l1_register_settings.h"
Charles MacNeill 7:1add29d51e72 18 #include "vl53l1_register_funcs.h"
Charles MacNeill 7:1add29d51e72 19 #include "vl53l1_core.h"
Charles MacNeill 7:1add29d51e72 20 #include "vl53l1_api_calibration.h"
Charles MacNeill 7:1add29d51e72 21 #include "vl53l1_wait.h"
Charles MacNeill 7:1add29d51e72 22 #include "vl53l1_preset_setup.h"
Charles MacNeill 7:1add29d51e72 23 #include "vl53l1_api_debug.h"
Charles MacNeill 7:1add29d51e72 24 #include "vl53l1_api_core.h"
Charles MacNeill 7:1add29d51e72 25 #include "vl53l1_nvm.h"
Charles MacNeill 7:1add29d51e72 26
Charles MacNeill 7:1add29d51e72 27
Charles MacNeill 7:1add29d51e72 28 #define ZONE_CHECK VL53L1_MAX_USER_ZONES
Charles MacNeill 7:1add29d51e72 29
Charles MacNeill 7:1add29d51e72 30 #if ZONE_CHECK < 5
Charles MacNeill 7:1add29d51e72 31 #error Must define at least 5 zones in MAX_USER_ZONES constant
Charles MacNeill 7:1add29d51e72 32 #endif
Charles MacNeill 7:1add29d51e72 33
Charles MacNeill 7:1add29d51e72 34 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 7:1add29d51e72 35 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_API, fmt, ##__VA_ARGS__)
Charles MacNeill 7:1add29d51e72 36 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 7:1add29d51e72 37 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_API, status, ##__VA_ARGS__)
Charles MacNeill 7:1add29d51e72 38 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 7:1add29d51e72 39 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_API, status, \
Charles MacNeill 7:1add29d51e72 40 fmt, ##__VA_ARGS__)
Charles MacNeill 7:1add29d51e72 41
Charles MacNeill 7:1add29d51e72 42 #ifdef VL53L1_LOG_ENABLE
Charles MacNeill 7:1add29d51e72 43 #define trace_print(level, ...) trace_print_module_function(\
Charles MacNeill 7:1add29d51e72 44 VL53L1_TRACE_MODULE_API, level, VL53L1_TRACE_FUNCTION_NONE, \
Charles MacNeill 7:1add29d51e72 45 ##__VA_ARGS__)
Charles MacNeill 7:1add29d51e72 46 #endif
Charles MacNeill 7:1add29d51e72 47
Charles MacNeill 7:1add29d51e72 48 #ifndef MIN
Charles MacNeill 7:1add29d51e72 49 #define MIN(v1, v2) ((v1) < (v2) ? (v1) : (v2))
Charles MacNeill 7:1add29d51e72 50 #endif
Charles MacNeill 7:1add29d51e72 51 #ifndef MAX
Charles MacNeill 7:1add29d51e72 52 #define MAX(v1, v2) ((v1) < (v2) ? (v2) : (v1))
Charles MacNeill 7:1add29d51e72 53 #endif
Charles MacNeill 7:1add29d51e72 54
Charles MacNeill 7:1add29d51e72 55 #define DMAX_REFLECTANCE_IDX 2
Charles MacNeill 7:1add29d51e72 56
Charles MacNeill 7:1add29d51e72 57
Charles MacNeill 7:1add29d51e72 58
Charles MacNeill 7:1add29d51e72 59 #define LOWPOWER_AUTO_VHV_LOOP_DURATION_US 245
Charles MacNeill 7:1add29d51e72 60 #define LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING 1448
Charles MacNeill 7:1add29d51e72 61 #define LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING 2100
Charles MacNeill 7:1add29d51e72 62
Charles MacNeill 7:1add29d51e72 63 #define FDA_MAX_TIMING_BUDGET_US 550000
Charles MacNeill 7:1add29d51e72 64
Charles MacNeill 7:1add29d51e72 65
Charles MacNeill 7:1add29d51e72 66
Charles MacNeill 7:1add29d51e72 67
Charles MacNeill 7:1add29d51e72 68
Charles MacNeill 7:1add29d51e72 69
Charles MacNeill 7:1add29d51e72 70 static int32_t BDTable[VL53L1_TUNING_MAX_TUNABLE_KEY] = {
Charles MacNeill 7:1add29d51e72 71 TUNING_VERSION,
Charles MacNeill 7:1add29d51e72 72 TUNING_PROXY_MIN,
Charles MacNeill 7:1add29d51e72 73 TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM,
Charles MacNeill 7:1add29d51e72 74 TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER,
Charles MacNeill 7:1add29d51e72 75 TUNING_MIN_AMBIENT_DMAX_VALID,
Charles MacNeill 7:1add29d51e72 76 TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER,
Charles MacNeill 7:1add29d51e72 77 TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM,
Charles MacNeill 7:1add29d51e72 78 TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT,
Charles MacNeill 7:1add29d51e72 79 TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN,
Charles MacNeill 7:1add29d51e72 80 TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET,
Charles MacNeill 7:1add29d51e72 81 TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR_DEFAULT,
Charles MacNeill 7:1add29d51e72 82 };
Charles MacNeill 7:1add29d51e72 83
Charles MacNeill 7:1add29d51e72 84
Charles MacNeill 7:1add29d51e72 85 static VL53L1_Error SingleTargetXTalkCalibration(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 86 {
Charles MacNeill 7:1add29d51e72 87 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 88
Charles MacNeill 7:1add29d51e72 89 uint32_t sum_ranging = 0;
Charles MacNeill 7:1add29d51e72 90 uint32_t sum_spads = 0;
Charles MacNeill 7:1add29d51e72 91 FixPoint1616_t sum_signalRate = 0;
Charles MacNeill 7:1add29d51e72 92 FixPoint1616_t total_count = 0;
Charles MacNeill 7:1add29d51e72 93 uint8_t xtalk_meas = 0;
Charles MacNeill 7:1add29d51e72 94 uint8_t xtalk_measmax =
Charles MacNeill 7:1add29d51e72 95 BDTable[VL53L1_TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER];
Charles MacNeill 7:1add29d51e72 96 VL53L1_RangingMeasurementData_t RMData;
Charles MacNeill 7:1add29d51e72 97 FixPoint1616_t xTalkStoredMeanSignalRate;
Charles MacNeill 7:1add29d51e72 98 FixPoint1616_t xTalkStoredMeanRange;
Charles MacNeill 7:1add29d51e72 99 FixPoint1616_t xTalkStoredMeanRtnSpads;
Charles MacNeill 7:1add29d51e72 100 uint32_t xTalkStoredMeanRtnSpadsAsInt;
Charles MacNeill 7:1add29d51e72 101 uint32_t xTalkCalDistanceAsInt;
Charles MacNeill 7:1add29d51e72 102 FixPoint1616_t XTalkCompensationRateMegaCps;
Charles MacNeill 7:1add29d51e72 103 uint32_t signalXTalkTotalPerSpad;
Charles MacNeill 7:1add29d51e72 104 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 105 VL53L1_CalibrationData_t CalibrationData;
Charles MacNeill 7:1add29d51e72 106 VL53L1_CustomerNvmManaged_t *pC;
Charles MacNeill 7:1add29d51e72 107
Charles MacNeill 7:1add29d51e72 108
Charles MacNeill 7:1add29d51e72 109 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 110
Charles MacNeill 7:1add29d51e72 111
Charles MacNeill 7:1add29d51e72 112 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 113
Charles MacNeill 7:1add29d51e72 114 if ((PresetMode != VL53L1_PRESETMODE_AUTONOMOUS) &&
Charles MacNeill 7:1add29d51e72 115 (PresetMode != VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS) &&
Charles MacNeill 7:1add29d51e72 116 (PresetMode != VL53L1_PRESETMODE_LITE_RANGING)) {
Charles MacNeill 7:1add29d51e72 117 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 118 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 119 }
Charles MacNeill 7:1add29d51e72 120
Charles MacNeill 7:1add29d51e72 121
Charles MacNeill 7:1add29d51e72 122 Status = VL53L1_disable_xtalk_compensation(Dev);
Charles MacNeill 7:1add29d51e72 123
Charles MacNeill 7:1add29d51e72 124 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 125 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 126
Charles MacNeill 7:1add29d51e72 127 Status = VL53L1_StartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 128
Charles MacNeill 7:1add29d51e72 129 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 130 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 131
Charles MacNeill 7:1add29d51e72 132
Charles MacNeill 7:1add29d51e72 133 VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 134 VL53L1_GetRangingMeasurementData(Dev, &RMData);
Charles MacNeill 7:1add29d51e72 135 VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 136
Charles MacNeill 7:1add29d51e72 137 sum_ranging = 0;
Charles MacNeill 7:1add29d51e72 138 sum_spads = 0;
Charles MacNeill 7:1add29d51e72 139 sum_signalRate = 0;
Charles MacNeill 7:1add29d51e72 140 total_count = 0;
Charles MacNeill 7:1add29d51e72 141 for (xtalk_meas = 0; xtalk_meas < xtalk_measmax; xtalk_meas++) {
Charles MacNeill 7:1add29d51e72 142 VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 143 VL53L1_GetRangingMeasurementData(Dev, &RMData);
Charles MacNeill 7:1add29d51e72 144 VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 145 if (RMData.RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
Charles MacNeill 7:1add29d51e72 146 sum_ranging += RMData.RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 147 sum_signalRate += RMData.SignalRateRtnMegaCps;
Charles MacNeill 7:1add29d51e72 148 sum_spads += RMData.EffectiveSpadRtnCount / 256;
Charles MacNeill 7:1add29d51e72 149 total_count++;
Charles MacNeill 7:1add29d51e72 150 }
Charles MacNeill 7:1add29d51e72 151 }
Charles MacNeill 7:1add29d51e72 152 Status = VL53L1_StopMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 153
Charles MacNeill 7:1add29d51e72 154 if (total_count > 0) {
Charles MacNeill 7:1add29d51e72 155
Charles MacNeill 7:1add29d51e72 156 xTalkStoredMeanSignalRate = sum_signalRate / total_count;
Charles MacNeill 7:1add29d51e72 157 xTalkStoredMeanRange = (FixPoint1616_t)(sum_ranging << 16);
Charles MacNeill 7:1add29d51e72 158 xTalkStoredMeanRange /= total_count;
Charles MacNeill 7:1add29d51e72 159 xTalkStoredMeanRtnSpads = (FixPoint1616_t)(sum_spads << 16);
Charles MacNeill 7:1add29d51e72 160 xTalkStoredMeanRtnSpads /= total_count;
Charles MacNeill 7:1add29d51e72 161
Charles MacNeill 7:1add29d51e72 162
Charles MacNeill 7:1add29d51e72 163 xTalkStoredMeanRtnSpadsAsInt = (xTalkStoredMeanRtnSpads +
Charles MacNeill 7:1add29d51e72 164 0x8000) >> 16;
Charles MacNeill 7:1add29d51e72 165
Charles MacNeill 7:1add29d51e72 166
Charles MacNeill 7:1add29d51e72 167 xTalkCalDistanceAsInt = ((uint32_t)BDTable[
Charles MacNeill 7:1add29d51e72 168 VL53L1_TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM]);
Charles MacNeill 7:1add29d51e72 169 if (xTalkStoredMeanRtnSpadsAsInt == 0 ||
Charles MacNeill 7:1add29d51e72 170 xTalkCalDistanceAsInt == 0 ||
Charles MacNeill 7:1add29d51e72 171 xTalkStoredMeanRange >= (xTalkCalDistanceAsInt << 16)) {
Charles MacNeill 7:1add29d51e72 172 XTalkCompensationRateMegaCps = 0;
Charles MacNeill 7:1add29d51e72 173 } else {
Charles MacNeill 7:1add29d51e72 174
Charles MacNeill 7:1add29d51e72 175 signalXTalkTotalPerSpad = (xTalkStoredMeanSignalRate) /
Charles MacNeill 7:1add29d51e72 176 xTalkStoredMeanRtnSpadsAsInt;
Charles MacNeill 7:1add29d51e72 177
Charles MacNeill 7:1add29d51e72 178
Charles MacNeill 7:1add29d51e72 179 signalXTalkTotalPerSpad *= (((uint32_t)1 << 16) -
Charles MacNeill 7:1add29d51e72 180 (xTalkStoredMeanRange / xTalkCalDistanceAsInt));
Charles MacNeill 7:1add29d51e72 181
Charles MacNeill 7:1add29d51e72 182
Charles MacNeill 7:1add29d51e72 183 XTalkCompensationRateMegaCps = (signalXTalkTotalPerSpad
Charles MacNeill 7:1add29d51e72 184 + 0x8000) >> 16;
Charles MacNeill 7:1add29d51e72 185 }
Charles MacNeill 7:1add29d51e72 186
Charles MacNeill 7:1add29d51e72 187
Charles MacNeill 7:1add29d51e72 188 Status = VL53L1_GetCalibrationData(Dev, &CalibrationData);
Charles MacNeill 7:1add29d51e72 189
Charles MacNeill 7:1add29d51e72 190 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 191 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 192
Charles MacNeill 7:1add29d51e72 193 pC = &CalibrationData.customer;
Charles MacNeill 7:1add29d51e72 194
Charles MacNeill 7:1add29d51e72 195 pC->algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 7:1add29d51e72 196 (uint32_t)(1000 * ((XTalkCompensationRateMegaCps +
Charles MacNeill 7:1add29d51e72 197 ((uint32_t)1<<6)) >> (16-9)));
Charles MacNeill 7:1add29d51e72 198
Charles MacNeill 7:1add29d51e72 199 Status = VL53L1_SetCalibrationData(Dev, &CalibrationData);
Charles MacNeill 7:1add29d51e72 200
Charles MacNeill 7:1add29d51e72 201 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 202 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 203
Charles MacNeill 7:1add29d51e72 204 Status = VL53L1_enable_xtalk_compensation(Dev);
Charles MacNeill 7:1add29d51e72 205
Charles MacNeill 7:1add29d51e72 206 } else
Charles MacNeill 7:1add29d51e72 207
Charles MacNeill 7:1add29d51e72 208 Status = VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL;
Charles MacNeill 7:1add29d51e72 209
Charles MacNeill 7:1add29d51e72 210 ENDFUNC:
Charles MacNeill 7:1add29d51e72 211 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 212 return Status;
Charles MacNeill 7:1add29d51e72 213
Charles MacNeill 7:1add29d51e72 214 }
Charles MacNeill 7:1add29d51e72 215
Charles MacNeill 7:1add29d51e72 216
Charles MacNeill 7:1add29d51e72 217 static VL53L1_Error CheckValidRectRoi(VL53L1_UserRoi_t ROI)
Charles MacNeill 7:1add29d51e72 218 {
Charles MacNeill 7:1add29d51e72 219 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 220
Charles MacNeill 7:1add29d51e72 221 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 222
Charles MacNeill 7:1add29d51e72 223
Charles MacNeill 7:1add29d51e72 224 if ((ROI.TopLeftX > 15) || (ROI.TopLeftY > 15) ||
Charles MacNeill 7:1add29d51e72 225 (ROI.BotRightX > 15) || (ROI.BotRightY > 15))
Charles MacNeill 7:1add29d51e72 226 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 227
Charles MacNeill 7:1add29d51e72 228 if ((ROI.TopLeftX > ROI.BotRightX) || (ROI.TopLeftY < ROI.BotRightY))
Charles MacNeill 7:1add29d51e72 229 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 230
Charles MacNeill 7:1add29d51e72 231 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 232 return Status;
Charles MacNeill 7:1add29d51e72 233 }
Charles MacNeill 7:1add29d51e72 234
Charles MacNeill 7:1add29d51e72 235 static VL53L1_GPIO_Interrupt_Mode ConvertModeToLLD(VL53L1_Error *pStatus,
Charles MacNeill 7:1add29d51e72 236 VL53L1_ThresholdMode CrossMode)
Charles MacNeill 7:1add29d51e72 237 {
Charles MacNeill 7:1add29d51e72 238 VL53L1_GPIO_Interrupt_Mode Mode;
Charles MacNeill 7:1add29d51e72 239
Charles MacNeill 7:1add29d51e72 240 switch (CrossMode) {
Charles MacNeill 7:1add29d51e72 241 case VL53L1_THRESHOLD_CROSSED_LOW:
Charles MacNeill 7:1add29d51e72 242 Mode = VL53L1_GPIOINTMODE_LEVEL_LOW;
Charles MacNeill 7:1add29d51e72 243 break;
Charles MacNeill 7:1add29d51e72 244 case VL53L1_THRESHOLD_CROSSED_HIGH:
Charles MacNeill 7:1add29d51e72 245 Mode = VL53L1_GPIOINTMODE_LEVEL_HIGH;
Charles MacNeill 7:1add29d51e72 246 break;
Charles MacNeill 7:1add29d51e72 247 case VL53L1_THRESHOLD_OUT_OF_WINDOW:
Charles MacNeill 7:1add29d51e72 248 Mode = VL53L1_GPIOINTMODE_OUT_OF_WINDOW;
Charles MacNeill 7:1add29d51e72 249 break;
Charles MacNeill 7:1add29d51e72 250 case VL53L1_THRESHOLD_IN_WINDOW:
Charles MacNeill 7:1add29d51e72 251 Mode = VL53L1_GPIOINTMODE_IN_WINDOW;
Charles MacNeill 7:1add29d51e72 252 break;
Charles MacNeill 7:1add29d51e72 253 default:
Charles MacNeill 7:1add29d51e72 254
Charles MacNeill 7:1add29d51e72 255 Mode = VL53L1_GPIOINTMODE_LEVEL_HIGH;
Charles MacNeill 7:1add29d51e72 256 *pStatus = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 257 }
Charles MacNeill 7:1add29d51e72 258 return Mode;
Charles MacNeill 7:1add29d51e72 259 }
Charles MacNeill 7:1add29d51e72 260
Charles MacNeill 7:1add29d51e72 261 static VL53L1_ThresholdMode ConvertModeFromLLD(VL53L1_Error *pStatus,
Charles MacNeill 7:1add29d51e72 262 VL53L1_GPIO_Interrupt_Mode CrossMode)
Charles MacNeill 7:1add29d51e72 263 {
Charles MacNeill 7:1add29d51e72 264 VL53L1_ThresholdMode Mode;
Charles MacNeill 7:1add29d51e72 265
Charles MacNeill 7:1add29d51e72 266 switch (CrossMode) {
Charles MacNeill 7:1add29d51e72 267 case VL53L1_GPIOINTMODE_LEVEL_LOW:
Charles MacNeill 7:1add29d51e72 268 Mode = VL53L1_THRESHOLD_CROSSED_LOW;
Charles MacNeill 7:1add29d51e72 269 break;
Charles MacNeill 7:1add29d51e72 270 case VL53L1_GPIOINTMODE_LEVEL_HIGH:
Charles MacNeill 7:1add29d51e72 271 Mode = VL53L1_THRESHOLD_CROSSED_HIGH;
Charles MacNeill 7:1add29d51e72 272 break;
Charles MacNeill 7:1add29d51e72 273 case VL53L1_GPIOINTMODE_OUT_OF_WINDOW:
Charles MacNeill 7:1add29d51e72 274 Mode = VL53L1_THRESHOLD_OUT_OF_WINDOW;
Charles MacNeill 7:1add29d51e72 275 break;
Charles MacNeill 7:1add29d51e72 276 case VL53L1_GPIOINTMODE_IN_WINDOW:
Charles MacNeill 7:1add29d51e72 277 Mode = VL53L1_THRESHOLD_IN_WINDOW;
Charles MacNeill 7:1add29d51e72 278 break;
Charles MacNeill 7:1add29d51e72 279 default:
Charles MacNeill 7:1add29d51e72 280
Charles MacNeill 7:1add29d51e72 281 Mode = VL53L1_THRESHOLD_CROSSED_HIGH;
Charles MacNeill 7:1add29d51e72 282 *pStatus = VL53L1_ERROR_UNDEFINED;
Charles MacNeill 7:1add29d51e72 283 }
Charles MacNeill 7:1add29d51e72 284 return Mode;
Charles MacNeill 7:1add29d51e72 285 }
Charles MacNeill 7:1add29d51e72 286
Charles MacNeill 7:1add29d51e72 287
Charles MacNeill 7:1add29d51e72 288
Charles MacNeill 7:1add29d51e72 289 VL53L1_Error VL53L1_GetVersion(VL53L1_Version_t *pVersion)
Charles MacNeill 7:1add29d51e72 290 {
Charles MacNeill 7:1add29d51e72 291 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 292
Charles MacNeill 7:1add29d51e72 293 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 294
Charles MacNeill 7:1add29d51e72 295 pVersion->major = VL53L1_IMPLEMENTATION_VER_MAJOR;
Charles MacNeill 7:1add29d51e72 296 pVersion->minor = VL53L1_IMPLEMENTATION_VER_MINOR;
Charles MacNeill 7:1add29d51e72 297 pVersion->build = VL53L1_IMPLEMENTATION_VER_SUB;
Charles MacNeill 7:1add29d51e72 298
Charles MacNeill 7:1add29d51e72 299 pVersion->revision = VL53L1_IMPLEMENTATION_VER_REVISION;
Charles MacNeill 7:1add29d51e72 300
Charles MacNeill 7:1add29d51e72 301 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 302 return Status;
Charles MacNeill 7:1add29d51e72 303 }
Charles MacNeill 7:1add29d51e72 304
Charles MacNeill 7:1add29d51e72 305 VL53L1_Error VL53L1_GetProductRevision(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 306 uint8_t *pProductRevisionMajor, uint8_t *pProductRevisionMinor)
Charles MacNeill 7:1add29d51e72 307 {
Charles MacNeill 7:1add29d51e72 308 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 309 uint8_t revision_id;
Charles MacNeill 7:1add29d51e72 310 VL53L1_LLDriverData_t *pLLData;
Charles MacNeill 7:1add29d51e72 311
Charles MacNeill 7:1add29d51e72 312 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 313
Charles MacNeill 7:1add29d51e72 314 pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 315 revision_id = pLLData->nvm_copy_data.identification__revision_id;
Charles MacNeill 7:1add29d51e72 316 *pProductRevisionMajor = 1;
Charles MacNeill 7:1add29d51e72 317 *pProductRevisionMinor = (revision_id & 0xF0) >> 4;
Charles MacNeill 7:1add29d51e72 318
Charles MacNeill 7:1add29d51e72 319 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 320 return Status;
Charles MacNeill 7:1add29d51e72 321
Charles MacNeill 7:1add29d51e72 322 }
Charles MacNeill 7:1add29d51e72 323
Charles MacNeill 7:1add29d51e72 324 VL53L1_Error VL53L1_GetDeviceInfo(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 325 VL53L1_DeviceInfo_t *pVL53L1_DeviceInfo)
Charles MacNeill 7:1add29d51e72 326 {
Charles MacNeill 7:1add29d51e72 327 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 328 uint8_t revision_id;
Charles MacNeill 7:1add29d51e72 329 VL53L1_LLDriverData_t *pLLData;
Charles MacNeill 7:1add29d51e72 330
Charles MacNeill 7:1add29d51e72 331 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 332
Charles MacNeill 7:1add29d51e72 333 pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 334
Charles MacNeill 7:1add29d51e72 335 strncpy(pVL53L1_DeviceInfo->ProductId, "",
Charles MacNeill 7:1add29d51e72 336 VL53L1_DEVINFO_STRLEN-1);
Charles MacNeill 7:1add29d51e72 337 pVL53L1_DeviceInfo->ProductType =
Charles MacNeill 7:1add29d51e72 338 pLLData->nvm_copy_data.identification__module_type;
Charles MacNeill 7:1add29d51e72 339
Charles MacNeill 7:1add29d51e72 340 revision_id = pLLData->nvm_copy_data.identification__revision_id;
Charles MacNeill 7:1add29d51e72 341 pVL53L1_DeviceInfo->ProductRevisionMajor = 1;
Charles MacNeill 7:1add29d51e72 342 pVL53L1_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4;
Charles MacNeill 7:1add29d51e72 343
Charles MacNeill 7:1add29d51e72 344 #ifndef VL53L1_USE_EMPTY_STRING
Charles MacNeill 7:1add29d51e72 345 if (pVL53L1_DeviceInfo->ProductRevisionMinor == 0)
Charles MacNeill 7:1add29d51e72 346 strncpy(pVL53L1_DeviceInfo->Name,
Charles MacNeill 7:1add29d51e72 347 VL53L1_STRING_DEVICE_INFO_NAME0,
Charles MacNeill 7:1add29d51e72 348 VL53L1_DEVINFO_STRLEN-1);
Charles MacNeill 7:1add29d51e72 349 else
Charles MacNeill 7:1add29d51e72 350 strncpy(pVL53L1_DeviceInfo->Name,
Charles MacNeill 7:1add29d51e72 351 VL53L1_STRING_DEVICE_INFO_NAME1,
Charles MacNeill 7:1add29d51e72 352 VL53L1_DEVINFO_STRLEN-1);
Charles MacNeill 7:1add29d51e72 353 strncpy(pVL53L1_DeviceInfo->Type,
Charles MacNeill 7:1add29d51e72 354 VL53L1_STRING_DEVICE_INFO_TYPE,
Charles MacNeill 7:1add29d51e72 355 VL53L1_DEVINFO_STRLEN-1);
Charles MacNeill 7:1add29d51e72 356
Charles MacNeill 7:1add29d51e72 357 if (pVL53L1_DeviceInfo->ProductType == 0xAA) {
Charles MacNeill 7:1add29d51e72 358 pVL53L1_DeviceInfo->Name[5] = '3';
Charles MacNeill 7:1add29d51e72 359 pVL53L1_DeviceInfo->Type[5] = '3';
Charles MacNeill 7:1add29d51e72 360 }
Charles MacNeill 7:1add29d51e72 361 #else
Charles MacNeill 7:1add29d51e72 362 pVL53L1_DeviceInfo->Name[0] = 0;
Charles MacNeill 7:1add29d51e72 363 pVL53L1_DeviceInfo->Type[0] = 0;
Charles MacNeill 7:1add29d51e72 364 #endif
Charles MacNeill 7:1add29d51e72 365
Charles MacNeill 7:1add29d51e72 366 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 367 return Status;
Charles MacNeill 7:1add29d51e72 368 }
Charles MacNeill 7:1add29d51e72 369
Charles MacNeill 7:1add29d51e72 370 VL53L1_Error VL53L1_GetUID(VL53L1_DEV Dev, uint64_t *pUid)
Charles MacNeill 7:1add29d51e72 371 {
Charles MacNeill 7:1add29d51e72 372 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 373 uint8_t fmtdata[8];
Charles MacNeill 7:1add29d51e72 374
Charles MacNeill 7:1add29d51e72 375 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 376
Charles MacNeill 7:1add29d51e72 377 Status = VL53L1_read_nvm_raw_data(Dev,
Charles MacNeill 7:1add29d51e72 378 (uint8_t)(0x1F8 >> 2),
Charles MacNeill 7:1add29d51e72 379 (uint8_t)(8 >> 2),
Charles MacNeill 7:1add29d51e72 380 fmtdata);
Charles MacNeill 7:1add29d51e72 381 memcpy(pUid, fmtdata, sizeof(uint64_t));
Charles MacNeill 7:1add29d51e72 382
Charles MacNeill 7:1add29d51e72 383 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 384 return Status;
Charles MacNeill 7:1add29d51e72 385 }
Charles MacNeill 7:1add29d51e72 386
Charles MacNeill 7:1add29d51e72 387 VL53L1_Error VL53L1_GetRangeStatusString(uint8_t RangeStatus,
Charles MacNeill 7:1add29d51e72 388 char *pRangeStatusString)
Charles MacNeill 7:1add29d51e72 389 {
Charles MacNeill 7:1add29d51e72 390 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 391
Charles MacNeill 7:1add29d51e72 392 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 393
Charles MacNeill 7:1add29d51e72 394 Status = VL53L1_get_range_status_string(RangeStatus,
Charles MacNeill 7:1add29d51e72 395 pRangeStatusString);
Charles MacNeill 7:1add29d51e72 396
Charles MacNeill 7:1add29d51e72 397 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 398 return Status;
Charles MacNeill 7:1add29d51e72 399 }
Charles MacNeill 7:1add29d51e72 400
Charles MacNeill 7:1add29d51e72 401 VL53L1_Error VL53L1_GetPalErrorString(VL53L1_Error PalErrorCode,
Charles MacNeill 7:1add29d51e72 402 char *pPalErrorString)
Charles MacNeill 7:1add29d51e72 403 {
Charles MacNeill 7:1add29d51e72 404 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 405
Charles MacNeill 7:1add29d51e72 406 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 407
Charles MacNeill 7:1add29d51e72 408 Status = VL53L1_get_pal_error_string(PalErrorCode, pPalErrorString);
Charles MacNeill 7:1add29d51e72 409
Charles MacNeill 7:1add29d51e72 410 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 411 return Status;
Charles MacNeill 7:1add29d51e72 412 }
Charles MacNeill 7:1add29d51e72 413
Charles MacNeill 7:1add29d51e72 414 VL53L1_Error VL53L1_GetPalStateString(VL53L1_State PalStateCode,
Charles MacNeill 7:1add29d51e72 415 char *pPalStateString)
Charles MacNeill 7:1add29d51e72 416 {
Charles MacNeill 7:1add29d51e72 417 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 418
Charles MacNeill 7:1add29d51e72 419 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 420
Charles MacNeill 7:1add29d51e72 421 Status = VL53L1_get_pal_state_string(PalStateCode, pPalStateString);
Charles MacNeill 7:1add29d51e72 422
Charles MacNeill 7:1add29d51e72 423 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 424 return Status;
Charles MacNeill 7:1add29d51e72 425 }
Charles MacNeill 7:1add29d51e72 426
Charles MacNeill 7:1add29d51e72 427 VL53L1_Error VL53L1_GetPalState(VL53L1_DEV Dev, VL53L1_State *pPalState)
Charles MacNeill 7:1add29d51e72 428 {
Charles MacNeill 7:1add29d51e72 429 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 430
Charles MacNeill 7:1add29d51e72 431 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 432
Charles MacNeill 7:1add29d51e72 433 *pPalState = VL53L1DevDataGet(Dev, PalState);
Charles MacNeill 7:1add29d51e72 434
Charles MacNeill 7:1add29d51e72 435 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 436 return Status;
Charles MacNeill 7:1add29d51e72 437 }
Charles MacNeill 7:1add29d51e72 438
Charles MacNeill 7:1add29d51e72 439
Charles MacNeill 7:1add29d51e72 440
Charles MacNeill 7:1add29d51e72 441
Charles MacNeill 7:1add29d51e72 442 VL53L1_Error VL53L1_SetDeviceAddress(VL53L1_DEV Dev, uint8_t DeviceAddress)
Charles MacNeill 7:1add29d51e72 443 {
Charles MacNeill 7:1add29d51e72 444 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 445 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 446 VL53L1_static_nvm_managed_t *pdata = &(pdev->stat_nvm);
Charles MacNeill 7:1add29d51e72 447
Charles MacNeill 7:1add29d51e72 448 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 449
Charles MacNeill 7:1add29d51e72 450 Status = VL53L1_WrByte(Dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS,
Charles MacNeill 7:1add29d51e72 451 DeviceAddress / 2);
Charles MacNeill 7:1add29d51e72 452
Charles MacNeill 7:1add29d51e72 453 pdata->i2c_slave__device_address = (DeviceAddress / 2) & 0x7F;
Charles MacNeill 7:1add29d51e72 454
Charles MacNeill 7:1add29d51e72 455 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 456 return Status;
Charles MacNeill 7:1add29d51e72 457 }
Charles MacNeill 7:1add29d51e72 458
Charles MacNeill 7:1add29d51e72 459 VL53L1_Error VL53L1_DataInit(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 460 {
Charles MacNeill 7:1add29d51e72 461 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 462 uint8_t i;
Charles MacNeill 7:1add29d51e72 463 VL53L1_LLDriverData_t *pdev;
Charles MacNeill 7:1add29d51e72 464
Charles MacNeill 7:1add29d51e72 465 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 466
Charles MacNeill 7:1add29d51e72 467
Charles MacNeill 7:1add29d51e72 468 #ifdef USE_I2C_2V8
Charles MacNeill 7:1add29d51e72 469 Status = VL53L1_RdByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, &i);
Charles MacNeill 7:1add29d51e72 470 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 471 i = (i & 0xfe) | 0x01;
Charles MacNeill 7:1add29d51e72 472 Status = VL53L1_WrByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG,
Charles MacNeill 7:1add29d51e72 473 i);
Charles MacNeill 7:1add29d51e72 474 }
Charles MacNeill 7:1add29d51e72 475 #endif
Charles MacNeill 7:1add29d51e72 476
Charles MacNeill 7:1add29d51e72 477 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 478 Status = VL53L1_data_init(Dev, 1);
Charles MacNeill 7:1add29d51e72 479
Charles MacNeill 7:1add29d51e72 480 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 481 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 482 memset(&pdev->per_vcsel_cal_data, 0,
Charles MacNeill 7:1add29d51e72 483 sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 7:1add29d51e72 484 }
Charles MacNeill 7:1add29d51e72 485
Charles MacNeill 7:1add29d51e72 486 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 487 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_WAIT_STATICINIT);
Charles MacNeill 7:1add29d51e72 488
Charles MacNeill 7:1add29d51e72 489
Charles MacNeill 7:1add29d51e72 490 for (i = 0; i < VL53L1_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
Charles MacNeill 7:1add29d51e72 491 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 492 Status |= VL53L1_SetLimitCheckEnable(Dev, i, 1);
Charles MacNeill 7:1add29d51e72 493 else
Charles MacNeill 7:1add29d51e72 494 break;
Charles MacNeill 7:1add29d51e72 495
Charles MacNeill 7:1add29d51e72 496 }
Charles MacNeill 7:1add29d51e72 497
Charles MacNeill 7:1add29d51e72 498
Charles MacNeill 7:1add29d51e72 499 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 500 Status = VL53L1_set_dmax_mode(Dev,
Charles MacNeill 7:1add29d51e72 501 VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA);
Charles MacNeill 7:1add29d51e72 502 }
Charles MacNeill 7:1add29d51e72 503
Charles MacNeill 7:1add29d51e72 504
Charles MacNeill 7:1add29d51e72 505 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 506 return Status;
Charles MacNeill 7:1add29d51e72 507 }
Charles MacNeill 7:1add29d51e72 508
Charles MacNeill 7:1add29d51e72 509
Charles MacNeill 7:1add29d51e72 510 VL53L1_Error VL53L1_StaticInit(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 511 {
Charles MacNeill 7:1add29d51e72 512 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 513 uint8_t measurement_mode;
Charles MacNeill 7:1add29d51e72 514
Charles MacNeill 7:1add29d51e72 515 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 516
Charles MacNeill 7:1add29d51e72 517 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_IDLE);
Charles MacNeill 7:1add29d51e72 518
Charles MacNeill 7:1add29d51e72 519 measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
Charles MacNeill 7:1add29d51e72 520 VL53L1DevDataSet(Dev, LLData.measurement_mode, measurement_mode);
Charles MacNeill 7:1add29d51e72 521
Charles MacNeill 7:1add29d51e72 522 Status = VL53L1_SetPresetMode(Dev,
Charles MacNeill 7:1add29d51e72 523 VL53L1_PRESETMODE_RANGING);
Charles MacNeill 7:1add29d51e72 524 VL53L1_SetDistanceMode(Dev,
Charles MacNeill 7:1add29d51e72 525 VL53L1_DISTANCEMODE_MEDIUM);
Charles MacNeill 7:1add29d51e72 526 VL53L1DevDataSet(Dev, CurrentParameters.OutputMode,
Charles MacNeill 7:1add29d51e72 527 VL53L1_OUTPUTMODE_NEAREST);
Charles MacNeill 7:1add29d51e72 528 VL53L1_SmudgeCorrectionEnable(Dev,
Charles MacNeill 7:1add29d51e72 529 VL53L1_SMUDGE_CORRECTION_NONE);
Charles MacNeill 7:1add29d51e72 530
Charles MacNeill 7:1add29d51e72 531 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 532 return Status;
Charles MacNeill 7:1add29d51e72 533 }
Charles MacNeill 7:1add29d51e72 534
Charles MacNeill 7:1add29d51e72 535 VL53L1_Error VL53L1_WaitDeviceBooted(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 536 {
Charles MacNeill 7:1add29d51e72 537 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 538
Charles MacNeill 7:1add29d51e72 539 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 540
Charles MacNeill 7:1add29d51e72 541 Status = VL53L1_poll_for_boot_completion(Dev,
Charles MacNeill 7:1add29d51e72 542 VL53L1_BOOT_COMPLETION_POLLING_TIMEOUT_MS);
Charles MacNeill 7:1add29d51e72 543
Charles MacNeill 7:1add29d51e72 544 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 545 return Status;
Charles MacNeill 7:1add29d51e72 546 }
Charles MacNeill 7:1add29d51e72 547
Charles MacNeill 7:1add29d51e72 548
Charles MacNeill 7:1add29d51e72 549
Charles MacNeill 7:1add29d51e72 550
Charles MacNeill 7:1add29d51e72 551 static VL53L1_Error ComputeDevicePresetMode(
Charles MacNeill 7:1add29d51e72 552 VL53L1_PresetModes PresetMode,
Charles MacNeill 7:1add29d51e72 553 VL53L1_DistanceModes DistanceMode,
Charles MacNeill 7:1add29d51e72 554 VL53L1_DevicePresetModes *pDevicePresetMode)
Charles MacNeill 7:1add29d51e72 555 {
Charles MacNeill 7:1add29d51e72 556 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 557
Charles MacNeill 7:1add29d51e72 558 uint8_t DistIdx;
Charles MacNeill 7:1add29d51e72 559 VL53L1_DevicePresetModes LightModes[3] = {
Charles MacNeill 7:1add29d51e72 560 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_SHORT_RANGE,
Charles MacNeill 7:1add29d51e72 561 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING,
Charles MacNeill 7:1add29d51e72 562 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_LONG_RANGE};
Charles MacNeill 7:1add29d51e72 563
Charles MacNeill 7:1add29d51e72 564 VL53L1_DevicePresetModes RangingModes[3] = {
Charles MacNeill 7:1add29d51e72 565 VL53L1_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE,
Charles MacNeill 7:1add29d51e72 566 VL53L1_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE,
Charles MacNeill 7:1add29d51e72 567 VL53L1_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE};
Charles MacNeill 7:1add29d51e72 568
Charles MacNeill 7:1add29d51e72 569 VL53L1_DevicePresetModes ScanningModes[3] = {
Charles MacNeill 7:1add29d51e72 570 VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_SHORT_RANGE,
Charles MacNeill 7:1add29d51e72 571 VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE,
Charles MacNeill 7:1add29d51e72 572 VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_LONG_RANGE};
Charles MacNeill 7:1add29d51e72 573
Charles MacNeill 7:1add29d51e72 574 VL53L1_DevicePresetModes TimedModes[3] = {
Charles MacNeill 7:1add29d51e72 575 VL53L1_DEVICEPRESETMODE_TIMED_RANGING_SHORT_RANGE,
Charles MacNeill 7:1add29d51e72 576 VL53L1_DEVICEPRESETMODE_TIMED_RANGING,
Charles MacNeill 7:1add29d51e72 577 VL53L1_DEVICEPRESETMODE_TIMED_RANGING_LONG_RANGE};
Charles MacNeill 7:1add29d51e72 578
Charles MacNeill 7:1add29d51e72 579 VL53L1_DevicePresetModes LowPowerTimedModes[3] = {
Charles MacNeill 7:1add29d51e72 580 VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_SHORT_RANGE,
Charles MacNeill 7:1add29d51e72 581 VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_MEDIUM_RANGE,
Charles MacNeill 7:1add29d51e72 582 VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_LONG_RANGE};
Charles MacNeill 7:1add29d51e72 583
Charles MacNeill 7:1add29d51e72 584 *pDevicePresetMode = VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
Charles MacNeill 7:1add29d51e72 585
Charles MacNeill 7:1add29d51e72 586 switch (DistanceMode) {
Charles MacNeill 7:1add29d51e72 587 case VL53L1_DISTANCEMODE_SHORT:
Charles MacNeill 7:1add29d51e72 588 DistIdx = 0;
Charles MacNeill 7:1add29d51e72 589 break;
Charles MacNeill 7:1add29d51e72 590 case VL53L1_DISTANCEMODE_MEDIUM:
Charles MacNeill 7:1add29d51e72 591 DistIdx = 1;
Charles MacNeill 7:1add29d51e72 592 break;
Charles MacNeill 7:1add29d51e72 593 default:
Charles MacNeill 7:1add29d51e72 594 DistIdx = 2;
Charles MacNeill 7:1add29d51e72 595 }
Charles MacNeill 7:1add29d51e72 596
Charles MacNeill 7:1add29d51e72 597 switch (PresetMode) {
Charles MacNeill 7:1add29d51e72 598 case VL53L1_PRESETMODE_LITE_RANGING:
Charles MacNeill 7:1add29d51e72 599 *pDevicePresetMode = LightModes[DistIdx];
Charles MacNeill 7:1add29d51e72 600 break;
Charles MacNeill 7:1add29d51e72 601
Charles MacNeill 7:1add29d51e72 602 case VL53L1_PRESETMODE_RANGING:
Charles MacNeill 7:1add29d51e72 603 *pDevicePresetMode = RangingModes[DistIdx];
Charles MacNeill 7:1add29d51e72 604 break;
Charles MacNeill 7:1add29d51e72 605
Charles MacNeill 7:1add29d51e72 606 case VL53L1_PRESETMODE_MULTIZONES_SCANNING:
Charles MacNeill 7:1add29d51e72 607 *pDevicePresetMode = ScanningModes[DistIdx];
Charles MacNeill 7:1add29d51e72 608 break;
Charles MacNeill 7:1add29d51e72 609
Charles MacNeill 7:1add29d51e72 610 case VL53L1_PRESETMODE_AUTONOMOUS:
Charles MacNeill 7:1add29d51e72 611 *pDevicePresetMode = TimedModes[DistIdx];
Charles MacNeill 7:1add29d51e72 612 break;
Charles MacNeill 7:1add29d51e72 613
Charles MacNeill 7:1add29d51e72 614 case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
Charles MacNeill 7:1add29d51e72 615 *pDevicePresetMode = LowPowerTimedModes[DistIdx];
Charles MacNeill 7:1add29d51e72 616 break;
Charles MacNeill 7:1add29d51e72 617 case VL53L1_PRESETMODE_OLT:
Charles MacNeill 7:1add29d51e72 618 *pDevicePresetMode = VL53L1_DEVICEPRESETMODE_OLT;
Charles MacNeill 7:1add29d51e72 619 break;
Charles MacNeill 7:1add29d51e72 620 case VL53L1_PRESETMODE_PROXY_RANGING_MODE:
Charles MacNeill 7:1add29d51e72 621 *pDevicePresetMode =
Charles MacNeill 7:1add29d51e72 622 VL53L1_DEVICEPRESETMODE_SPECIAL_HISTOGRAM_SHORT_RANGE;
Charles MacNeill 7:1add29d51e72 623 break;
Charles MacNeill 7:1add29d51e72 624
Charles MacNeill 7:1add29d51e72 625 default:
Charles MacNeill 7:1add29d51e72 626
Charles MacNeill 7:1add29d51e72 627 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 628 }
Charles MacNeill 7:1add29d51e72 629
Charles MacNeill 7:1add29d51e72 630 return Status;
Charles MacNeill 7:1add29d51e72 631 }
Charles MacNeill 7:1add29d51e72 632
Charles MacNeill 7:1add29d51e72 633 static VL53L1_Error SetPresetMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 634 VL53L1_PresetModes PresetMode,
Charles MacNeill 7:1add29d51e72 635 VL53L1_DistanceModes DistanceMode,
Charles MacNeill 7:1add29d51e72 636 uint32_t inter_measurement_period_ms)
Charles MacNeill 7:1add29d51e72 637 {
Charles MacNeill 7:1add29d51e72 638 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 639 VL53L1_DevicePresetModes device_preset_mode;
Charles MacNeill 7:1add29d51e72 640 uint8_t measurement_mode;
lugandc 18:0696efe39d08 641 uint16_t dss_config__target_total_rate_mcps = 0;
lugandc 18:0696efe39d08 642 uint32_t phasecal_config_timeout_us = 0;
lugandc 18:0696efe39d08 643 uint32_t mm_config_timeout_us = 0;
lugandc 18:0696efe39d08 644 uint32_t lld_range_config_timeout_us = 0;
Charles MacNeill 7:1add29d51e72 645
Charles MacNeill 7:1add29d51e72 646 LOG_FUNCTION_START("%d", (int)PresetMode);
Charles MacNeill 7:1add29d51e72 647
Charles MacNeill 7:1add29d51e72 648 if ((PresetMode == VL53L1_PRESETMODE_AUTONOMOUS) ||
Charles MacNeill 7:1add29d51e72 649 (PresetMode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS))
Charles MacNeill 7:1add29d51e72 650 measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_TIMED;
Charles MacNeill 7:1add29d51e72 651 else
Charles MacNeill 7:1add29d51e72 652 measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
Charles MacNeill 7:1add29d51e72 653
Charles MacNeill 7:1add29d51e72 654
Charles MacNeill 7:1add29d51e72 655 Status = ComputeDevicePresetMode(PresetMode, DistanceMode,
Charles MacNeill 7:1add29d51e72 656 &device_preset_mode);
Charles MacNeill 7:1add29d51e72 657
Charles MacNeill 7:1add29d51e72 658 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 659 Status = VL53L1_get_preset_mode_timing_cfg(Dev,
Charles MacNeill 7:1add29d51e72 660 device_preset_mode,
Charles MacNeill 7:1add29d51e72 661 &dss_config__target_total_rate_mcps,
Charles MacNeill 7:1add29d51e72 662 &phasecal_config_timeout_us,
Charles MacNeill 7:1add29d51e72 663 &mm_config_timeout_us,
Charles MacNeill 7:1add29d51e72 664 &lld_range_config_timeout_us);
Charles MacNeill 7:1add29d51e72 665
Charles MacNeill 7:1add29d51e72 666 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 667 Status = VL53L1_set_preset_mode(
Charles MacNeill 7:1add29d51e72 668 Dev,
Charles MacNeill 7:1add29d51e72 669 device_preset_mode,
Charles MacNeill 7:1add29d51e72 670 dss_config__target_total_rate_mcps,
Charles MacNeill 7:1add29d51e72 671 phasecal_config_timeout_us,
Charles MacNeill 7:1add29d51e72 672 mm_config_timeout_us,
Charles MacNeill 7:1add29d51e72 673 lld_range_config_timeout_us,
Charles MacNeill 7:1add29d51e72 674 inter_measurement_period_ms);
Charles MacNeill 7:1add29d51e72 675
Charles MacNeill 7:1add29d51e72 676 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 677 VL53L1DevDataSet(Dev, LLData.measurement_mode,
Charles MacNeill 7:1add29d51e72 678 measurement_mode);
Charles MacNeill 7:1add29d51e72 679
Charles MacNeill 7:1add29d51e72 680 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 681 VL53L1DevDataSet(Dev, CurrentParameters.PresetMode, PresetMode);
Charles MacNeill 7:1add29d51e72 682
Charles MacNeill 7:1add29d51e72 683 VL53L1DevDataSet(Dev, CurrentParameters.OutputMode,
Charles MacNeill 7:1add29d51e72 684 VL53L1_OUTPUTMODE_NEAREST);
Charles MacNeill 7:1add29d51e72 685 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 686 return Status;
Charles MacNeill 7:1add29d51e72 687 }
Charles MacNeill 7:1add29d51e72 688
Charles MacNeill 7:1add29d51e72 689
Charles MacNeill 7:1add29d51e72 690 VL53L1_Error VL53L1_SetPresetMode(VL53L1_DEV Dev, VL53L1_PresetModes PresetMode)
Charles MacNeill 7:1add29d51e72 691 {
Charles MacNeill 7:1add29d51e72 692 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 693 VL53L1_DistanceModes DistanceMode = VL53L1_DISTANCEMODE_LONG;
Charles MacNeill 7:1add29d51e72 694
Charles MacNeill 7:1add29d51e72 695 LOG_FUNCTION_START("%d", (int)PresetMode);
Charles MacNeill 7:1add29d51e72 696
Charles MacNeill 7:1add29d51e72 697
Charles MacNeill 7:1add29d51e72 698 Status = VL53L1_low_power_auto_data_init(Dev);
Charles MacNeill 7:1add29d51e72 699
Charles MacNeill 7:1add29d51e72 700 if (PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE)
Charles MacNeill 7:1add29d51e72 701 DistanceMode = VL53L1_DISTANCEMODE_SHORT;
Charles MacNeill 7:1add29d51e72 702 Status = SetPresetMode(Dev,
Charles MacNeill 7:1add29d51e72 703 PresetMode,
Charles MacNeill 7:1add29d51e72 704 DistanceMode,
Charles MacNeill 7:1add29d51e72 705 1000);
Charles MacNeill 7:1add29d51e72 706
Charles MacNeill 7:1add29d51e72 707 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 708 if ((PresetMode == VL53L1_PRESETMODE_LITE_RANGING) ||
Charles MacNeill 7:1add29d51e72 709 (PresetMode == VL53L1_PRESETMODE_AUTONOMOUS) ||
Charles MacNeill 7:1add29d51e72 710 (PresetMode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS))
Charles MacNeill 7:1add29d51e72 711 Status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(
Charles MacNeill 7:1add29d51e72 712 Dev, 41000);
Charles MacNeill 7:1add29d51e72 713 else
Charles MacNeill 7:1add29d51e72 714
Charles MacNeill 7:1add29d51e72 715 Status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(
Charles MacNeill 7:1add29d51e72 716 Dev, 33333);
Charles MacNeill 7:1add29d51e72 717 }
Charles MacNeill 7:1add29d51e72 718
Charles MacNeill 7:1add29d51e72 719 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 720
Charles MacNeill 7:1add29d51e72 721 Status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev,
Charles MacNeill 7:1add29d51e72 722 1000);
Charles MacNeill 7:1add29d51e72 723 }
Charles MacNeill 7:1add29d51e72 724
Charles MacNeill 7:1add29d51e72 725 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 726 return Status;
Charles MacNeill 7:1add29d51e72 727 }
Charles MacNeill 7:1add29d51e72 728
Charles MacNeill 7:1add29d51e72 729
Charles MacNeill 7:1add29d51e72 730 VL53L1_Error VL53L1_GetPresetMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 731 VL53L1_PresetModes *pPresetMode)
Charles MacNeill 7:1add29d51e72 732 {
Charles MacNeill 7:1add29d51e72 733 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 734
Charles MacNeill 7:1add29d51e72 735 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 736
Charles MacNeill 7:1add29d51e72 737 *pPresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 738
Charles MacNeill 7:1add29d51e72 739 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 740 return Status;
Charles MacNeill 7:1add29d51e72 741 }
Charles MacNeill 7:1add29d51e72 742
Charles MacNeill 7:1add29d51e72 743 VL53L1_Error VL53L1_SetDistanceMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 744 VL53L1_DistanceModes DistanceMode)
Charles MacNeill 7:1add29d51e72 745 {
Charles MacNeill 7:1add29d51e72 746 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 747 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 748 uint32_t inter_measurement_period_ms;
lugandc 18:0696efe39d08 749 uint32_t TimingBudget = 0;
lugandc 18:0696efe39d08 750 uint32_t MmTimeoutUs = 0;
lugandc 18:0696efe39d08 751 uint32_t PhaseCalTimeoutUs = 0;
Charles MacNeill 7:1add29d51e72 752 VL53L1_zone_config_t zone_config;
Charles MacNeill 7:1add29d51e72 753
Charles MacNeill 7:1add29d51e72 754 LOG_FUNCTION_START("%d", (int)DistanceMode);
Charles MacNeill 7:1add29d51e72 755
Charles MacNeill 7:1add29d51e72 756 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 757
Charles MacNeill 7:1add29d51e72 758
Charles MacNeill 7:1add29d51e72 759
Charles MacNeill 7:1add29d51e72 760 if ((PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE) &&
Charles MacNeill 7:1add29d51e72 761 (DistanceMode != VL53L1_DISTANCEMODE_SHORT))
Charles MacNeill 7:1add29d51e72 762 return VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 763 if ((DistanceMode != VL53L1_DISTANCEMODE_SHORT) &&
Charles MacNeill 7:1add29d51e72 764 (DistanceMode != VL53L1_DISTANCEMODE_MEDIUM) &&
Charles MacNeill 7:1add29d51e72 765 (DistanceMode != VL53L1_DISTANCEMODE_LONG))
Charles MacNeill 7:1add29d51e72 766 return VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 767
Charles MacNeill 7:1add29d51e72 768 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 769 Status = VL53L1_get_zone_config(Dev, &zone_config);
Charles MacNeill 7:1add29d51e72 770
Charles MacNeill 7:1add29d51e72 771 inter_measurement_period_ms = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 772 LLData.inter_measurement_period_ms);
Charles MacNeill 7:1add29d51e72 773
Charles MacNeill 7:1add29d51e72 774 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 775 Status = VL53L1_get_timeouts_us(Dev, &PhaseCalTimeoutUs,
Charles MacNeill 7:1add29d51e72 776 &MmTimeoutUs, &TimingBudget);
Charles MacNeill 7:1add29d51e72 777
Charles MacNeill 7:1add29d51e72 778 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 779 Status = SetPresetMode(Dev,
Charles MacNeill 7:1add29d51e72 780 PresetMode,
Charles MacNeill 7:1add29d51e72 781 DistanceMode,
Charles MacNeill 7:1add29d51e72 782 inter_measurement_period_ms);
Charles MacNeill 7:1add29d51e72 783
Charles MacNeill 7:1add29d51e72 784 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 785 VL53L1DevDataSet(Dev, CurrentParameters.DistanceMode,
Charles MacNeill 7:1add29d51e72 786 DistanceMode);
Charles MacNeill 7:1add29d51e72 787 }
Charles MacNeill 7:1add29d51e72 788
Charles MacNeill 7:1add29d51e72 789 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 790 Status = VL53L1_set_timeouts_us(Dev, PhaseCalTimeoutUs,
Charles MacNeill 7:1add29d51e72 791 MmTimeoutUs, TimingBudget);
Charles MacNeill 7:1add29d51e72 792
Charles MacNeill 7:1add29d51e72 793 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 794 VL53L1DevDataSet(Dev, LLData.range_config_timeout_us,
Charles MacNeill 7:1add29d51e72 795 TimingBudget);
Charles MacNeill 7:1add29d51e72 796 }
Charles MacNeill 7:1add29d51e72 797
Charles MacNeill 7:1add29d51e72 798 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 799 Status = VL53L1_set_zone_config(Dev, &zone_config);
Charles MacNeill 7:1add29d51e72 800
Charles MacNeill 7:1add29d51e72 801 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 802 return Status;
Charles MacNeill 7:1add29d51e72 803 }
Charles MacNeill 7:1add29d51e72 804
Charles MacNeill 7:1add29d51e72 805 VL53L1_Error VL53L1_GetDistanceMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 806 VL53L1_DistanceModes *pDistanceMode)
Charles MacNeill 7:1add29d51e72 807 {
Charles MacNeill 7:1add29d51e72 808 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 809
Charles MacNeill 7:1add29d51e72 810 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 811
Charles MacNeill 7:1add29d51e72 812 *pDistanceMode = VL53L1DevDataGet(Dev, CurrentParameters.DistanceMode);
Charles MacNeill 7:1add29d51e72 813
Charles MacNeill 7:1add29d51e72 814 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 815 return Status;
Charles MacNeill 7:1add29d51e72 816 }
Charles MacNeill 7:1add29d51e72 817
Charles MacNeill 7:1add29d51e72 818 VL53L1_Error VL53L1_SetOutputMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 819 VL53L1_OutputModes OutputMode)
Charles MacNeill 7:1add29d51e72 820 {
Charles MacNeill 7:1add29d51e72 821 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 822
Charles MacNeill 7:1add29d51e72 823 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 824
Charles MacNeill 7:1add29d51e72 825 if ((OutputMode != VL53L1_OUTPUTMODE_NEAREST) &&
Charles MacNeill 7:1add29d51e72 826 (OutputMode != VL53L1_OUTPUTMODE_STRONGEST))
Charles MacNeill 7:1add29d51e72 827 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 828 else
Charles MacNeill 7:1add29d51e72 829 VL53L1DevDataSet(Dev, CurrentParameters.OutputMode, OutputMode);
Charles MacNeill 7:1add29d51e72 830
Charles MacNeill 7:1add29d51e72 831 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 832 return Status;
Charles MacNeill 7:1add29d51e72 833 }
Charles MacNeill 7:1add29d51e72 834
Charles MacNeill 7:1add29d51e72 835 VL53L1_Error VL53L1_GetOutputMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 836 VL53L1_OutputModes *pOutputMode)
Charles MacNeill 7:1add29d51e72 837 {
Charles MacNeill 7:1add29d51e72 838 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 839
Charles MacNeill 7:1add29d51e72 840 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 841
Charles MacNeill 7:1add29d51e72 842 *pOutputMode = VL53L1DevDataGet(Dev, CurrentParameters.OutputMode);
Charles MacNeill 7:1add29d51e72 843
Charles MacNeill 7:1add29d51e72 844 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 845 return Status;
Charles MacNeill 7:1add29d51e72 846 }
Charles MacNeill 7:1add29d51e72 847
Charles MacNeill 7:1add29d51e72 848
Charles MacNeill 7:1add29d51e72 849
Charles MacNeill 7:1add29d51e72 850 VL53L1_Error VL53L1_SetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 851 uint32_t MeasurementTimingBudgetMicroSeconds)
Charles MacNeill 7:1add29d51e72 852 {
Charles MacNeill 7:1add29d51e72 853 VL53L1_Error Status = VL53L1_ERROR_NONE;
lugandc 18:0696efe39d08 854 uint8_t Mm1Enabled = 0;
lugandc 18:0696efe39d08 855 uint8_t Mm2Enabled = 0;
Charles MacNeill 7:1add29d51e72 856 uint32_t TimingGuard;
Charles MacNeill 7:1add29d51e72 857 uint32_t divisor;
lugandc 18:0696efe39d08 858 uint32_t TimingBudget = 0;
lugandc 18:0696efe39d08 859 uint32_t MmTimeoutUs = 0;
Charles MacNeill 7:1add29d51e72 860 VL53L1_PresetModes PresetMode;
lugandc 18:0696efe39d08 861 uint32_t PhaseCalTimeoutUs = 0;
Charles MacNeill 7:1add29d51e72 862 uint32_t vhv;
Charles MacNeill 7:1add29d51e72 863 int32_t vhv_loops;
Charles MacNeill 7:1add29d51e72 864 uint32_t FDAMaxTimingBudgetUs = FDA_MAX_TIMING_BUDGET_US;
Charles MacNeill 7:1add29d51e72 865
Charles MacNeill 7:1add29d51e72 866 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 867
Charles MacNeill 7:1add29d51e72 868
Charles MacNeill 7:1add29d51e72 869 if (MeasurementTimingBudgetMicroSeconds > 10000000)
Charles MacNeill 7:1add29d51e72 870 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 871
Charles MacNeill 7:1add29d51e72 872 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 873 Status = VL53L1_GetSequenceStepEnable(Dev,
Charles MacNeill 7:1add29d51e72 874 VL53L1_SEQUENCESTEP_MM1, &Mm1Enabled);
Charles MacNeill 7:1add29d51e72 875 }
Charles MacNeill 7:1add29d51e72 876
Charles MacNeill 7:1add29d51e72 877 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 878 Status = VL53L1_GetSequenceStepEnable(Dev,
Charles MacNeill 7:1add29d51e72 879 VL53L1_SEQUENCESTEP_MM2, &Mm2Enabled);
Charles MacNeill 7:1add29d51e72 880 }
Charles MacNeill 7:1add29d51e72 881
Charles MacNeill 7:1add29d51e72 882 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 883 Status = VL53L1_get_timeouts_us(Dev,
Charles MacNeill 7:1add29d51e72 884 &PhaseCalTimeoutUs,
Charles MacNeill 7:1add29d51e72 885 &MmTimeoutUs,
Charles MacNeill 7:1add29d51e72 886 &TimingBudget);
Charles MacNeill 7:1add29d51e72 887
Charles MacNeill 7:1add29d51e72 888 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 889 PresetMode = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 890 CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 891
Charles MacNeill 7:1add29d51e72 892 TimingGuard = 0;
Charles MacNeill 7:1add29d51e72 893 divisor = 1;
Charles MacNeill 7:1add29d51e72 894 switch (PresetMode) {
Charles MacNeill 7:1add29d51e72 895 case VL53L1_PRESETMODE_LITE_RANGING:
Charles MacNeill 7:1add29d51e72 896 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
Charles MacNeill 7:1add29d51e72 897 TimingGuard = 5000;
Charles MacNeill 7:1add29d51e72 898 else
Charles MacNeill 7:1add29d51e72 899 TimingGuard = 1000;
Charles MacNeill 7:1add29d51e72 900 break;
Charles MacNeill 7:1add29d51e72 901
Charles MacNeill 7:1add29d51e72 902 case VL53L1_PRESETMODE_AUTONOMOUS:
Charles MacNeill 7:1add29d51e72 903 FDAMaxTimingBudgetUs *= 2;
Charles MacNeill 7:1add29d51e72 904 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
Charles MacNeill 7:1add29d51e72 905 TimingGuard = 26600;
Charles MacNeill 7:1add29d51e72 906 else
Charles MacNeill 7:1add29d51e72 907 TimingGuard = 21600;
Charles MacNeill 7:1add29d51e72 908 divisor = 2;
Charles MacNeill 7:1add29d51e72 909 break;
Charles MacNeill 7:1add29d51e72 910
Charles MacNeill 7:1add29d51e72 911 case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
Charles MacNeill 7:1add29d51e72 912 FDAMaxTimingBudgetUs *= 2;
Charles MacNeill 7:1add29d51e72 913 vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
Charles MacNeill 7:1add29d51e72 914 VL53L1_get_tuning_parm(Dev,
Charles MacNeill 7:1add29d51e72 915 VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND,
Charles MacNeill 7:1add29d51e72 916 &vhv_loops);
Charles MacNeill 7:1add29d51e72 917 if (vhv_loops > 0) {
Charles MacNeill 7:1add29d51e72 918 vhv += vhv_loops *
Charles MacNeill 7:1add29d51e72 919 LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
Charles MacNeill 7:1add29d51e72 920 }
Charles MacNeill 7:1add29d51e72 921 TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING +
Charles MacNeill 7:1add29d51e72 922 LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING +
Charles MacNeill 7:1add29d51e72 923 vhv;
Charles MacNeill 7:1add29d51e72 924 divisor = 2;
Charles MacNeill 7:1add29d51e72 925 break;
Charles MacNeill 7:1add29d51e72 926
Charles MacNeill 7:1add29d51e72 927 case VL53L1_PRESETMODE_RANGING:
Charles MacNeill 7:1add29d51e72 928 case VL53L1_PRESETMODE_MULTIZONES_SCANNING:
Charles MacNeill 7:1add29d51e72 929 case VL53L1_PRESETMODE_PROXY_RANGING_MODE:
Charles MacNeill 7:1add29d51e72 930 TimingGuard = 1700;
Charles MacNeill 7:1add29d51e72 931 divisor = 6;
Charles MacNeill 7:1add29d51e72 932 break;
Charles MacNeill 7:1add29d51e72 933
Charles MacNeill 7:1add29d51e72 934 case VL53L1_PRESETMODE_OLT:
Charles MacNeill 7:1add29d51e72 935 TimingGuard = MmTimeoutUs + 5000;
Charles MacNeill 7:1add29d51e72 936 break;
Charles MacNeill 7:1add29d51e72 937 default:
Charles MacNeill 7:1add29d51e72 938
Charles MacNeill 7:1add29d51e72 939 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 940 }
Charles MacNeill 7:1add29d51e72 941
Charles MacNeill 7:1add29d51e72 942 if (MeasurementTimingBudgetMicroSeconds <= TimingGuard)
Charles MacNeill 7:1add29d51e72 943 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 944 else {
Charles MacNeill 7:1add29d51e72 945 TimingBudget = (MeasurementTimingBudgetMicroSeconds
Charles MacNeill 7:1add29d51e72 946 - TimingGuard);
Charles MacNeill 7:1add29d51e72 947 }
Charles MacNeill 7:1add29d51e72 948
Charles MacNeill 7:1add29d51e72 949 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 950 if (TimingBudget > FDAMaxTimingBudgetUs)
Charles MacNeill 7:1add29d51e72 951 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 952 else {
Charles MacNeill 7:1add29d51e72 953 TimingBudget /= divisor;
Charles MacNeill 7:1add29d51e72 954 Status = VL53L1_set_timeouts_us(
Charles MacNeill 7:1add29d51e72 955 Dev,
Charles MacNeill 7:1add29d51e72 956 PhaseCalTimeoutUs,
Charles MacNeill 7:1add29d51e72 957 MmTimeoutUs,
Charles MacNeill 7:1add29d51e72 958 TimingBudget);
Charles MacNeill 7:1add29d51e72 959 }
Charles MacNeill 7:1add29d51e72 960
Charles MacNeill 7:1add29d51e72 961 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 962 VL53L1DevDataSet(Dev,
Charles MacNeill 7:1add29d51e72 963 LLData.range_config_timeout_us,
Charles MacNeill 7:1add29d51e72 964 TimingBudget);
Charles MacNeill 7:1add29d51e72 965 }
Charles MacNeill 7:1add29d51e72 966 }
Charles MacNeill 7:1add29d51e72 967 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 968 VL53L1DevDataSet(Dev,
Charles MacNeill 7:1add29d51e72 969 CurrentParameters.MeasurementTimingBudgetMicroSeconds,
Charles MacNeill 7:1add29d51e72 970 MeasurementTimingBudgetMicroSeconds);
Charles MacNeill 7:1add29d51e72 971 }
Charles MacNeill 7:1add29d51e72 972
Charles MacNeill 7:1add29d51e72 973 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 974 return Status;
Charles MacNeill 7:1add29d51e72 975 }
Charles MacNeill 7:1add29d51e72 976
Charles MacNeill 7:1add29d51e72 977
Charles MacNeill 7:1add29d51e72 978 VL53L1_Error VL53L1_GetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 979 uint32_t *pMeasurementTimingBudgetMicroSeconds)
Charles MacNeill 7:1add29d51e72 980 {
Charles MacNeill 7:1add29d51e72 981 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 982 uint8_t Mm1Enabled = 0;
Charles MacNeill 7:1add29d51e72 983 uint8_t Mm2Enabled = 0;
Charles MacNeill 7:1add29d51e72 984 uint32_t MmTimeoutUs = 0;
Charles MacNeill 7:1add29d51e72 985 uint32_t RangeTimeoutUs = 0;
Charles MacNeill 7:1add29d51e72 986 uint32_t MeasTimingBdg = 0;
Charles MacNeill 7:1add29d51e72 987 uint32_t PhaseCalTimeoutUs = 0;
Charles MacNeill 7:1add29d51e72 988 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 989 uint32_t TimingGuard;
Charles MacNeill 7:1add29d51e72 990 uint32_t vhv;
Charles MacNeill 7:1add29d51e72 991 int32_t vhv_loops;
Charles MacNeill 7:1add29d51e72 992
Charles MacNeill 7:1add29d51e72 993 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 994
Charles MacNeill 7:1add29d51e72 995 *pMeasurementTimingBudgetMicroSeconds = 0;
Charles MacNeill 7:1add29d51e72 996
Charles MacNeill 7:1add29d51e72 997 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 998 Status = VL53L1_GetSequenceStepEnable(Dev,
Charles MacNeill 7:1add29d51e72 999 VL53L1_SEQUENCESTEP_MM1, &Mm1Enabled);
Charles MacNeill 7:1add29d51e72 1000
Charles MacNeill 7:1add29d51e72 1001 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1002 Status = VL53L1_GetSequenceStepEnable(Dev,
Charles MacNeill 7:1add29d51e72 1003 VL53L1_SEQUENCESTEP_MM2, &Mm2Enabled);
Charles MacNeill 7:1add29d51e72 1004
Charles MacNeill 7:1add29d51e72 1005 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1006 Status = VL53L1_get_timeouts_us(Dev,
Charles MacNeill 7:1add29d51e72 1007 &PhaseCalTimeoutUs,
Charles MacNeill 7:1add29d51e72 1008 &MmTimeoutUs,
Charles MacNeill 7:1add29d51e72 1009 &RangeTimeoutUs);
Charles MacNeill 7:1add29d51e72 1010
Charles MacNeill 7:1add29d51e72 1011 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1012 PresetMode = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 1013 CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 1014
Charles MacNeill 7:1add29d51e72 1015 switch (PresetMode) {
Charles MacNeill 7:1add29d51e72 1016 case VL53L1_PRESETMODE_LITE_RANGING:
Charles MacNeill 7:1add29d51e72 1017 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
Charles MacNeill 7:1add29d51e72 1018 MeasTimingBdg = RangeTimeoutUs + 5000;
Charles MacNeill 7:1add29d51e72 1019 else
Charles MacNeill 7:1add29d51e72 1020 MeasTimingBdg = RangeTimeoutUs + 1000;
Charles MacNeill 7:1add29d51e72 1021
Charles MacNeill 7:1add29d51e72 1022 break;
Charles MacNeill 7:1add29d51e72 1023
Charles MacNeill 7:1add29d51e72 1024 case VL53L1_PRESETMODE_AUTONOMOUS:
Charles MacNeill 7:1add29d51e72 1025 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
Charles MacNeill 7:1add29d51e72 1026 MeasTimingBdg = 2 * RangeTimeoutUs + 26600;
Charles MacNeill 7:1add29d51e72 1027 else
Charles MacNeill 7:1add29d51e72 1028 MeasTimingBdg = 2 * RangeTimeoutUs + 21600;
Charles MacNeill 7:1add29d51e72 1029
Charles MacNeill 7:1add29d51e72 1030 break;
Charles MacNeill 7:1add29d51e72 1031
Charles MacNeill 7:1add29d51e72 1032 case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
Charles MacNeill 7:1add29d51e72 1033 vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
Charles MacNeill 7:1add29d51e72 1034 VL53L1_get_tuning_parm(Dev,
Charles MacNeill 7:1add29d51e72 1035 VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND,
Charles MacNeill 7:1add29d51e72 1036 &vhv_loops);
Charles MacNeill 7:1add29d51e72 1037 if (vhv_loops > 0) {
Charles MacNeill 7:1add29d51e72 1038 vhv += vhv_loops *
Charles MacNeill 7:1add29d51e72 1039 LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
Charles MacNeill 7:1add29d51e72 1040 }
Charles MacNeill 7:1add29d51e72 1041 TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING +
Charles MacNeill 7:1add29d51e72 1042 LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING +
Charles MacNeill 7:1add29d51e72 1043 vhv;
Charles MacNeill 7:1add29d51e72 1044 MeasTimingBdg = 2 * RangeTimeoutUs + TimingGuard;
Charles MacNeill 7:1add29d51e72 1045 break;
Charles MacNeill 7:1add29d51e72 1046
Charles MacNeill 7:1add29d51e72 1047 case VL53L1_PRESETMODE_RANGING:
Charles MacNeill 7:1add29d51e72 1048 case VL53L1_PRESETMODE_MULTIZONES_SCANNING:
Charles MacNeill 7:1add29d51e72 1049 case VL53L1_PRESETMODE_PROXY_RANGING_MODE:
Charles MacNeill 7:1add29d51e72 1050 MeasTimingBdg = (6 * RangeTimeoutUs) + 1700;
Charles MacNeill 7:1add29d51e72 1051 break;
Charles MacNeill 7:1add29d51e72 1052
Charles MacNeill 7:1add29d51e72 1053 case VL53L1_PRESETMODE_OLT:
Charles MacNeill 7:1add29d51e72 1054 MeasTimingBdg = RangeTimeoutUs + MmTimeoutUs + 5000;
Charles MacNeill 7:1add29d51e72 1055 break;
Charles MacNeill 7:1add29d51e72 1056 default:
Charles MacNeill 7:1add29d51e72 1057
Charles MacNeill 7:1add29d51e72 1058 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 1059 }
Charles MacNeill 7:1add29d51e72 1060 }
Charles MacNeill 7:1add29d51e72 1061 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1062 *pMeasurementTimingBudgetMicroSeconds = MeasTimingBdg;
Charles MacNeill 7:1add29d51e72 1063
Charles MacNeill 7:1add29d51e72 1064 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1065 return Status;
Charles MacNeill 7:1add29d51e72 1066 }
Charles MacNeill 7:1add29d51e72 1067
Charles MacNeill 7:1add29d51e72 1068
Charles MacNeill 7:1add29d51e72 1069
Charles MacNeill 7:1add29d51e72 1070 VL53L1_Error VL53L1_SetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1071 uint32_t InterMeasurementPeriodMilliSeconds)
Charles MacNeill 7:1add29d51e72 1072 {
Charles MacNeill 7:1add29d51e72 1073 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1074 uint32_t adjustedIMP;
Charles MacNeill 7:1add29d51e72 1075
Charles MacNeill 7:1add29d51e72 1076 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1077
Charles MacNeill 7:1add29d51e72 1078
Charles MacNeill 7:1add29d51e72 1079 adjustedIMP = InterMeasurementPeriodMilliSeconds;
Charles MacNeill 7:1add29d51e72 1080 adjustedIMP += (adjustedIMP * 64) / 1000;
Charles MacNeill 7:1add29d51e72 1081
Charles MacNeill 7:1add29d51e72 1082 Status = VL53L1_set_inter_measurement_period_ms(Dev,
Charles MacNeill 7:1add29d51e72 1083 adjustedIMP);
Charles MacNeill 7:1add29d51e72 1084
Charles MacNeill 7:1add29d51e72 1085 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1086 return Status;
Charles MacNeill 7:1add29d51e72 1087 }
Charles MacNeill 7:1add29d51e72 1088
Charles MacNeill 7:1add29d51e72 1089 VL53L1_Error VL53L1_GetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1090 uint32_t *pInterMeasurementPeriodMilliSeconds)
Charles MacNeill 7:1add29d51e72 1091 {
Charles MacNeill 7:1add29d51e72 1092 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1093 uint32_t adjustedIMP;
Charles MacNeill 7:1add29d51e72 1094
Charles MacNeill 7:1add29d51e72 1095 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1096
Charles MacNeill 7:1add29d51e72 1097 Status = VL53L1_get_inter_measurement_period_ms(Dev, &adjustedIMP);
Charles MacNeill 7:1add29d51e72 1098
Charles MacNeill 7:1add29d51e72 1099 adjustedIMP -= (adjustedIMP * 64) / 1000;
Charles MacNeill 7:1add29d51e72 1100 *pInterMeasurementPeriodMilliSeconds = adjustedIMP;
Charles MacNeill 7:1add29d51e72 1101
Charles MacNeill 7:1add29d51e72 1102
Charles MacNeill 7:1add29d51e72 1103 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1104 return Status;
Charles MacNeill 7:1add29d51e72 1105 }
Charles MacNeill 7:1add29d51e72 1106
Charles MacNeill 7:1add29d51e72 1107 VL53L1_Error VL53L1_SetDmaxReflectance(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1108 FixPoint1616_t DmaxReflectance)
Charles MacNeill 7:1add29d51e72 1109 {
Charles MacNeill 7:1add29d51e72 1110 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1111 VL53L1_dmax_reflectance_array_t dmax_reflectances;
Charles MacNeill 7:1add29d51e72 1112
Charles MacNeill 7:1add29d51e72 1113 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1114
Charles MacNeill 7:1add29d51e72 1115 if (DmaxReflectance > 100*65536)
Charles MacNeill 7:1add29d51e72 1116 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1117
Charles MacNeill 7:1add29d51e72 1118 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1119 Status = VL53L1_get_dmax_reflectance_values(Dev,
Charles MacNeill 7:1add29d51e72 1120 &dmax_reflectances);
Charles MacNeill 7:1add29d51e72 1121
Charles MacNeill 7:1add29d51e72 1122 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1123 dmax_reflectances.target_reflectance_for_dmax[
Charles MacNeill 7:1add29d51e72 1124 DMAX_REFLECTANCE_IDX] =
Charles MacNeill 7:1add29d51e72 1125 VL53L1_FIXPOINT1616TOFIXPOINT72(DmaxReflectance);
Charles MacNeill 7:1add29d51e72 1126 Status = VL53L1_set_dmax_reflectance_values(Dev,
Charles MacNeill 7:1add29d51e72 1127 &dmax_reflectances);
Charles MacNeill 7:1add29d51e72 1128 }
Charles MacNeill 7:1add29d51e72 1129
Charles MacNeill 7:1add29d51e72 1130 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1131 return Status;
Charles MacNeill 7:1add29d51e72 1132 }
Charles MacNeill 7:1add29d51e72 1133
Charles MacNeill 7:1add29d51e72 1134 VL53L1_Error VL53L1_GetDmaxReflectance(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1135 FixPoint1616_t *pDmaxReflectance)
Charles MacNeill 7:1add29d51e72 1136 {
Charles MacNeill 7:1add29d51e72 1137 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1138 VL53L1_dmax_reflectance_array_t dmax_reflectances;
Charles MacNeill 7:1add29d51e72 1139 uint16_t r;
Charles MacNeill 7:1add29d51e72 1140
Charles MacNeill 7:1add29d51e72 1141 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1142 Status = VL53L1_get_dmax_reflectance_values(Dev, &dmax_reflectances);
Charles MacNeill 7:1add29d51e72 1143 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1144 r = dmax_reflectances.target_reflectance_for_dmax[
Charles MacNeill 7:1add29d51e72 1145 DMAX_REFLECTANCE_IDX];
Charles MacNeill 7:1add29d51e72 1146 *pDmaxReflectance = VL53L1_FIXPOINT72TOFIXPOINT1616(r);
Charles MacNeill 7:1add29d51e72 1147 }
Charles MacNeill 7:1add29d51e72 1148
Charles MacNeill 7:1add29d51e72 1149 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1150 return Status;
Charles MacNeill 7:1add29d51e72 1151 }
Charles MacNeill 7:1add29d51e72 1152
Charles MacNeill 7:1add29d51e72 1153
Charles MacNeill 7:1add29d51e72 1154 VL53L1_Error VL53L1_SetDmaxMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1155 VL53L1_DeviceDmaxModes DmaxMode)
Charles MacNeill 7:1add29d51e72 1156 {
Charles MacNeill 7:1add29d51e72 1157
Charles MacNeill 7:1add29d51e72 1158 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1159 VL53L1_DeviceDmaxMode dmax_mode;
Charles MacNeill 7:1add29d51e72 1160
Charles MacNeill 7:1add29d51e72 1161 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1162
Charles MacNeill 7:1add29d51e72 1163 switch (DmaxMode) {
Charles MacNeill 7:1add29d51e72 1164 case VL53L1_DMAXMODE_FMT_CAL_DATA:
Charles MacNeill 7:1add29d51e72 1165 dmax_mode = VL53L1_DEVICEDMAXMODE__FMT_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1166 break;
Charles MacNeill 7:1add29d51e72 1167 case VL53L1_DMAXMODE_CUSTCAL_DATA:
Charles MacNeill 7:1add29d51e72 1168 dmax_mode = VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1169 break;
Charles MacNeill 7:1add29d51e72 1170 case VL53L1_DMAXMODE_PER_ZONE_CAL_DATA:
Charles MacNeill 7:1add29d51e72 1171 dmax_mode = VL53L1_DEVICEDMAXMODE__PER_ZONE_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1172 break;
Charles MacNeill 7:1add29d51e72 1173 default:
lugandc 18:0696efe39d08 1174 dmax_mode = VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1175 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1176 break;
Charles MacNeill 7:1add29d51e72 1177 }
Charles MacNeill 7:1add29d51e72 1178 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1179 Status = VL53L1_set_dmax_mode(Dev, dmax_mode);
Charles MacNeill 7:1add29d51e72 1180
Charles MacNeill 7:1add29d51e72 1181 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1182 return Status;
Charles MacNeill 7:1add29d51e72 1183 }
Charles MacNeill 7:1add29d51e72 1184
Charles MacNeill 7:1add29d51e72 1185
Charles MacNeill 7:1add29d51e72 1186 VL53L1_Error VL53L1_GetDmaxMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1187 VL53L1_DeviceDmaxModes *pDmaxMode)
Charles MacNeill 7:1add29d51e72 1188 {
Charles MacNeill 7:1add29d51e72 1189 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1190 VL53L1_DeviceDmaxMode dmax_mode;
Charles MacNeill 7:1add29d51e72 1191
Charles MacNeill 7:1add29d51e72 1192 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1193
Charles MacNeill 7:1add29d51e72 1194 Status = VL53L1_get_dmax_mode(Dev, &dmax_mode);
Charles MacNeill 7:1add29d51e72 1195 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1196 switch (dmax_mode) {
Charles MacNeill 7:1add29d51e72 1197 case VL53L1_DEVICEDMAXMODE__FMT_CAL_DATA:
Charles MacNeill 7:1add29d51e72 1198 *pDmaxMode = VL53L1_DMAXMODE_FMT_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1199 break;
Charles MacNeill 7:1add29d51e72 1200 case VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA:
Charles MacNeill 7:1add29d51e72 1201 *pDmaxMode = VL53L1_DMAXMODE_CUSTCAL_DATA;
Charles MacNeill 7:1add29d51e72 1202 break;
Charles MacNeill 7:1add29d51e72 1203 case VL53L1_DEVICEDMAXMODE__PER_ZONE_CAL_DATA:
Charles MacNeill 7:1add29d51e72 1204 *pDmaxMode = VL53L1_DMAXMODE_PER_ZONE_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1205 break;
Charles MacNeill 7:1add29d51e72 1206 default:
Charles MacNeill 7:1add29d51e72 1207 *pDmaxMode = VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA;
Charles MacNeill 7:1add29d51e72 1208 Status = VL53L1_ERROR_NOT_IMPLEMENTED;
Charles MacNeill 7:1add29d51e72 1209 break;
Charles MacNeill 7:1add29d51e72 1210 }
Charles MacNeill 7:1add29d51e72 1211 }
Charles MacNeill 7:1add29d51e72 1212
Charles MacNeill 7:1add29d51e72 1213 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1214 return Status;
Charles MacNeill 7:1add29d51e72 1215 }
Charles MacNeill 7:1add29d51e72 1216
Charles MacNeill 7:1add29d51e72 1217
Charles MacNeill 7:1add29d51e72 1218
Charles MacNeill 7:1add29d51e72 1219
Charles MacNeill 7:1add29d51e72 1220
Charles MacNeill 7:1add29d51e72 1221
Charles MacNeill 7:1add29d51e72 1222 VL53L1_Error VL53L1_GetNumberOfLimitCheck(uint16_t *pNumberOfLimitCheck)
Charles MacNeill 7:1add29d51e72 1223 {
Charles MacNeill 7:1add29d51e72 1224 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1225
Charles MacNeill 7:1add29d51e72 1226 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1227
Charles MacNeill 7:1add29d51e72 1228 *pNumberOfLimitCheck = VL53L1_CHECKENABLE_NUMBER_OF_CHECKS;
Charles MacNeill 7:1add29d51e72 1229
Charles MacNeill 7:1add29d51e72 1230 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1231 return Status;
Charles MacNeill 7:1add29d51e72 1232 }
Charles MacNeill 7:1add29d51e72 1233
Charles MacNeill 7:1add29d51e72 1234 VL53L1_Error VL53L1_GetLimitCheckInfo(uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1235 char *pLimitCheckString)
Charles MacNeill 7:1add29d51e72 1236 {
Charles MacNeill 7:1add29d51e72 1237 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1238
Charles MacNeill 7:1add29d51e72 1239 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1240
Charles MacNeill 7:1add29d51e72 1241 Status = VL53L1_get_limit_check_info(LimitCheckId,
Charles MacNeill 7:1add29d51e72 1242 pLimitCheckString);
Charles MacNeill 7:1add29d51e72 1243
Charles MacNeill 7:1add29d51e72 1244 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1245 return Status;
Charles MacNeill 7:1add29d51e72 1246 }
Charles MacNeill 7:1add29d51e72 1247
Charles MacNeill 7:1add29d51e72 1248 VL53L1_Error VL53L1_GetLimitCheckStatus(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1249 uint8_t *pLimitCheckStatus)
Charles MacNeill 7:1add29d51e72 1250 {
Charles MacNeill 7:1add29d51e72 1251 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1252 uint8_t Temp8;
Charles MacNeill 7:1add29d51e72 1253
Charles MacNeill 7:1add29d51e72 1254 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1255
Charles MacNeill 7:1add29d51e72 1256 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Charles MacNeill 7:1add29d51e72 1257 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1258 } else {
Charles MacNeill 7:1add29d51e72 1259 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
Charles MacNeill 7:1add29d51e72 1260 LimitCheckId, Temp8);
Charles MacNeill 7:1add29d51e72 1261 *pLimitCheckStatus = Temp8;
Charles MacNeill 7:1add29d51e72 1262 }
Charles MacNeill 7:1add29d51e72 1263
Charles MacNeill 7:1add29d51e72 1264 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1265 return Status;
Charles MacNeill 7:1add29d51e72 1266 }
Charles MacNeill 7:1add29d51e72 1267
Charles MacNeill 7:1add29d51e72 1268 static VL53L1_Error SetLimitValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1269 FixPoint1616_t value)
Charles MacNeill 7:1add29d51e72 1270 {
Charles MacNeill 7:1add29d51e72 1271 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1272 uint16_t tmpuint16;
Charles MacNeill 7:1add29d51e72 1273
Charles MacNeill 7:1add29d51e72 1274 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1275
Charles MacNeill 7:1add29d51e72 1276 switch (LimitCheckId) {
Charles MacNeill 7:1add29d51e72 1277 case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
Charles MacNeill 7:1add29d51e72 1278 tmpuint16 = VL53L1_FIXPOINT1616TOFIXPOINT142(value);
Charles MacNeill 7:1add29d51e72 1279 VL53L1_set_lite_sigma_threshold(Dev, tmpuint16);
Charles MacNeill 7:1add29d51e72 1280 break;
Charles MacNeill 7:1add29d51e72 1281 case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
Charles MacNeill 7:1add29d51e72 1282 tmpuint16 = VL53L1_FIXPOINT1616TOFIXPOINT97(value);
Charles MacNeill 7:1add29d51e72 1283 VL53L1_set_lite_min_count_rate(Dev, tmpuint16);
Charles MacNeill 7:1add29d51e72 1284 break;
Charles MacNeill 7:1add29d51e72 1285 default:
Charles MacNeill 7:1add29d51e72 1286 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1287 }
Charles MacNeill 7:1add29d51e72 1288
Charles MacNeill 7:1add29d51e72 1289 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1290 return Status;
Charles MacNeill 7:1add29d51e72 1291 }
Charles MacNeill 7:1add29d51e72 1292
Charles MacNeill 7:1add29d51e72 1293
Charles MacNeill 7:1add29d51e72 1294 VL53L1_Error VL53L1_SetLimitCheckEnable(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1295 uint8_t LimitCheckEnable)
Charles MacNeill 7:1add29d51e72 1296 {
Charles MacNeill 7:1add29d51e72 1297 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1298 FixPoint1616_t TempFix1616 = 0;
Charles MacNeill 7:1add29d51e72 1299
Charles MacNeill 7:1add29d51e72 1300 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1301
Charles MacNeill 7:1add29d51e72 1302
Charles MacNeill 7:1add29d51e72 1303 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Charles MacNeill 7:1add29d51e72 1304 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1305 } else {
Charles MacNeill 7:1add29d51e72 1306
Charles MacNeill 7:1add29d51e72 1307 if (LimitCheckEnable == 0)
Charles MacNeill 7:1add29d51e72 1308 TempFix1616 = 0;
Charles MacNeill 7:1add29d51e72 1309 else
Charles MacNeill 7:1add29d51e72 1310 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
Charles MacNeill 7:1add29d51e72 1311 LimitCheckId, TempFix1616);
Charles MacNeill 7:1add29d51e72 1312
Charles MacNeill 7:1add29d51e72 1313 Status = SetLimitValue(Dev, LimitCheckId, TempFix1616);
Charles MacNeill 7:1add29d51e72 1314 }
Charles MacNeill 7:1add29d51e72 1315
Charles MacNeill 7:1add29d51e72 1316 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1317 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 1318 LimitChecksEnable,
Charles MacNeill 7:1add29d51e72 1319 LimitCheckId,
Charles MacNeill 7:1add29d51e72 1320 ((LimitCheckEnable == 0) ? 0 : 1));
Charles MacNeill 7:1add29d51e72 1321
Charles MacNeill 7:1add29d51e72 1322
Charles MacNeill 7:1add29d51e72 1323
Charles MacNeill 7:1add29d51e72 1324 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1325 return Status;
Charles MacNeill 7:1add29d51e72 1326 }
Charles MacNeill 7:1add29d51e72 1327
Charles MacNeill 7:1add29d51e72 1328 VL53L1_Error VL53L1_GetLimitCheckEnable(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1329 uint8_t *pLimitCheckEnable)
Charles MacNeill 7:1add29d51e72 1330 {
Charles MacNeill 7:1add29d51e72 1331 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1332 uint8_t Temp8;
Charles MacNeill 7:1add29d51e72 1333
Charles MacNeill 7:1add29d51e72 1334 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1335
Charles MacNeill 7:1add29d51e72 1336 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Charles MacNeill 7:1add29d51e72 1337 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1338 *pLimitCheckEnable = 0;
Charles MacNeill 7:1add29d51e72 1339 } else {
Charles MacNeill 7:1add29d51e72 1340 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
Charles MacNeill 7:1add29d51e72 1341 LimitCheckId, Temp8);
Charles MacNeill 7:1add29d51e72 1342 *pLimitCheckEnable = Temp8;
Charles MacNeill 7:1add29d51e72 1343 }
Charles MacNeill 7:1add29d51e72 1344
Charles MacNeill 7:1add29d51e72 1345
Charles MacNeill 7:1add29d51e72 1346 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1347 return Status;
Charles MacNeill 7:1add29d51e72 1348 }
Charles MacNeill 7:1add29d51e72 1349
Charles MacNeill 7:1add29d51e72 1350 VL53L1_Error VL53L1_SetLimitCheckValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1351 FixPoint1616_t LimitCheckValue)
Charles MacNeill 7:1add29d51e72 1352 {
Charles MacNeill 7:1add29d51e72 1353 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1354 uint8_t LimitChecksEnable;
Charles MacNeill 7:1add29d51e72 1355
Charles MacNeill 7:1add29d51e72 1356 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1357
Charles MacNeill 7:1add29d51e72 1358 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Charles MacNeill 7:1add29d51e72 1359 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1360 } else {
Charles MacNeill 7:1add29d51e72 1361
Charles MacNeill 7:1add29d51e72 1362 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
Charles MacNeill 7:1add29d51e72 1363 LimitCheckId,
Charles MacNeill 7:1add29d51e72 1364 LimitChecksEnable);
Charles MacNeill 7:1add29d51e72 1365
Charles MacNeill 7:1add29d51e72 1366 if (LimitChecksEnable == 0) {
Charles MacNeill 7:1add29d51e72 1367
Charles MacNeill 7:1add29d51e72 1368 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
Charles MacNeill 7:1add29d51e72 1369 LimitCheckId, LimitCheckValue);
Charles MacNeill 7:1add29d51e72 1370 } else {
Charles MacNeill 7:1add29d51e72 1371
Charles MacNeill 7:1add29d51e72 1372 Status = SetLimitValue(Dev, LimitCheckId,
Charles MacNeill 7:1add29d51e72 1373 LimitCheckValue);
Charles MacNeill 7:1add29d51e72 1374
Charles MacNeill 7:1add29d51e72 1375 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1376 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 1377 LimitChecksValue,
Charles MacNeill 7:1add29d51e72 1378 LimitCheckId, LimitCheckValue);
Charles MacNeill 7:1add29d51e72 1379 }
Charles MacNeill 7:1add29d51e72 1380 }
Charles MacNeill 7:1add29d51e72 1381 }
Charles MacNeill 7:1add29d51e72 1382
Charles MacNeill 7:1add29d51e72 1383 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1384 return Status;
Charles MacNeill 7:1add29d51e72 1385 }
Charles MacNeill 7:1add29d51e72 1386
Charles MacNeill 7:1add29d51e72 1387 VL53L1_Error VL53L1_GetLimitCheckValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1388 FixPoint1616_t *pLimitCheckValue)
Charles MacNeill 7:1add29d51e72 1389 {
Charles MacNeill 7:1add29d51e72 1390 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1391 uint16_t MinCountRate;
lugandc 18:0696efe39d08 1392 FixPoint1616_t TempFix1616 = 0;
Charles MacNeill 7:1add29d51e72 1393 uint16_t SigmaThresh;
Charles MacNeill 7:1add29d51e72 1394
Charles MacNeill 7:1add29d51e72 1395 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1396
Charles MacNeill 7:1add29d51e72 1397 switch (LimitCheckId) {
Charles MacNeill 7:1add29d51e72 1398 case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
Charles MacNeill 7:1add29d51e72 1399 Status = VL53L1_get_lite_sigma_threshold(Dev, &SigmaThresh);
Charles MacNeill 7:1add29d51e72 1400 TempFix1616 = VL53L1_FIXPOINT142TOFIXPOINT1616(SigmaThresh);
Charles MacNeill 7:1add29d51e72 1401 break;
Charles MacNeill 7:1add29d51e72 1402 case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
Charles MacNeill 7:1add29d51e72 1403 Status = VL53L1_get_lite_min_count_rate(Dev, &MinCountRate);
Charles MacNeill 7:1add29d51e72 1404 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(MinCountRate);
Charles MacNeill 7:1add29d51e72 1405 break;
Charles MacNeill 7:1add29d51e72 1406 default:
Charles MacNeill 7:1add29d51e72 1407 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1408 }
Charles MacNeill 7:1add29d51e72 1409
Charles MacNeill 7:1add29d51e72 1410 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1411
Charles MacNeill 7:1add29d51e72 1412 if (TempFix1616 == 0) {
Charles MacNeill 7:1add29d51e72 1413
Charles MacNeill 7:1add29d51e72 1414 VL53L1_GETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 1415 LimitChecksValue, LimitCheckId,
Charles MacNeill 7:1add29d51e72 1416 TempFix1616);
Charles MacNeill 7:1add29d51e72 1417 *pLimitCheckValue = TempFix1616;
Charles MacNeill 7:1add29d51e72 1418 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 1419 LimitChecksEnable, LimitCheckId, 0);
Charles MacNeill 7:1add29d51e72 1420 } else {
Charles MacNeill 7:1add29d51e72 1421 *pLimitCheckValue = TempFix1616;
Charles MacNeill 7:1add29d51e72 1422 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 1423 LimitChecksValue, LimitCheckId,
Charles MacNeill 7:1add29d51e72 1424 TempFix1616);
Charles MacNeill 7:1add29d51e72 1425 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 1426 LimitChecksEnable, LimitCheckId, 1);
Charles MacNeill 7:1add29d51e72 1427 }
Charles MacNeill 7:1add29d51e72 1428 }
Charles MacNeill 7:1add29d51e72 1429 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1430 return Status;
Charles MacNeill 7:1add29d51e72 1431
Charles MacNeill 7:1add29d51e72 1432 }
Charles MacNeill 7:1add29d51e72 1433
Charles MacNeill 7:1add29d51e72 1434 VL53L1_Error VL53L1_GetLimitCheckCurrent(VL53L1_DEV Dev, uint16_t LimitCheckId,
Charles MacNeill 7:1add29d51e72 1435 FixPoint1616_t *pLimitCheckCurrent)
Charles MacNeill 7:1add29d51e72 1436 {
Charles MacNeill 7:1add29d51e72 1437 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1438 FixPoint1616_t TempFix1616 = 0;
Charles MacNeill 7:1add29d51e72 1439
Charles MacNeill 7:1add29d51e72 1440 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1441
Charles MacNeill 7:1add29d51e72 1442 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Charles MacNeill 7:1add29d51e72 1443 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1444 } else {
Charles MacNeill 7:1add29d51e72 1445 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksCurrent,
Charles MacNeill 7:1add29d51e72 1446 LimitCheckId, TempFix1616);
Charles MacNeill 7:1add29d51e72 1447 *pLimitCheckCurrent = TempFix1616;
Charles MacNeill 7:1add29d51e72 1448 }
Charles MacNeill 7:1add29d51e72 1449
Charles MacNeill 7:1add29d51e72 1450 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1451 return Status;
Charles MacNeill 7:1add29d51e72 1452
Charles MacNeill 7:1add29d51e72 1453 }
Charles MacNeill 7:1add29d51e72 1454
Charles MacNeill 7:1add29d51e72 1455
Charles MacNeill 7:1add29d51e72 1456
Charles MacNeill 7:1add29d51e72 1457
Charles MacNeill 7:1add29d51e72 1458
Charles MacNeill 7:1add29d51e72 1459
Charles MacNeill 7:1add29d51e72 1460
Charles MacNeill 7:1add29d51e72 1461
Charles MacNeill 7:1add29d51e72 1462 VL53L1_Error VL53L1_GetMaxNumberOfROI(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1463 uint8_t *pMaxNumberOfROI)
Charles MacNeill 7:1add29d51e72 1464 {
Charles MacNeill 7:1add29d51e72 1465 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1466 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 1467
Charles MacNeill 7:1add29d51e72 1468 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1469
Charles MacNeill 7:1add29d51e72 1470 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 1471
Charles MacNeill 7:1add29d51e72 1472
Charles MacNeill 7:1add29d51e72 1473 if (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING)
Charles MacNeill 7:1add29d51e72 1474 *pMaxNumberOfROI = VL53L1_MAX_USER_ZONES;
Charles MacNeill 7:1add29d51e72 1475 else
Charles MacNeill 7:1add29d51e72 1476 *pMaxNumberOfROI = 1;
Charles MacNeill 7:1add29d51e72 1477
Charles MacNeill 7:1add29d51e72 1478 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1479 return Status;
Charles MacNeill 7:1add29d51e72 1480 }
Charles MacNeill 7:1add29d51e72 1481
Charles MacNeill 7:1add29d51e72 1482 VL53L1_Error VL53L1_SetROI(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1483 VL53L1_RoiConfig_t *pRoiConfig)
Charles MacNeill 7:1add29d51e72 1484 {
Charles MacNeill 7:1add29d51e72 1485 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1486 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 1487 uint8_t MaxNumberOfROI = 1;
Charles MacNeill 7:1add29d51e72 1488 VL53L1_zone_config_t zone_cfg;
Charles MacNeill 7:1add29d51e72 1489 VL53L1_UserRoi_t CurrROI;
Charles MacNeill 7:1add29d51e72 1490 uint8_t i;
Charles MacNeill 7:1add29d51e72 1491 uint8_t x_centre;
Charles MacNeill 7:1add29d51e72 1492 uint8_t y_centre;
Charles MacNeill 7:1add29d51e72 1493 uint8_t width, height;
Charles MacNeill 7:1add29d51e72 1494
Charles MacNeill 7:1add29d51e72 1495 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1496
Charles MacNeill 7:1add29d51e72 1497
Charles MacNeill 7:1add29d51e72 1498 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 1499
Charles MacNeill 7:1add29d51e72 1500
Charles MacNeill 7:1add29d51e72 1501 if (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING)
Charles MacNeill 7:1add29d51e72 1502 MaxNumberOfROI = VL53L1_MAX_USER_ZONES;
Charles MacNeill 7:1add29d51e72 1503
Charles MacNeill 7:1add29d51e72 1504 if ((pRoiConfig->NumberOfRoi > MaxNumberOfROI) ||
Charles MacNeill 7:1add29d51e72 1505 (pRoiConfig->NumberOfRoi < 1))
Charles MacNeill 7:1add29d51e72 1506 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1507
Charles MacNeill 7:1add29d51e72 1508 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1509
Charles MacNeill 7:1add29d51e72 1510
Charles MacNeill 7:1add29d51e72 1511 zone_cfg.max_zones = MaxNumberOfROI;
Charles MacNeill 7:1add29d51e72 1512 zone_cfg.active_zones = pRoiConfig->NumberOfRoi - 1;
Charles MacNeill 7:1add29d51e72 1513
Charles MacNeill 7:1add29d51e72 1514 for (i = 0; i < pRoiConfig->NumberOfRoi; i++) {
Charles MacNeill 7:1add29d51e72 1515 CurrROI = pRoiConfig->UserRois[i];
Charles MacNeill 7:1add29d51e72 1516
Charles MacNeill 7:1add29d51e72 1517 Status = CheckValidRectRoi(CurrROI);
Charles MacNeill 7:1add29d51e72 1518 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1519 break;
Charles MacNeill 7:1add29d51e72 1520
Charles MacNeill 7:1add29d51e72 1521 x_centre = (CurrROI.BotRightX + CurrROI.TopLeftX + 1)
Charles MacNeill 7:1add29d51e72 1522 / 2;
Charles MacNeill 7:1add29d51e72 1523 y_centre = (CurrROI.TopLeftY + CurrROI.BotRightY + 1)
Charles MacNeill 7:1add29d51e72 1524 / 2;
Charles MacNeill 7:1add29d51e72 1525 width = (CurrROI.BotRightX - CurrROI.TopLeftX);
Charles MacNeill 7:1add29d51e72 1526 height = (CurrROI.TopLeftY - CurrROI.BotRightY);
Charles MacNeill 7:1add29d51e72 1527 if ((width < 3) || (height < 3)) {
Charles MacNeill 7:1add29d51e72 1528 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1529 break;
Charles MacNeill 7:1add29d51e72 1530 }
Charles MacNeill 7:1add29d51e72 1531 zone_cfg.user_zones[i].x_centre = x_centre;
Charles MacNeill 7:1add29d51e72 1532 zone_cfg.user_zones[i].y_centre = y_centre;
Charles MacNeill 7:1add29d51e72 1533 zone_cfg.user_zones[i].width = width;
Charles MacNeill 7:1add29d51e72 1534 zone_cfg.user_zones[i].height = height;
Charles MacNeill 7:1add29d51e72 1535 }
Charles MacNeill 7:1add29d51e72 1536 }
Charles MacNeill 7:1add29d51e72 1537
Charles MacNeill 7:1add29d51e72 1538 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1539 Status = VL53L1_set_zone_config(Dev, &zone_cfg);
Charles MacNeill 7:1add29d51e72 1540
Charles MacNeill 7:1add29d51e72 1541 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1542 return Status;
Charles MacNeill 7:1add29d51e72 1543 }
Charles MacNeill 7:1add29d51e72 1544
Charles MacNeill 7:1add29d51e72 1545 VL53L1_Error VL53L1_GetROI(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1546 VL53L1_RoiConfig_t *pRoiConfig)
Charles MacNeill 7:1add29d51e72 1547 {
Charles MacNeill 7:1add29d51e72 1548 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1549 VL53L1_zone_config_t zone_cfg;
Charles MacNeill 7:1add29d51e72 1550 uint8_t i;
Charles MacNeill 7:1add29d51e72 1551 uint8_t TopLeftX;
Charles MacNeill 7:1add29d51e72 1552 uint8_t TopLeftY;
Charles MacNeill 7:1add29d51e72 1553 uint8_t BotRightX;
Charles MacNeill 7:1add29d51e72 1554 uint8_t BotRightY;
Charles MacNeill 7:1add29d51e72 1555
Charles MacNeill 7:1add29d51e72 1556 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1557
Charles MacNeill 7:1add29d51e72 1558 VL53L1_get_zone_config(Dev, &zone_cfg);
Charles MacNeill 7:1add29d51e72 1559
Charles MacNeill 7:1add29d51e72 1560 pRoiConfig->NumberOfRoi = zone_cfg.active_zones + 1;
Charles MacNeill 7:1add29d51e72 1561
Charles MacNeill 7:1add29d51e72 1562 for (i = 0; i < pRoiConfig->NumberOfRoi; i++) {
Charles MacNeill 7:1add29d51e72 1563 TopLeftX = (2 * zone_cfg.user_zones[i].x_centre -
Charles MacNeill 7:1add29d51e72 1564 zone_cfg.user_zones[i].width) >> 1;
Charles MacNeill 7:1add29d51e72 1565 TopLeftY = (2 * zone_cfg.user_zones[i].y_centre +
Charles MacNeill 7:1add29d51e72 1566 zone_cfg.user_zones[i].height) >> 1;
Charles MacNeill 7:1add29d51e72 1567 BotRightX = (2 * zone_cfg.user_zones[i].x_centre +
Charles MacNeill 7:1add29d51e72 1568 zone_cfg.user_zones[i].width) >> 1;
Charles MacNeill 7:1add29d51e72 1569 BotRightY = (2 * zone_cfg.user_zones[i].y_centre -
Charles MacNeill 7:1add29d51e72 1570 zone_cfg.user_zones[i].height) >> 1;
Charles MacNeill 7:1add29d51e72 1571 pRoiConfig->UserRois[i].TopLeftX = TopLeftX;
Charles MacNeill 7:1add29d51e72 1572 pRoiConfig->UserRois[i].TopLeftY = TopLeftY;
Charles MacNeill 7:1add29d51e72 1573 pRoiConfig->UserRois[i].BotRightX = BotRightX;
Charles MacNeill 7:1add29d51e72 1574 pRoiConfig->UserRois[i].BotRightY = BotRightY;
Charles MacNeill 7:1add29d51e72 1575 }
Charles MacNeill 7:1add29d51e72 1576
Charles MacNeill 7:1add29d51e72 1577 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1578 return Status;
Charles MacNeill 7:1add29d51e72 1579 }
Charles MacNeill 7:1add29d51e72 1580
Charles MacNeill 7:1add29d51e72 1581
Charles MacNeill 7:1add29d51e72 1582
Charles MacNeill 7:1add29d51e72 1583
Charles MacNeill 7:1add29d51e72 1584
Charles MacNeill 7:1add29d51e72 1585
Charles MacNeill 7:1add29d51e72 1586
Charles MacNeill 7:1add29d51e72 1587 VL53L1_Error VL53L1_GetNumberOfSequenceSteps(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1588 uint8_t *pNumberOfSequenceSteps)
Charles MacNeill 7:1add29d51e72 1589 {
Charles MacNeill 7:1add29d51e72 1590 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1591
Charles MacNeill 7:1add29d51e72 1592 SUPPRESS_UNUSED_WARNING(Dev);
Charles MacNeill 7:1add29d51e72 1593
Charles MacNeill 7:1add29d51e72 1594 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1595
Charles MacNeill 7:1add29d51e72 1596 *pNumberOfSequenceSteps = VL53L1_SEQUENCESTEP_NUMBER_OF_ITEMS;
Charles MacNeill 7:1add29d51e72 1597
Charles MacNeill 7:1add29d51e72 1598 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1599 return Status;
Charles MacNeill 7:1add29d51e72 1600 }
Charles MacNeill 7:1add29d51e72 1601
Charles MacNeill 7:1add29d51e72 1602 VL53L1_Error VL53L1_GetSequenceStepsInfo(VL53L1_SequenceStepId SequenceStepId,
Charles MacNeill 7:1add29d51e72 1603 char *pSequenceStepsString)
Charles MacNeill 7:1add29d51e72 1604 {
Charles MacNeill 7:1add29d51e72 1605 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1606
Charles MacNeill 7:1add29d51e72 1607 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1608
Charles MacNeill 7:1add29d51e72 1609 Status = VL53L1_get_sequence_steps_info(
Charles MacNeill 7:1add29d51e72 1610 SequenceStepId,
Charles MacNeill 7:1add29d51e72 1611 pSequenceStepsString);
Charles MacNeill 7:1add29d51e72 1612
Charles MacNeill 7:1add29d51e72 1613 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1614
Charles MacNeill 7:1add29d51e72 1615 return Status;
Charles MacNeill 7:1add29d51e72 1616 }
Charles MacNeill 7:1add29d51e72 1617
Charles MacNeill 7:1add29d51e72 1618 VL53L1_Error VL53L1_SetSequenceStepEnable(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1619 VL53L1_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled)
Charles MacNeill 7:1add29d51e72 1620 {
Charles MacNeill 7:1add29d51e72 1621 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1622 uint32_t MeasurementTimingBudgetMicroSeconds;
Charles MacNeill 7:1add29d51e72 1623
Charles MacNeill 7:1add29d51e72 1624 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1625
Charles MacNeill 7:1add29d51e72 1626
Charles MacNeill 7:1add29d51e72 1627
Charles MacNeill 7:1add29d51e72 1628 Status = VL53L1_set_sequence_config_bit(Dev,
Charles MacNeill 7:1add29d51e72 1629 (VL53L1_DeviceSequenceConfig)SequenceStepId,
Charles MacNeill 7:1add29d51e72 1630 SequenceStepEnabled);
Charles MacNeill 7:1add29d51e72 1631
Charles MacNeill 7:1add29d51e72 1632
Charles MacNeill 7:1add29d51e72 1633 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 1634
Charles MacNeill 7:1add29d51e72 1635
Charles MacNeill 7:1add29d51e72 1636 MeasurementTimingBudgetMicroSeconds = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 1637 CurrentParameters.MeasurementTimingBudgetMicroSeconds);
Charles MacNeill 7:1add29d51e72 1638
Charles MacNeill 7:1add29d51e72 1639 VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev,
Charles MacNeill 7:1add29d51e72 1640 MeasurementTimingBudgetMicroSeconds);
Charles MacNeill 7:1add29d51e72 1641 }
Charles MacNeill 7:1add29d51e72 1642
Charles MacNeill 7:1add29d51e72 1643 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1644
Charles MacNeill 7:1add29d51e72 1645 return Status;
Charles MacNeill 7:1add29d51e72 1646 }
Charles MacNeill 7:1add29d51e72 1647
Charles MacNeill 7:1add29d51e72 1648
Charles MacNeill 7:1add29d51e72 1649 VL53L1_Error VL53L1_GetSequenceStepEnable(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1650 VL53L1_SequenceStepId SequenceStepId, uint8_t *pSequenceStepEnabled)
Charles MacNeill 7:1add29d51e72 1651 {
Charles MacNeill 7:1add29d51e72 1652 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1653
Charles MacNeill 7:1add29d51e72 1654 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1655
Charles MacNeill 7:1add29d51e72 1656 Status = VL53L1_get_sequence_config_bit(Dev,
Charles MacNeill 7:1add29d51e72 1657 (VL53L1_DeviceSequenceConfig)SequenceStepId,
Charles MacNeill 7:1add29d51e72 1658 pSequenceStepEnabled);
Charles MacNeill 7:1add29d51e72 1659
Charles MacNeill 7:1add29d51e72 1660 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1661 return Status;
Charles MacNeill 7:1add29d51e72 1662 }
Charles MacNeill 7:1add29d51e72 1663
Charles MacNeill 7:1add29d51e72 1664
Charles MacNeill 7:1add29d51e72 1665
Charles MacNeill 7:1add29d51e72 1666
Charles MacNeill 7:1add29d51e72 1667
Charles MacNeill 7:1add29d51e72 1668
Charles MacNeill 7:1add29d51e72 1669
Charles MacNeill 7:1add29d51e72 1670
Charles MacNeill 7:1add29d51e72 1671
Charles MacNeill 7:1add29d51e72 1672
Charles MacNeill 7:1add29d51e72 1673 VL53L1_Error VL53L1_StartMeasurement(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 1674 {
Charles MacNeill 7:1add29d51e72 1675 #define TIMED_MODE_TIMING_GUARD_MILLISECONDS 4
Charles MacNeill 7:1add29d51e72 1676 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1677 uint8_t DeviceMeasurementMode;
Charles MacNeill 7:1add29d51e72 1678 VL53L1_State CurrPalState;
Charles MacNeill 7:1add29d51e72 1679 VL53L1_Error lStatus;
Charles MacNeill 7:1add29d51e72 1680 uint32_t MTBus, IMPms;
Charles MacNeill 7:1add29d51e72 1681 uint8_t i;
Charles MacNeill 7:1add29d51e72 1682 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 1683
Charles MacNeill 7:1add29d51e72 1684
Charles MacNeill 7:1add29d51e72 1685 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1686 VL53L1_load_patch(Dev);
Charles MacNeill 7:1add29d51e72 1687 for (i = 0; i < VL53L1_MAX_RANGE_RESULTS; i++) {
Charles MacNeill 7:1add29d51e72 1688 pdev->PreviousRangeMilliMeter[i] = 0;
Charles MacNeill 7:1add29d51e72 1689 pdev->PreviousRangeStatus[i] = 255;
Charles MacNeill 7:1add29d51e72 1690 pdev->PreviousExtendedRange[i] = 0;
Charles MacNeill 7:1add29d51e72 1691 }
Charles MacNeill 7:1add29d51e72 1692 pdev->PreviousStreamCount = 0;
Charles MacNeill 7:1add29d51e72 1693 CurrPalState = VL53L1DevDataGet(Dev, PalState);
Charles MacNeill 7:1add29d51e72 1694 switch (CurrPalState) {
Charles MacNeill 7:1add29d51e72 1695 case VL53L1_STATE_IDLE:
Charles MacNeill 7:1add29d51e72 1696 Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1697 break;
Charles MacNeill 7:1add29d51e72 1698 case VL53L1_STATE_POWERDOWN:
Charles MacNeill 7:1add29d51e72 1699 case VL53L1_STATE_WAIT_STATICINIT:
Charles MacNeill 7:1add29d51e72 1700 case VL53L1_STATE_STANDBY:
Charles MacNeill 7:1add29d51e72 1701 case VL53L1_STATE_RUNNING:
Charles MacNeill 7:1add29d51e72 1702 case VL53L1_STATE_RESET:
Charles MacNeill 7:1add29d51e72 1703 case VL53L1_STATE_UNKNOWN:
Charles MacNeill 7:1add29d51e72 1704 case VL53L1_STATE_ERROR:
Charles MacNeill 7:1add29d51e72 1705 Status = VL53L1_ERROR_INVALID_COMMAND;
Charles MacNeill 7:1add29d51e72 1706 break;
Charles MacNeill 7:1add29d51e72 1707 default:
Charles MacNeill 7:1add29d51e72 1708 Status = VL53L1_ERROR_UNDEFINED;
Charles MacNeill 7:1add29d51e72 1709 }
Charles MacNeill 7:1add29d51e72 1710
Charles MacNeill 7:1add29d51e72 1711 DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
Charles MacNeill 7:1add29d51e72 1712
Charles MacNeill 7:1add29d51e72 1713
Charles MacNeill 7:1add29d51e72 1714 if ((Status == VL53L1_ERROR_NONE) &&
Charles MacNeill 7:1add29d51e72 1715 (DeviceMeasurementMode == VL53L1_DEVICEMEASUREMENTMODE_TIMED)) {
Charles MacNeill 7:1add29d51e72 1716 lStatus = VL53L1_GetMeasurementTimingBudgetMicroSeconds(Dev,
Charles MacNeill 7:1add29d51e72 1717 &MTBus);
Charles MacNeill 7:1add29d51e72 1718
Charles MacNeill 7:1add29d51e72 1719 MTBus /= 1000;
Charles MacNeill 7:1add29d51e72 1720 lStatus = VL53L1_GetInterMeasurementPeriodMilliSeconds(Dev,
Charles MacNeill 7:1add29d51e72 1721 &IMPms);
Charles MacNeill 7:1add29d51e72 1722
Charles MacNeill 7:1add29d51e72 1723 SUPPRESS_UNUSED_WARNING(lStatus);
Charles MacNeill 7:1add29d51e72 1724 if (IMPms < MTBus + TIMED_MODE_TIMING_GUARD_MILLISECONDS)
Charles MacNeill 7:1add29d51e72 1725 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 1726 }
Charles MacNeill 7:1add29d51e72 1727
Charles MacNeill 7:1add29d51e72 1728 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1729 Status = VL53L1_init_and_start_range(
Charles MacNeill 7:1add29d51e72 1730 Dev,
Charles MacNeill 7:1add29d51e72 1731 DeviceMeasurementMode,
Charles MacNeill 7:1add29d51e72 1732 VL53L1_DEVICECONFIGLEVEL_FULL);
Charles MacNeill 7:1add29d51e72 1733
Charles MacNeill 7:1add29d51e72 1734
Charles MacNeill 7:1add29d51e72 1735 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1736 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_RUNNING);
Charles MacNeill 7:1add29d51e72 1737
Charles MacNeill 7:1add29d51e72 1738
Charles MacNeill 7:1add29d51e72 1739 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1740 return Status;
Charles MacNeill 7:1add29d51e72 1741 }
Charles MacNeill 7:1add29d51e72 1742
Charles MacNeill 7:1add29d51e72 1743 VL53L1_Error VL53L1_StopMeasurement(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 1744 {
Charles MacNeill 7:1add29d51e72 1745 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1746
Charles MacNeill 7:1add29d51e72 1747 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1748
Charles MacNeill 7:1add29d51e72 1749 Status = VL53L1_stop_range(Dev);
Charles MacNeill 7:1add29d51e72 1750 VL53L1_unload_patch(Dev);
Charles MacNeill 7:1add29d51e72 1751
Charles MacNeill 7:1add29d51e72 1752 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 1753 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_IDLE);
Charles MacNeill 7:1add29d51e72 1754
Charles MacNeill 7:1add29d51e72 1755 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1756 return Status;
Charles MacNeill 7:1add29d51e72 1757 }
Charles MacNeill 7:1add29d51e72 1758
Charles MacNeill 7:1add29d51e72 1759
Charles MacNeill 7:1add29d51e72 1760 VL53L1_Error VL53L1_ClearInterruptAndStartMeasurement(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 1761 {
Charles MacNeill 7:1add29d51e72 1762 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1763 uint8_t DeviceMeasurementMode;
Charles MacNeill 7:1add29d51e72 1764
Charles MacNeill 7:1add29d51e72 1765 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1766
Charles MacNeill 7:1add29d51e72 1767 DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
Charles MacNeill 7:1add29d51e72 1768
Charles MacNeill 7:1add29d51e72 1769 Status = VL53L1_clear_interrupt_and_enable_next_range(Dev,
Charles MacNeill 7:1add29d51e72 1770 DeviceMeasurementMode);
Charles MacNeill 7:1add29d51e72 1771
Charles MacNeill 7:1add29d51e72 1772 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1773 return Status;
Charles MacNeill 7:1add29d51e72 1774 }
Charles MacNeill 7:1add29d51e72 1775
Charles MacNeill 7:1add29d51e72 1776
Charles MacNeill 7:1add29d51e72 1777 VL53L1_Error VL53L1_GetMeasurementDataReady(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1778 uint8_t *pMeasurementDataReady)
Charles MacNeill 7:1add29d51e72 1779 {
Charles MacNeill 7:1add29d51e72 1780 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1781
Charles MacNeill 7:1add29d51e72 1782 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1783
Charles MacNeill 7:1add29d51e72 1784 Status = VL53L1_is_new_data_ready(Dev, pMeasurementDataReady);
Charles MacNeill 7:1add29d51e72 1785
Charles MacNeill 7:1add29d51e72 1786 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1787 return Status;
Charles MacNeill 7:1add29d51e72 1788 }
Charles MacNeill 7:1add29d51e72 1789
Charles MacNeill 7:1add29d51e72 1790 VL53L1_Error VL53L1_WaitMeasurementDataReady(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 1791 {
Charles MacNeill 7:1add29d51e72 1792 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 1793
Charles MacNeill 7:1add29d51e72 1794 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 1795
Charles MacNeill 7:1add29d51e72 1796
Charles MacNeill 7:1add29d51e72 1797
Charles MacNeill 7:1add29d51e72 1798 Status = VL53L1_poll_for_range_completion(Dev,
Charles MacNeill 7:1add29d51e72 1799 VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS);
Charles MacNeill 7:1add29d51e72 1800
Charles MacNeill 7:1add29d51e72 1801 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 1802 return Status;
Charles MacNeill 7:1add29d51e72 1803 }
Charles MacNeill 7:1add29d51e72 1804
Charles MacNeill 7:1add29d51e72 1805 static void GenNewPresetMode(int16_t RefRange,
Charles MacNeill 7:1add29d51e72 1806 VL53L1_DistanceModes InternalDistanceMode,
Charles MacNeill 7:1add29d51e72 1807 VL53L1_DistanceModes *pNewDistanceMode)
Charles MacNeill 7:1add29d51e72 1808 {
Charles MacNeill 7:1add29d51e72 1809 uint16_t HRLI = 600;
Charles MacNeill 7:1add29d51e72 1810 uint16_t HRLH = 700;
Charles MacNeill 7:1add29d51e72 1811 uint16_t MRLI = 1400;
Charles MacNeill 7:1add29d51e72 1812 uint16_t MRLH = 1500;
Charles MacNeill 7:1add29d51e72 1813
Charles MacNeill 7:1add29d51e72 1814 switch (InternalDistanceMode) {
Charles MacNeill 7:1add29d51e72 1815 case VL53L1_DISTANCEMODE_SHORT:
Charles MacNeill 7:1add29d51e72 1816
Charles MacNeill 7:1add29d51e72 1817 if (RefRange > MRLH)
Charles MacNeill 7:1add29d51e72 1818 *pNewDistanceMode = VL53L1_DISTANCEMODE_LONG;
Charles MacNeill 7:1add29d51e72 1819 else if (RefRange > HRLH)
Charles MacNeill 7:1add29d51e72 1820 *pNewDistanceMode = VL53L1_DISTANCEMODE_MEDIUM;
Charles MacNeill 7:1add29d51e72 1821 break;
Charles MacNeill 7:1add29d51e72 1822 case VL53L1_DISTANCEMODE_MEDIUM:
Charles MacNeill 7:1add29d51e72 1823
Charles MacNeill 7:1add29d51e72 1824 if (RefRange > MRLH)
Charles MacNeill 7:1add29d51e72 1825 *pNewDistanceMode = VL53L1_DISTANCEMODE_LONG;
Charles MacNeill 7:1add29d51e72 1826 else if (RefRange < HRLI)
Charles MacNeill 7:1add29d51e72 1827 *pNewDistanceMode = VL53L1_DISTANCEMODE_SHORT;
Charles MacNeill 7:1add29d51e72 1828 break;
Charles MacNeill 7:1add29d51e72 1829 default:
Charles MacNeill 7:1add29d51e72 1830
Charles MacNeill 7:1add29d51e72 1831 if (RefRange < HRLI)
Charles MacNeill 7:1add29d51e72 1832 *pNewDistanceMode = VL53L1_DISTANCEMODE_SHORT;
Charles MacNeill 7:1add29d51e72 1833 else if (RefRange < MRLI)
Charles MacNeill 7:1add29d51e72 1834 *pNewDistanceMode = VL53L1_DISTANCEMODE_MEDIUM;
Charles MacNeill 7:1add29d51e72 1835 break;
Charles MacNeill 7:1add29d51e72 1836 }
Charles MacNeill 7:1add29d51e72 1837 }
Charles MacNeill 7:1add29d51e72 1838
Charles MacNeill 7:1add29d51e72 1839 static void CheckAndChangeDistanceMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 1840 VL53L1_TargetRangeData_t *pRangeData,
Charles MacNeill 7:1add29d51e72 1841 int16_t Ambient100DmaxMm,
Charles MacNeill 7:1add29d51e72 1842 VL53L1_DistanceModes *pNewDistanceMode
Charles MacNeill 7:1add29d51e72 1843 )
Charles MacNeill 7:1add29d51e72 1844 {
Charles MacNeill 7:1add29d51e72 1845 VL53L1_DistanceModes DistanceMode;
Charles MacNeill 7:1add29d51e72 1846 uint8_t RangeStatus = pRangeData->RangeStatus;
Charles MacNeill 7:1add29d51e72 1847 uint8_t DmaxValid;
Charles MacNeill 7:1add29d51e72 1848 int32_t MinAmbient = BDTable[VL53L1_TUNING_MIN_AMBIENT_DMAX_VALID];
Charles MacNeill 7:1add29d51e72 1849 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 1850 int32_t tmpint32;
Charles MacNeill 7:1add29d51e72 1851
Charles MacNeill 7:1add29d51e72 1852
Charles MacNeill 7:1add29d51e72 1853 switch (RangeStatus) {
Charles MacNeill 7:1add29d51e72 1854 case VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL:
Charles MacNeill 7:1add29d51e72 1855 case VL53L1_RANGESTATUS_WRAP_TARGET_FAIL:
Charles MacNeill 7:1add29d51e72 1856 case VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE:
Charles MacNeill 7:1add29d51e72 1857 case VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL:
Charles MacNeill 7:1add29d51e72 1858 case VL53L1_RANGESTATUS_SYNCRONISATION_INT:
Charles MacNeill 7:1add29d51e72 1859 case VL53L1_RANGESTATUS_NONE:
Charles MacNeill 7:1add29d51e72 1860 return;
Charles MacNeill 7:1add29d51e72 1861 default:
Charles MacNeill 7:1add29d51e72 1862
Charles MacNeill 7:1add29d51e72 1863 break;
Charles MacNeill 7:1add29d51e72 1864 }
Charles MacNeill 7:1add29d51e72 1865
Charles MacNeill 7:1add29d51e72 1866 DmaxValid = 1;
Charles MacNeill 7:1add29d51e72 1867 tmpint32 = pdev->hist_data.VL53L1_p_004;
Charles MacNeill 7:1add29d51e72 1868 if ((tmpint32 < MinAmbient) || (Ambient100DmaxMm == 0))
Charles MacNeill 7:1add29d51e72 1869 DmaxValid = 0;
Charles MacNeill 7:1add29d51e72 1870
Charles MacNeill 7:1add29d51e72 1871 DistanceMode = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 1872 CurrentParameters.DistanceMode);
Charles MacNeill 7:1add29d51e72 1873
Charles MacNeill 7:1add29d51e72 1874 *pNewDistanceMode = DistanceMode;
Charles MacNeill 7:1add29d51e72 1875
Charles MacNeill 7:1add29d51e72 1876 if (RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID)
Charles MacNeill 7:1add29d51e72 1877 GenNewPresetMode(pRangeData->RangeMilliMeter,
Charles MacNeill 7:1add29d51e72 1878 DistanceMode, pNewDistanceMode);
Charles MacNeill 7:1add29d51e72 1879 else {
Charles MacNeill 7:1add29d51e72 1880 if (DmaxValid)
Charles MacNeill 7:1add29d51e72 1881 GenNewPresetMode(Ambient100DmaxMm,
Charles MacNeill 7:1add29d51e72 1882 DistanceMode, pNewDistanceMode);
Charles MacNeill 7:1add29d51e72 1883 else
Charles MacNeill 7:1add29d51e72 1884 *pNewDistanceMode = VL53L1_DISTANCEMODE_LONG;
Charles MacNeill 7:1add29d51e72 1885 }
Charles MacNeill 7:1add29d51e72 1886 }
Charles MacNeill 7:1add29d51e72 1887
Charles MacNeill 7:1add29d51e72 1888 static uint8_t ComputeRQL(uint8_t active_results,
Charles MacNeill 7:1add29d51e72 1889 uint8_t FilteredRangeStatus,
Charles MacNeill 7:1add29d51e72 1890 VL53L1_range_data_t *presults_data)
Charles MacNeill 7:1add29d51e72 1891 {
Charles MacNeill 7:1add29d51e72 1892 int16_t T_Wide = 150;
Charles MacNeill 7:1add29d51e72 1893 int16_t SRL = 300;
Charles MacNeill 7:1add29d51e72 1894 uint16_t SRAS = 30;
Charles MacNeill 7:1add29d51e72 1895 FixPoint1616_t RAS;
Charles MacNeill 7:1add29d51e72 1896 FixPoint1616_t SRQL;
Charles MacNeill 7:1add29d51e72 1897 FixPoint1616_t GI = 7713587;
Charles MacNeill 7:1add29d51e72 1898 FixPoint1616_t GGm = 3198157;
Charles MacNeill 7:1add29d51e72 1899 FixPoint1616_t LRAP = 6554;
Charles MacNeill 7:1add29d51e72 1900 FixPoint1616_t partial;
Charles MacNeill 7:1add29d51e72 1901 uint8_t finalvalue;
Charles MacNeill 7:1add29d51e72 1902 uint8_t returnvalue;
Charles MacNeill 7:1add29d51e72 1903
Charles MacNeill 7:1add29d51e72 1904 if (active_results == 0)
Charles MacNeill 7:1add29d51e72 1905 returnvalue = 0;
Charles MacNeill 7:1add29d51e72 1906 else if (((presults_data->max_range_mm -
Charles MacNeill 7:1add29d51e72 1907 presults_data->min_range_mm) >= T_Wide) ||
Charles MacNeill 7:1add29d51e72 1908 (FilteredRangeStatus == VL53L1_DEVICEERROR_PHASECONSISTENCY))
Charles MacNeill 7:1add29d51e72 1909 returnvalue = 50;
Charles MacNeill 7:1add29d51e72 1910 else {
Charles MacNeill 7:1add29d51e72 1911 if (presults_data->median_range_mm < SRL)
Charles MacNeill 7:1add29d51e72 1912 RAS = SRAS * 65536;
Charles MacNeill 7:1add29d51e72 1913 else
Charles MacNeill 7:1add29d51e72 1914 RAS = LRAP * presults_data->median_range_mm;
Charles MacNeill 7:1add29d51e72 1915
Charles MacNeill 7:1add29d51e72 1916
Charles MacNeill 7:1add29d51e72 1917 if (RAS != 0) {
Charles MacNeill 7:1add29d51e72 1918 partial = (GGm * presults_data->VL53L1_p_005);
Charles MacNeill 7:1add29d51e72 1919 partial = partial + (RAS >> 1);
Charles MacNeill 7:1add29d51e72 1920 partial = partial / RAS;
Charles MacNeill 7:1add29d51e72 1921 partial = partial * 65536;
Charles MacNeill 7:1add29d51e72 1922 if (partial <= GI)
Charles MacNeill 7:1add29d51e72 1923 SRQL = GI - partial;
Charles MacNeill 7:1add29d51e72 1924 else
Charles MacNeill 7:1add29d51e72 1925 SRQL = 50 * 65536;
Charles MacNeill 7:1add29d51e72 1926 } else
Charles MacNeill 7:1add29d51e72 1927 SRQL = 100 * 65536;
Charles MacNeill 7:1add29d51e72 1928
Charles MacNeill 7:1add29d51e72 1929 finalvalue = (uint8_t)(SRQL >> 16);
Charles MacNeill 7:1add29d51e72 1930 returnvalue = MAX(50, MIN(100, finalvalue));
Charles MacNeill 7:1add29d51e72 1931 }
Charles MacNeill 7:1add29d51e72 1932
Charles MacNeill 7:1add29d51e72 1933 return returnvalue;
Charles MacNeill 7:1add29d51e72 1934 }
Charles MacNeill 7:1add29d51e72 1935
Charles MacNeill 7:1add29d51e72 1936
Charles MacNeill 7:1add29d51e72 1937 static uint8_t ConvertStatusLite(uint8_t FilteredRangeStatus)
Charles MacNeill 7:1add29d51e72 1938 {
Charles MacNeill 7:1add29d51e72 1939 uint8_t RangeStatus;
Charles MacNeill 7:1add29d51e72 1940
Charles MacNeill 7:1add29d51e72 1941 switch (FilteredRangeStatus) {
Charles MacNeill 7:1add29d51e72 1942 case VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY:
Charles MacNeill 7:1add29d51e72 1943 RangeStatus = VL53L1_RANGESTATUS_SYNCRONISATION_INT;
Charles MacNeill 7:1add29d51e72 1944 break;
Charles MacNeill 7:1add29d51e72 1945 case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
Charles MacNeill 7:1add29d51e72 1946 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
Charles MacNeill 7:1add29d51e72 1947 break;
Charles MacNeill 7:1add29d51e72 1948 case VL53L1_DEVICEERROR_RANGEPHASECHECK:
Charles MacNeill 7:1add29d51e72 1949 RangeStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
Charles MacNeill 7:1add29d51e72 1950 break;
Charles MacNeill 7:1add29d51e72 1951 case VL53L1_DEVICEERROR_MSRCNOTARGET:
Charles MacNeill 7:1add29d51e72 1952 RangeStatus = VL53L1_RANGESTATUS_SIGNAL_FAIL;
Charles MacNeill 7:1add29d51e72 1953 break;
Charles MacNeill 7:1add29d51e72 1954 case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
Charles MacNeill 7:1add29d51e72 1955 RangeStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
Charles MacNeill 7:1add29d51e72 1956 break;
Charles MacNeill 7:1add29d51e72 1957 case VL53L1_DEVICEERROR_PHASECONSISTENCY:
Charles MacNeill 7:1add29d51e72 1958 RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
Charles MacNeill 7:1add29d51e72 1959 break;
Charles MacNeill 7:1add29d51e72 1960 case VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD:
Charles MacNeill 7:1add29d51e72 1961 RangeStatus = VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL;
Charles MacNeill 7:1add29d51e72 1962 break;
Charles MacNeill 7:1add29d51e72 1963 case VL53L1_DEVICEERROR_MINCLIP:
Charles MacNeill 7:1add29d51e72 1964 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED;
Charles MacNeill 7:1add29d51e72 1965 break;
Charles MacNeill 7:1add29d51e72 1966 case VL53L1_DEVICEERROR_RANGECOMPLETE:
Charles MacNeill 7:1add29d51e72 1967 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
Charles MacNeill 7:1add29d51e72 1968 break;
Charles MacNeill 7:1add29d51e72 1969 default:
Charles MacNeill 7:1add29d51e72 1970 RangeStatus = VL53L1_RANGESTATUS_NONE;
Charles MacNeill 7:1add29d51e72 1971 }
Charles MacNeill 7:1add29d51e72 1972
Charles MacNeill 7:1add29d51e72 1973 return RangeStatus;
Charles MacNeill 7:1add29d51e72 1974 }
Charles MacNeill 7:1add29d51e72 1975
Charles MacNeill 7:1add29d51e72 1976
Charles MacNeill 7:1add29d51e72 1977 static uint8_t ConvertStatusHisto(uint8_t FilteredRangeStatus)
Charles MacNeill 7:1add29d51e72 1978 {
Charles MacNeill 7:1add29d51e72 1979 uint8_t RangeStatus;
Charles MacNeill 7:1add29d51e72 1980
Charles MacNeill 7:1add29d51e72 1981 switch (FilteredRangeStatus) {
Charles MacNeill 7:1add29d51e72 1982 case VL53L1_DEVICEERROR_RANGEPHASECHECK:
Charles MacNeill 7:1add29d51e72 1983 RangeStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
Charles MacNeill 7:1add29d51e72 1984 break;
Charles MacNeill 7:1add29d51e72 1985 case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
Charles MacNeill 7:1add29d51e72 1986 RangeStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
Charles MacNeill 7:1add29d51e72 1987 break;
Charles MacNeill 7:1add29d51e72 1988 case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
Charles MacNeill 7:1add29d51e72 1989 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
Charles MacNeill 7:1add29d51e72 1990 break;
Charles MacNeill 7:1add29d51e72 1991 case VL53L1_DEVICEERROR_PHASECONSISTENCY:
Charles MacNeill 7:1add29d51e72 1992 RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
Charles MacNeill 7:1add29d51e72 1993 break;
Charles MacNeill 7:1add29d51e72 1994 case VL53L1_DEVICEERROR_PREV_RANGE_NO_TARGETS:
Charles MacNeill 7:1add29d51e72 1995 RangeStatus = VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL;
Charles MacNeill 7:1add29d51e72 1996 break;
Charles MacNeill 7:1add29d51e72 1997 case VL53L1_DEVICEERROR_EVENTCONSISTENCY:
Charles MacNeill 7:1add29d51e72 1998 RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
Charles MacNeill 7:1add29d51e72 1999 break;
Charles MacNeill 7:1add29d51e72 2000 case VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE:
Charles MacNeill 7:1add29d51e72 2001 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE;
Charles MacNeill 7:1add29d51e72 2002 break;
Charles MacNeill 7:1add29d51e72 2003 case VL53L1_DEVICEERROR_RANGECOMPLETE:
Charles MacNeill 7:1add29d51e72 2004 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
Charles MacNeill 7:1add29d51e72 2005 break;
Charles MacNeill 7:1add29d51e72 2006 default:
Charles MacNeill 7:1add29d51e72 2007 RangeStatus = VL53L1_RANGESTATUS_NONE;
Charles MacNeill 7:1add29d51e72 2008 }
Charles MacNeill 7:1add29d51e72 2009
Charles MacNeill 7:1add29d51e72 2010 return RangeStatus;
Charles MacNeill 7:1add29d51e72 2011 }
Charles MacNeill 7:1add29d51e72 2012
Charles MacNeill 7:1add29d51e72 2013 static VL53L1_Error SetSimpleData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2014 uint8_t active_results, uint8_t device_status,
Charles MacNeill 7:1add29d51e72 2015 VL53L1_range_data_t *presults_data,
Charles MacNeill 7:1add29d51e72 2016 VL53L1_RangingMeasurementData_t *pRangeData)
Charles MacNeill 7:1add29d51e72 2017 {
Charles MacNeill 7:1add29d51e72 2018 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2019 uint8_t FilteredRangeStatus;
Charles MacNeill 7:1add29d51e72 2020 uint8_t SigmaLimitflag;
Charles MacNeill 7:1add29d51e72 2021 uint8_t SignalLimitflag;
Charles MacNeill 7:1add29d51e72 2022 uint8_t Temp8Enable;
Charles MacNeill 7:1add29d51e72 2023 uint8_t Temp8;
Charles MacNeill 7:1add29d51e72 2024 FixPoint1616_t AmbientRate;
Charles MacNeill 7:1add29d51e72 2025 FixPoint1616_t SignalRate;
Charles MacNeill 7:1add29d51e72 2026 FixPoint1616_t TempFix1616;
Charles MacNeill 7:1add29d51e72 2027 FixPoint1616_t LimitCheckValue;
Charles MacNeill 7:1add29d51e72 2028 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 2029 int16_t Range;
Charles MacNeill 7:1add29d51e72 2030
Charles MacNeill 7:1add29d51e72 2031 pRangeData->TimeStamp = presults_data->time_stamp;
Charles MacNeill 7:1add29d51e72 2032
Charles MacNeill 7:1add29d51e72 2033 FilteredRangeStatus = presults_data->range_status & 0x1F;
Charles MacNeill 7:1add29d51e72 2034
Charles MacNeill 7:1add29d51e72 2035 pRangeData->RangeQualityLevel = ComputeRQL(active_results,
Charles MacNeill 7:1add29d51e72 2036 FilteredRangeStatus,
Charles MacNeill 7:1add29d51e72 2037 presults_data);
Charles MacNeill 7:1add29d51e72 2038
Charles MacNeill 7:1add29d51e72 2039 SignalRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2040 presults_data->peak_signal_count_rate_mcps);
Charles MacNeill 7:1add29d51e72 2041 pRangeData->SignalRateRtnMegaCps
Charles MacNeill 7:1add29d51e72 2042 = SignalRate;
Charles MacNeill 7:1add29d51e72 2043
Charles MacNeill 7:1add29d51e72 2044 AmbientRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2045 presults_data->ambient_count_rate_mcps);
Charles MacNeill 7:1add29d51e72 2046 pRangeData->AmbientRateRtnMegaCps = AmbientRate;
Charles MacNeill 7:1add29d51e72 2047
Charles MacNeill 7:1add29d51e72 2048 pRangeData->EffectiveSpadRtnCount =
Charles MacNeill 7:1add29d51e72 2049 presults_data->VL53L1_p_006;
Charles MacNeill 7:1add29d51e72 2050
Charles MacNeill 7:1add29d51e72 2051 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2052 presults_data->VL53L1_p_005);
Charles MacNeill 7:1add29d51e72 2053
Charles MacNeill 7:1add29d51e72 2054 pRangeData->SigmaMilliMeter = TempFix1616;
Charles MacNeill 7:1add29d51e72 2055
Charles MacNeill 7:1add29d51e72 2056 pRangeData->RangeMilliMeter = presults_data->median_range_mm;
Charles MacNeill 7:1add29d51e72 2057
Charles MacNeill 7:1add29d51e72 2058 pRangeData->RangeFractionalPart = 0;
Charles MacNeill 7:1add29d51e72 2059
Charles MacNeill 7:1add29d51e72 2060
Charles MacNeill 7:1add29d51e72 2061 switch (device_status) {
Charles MacNeill 7:1add29d51e72 2062 case VL53L1_DEVICEERROR_MULTCLIPFAIL:
Charles MacNeill 7:1add29d51e72 2063 case VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
Charles MacNeill 7:1add29d51e72 2064 case VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
Charles MacNeill 7:1add29d51e72 2065 case VL53L1_DEVICEERROR_NOVHVVALUEFOUND:
Charles MacNeill 7:1add29d51e72 2066 pRangeData->RangeStatus = VL53L1_RANGESTATUS_HARDWARE_FAIL;
Charles MacNeill 7:1add29d51e72 2067 break;
Charles MacNeill 7:1add29d51e72 2068 case VL53L1_DEVICEERROR_USERROICLIP:
Charles MacNeill 7:1add29d51e72 2069 pRangeData->RangeStatus = VL53L1_RANGESTATUS_MIN_RANGE_FAIL;
Charles MacNeill 7:1add29d51e72 2070 break;
Charles MacNeill 7:1add29d51e72 2071 default:
Charles MacNeill 7:1add29d51e72 2072 pRangeData->RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
Charles MacNeill 7:1add29d51e72 2073 }
Charles MacNeill 7:1add29d51e72 2074
Charles MacNeill 7:1add29d51e72 2075
Charles MacNeill 7:1add29d51e72 2076 if (pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
Charles MacNeill 7:1add29d51e72 2077 PresetMode = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 2078 CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 2079 if ((PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) ||
Charles MacNeill 7:1add29d51e72 2080 (PresetMode == VL53L1_PRESETMODE_RANGING) ||
Charles MacNeill 7:1add29d51e72 2081 (PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE))
Charles MacNeill 7:1add29d51e72 2082 pRangeData->RangeStatus =
Charles MacNeill 7:1add29d51e72 2083 ConvertStatusHisto(FilteredRangeStatus);
Charles MacNeill 7:1add29d51e72 2084 else
Charles MacNeill 7:1add29d51e72 2085 pRangeData->RangeStatus =
Charles MacNeill 7:1add29d51e72 2086 ConvertStatusLite(FilteredRangeStatus);
Charles MacNeill 7:1add29d51e72 2087 }
Charles MacNeill 7:1add29d51e72 2088
Charles MacNeill 7:1add29d51e72 2089
Charles MacNeill 7:1add29d51e72 2090 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2091 presults_data->VL53L1_p_005);
Charles MacNeill 7:1add29d51e72 2092 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 2093 LimitChecksCurrent, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2094 TempFix1616);
Charles MacNeill 7:1add29d51e72 2095
Charles MacNeill 7:1add29d51e72 2096 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2097 presults_data->peak_signal_count_rate_mcps);
Charles MacNeill 7:1add29d51e72 2098 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 2099 LimitChecksCurrent, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2100 TempFix1616);
Charles MacNeill 7:1add29d51e72 2101
Charles MacNeill 7:1add29d51e72 2102
Charles MacNeill 7:1add29d51e72 2103
Charles MacNeill 7:1add29d51e72 2104 VL53L1_GetLimitCheckValue(Dev,
Charles MacNeill 7:1add29d51e72 2105 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2106 &LimitCheckValue);
Charles MacNeill 7:1add29d51e72 2107
Charles MacNeill 7:1add29d51e72 2108 SigmaLimitflag = (FilteredRangeStatus ==
Charles MacNeill 7:1add29d51e72 2109 VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK)
Charles MacNeill 7:1add29d51e72 2110 ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2111
Charles MacNeill 7:1add29d51e72 2112 VL53L1_GetLimitCheckEnable(Dev,
Charles MacNeill 7:1add29d51e72 2113 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2114 &Temp8Enable);
Charles MacNeill 7:1add29d51e72 2115
Charles MacNeill 7:1add29d51e72 2116 Temp8 = ((Temp8Enable == 1) && (SigmaLimitflag == 1)) ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2117 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
Charles MacNeill 7:1add29d51e72 2118 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
Charles MacNeill 7:1add29d51e72 2119
Charles MacNeill 7:1add29d51e72 2120
Charles MacNeill 7:1add29d51e72 2121 VL53L1_GetLimitCheckValue(Dev,
Charles MacNeill 7:1add29d51e72 2122 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2123 &LimitCheckValue);
Charles MacNeill 7:1add29d51e72 2124
Charles MacNeill 7:1add29d51e72 2125 SignalLimitflag = (FilteredRangeStatus ==
Charles MacNeill 7:1add29d51e72 2126 VL53L1_DEVICEERROR_MSRCNOTARGET)
Charles MacNeill 7:1add29d51e72 2127 ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2128
Charles MacNeill 7:1add29d51e72 2129 VL53L1_GetLimitCheckEnable(Dev,
Charles MacNeill 7:1add29d51e72 2130 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2131 &Temp8Enable);
Charles MacNeill 7:1add29d51e72 2132
Charles MacNeill 7:1add29d51e72 2133 Temp8 = ((Temp8Enable == 1) && (SignalLimitflag == 1)) ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2134 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
Charles MacNeill 7:1add29d51e72 2135 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, Temp8);
Charles MacNeill 7:1add29d51e72 2136
Charles MacNeill 7:1add29d51e72 2137 Range = pRangeData->RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 2138 if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
Charles MacNeill 7:1add29d51e72 2139 (Range < 0)) {
Charles MacNeill 7:1add29d51e72 2140 if (Range < BDTable[VL53L1_TUNING_PROXY_MIN])
Charles MacNeill 7:1add29d51e72 2141 pRangeData->RangeStatus =
Charles MacNeill 7:1add29d51e72 2142 VL53L1_RANGESTATUS_RANGE_INVALID;
Charles MacNeill 7:1add29d51e72 2143 else
Charles MacNeill 7:1add29d51e72 2144 pRangeData->RangeMilliMeter = 0;
Charles MacNeill 7:1add29d51e72 2145 }
Charles MacNeill 7:1add29d51e72 2146
Charles MacNeill 7:1add29d51e72 2147 return Status;
Charles MacNeill 7:1add29d51e72 2148 }
Charles MacNeill 7:1add29d51e72 2149
Charles MacNeill 7:1add29d51e72 2150 static VL53L1_Error SetTargetData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2151 uint8_t active_results, uint8_t streamcount, uint8_t iteration,
Charles MacNeill 7:1add29d51e72 2152 uint8_t device_status, VL53L1_range_data_t *presults_data,
Charles MacNeill 7:1add29d51e72 2153 VL53L1_TargetRangeData_t *pRangeData)
Charles MacNeill 7:1add29d51e72 2154 {
Charles MacNeill 7:1add29d51e72 2155 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2156 VL53L1_LLDriverData_t *pdev =
Charles MacNeill 7:1add29d51e72 2157 VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 2158 VL53L1_tuning_parm_storage_t *tp =
Charles MacNeill 7:1add29d51e72 2159 &(pdev->tuning_parms);
Charles MacNeill 7:1add29d51e72 2160 uint8_t sequency;
Charles MacNeill 7:1add29d51e72 2161 uint8_t FilteredRangeStatus;
Charles MacNeill 7:1add29d51e72 2162 uint8_t SigmaLimitflag;
Charles MacNeill 7:1add29d51e72 2163 uint8_t SignalLimitflag;
Charles MacNeill 7:1add29d51e72 2164 uint8_t Temp8Enable;
Charles MacNeill 7:1add29d51e72 2165 uint8_t Temp8;
Charles MacNeill 7:1add29d51e72 2166 FixPoint1616_t AmbientRate;
Charles MacNeill 7:1add29d51e72 2167 FixPoint1616_t SignalRate;
Charles MacNeill 7:1add29d51e72 2168 FixPoint1616_t TempFix1616;
Charles MacNeill 7:1add29d51e72 2169 FixPoint1616_t LimitCheckValue;
Charles MacNeill 7:1add29d51e72 2170 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 2171 int16_t Range, RangeDiff, RangeMillimeterInit;
Charles MacNeill 7:1add29d51e72 2172 int32_t ExtendedRangeEnabled = 0;
Charles MacNeill 7:1add29d51e72 2173 uint8_t uwr_status;
Charles MacNeill 7:1add29d51e72 2174 int16_t AddOffset;
lugandc 18:0696efe39d08 2175 int32_t TuningMinValidSignal;
lugandc 18:0696efe39d08 2176 FixPoint1616_t MinValidSignal;
Charles MacNeill 7:1add29d51e72 2177
Charles MacNeill 7:1add29d51e72 2178 FilteredRangeStatus = presults_data->range_status & 0x1F;
Charles MacNeill 7:1add29d51e72 2179
Charles MacNeill 7:1add29d51e72 2180 pRangeData->RangeQualityLevel = ComputeRQL(active_results,
Charles MacNeill 7:1add29d51e72 2181 FilteredRangeStatus,
Charles MacNeill 7:1add29d51e72 2182 presults_data);
Charles MacNeill 7:1add29d51e72 2183
Charles MacNeill 7:1add29d51e72 2184 SignalRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2185 presults_data->peak_signal_count_rate_mcps);
Charles MacNeill 7:1add29d51e72 2186 pRangeData->SignalRateRtnMegaCps
Charles MacNeill 7:1add29d51e72 2187 = SignalRate;
Charles MacNeill 7:1add29d51e72 2188
Charles MacNeill 7:1add29d51e72 2189 AmbientRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2190 presults_data->ambient_count_rate_mcps);
Charles MacNeill 7:1add29d51e72 2191 pRangeData->AmbientRateRtnMegaCps = AmbientRate;
Charles MacNeill 7:1add29d51e72 2192
Charles MacNeill 7:1add29d51e72 2193 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2194 presults_data->VL53L1_p_005);
Charles MacNeill 7:1add29d51e72 2195
Charles MacNeill 7:1add29d51e72 2196 pRangeData->SigmaMilliMeter = TempFix1616;
Charles MacNeill 7:1add29d51e72 2197
Charles MacNeill 7:1add29d51e72 2198 pRangeData->RangeMilliMeter = presults_data->median_range_mm;
Charles MacNeill 7:1add29d51e72 2199 pRangeData->RangeMaxMilliMeter = presults_data->max_range_mm;
Charles MacNeill 7:1add29d51e72 2200 pRangeData->RangeMinMilliMeter = presults_data->min_range_mm;
Charles MacNeill 7:1add29d51e72 2201
Charles MacNeill 7:1add29d51e72 2202 pRangeData->RangeFractionalPart = 0;
Charles MacNeill 7:1add29d51e72 2203
Charles MacNeill 7:1add29d51e72 2204
Charles MacNeill 7:1add29d51e72 2205 switch (device_status) {
Charles MacNeill 7:1add29d51e72 2206 case VL53L1_DEVICEERROR_MULTCLIPFAIL:
Charles MacNeill 7:1add29d51e72 2207 case VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
Charles MacNeill 7:1add29d51e72 2208 case VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
Charles MacNeill 7:1add29d51e72 2209 case VL53L1_DEVICEERROR_NOVHVVALUEFOUND:
Charles MacNeill 7:1add29d51e72 2210 pRangeData->RangeStatus = VL53L1_RANGESTATUS_HARDWARE_FAIL;
Charles MacNeill 7:1add29d51e72 2211 break;
Charles MacNeill 7:1add29d51e72 2212 case VL53L1_DEVICEERROR_USERROICLIP:
Charles MacNeill 7:1add29d51e72 2213 pRangeData->RangeStatus = VL53L1_RANGESTATUS_MIN_RANGE_FAIL;
Charles MacNeill 7:1add29d51e72 2214 break;
Charles MacNeill 7:1add29d51e72 2215 default:
Charles MacNeill 7:1add29d51e72 2216 pRangeData->RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
Charles MacNeill 7:1add29d51e72 2217 }
Charles MacNeill 7:1add29d51e72 2218
Charles MacNeill 7:1add29d51e72 2219
Charles MacNeill 7:1add29d51e72 2220 if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
Charles MacNeill 7:1add29d51e72 2221 (active_results == 0)) {
Charles MacNeill 7:1add29d51e72 2222 pRangeData->RangeStatus = VL53L1_RANGESTATUS_NONE;
Charles MacNeill 7:1add29d51e72 2223 pRangeData->SignalRateRtnMegaCps = 0;
Charles MacNeill 7:1add29d51e72 2224 pRangeData->SigmaMilliMeter = 0;
Charles MacNeill 7:1add29d51e72 2225 pRangeData->RangeMilliMeter = 8191;
Charles MacNeill 7:1add29d51e72 2226 pRangeData->RangeMaxMilliMeter = 8191;
Charles MacNeill 7:1add29d51e72 2227 pRangeData->RangeMinMilliMeter = 8191;
Charles MacNeill 7:1add29d51e72 2228 }
Charles MacNeill 7:1add29d51e72 2229
Charles MacNeill 7:1add29d51e72 2230
Charles MacNeill 7:1add29d51e72 2231 if (pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
Charles MacNeill 7:1add29d51e72 2232 PresetMode = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 2233 CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 2234 if ((PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) ||
Charles MacNeill 7:1add29d51e72 2235 (PresetMode == VL53L1_PRESETMODE_RANGING) ||
Charles MacNeill 7:1add29d51e72 2236 (PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE))
Charles MacNeill 7:1add29d51e72 2237 pRangeData->RangeStatus =
Charles MacNeill 7:1add29d51e72 2238 ConvertStatusHisto(FilteredRangeStatus);
Charles MacNeill 7:1add29d51e72 2239 else
Charles MacNeill 7:1add29d51e72 2240 pRangeData->RangeStatus =
Charles MacNeill 7:1add29d51e72 2241 ConvertStatusLite(FilteredRangeStatus);
Charles MacNeill 7:1add29d51e72 2242 }
Charles MacNeill 7:1add29d51e72 2243
Charles MacNeill 7:1add29d51e72 2244
Charles MacNeill 7:1add29d51e72 2245 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2246 presults_data->VL53L1_p_005);
Charles MacNeill 7:1add29d51e72 2247 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 2248 LimitChecksCurrent, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2249 TempFix1616);
Charles MacNeill 7:1add29d51e72 2250
Charles MacNeill 7:1add29d51e72 2251 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 2252 presults_data->peak_signal_count_rate_mcps);
Charles MacNeill 7:1add29d51e72 2253 VL53L1_SETARRAYPARAMETERFIELD(Dev,
Charles MacNeill 7:1add29d51e72 2254 LimitChecksCurrent, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2255 TempFix1616);
Charles MacNeill 7:1add29d51e72 2256
Charles MacNeill 7:1add29d51e72 2257
Charles MacNeill 7:1add29d51e72 2258
Charles MacNeill 7:1add29d51e72 2259 VL53L1_GetLimitCheckValue(Dev,
Charles MacNeill 7:1add29d51e72 2260 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2261 &LimitCheckValue);
Charles MacNeill 7:1add29d51e72 2262
Charles MacNeill 7:1add29d51e72 2263 SigmaLimitflag = (FilteredRangeStatus ==
Charles MacNeill 7:1add29d51e72 2264 VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK)
Charles MacNeill 7:1add29d51e72 2265 ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2266
Charles MacNeill 7:1add29d51e72 2267 VL53L1_GetLimitCheckEnable(Dev,
Charles MacNeill 7:1add29d51e72 2268 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2269 &Temp8Enable);
Charles MacNeill 7:1add29d51e72 2270
Charles MacNeill 7:1add29d51e72 2271 Temp8 = ((Temp8Enable == 1) && (SigmaLimitflag == 1)) ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2272 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
Charles MacNeill 7:1add29d51e72 2273 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
Charles MacNeill 7:1add29d51e72 2274
Charles MacNeill 7:1add29d51e72 2275
Charles MacNeill 7:1add29d51e72 2276 VL53L1_GetLimitCheckValue(Dev,
Charles MacNeill 7:1add29d51e72 2277 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2278 &LimitCheckValue);
Charles MacNeill 7:1add29d51e72 2279
Charles MacNeill 7:1add29d51e72 2280 SignalLimitflag = (FilteredRangeStatus ==
Charles MacNeill 7:1add29d51e72 2281 VL53L1_DEVICEERROR_MSRCNOTARGET)
Charles MacNeill 7:1add29d51e72 2282 ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2283
Charles MacNeill 7:1add29d51e72 2284 VL53L1_GetLimitCheckEnable(Dev,
Charles MacNeill 7:1add29d51e72 2285 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Charles MacNeill 7:1add29d51e72 2286 &Temp8Enable);
Charles MacNeill 7:1add29d51e72 2287
Charles MacNeill 7:1add29d51e72 2288 Temp8 = ((Temp8Enable == 1) && (SignalLimitflag == 1)) ? 1 : 0;
Charles MacNeill 7:1add29d51e72 2289 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
Charles MacNeill 7:1add29d51e72 2290 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, Temp8);
Charles MacNeill 7:1add29d51e72 2291
Charles MacNeill 7:1add29d51e72 2292 Range = pRangeData->RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 2293 if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
Charles MacNeill 7:1add29d51e72 2294 (Range < 0)) {
Charles MacNeill 7:1add29d51e72 2295 if (Range < BDTable[VL53L1_TUNING_PROXY_MIN])
Charles MacNeill 7:1add29d51e72 2296 pRangeData->RangeStatus =
Charles MacNeill 7:1add29d51e72 2297 VL53L1_RANGESTATUS_RANGE_INVALID;
Charles MacNeill 7:1add29d51e72 2298 else
Charles MacNeill 7:1add29d51e72 2299 pRangeData->RangeMilliMeter = 0;
Charles MacNeill 7:1add29d51e72 2300 }
Charles MacNeill 7:1add29d51e72 2301
Charles MacNeill 7:1add29d51e72 2302
lugandc 18:0696efe39d08 2303 VL53L1_get_tuning_parm(Dev,
lugandc 18:0696efe39d08 2304 VL53L1_TUNINGPARM_MIN_SIGNAL_SECONDARY_TARGETS,
lugandc 18:0696efe39d08 2305 &TuningMinValidSignal);
lugandc 18:0696efe39d08 2306 MinValidSignal = (FixPoint1616_t)TuningMinValidSignal;
lugandc 18:0696efe39d08 2307 if ((iteration > 0) &&
lugandc 18:0696efe39d08 2308 (pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
lugandc 18:0696efe39d08 2309 (pRangeData->SignalRateRtnMegaCps < MinValidSignal))
lugandc 18:0696efe39d08 2310 pRangeData->RangeStatus =
lugandc 18:0696efe39d08 2311 VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL;
lugandc 18:0696efe39d08 2312
lugandc 18:0696efe39d08 2313
lugandc 18:0696efe39d08 2314
Charles MacNeill 7:1add29d51e72 2315
Charles MacNeill 7:1add29d51e72 2316 VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_UWR_ENABLE,
Charles MacNeill 7:1add29d51e72 2317 &ExtendedRangeEnabled);
Charles MacNeill 7:1add29d51e72 2318
Charles MacNeill 7:1add29d51e72 2319 sequency = streamcount % 2;
Charles MacNeill 7:1add29d51e72 2320 uwr_status = 0;
Charles MacNeill 7:1add29d51e72 2321 RangeMillimeterInit = pRangeData->RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 2322 AddOffset = 0;
Charles MacNeill 7:1add29d51e72 2323
Charles MacNeill 7:1add29d51e72 2324 pRangeData->ExtendedRange = 0;
Charles MacNeill 7:1add29d51e72 2325
Charles MacNeill 7:1add29d51e72 2326 if (ExtendedRangeEnabled &&
Charles MacNeill 7:1add29d51e72 2327 (pRangeData->RangeStatus ==
Charles MacNeill 7:1add29d51e72 2328 VL53L1_RANGESTATUS_WRAP_TARGET_FAIL ||
Charles MacNeill 7:1add29d51e72 2329 pRangeData->RangeStatus ==
Charles MacNeill 7:1add29d51e72 2330 VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL)
Charles MacNeill 7:1add29d51e72 2331 && (pdev->PreviousRangeStatus[iteration] ==
Charles MacNeill 7:1add29d51e72 2332 VL53L1_RANGESTATUS_WRAP_TARGET_FAIL ||
Charles MacNeill 7:1add29d51e72 2333 pdev->PreviousRangeStatus[iteration] ==
Charles MacNeill 7:1add29d51e72 2334 VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL ||
Charles MacNeill 7:1add29d51e72 2335 (pdev->PreviousRangeStatus[iteration] ==
Charles MacNeill 7:1add29d51e72 2336 VL53L1_RANGESTATUS_RANGE_VALID &&
Charles MacNeill 7:1add29d51e72 2337 pdev->PreviousExtendedRange[iteration] == 1)))
Charles MacNeill 7:1add29d51e72 2338 {
Charles MacNeill 7:1add29d51e72 2339 if (((pdev->PreviousStreamCount) ==
Charles MacNeill 7:1add29d51e72 2340 (pdev->hist_data.result__stream_count - 1 ))
Charles MacNeill 7:1add29d51e72 2341 || ((pdev->PreviousStreamCount) ==
Charles MacNeill 7:1add29d51e72 2342 (pdev->hist_data.result__stream_count + 127)))
Charles MacNeill 7:1add29d51e72 2343 {
Charles MacNeill 7:1add29d51e72 2344 RangeDiff = pRangeData->RangeMilliMeter -
Charles MacNeill 7:1add29d51e72 2345 pdev->PreviousRangeMilliMeter[iteration];
Charles MacNeill 7:1add29d51e72 2346
Charles MacNeill 7:1add29d51e72 2347 uwr_status = 1;
Charles MacNeill 7:1add29d51e72 2348 switch (pdev->preset_mode) {
Charles MacNeill 7:1add29d51e72 2349 case VL53L1_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE:
Charles MacNeill 7:1add29d51e72 2350
Charles MacNeill 7:1add29d51e72 2351 uwr_status = 0;
Charles MacNeill 7:1add29d51e72 2352 break;
Charles MacNeill 7:1add29d51e72 2353
Charles MacNeill 7:1add29d51e72 2354 case VL53L1_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE:
Charles MacNeill 7:1add29d51e72 2355 if (RangeDiff > tp->tp_uwr_med_z_1_min &&
Charles MacNeill 7:1add29d51e72 2356 RangeDiff < tp->tp_uwr_med_z_1_max &&
Charles MacNeill 7:1add29d51e72 2357 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2358 AddOffset =
Charles MacNeill 7:1add29d51e72 2359 tp->tp_uwr_med_corr_z_1_rangeb;
Charles MacNeill 7:1add29d51e72 2360 }
Charles MacNeill 7:1add29d51e72 2361 else
Charles MacNeill 7:1add29d51e72 2362 if (RangeDiff < -tp->tp_uwr_med_z_1_min &&
Charles MacNeill 7:1add29d51e72 2363 RangeDiff > -tp->tp_uwr_med_z_1_max &&
Charles MacNeill 7:1add29d51e72 2364 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2365 AddOffset =
Charles MacNeill 7:1add29d51e72 2366 tp->tp_uwr_med_corr_z_1_rangea;
Charles MacNeill 7:1add29d51e72 2367 }
Charles MacNeill 7:1add29d51e72 2368 else
Charles MacNeill 7:1add29d51e72 2369 if (RangeDiff > tp->tp_uwr_med_z_2_min &&
Charles MacNeill 7:1add29d51e72 2370 RangeDiff < tp->tp_uwr_med_z_2_max &&
Charles MacNeill 7:1add29d51e72 2371 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2372 AddOffset =
Charles MacNeill 7:1add29d51e72 2373 tp->tp_uwr_med_corr_z_2_rangea;
Charles MacNeill 7:1add29d51e72 2374 }
Charles MacNeill 7:1add29d51e72 2375 else
Charles MacNeill 7:1add29d51e72 2376 if (RangeDiff < -tp->tp_uwr_med_z_2_min &&
Charles MacNeill 7:1add29d51e72 2377 RangeDiff > -tp->tp_uwr_med_z_2_max &&
Charles MacNeill 7:1add29d51e72 2378 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2379 AddOffset =
Charles MacNeill 7:1add29d51e72 2380 tp->tp_uwr_med_corr_z_2_rangeb;
Charles MacNeill 7:1add29d51e72 2381 }
Charles MacNeill 7:1add29d51e72 2382 else
Charles MacNeill 7:1add29d51e72 2383 if (RangeDiff > tp->tp_uwr_med_z_3_min &&
Charles MacNeill 7:1add29d51e72 2384 RangeDiff < tp->tp_uwr_med_z_3_max &&
Charles MacNeill 7:1add29d51e72 2385 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2386 AddOffset =
Charles MacNeill 7:1add29d51e72 2387 tp->tp_uwr_med_corr_z_3_rangeb;
Charles MacNeill 7:1add29d51e72 2388 }
Charles MacNeill 7:1add29d51e72 2389 else
Charles MacNeill 7:1add29d51e72 2390 if (RangeDiff < -tp->tp_uwr_med_z_3_min &&
Charles MacNeill 7:1add29d51e72 2391 RangeDiff > -tp->tp_uwr_med_z_3_max &&
Charles MacNeill 7:1add29d51e72 2392 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2393 AddOffset =
Charles MacNeill 7:1add29d51e72 2394 tp->tp_uwr_med_corr_z_3_rangea;
Charles MacNeill 7:1add29d51e72 2395 }
Charles MacNeill 7:1add29d51e72 2396 else
Charles MacNeill 7:1add29d51e72 2397 if (RangeDiff > tp->tp_uwr_med_z_4_min &&
Charles MacNeill 7:1add29d51e72 2398 RangeDiff < tp->tp_uwr_med_z_4_max &&
Charles MacNeill 7:1add29d51e72 2399 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2400 AddOffset =
Charles MacNeill 7:1add29d51e72 2401 tp->tp_uwr_med_corr_z_4_rangea;
Charles MacNeill 7:1add29d51e72 2402 }
Charles MacNeill 7:1add29d51e72 2403 else
Charles MacNeill 7:1add29d51e72 2404 if (RangeDiff < -tp->tp_uwr_med_z_4_min &&
Charles MacNeill 7:1add29d51e72 2405 RangeDiff > -tp->tp_uwr_med_z_4_max &&
Charles MacNeill 7:1add29d51e72 2406 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2407 AddOffset =
Charles MacNeill 7:1add29d51e72 2408 tp->tp_uwr_med_corr_z_4_rangeb;
Charles MacNeill 7:1add29d51e72 2409 }
Charles MacNeill 7:1add29d51e72 2410 else
Charles MacNeill 7:1add29d51e72 2411 if (RangeDiff < tp->tp_uwr_med_z_5_max &&
Charles MacNeill 7:1add29d51e72 2412 RangeDiff > tp->tp_uwr_med_z_5_min) {
Charles MacNeill 7:1add29d51e72 2413 AddOffset =
Charles MacNeill 7:1add29d51e72 2414 tp->tp_uwr_med_corr_z_5_rangea;
Charles MacNeill 7:1add29d51e72 2415 }
Charles MacNeill 7:1add29d51e72 2416 else
Charles MacNeill 7:1add29d51e72 2417 if (RangeDiff > tp->tp_uwr_med_z_6_min &&
Charles MacNeill 7:1add29d51e72 2418 RangeDiff < tp->tp_uwr_med_z_6_max &&
Charles MacNeill 7:1add29d51e72 2419 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2420 AddOffset =
Charles MacNeill 7:1add29d51e72 2421 tp->tp_uwr_med_corr_z_6_rangeb;
Charles MacNeill 7:1add29d51e72 2422 }
Charles MacNeill 7:1add29d51e72 2423 else
Charles MacNeill 7:1add29d51e72 2424 if (RangeDiff < -tp->tp_uwr_med_z_6_min &&
Charles MacNeill 7:1add29d51e72 2425 RangeDiff > -tp->tp_uwr_med_z_6_max &&
Charles MacNeill 7:1add29d51e72 2426 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2427 AddOffset =
Charles MacNeill 7:1add29d51e72 2428 tp->tp_uwr_med_corr_z_6_rangea;
Charles MacNeill 7:1add29d51e72 2429 }
Charles MacNeill 7:1add29d51e72 2430 else
Charles MacNeill 7:1add29d51e72 2431 uwr_status = 0;
Charles MacNeill 7:1add29d51e72 2432 break;
Charles MacNeill 7:1add29d51e72 2433
Charles MacNeill 7:1add29d51e72 2434 case VL53L1_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE:
Charles MacNeill 7:1add29d51e72 2435 if (RangeDiff > tp->tp_uwr_lng_z_1_min &&
Charles MacNeill 7:1add29d51e72 2436 RangeDiff < tp->tp_uwr_lng_z_1_max &&
Charles MacNeill 7:1add29d51e72 2437 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2438 AddOffset =
Charles MacNeill 7:1add29d51e72 2439 tp->tp_uwr_lng_corr_z_1_rangea;
Charles MacNeill 7:1add29d51e72 2440 }
Charles MacNeill 7:1add29d51e72 2441 else
Charles MacNeill 7:1add29d51e72 2442 if (RangeDiff < -tp->tp_uwr_lng_z_1_min &&
Charles MacNeill 7:1add29d51e72 2443 RangeDiff > -tp->tp_uwr_lng_z_1_max &&
Charles MacNeill 7:1add29d51e72 2444 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2445 AddOffset =
Charles MacNeill 7:1add29d51e72 2446 tp->tp_uwr_lng_corr_z_1_rangeb;
Charles MacNeill 7:1add29d51e72 2447 }
Charles MacNeill 7:1add29d51e72 2448 else
Charles MacNeill 7:1add29d51e72 2449 if (RangeDiff > tp->tp_uwr_lng_z_2_min &&
Charles MacNeill 7:1add29d51e72 2450 RangeDiff < tp->tp_uwr_lng_z_2_max &&
Charles MacNeill 7:1add29d51e72 2451 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2452 AddOffset =
Charles MacNeill 7:1add29d51e72 2453 tp->tp_uwr_lng_corr_z_2_rangeb;
Charles MacNeill 7:1add29d51e72 2454 }
Charles MacNeill 7:1add29d51e72 2455 else
Charles MacNeill 7:1add29d51e72 2456 if (RangeDiff < -tp->tp_uwr_lng_z_2_min &&
Charles MacNeill 7:1add29d51e72 2457 RangeDiff > -tp->tp_uwr_lng_z_2_max &&
Charles MacNeill 7:1add29d51e72 2458 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2459 AddOffset =
Charles MacNeill 7:1add29d51e72 2460 tp->tp_uwr_lng_corr_z_2_rangea;
Charles MacNeill 7:1add29d51e72 2461 }
Charles MacNeill 7:1add29d51e72 2462 else
Charles MacNeill 7:1add29d51e72 2463 if (RangeDiff < tp->tp_uwr_lng_z_3_max &&
Charles MacNeill 7:1add29d51e72 2464 RangeDiff > tp->tp_uwr_lng_z_3_min) {
Charles MacNeill 7:1add29d51e72 2465 AddOffset =
Charles MacNeill 7:1add29d51e72 2466 tp->tp_uwr_lng_corr_z_3_rangea;
Charles MacNeill 7:1add29d51e72 2467 }
Charles MacNeill 7:1add29d51e72 2468 else
Charles MacNeill 7:1add29d51e72 2469 if (RangeDiff > tp->tp_uwr_lng_z_4_min &&
Charles MacNeill 7:1add29d51e72 2470 RangeDiff < tp->tp_uwr_lng_z_4_max &&
Charles MacNeill 7:1add29d51e72 2471 sequency == 1) {
Charles MacNeill 7:1add29d51e72 2472 AddOffset =
Charles MacNeill 7:1add29d51e72 2473 tp->tp_uwr_lng_corr_z_4_rangeb;
Charles MacNeill 7:1add29d51e72 2474 }
Charles MacNeill 7:1add29d51e72 2475 else
Charles MacNeill 7:1add29d51e72 2476 if (RangeDiff < -tp->tp_uwr_lng_z_4_min &&
Charles MacNeill 7:1add29d51e72 2477 RangeDiff > -tp->tp_uwr_lng_z_4_max &&
Charles MacNeill 7:1add29d51e72 2478 sequency == 0) {
Charles MacNeill 7:1add29d51e72 2479 AddOffset =
Charles MacNeill 7:1add29d51e72 2480 tp->tp_uwr_lng_corr_z_4_rangea;
Charles MacNeill 7:1add29d51e72 2481 }
Charles MacNeill 7:1add29d51e72 2482 else
Charles MacNeill 7:1add29d51e72 2483 uwr_status = 0;
Charles MacNeill 7:1add29d51e72 2484 break;
Charles MacNeill 7:1add29d51e72 2485
Charles MacNeill 7:1add29d51e72 2486 default:
Charles MacNeill 7:1add29d51e72 2487 uwr_status = 0;
Charles MacNeill 7:1add29d51e72 2488 break;
Charles MacNeill 7:1add29d51e72 2489 }
Charles MacNeill 7:1add29d51e72 2490 }
Charles MacNeill 7:1add29d51e72 2491
Charles MacNeill 7:1add29d51e72 2492 if (uwr_status) {
Charles MacNeill 7:1add29d51e72 2493 pRangeData->RangeMilliMeter += AddOffset;
Charles MacNeill 7:1add29d51e72 2494 pRangeData->RangeMinMilliMeter += AddOffset;
Charles MacNeill 7:1add29d51e72 2495 pRangeData->RangeMaxMilliMeter += AddOffset;
Charles MacNeill 7:1add29d51e72 2496 pRangeData->ExtendedRange = 1;
Charles MacNeill 7:1add29d51e72 2497 pRangeData->RangeStatus = 0;
Charles MacNeill 7:1add29d51e72 2498 }
Charles MacNeill 7:1add29d51e72 2499
Charles MacNeill 7:1add29d51e72 2500 }
Charles MacNeill 7:1add29d51e72 2501
Charles MacNeill 7:1add29d51e72 2502 pdev->PreviousRangeMilliMeter[iteration] = RangeMillimeterInit;
Charles MacNeill 7:1add29d51e72 2503 pdev->PreviousRangeStatus[iteration] = pRangeData->RangeStatus;
Charles MacNeill 7:1add29d51e72 2504 pdev->PreviousExtendedRange[iteration] = pRangeData->ExtendedRange;
Charles MacNeill 7:1add29d51e72 2505
Charles MacNeill 7:1add29d51e72 2506 return Status;
Charles MacNeill 7:1add29d51e72 2507 }
Charles MacNeill 7:1add29d51e72 2508
Charles MacNeill 7:1add29d51e72 2509 static uint8_t GetOutputDataIndex(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2510 VL53L1_range_results_t *presults)
Charles MacNeill 7:1add29d51e72 2511 {
Charles MacNeill 7:1add29d51e72 2512 uint8_t i;
Charles MacNeill 7:1add29d51e72 2513 uint8_t index = 0;
Charles MacNeill 7:1add29d51e72 2514 VL53L1_OutputModes OutputMode;
Charles MacNeill 7:1add29d51e72 2515
Charles MacNeill 7:1add29d51e72 2516 OutputMode = VL53L1DevDataGet(Dev, CurrentParameters.OutputMode);
Charles MacNeill 7:1add29d51e72 2517
Charles MacNeill 7:1add29d51e72 2518
Charles MacNeill 7:1add29d51e72 2519 if (OutputMode == VL53L1_OUTPUTMODE_NEAREST)
Charles MacNeill 7:1add29d51e72 2520 return 0;
Charles MacNeill 7:1add29d51e72 2521
Charles MacNeill 7:1add29d51e72 2522
Charles MacNeill 7:1add29d51e72 2523 for (i = 1; i < presults->active_results; i++) {
Charles MacNeill 7:1add29d51e72 2524 if (presults->VL53L1_p_002[i].peak_signal_count_rate_mcps >
Charles MacNeill 7:1add29d51e72 2525 presults->VL53L1_p_002[index].peak_signal_count_rate_mcps)
Charles MacNeill 7:1add29d51e72 2526 index = i;
Charles MacNeill 7:1add29d51e72 2527 }
Charles MacNeill 7:1add29d51e72 2528
Charles MacNeill 7:1add29d51e72 2529 return index;
Charles MacNeill 7:1add29d51e72 2530 }
Charles MacNeill 7:1add29d51e72 2531
Charles MacNeill 7:1add29d51e72 2532 VL53L1_Error VL53L1_GetRangingMeasurementData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2533 VL53L1_RangingMeasurementData_t *pRangingMeasurementData)
Charles MacNeill 7:1add29d51e72 2534 {
Charles MacNeill 7:1add29d51e72 2535 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2536 VL53L1_LLDriverData_t *pdev =
Charles MacNeill 7:1add29d51e72 2537 VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 2538 VL53L1_range_results_t *presults =
Charles MacNeill 7:1add29d51e72 2539 (VL53L1_range_results_t *) pdev->wArea1;
Charles MacNeill 7:1add29d51e72 2540 VL53L1_range_data_t *presults_data;
Charles MacNeill 7:1add29d51e72 2541 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 2542 uint8_t index = 0;
Charles MacNeill 7:1add29d51e72 2543
Charles MacNeill 7:1add29d51e72 2544 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2545
Charles MacNeill 7:1add29d51e72 2546
Charles MacNeill 7:1add29d51e72 2547 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 2548
Charles MacNeill 7:1add29d51e72 2549 if (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) {
Charles MacNeill 7:1add29d51e72 2550 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 2551 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2552 return Status;
Charles MacNeill 7:1add29d51e72 2553 }
Charles MacNeill 7:1add29d51e72 2554
Charles MacNeill 7:1add29d51e72 2555
Charles MacNeill 7:1add29d51e72 2556 memset(pRangingMeasurementData, 0xFF,
Charles MacNeill 7:1add29d51e72 2557 sizeof(VL53L1_RangingMeasurementData_t));
Charles MacNeill 7:1add29d51e72 2558
Charles MacNeill 7:1add29d51e72 2559
Charles MacNeill 7:1add29d51e72 2560 Status = VL53L1_get_device_results(
Charles MacNeill 7:1add29d51e72 2561 Dev,
Charles MacNeill 7:1add29d51e72 2562 VL53L1_DEVICERESULTSLEVEL_FULL,
Charles MacNeill 7:1add29d51e72 2563 presults);
Charles MacNeill 7:1add29d51e72 2564
Charles MacNeill 7:1add29d51e72 2565 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 2566 pRangingMeasurementData->StreamCount = presults->stream_count;
Charles MacNeill 7:1add29d51e72 2567
Charles MacNeill 7:1add29d51e72 2568
Charles MacNeill 7:1add29d51e72 2569 index = GetOutputDataIndex(Dev, presults);
Charles MacNeill 7:1add29d51e72 2570 presults_data = &(presults->VL53L1_p_002[index]);
Charles MacNeill 7:1add29d51e72 2571 Status = SetSimpleData(Dev, presults->active_results,
Charles MacNeill 7:1add29d51e72 2572 presults->device_status,
Charles MacNeill 7:1add29d51e72 2573 presults_data,
Charles MacNeill 7:1add29d51e72 2574 pRangingMeasurementData);
Charles MacNeill 7:1add29d51e72 2575 }
Charles MacNeill 7:1add29d51e72 2576
Charles MacNeill 7:1add29d51e72 2577 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2578 return Status;
Charles MacNeill 7:1add29d51e72 2579 }
Charles MacNeill 7:1add29d51e72 2580
Charles MacNeill 7:1add29d51e72 2581 static VL53L1_Error SetMeasurementData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2582 VL53L1_range_results_t *presults,
Charles MacNeill 7:1add29d51e72 2583 VL53L1_MultiRangingData_t *pMultiRangingData)
Charles MacNeill 7:1add29d51e72 2584 {
Charles MacNeill 7:1add29d51e72 2585 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 2586 uint8_t i;
Charles MacNeill 7:1add29d51e72 2587 uint8_t iteration;
Charles MacNeill 7:1add29d51e72 2588 VL53L1_TargetRangeData_t *pRangeData;
Charles MacNeill 7:1add29d51e72 2589 VL53L1_range_data_t *presults_data;
Charles MacNeill 7:1add29d51e72 2590 int16_t dmax_min;
Charles MacNeill 7:1add29d51e72 2591 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2592 uint8_t Furthest_idx = 0;
Charles MacNeill 7:1add29d51e72 2593 int16_t Furthest_range = 0;
Charles MacNeill 7:1add29d51e72 2594 uint8_t ActiveResults, amb_idx;
Charles MacNeill 7:1add29d51e72 2595
Charles MacNeill 7:1add29d51e72 2596 pMultiRangingData->NumberOfObjectsFound = presults->active_results;
Charles MacNeill 7:1add29d51e72 2597 pMultiRangingData->RoiNumber = presults->zone_id;
Charles MacNeill 7:1add29d51e72 2598 pMultiRangingData->HasXtalkValueChanged =
Charles MacNeill 7:1add29d51e72 2599 presults->smudge_corrector_data.new_xtalk_applied_flag;
Charles MacNeill 7:1add29d51e72 2600 dmax_min = MIN(presults->wrap_dmax_mm,
Charles MacNeill 7:1add29d51e72 2601 presults->VL53L1_p_007[DMAX_REFLECTANCE_IDX]);
Charles MacNeill 7:1add29d51e72 2602 pMultiRangingData->DmaxMilliMeter = dmax_min;
Charles MacNeill 7:1add29d51e72 2603
Charles MacNeill 7:1add29d51e72 2604
Charles MacNeill 7:1add29d51e72 2605 pMultiRangingData->TimeStamp = 0;
Charles MacNeill 7:1add29d51e72 2606
Charles MacNeill 7:1add29d51e72 2607 pMultiRangingData->StreamCount = presults->stream_count;
Charles MacNeill 7:1add29d51e72 2608
Charles MacNeill 7:1add29d51e72 2609 pMultiRangingData->RecommendedDistanceMode =
Charles MacNeill 7:1add29d51e72 2610 VL53L1DevDataGet(Dev, CurrentParameters.DistanceMode);
Charles MacNeill 7:1add29d51e72 2611 ActiveResults = presults->active_results;
Charles MacNeill 7:1add29d51e72 2612 if (ActiveResults < 1)
Charles MacNeill 7:1add29d51e72 2613
Charles MacNeill 7:1add29d51e72 2614 iteration = 1;
Charles MacNeill 7:1add29d51e72 2615 else
Charles MacNeill 7:1add29d51e72 2616 iteration = ActiveResults;
Charles MacNeill 7:1add29d51e72 2617 for (i = 0; i < iteration; i++) {
Charles MacNeill 7:1add29d51e72 2618 pRangeData = &(pMultiRangingData->RangeData[i]);
Charles MacNeill 7:1add29d51e72 2619
Charles MacNeill 7:1add29d51e72 2620 presults_data = &(presults->VL53L1_p_002[i]);
Charles MacNeill 7:1add29d51e72 2621 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 2622 Status = SetTargetData(Dev, ActiveResults,
Charles MacNeill 7:1add29d51e72 2623 pMultiRangingData->StreamCount,
Charles MacNeill 7:1add29d51e72 2624 i,
Charles MacNeill 7:1add29d51e72 2625 presults->device_status,
Charles MacNeill 7:1add29d51e72 2626 presults_data,
Charles MacNeill 7:1add29d51e72 2627 pRangeData);
Charles MacNeill 7:1add29d51e72 2628
Charles MacNeill 7:1add29d51e72 2629 pMultiRangingData->EffectiveSpadRtnCount =
Charles MacNeill 7:1add29d51e72 2630 presults_data->VL53L1_p_006;
Charles MacNeill 7:1add29d51e72 2631
Charles MacNeill 7:1add29d51e72 2632 if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID)
Charles MacNeill 7:1add29d51e72 2633 && (pRangeData->RangeMilliMeter > Furthest_range)) {
Charles MacNeill 7:1add29d51e72 2634 Furthest_range = pRangeData->RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 2635 Furthest_idx = i;
Charles MacNeill 7:1add29d51e72 2636 }
Charles MacNeill 7:1add29d51e72 2637 }
Charles MacNeill 7:1add29d51e72 2638 pdev->PreviousStreamCount = pdev->hist_data.result__stream_count;
Charles MacNeill 7:1add29d51e72 2639 for (i = iteration; i < VL53L1_MAX_RANGE_RESULTS; i++) {
Charles MacNeill 7:1add29d51e72 2640 pdev->PreviousRangeMilliMeter[i] = 0;
Charles MacNeill 7:1add29d51e72 2641 pdev->PreviousRangeStatus[i] = 255;
Charles MacNeill 7:1add29d51e72 2642 pdev->PreviousExtendedRange[i] = 0;
Charles MacNeill 7:1add29d51e72 2643 }
Charles MacNeill 7:1add29d51e72 2644
Charles MacNeill 7:1add29d51e72 2645 if ((Status == VL53L1_ERROR_NONE) && (ActiveResults > 0)) {
Charles MacNeill 7:1add29d51e72 2646 pRangeData = &(pMultiRangingData->RangeData[Furthest_idx]);
Charles MacNeill 7:1add29d51e72 2647 amb_idx = VL53L1_MAX_AMBIENT_DMAX_VALUES-1;
Charles MacNeill 7:1add29d51e72 2648 CheckAndChangeDistanceMode(Dev, pRangeData,
Charles MacNeill 7:1add29d51e72 2649 presults->VL53L1_p_007[amb_idx],
Charles MacNeill 7:1add29d51e72 2650 &pMultiRangingData->RecommendedDistanceMode);
Charles MacNeill 7:1add29d51e72 2651 }
Charles MacNeill 7:1add29d51e72 2652
Charles MacNeill 7:1add29d51e72 2653 return Status;
Charles MacNeill 7:1add29d51e72 2654 }
Charles MacNeill 7:1add29d51e72 2655
Charles MacNeill 7:1add29d51e72 2656 VL53L1_Error VL53L1_GetMultiRangingData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2657 VL53L1_MultiRangingData_t *pMultiRangingData)
Charles MacNeill 7:1add29d51e72 2658 {
Charles MacNeill 7:1add29d51e72 2659 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2660 VL53L1_LLDriverData_t *pdev =
Charles MacNeill 7:1add29d51e72 2661 VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 2662 VL53L1_range_results_t *presults =
Charles MacNeill 7:1add29d51e72 2663 (VL53L1_range_results_t *) pdev->wArea1;
Charles MacNeill 7:1add29d51e72 2664
Charles MacNeill 7:1add29d51e72 2665 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2666
Charles MacNeill 7:1add29d51e72 2667
Charles MacNeill 7:1add29d51e72 2668 memset(pMultiRangingData, 0xFF,
Charles MacNeill 7:1add29d51e72 2669 sizeof(VL53L1_MultiRangingData_t));
Charles MacNeill 7:1add29d51e72 2670
Charles MacNeill 7:1add29d51e72 2671
Charles MacNeill 7:1add29d51e72 2672 Status = VL53L1_get_device_results(
Charles MacNeill 7:1add29d51e72 2673 Dev,
Charles MacNeill 7:1add29d51e72 2674 VL53L1_DEVICERESULTSLEVEL_FULL,
Charles MacNeill 7:1add29d51e72 2675 presults);
Charles MacNeill 7:1add29d51e72 2676
Charles MacNeill 7:1add29d51e72 2677
Charles MacNeill 7:1add29d51e72 2678 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 2679 switch (presults->rd_device_state) {
Charles MacNeill 7:1add29d51e72 2680 case VL53L1_DEVICESTATE_RANGING_GATHER_DATA:
Charles MacNeill 7:1add29d51e72 2681 pMultiRangingData->RoiStatus =
Charles MacNeill 7:1add29d51e72 2682 VL53L1_ROISTATUS_VALID_NOT_LAST;
Charles MacNeill 7:1add29d51e72 2683 break;
Charles MacNeill 7:1add29d51e72 2684 case VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA:
Charles MacNeill 7:1add29d51e72 2685 pMultiRangingData->RoiStatus =
Charles MacNeill 7:1add29d51e72 2686 VL53L1_ROISTATUS_VALID_LAST;
Charles MacNeill 7:1add29d51e72 2687 break;
Charles MacNeill 7:1add29d51e72 2688 default:
Charles MacNeill 7:1add29d51e72 2689 pMultiRangingData->RoiStatus =
Charles MacNeill 7:1add29d51e72 2690 VL53L1_ROISTATUS_NOT_VALID;
Charles MacNeill 7:1add29d51e72 2691 }
Charles MacNeill 7:1add29d51e72 2692
Charles MacNeill 7:1add29d51e72 2693 Status = SetMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 2694 presults,
Charles MacNeill 7:1add29d51e72 2695 pMultiRangingData);
Charles MacNeill 7:1add29d51e72 2696
Charles MacNeill 7:1add29d51e72 2697 }
Charles MacNeill 7:1add29d51e72 2698
Charles MacNeill 7:1add29d51e72 2699 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2700 return Status;
Charles MacNeill 7:1add29d51e72 2701 }
Charles MacNeill 7:1add29d51e72 2702
Charles MacNeill 7:1add29d51e72 2703 VL53L1_Error VL53L1_GetAdditionalData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2704 VL53L1_AdditionalData_t *pAdditionalData)
Charles MacNeill 7:1add29d51e72 2705 {
Charles MacNeill 7:1add29d51e72 2706 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2707
Charles MacNeill 7:1add29d51e72 2708 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2709
Charles MacNeill 7:1add29d51e72 2710 Status = VL53L1_get_additional_data(Dev, pAdditionalData);
Charles MacNeill 7:1add29d51e72 2711
Charles MacNeill 7:1add29d51e72 2712 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2713 return Status;
Charles MacNeill 7:1add29d51e72 2714 }
Charles MacNeill 7:1add29d51e72 2715
Charles MacNeill 7:1add29d51e72 2716
Charles MacNeill 7:1add29d51e72 2717
Charles MacNeill 7:1add29d51e72 2718
Charles MacNeill 7:1add29d51e72 2719
Charles MacNeill 7:1add29d51e72 2720
Charles MacNeill 7:1add29d51e72 2721 VL53L1_Error VL53L1_SetTuningParameter(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2722 uint16_t TuningParameterId, int32_t TuningParameterValue)
Charles MacNeill 7:1add29d51e72 2723 {
Charles MacNeill 7:1add29d51e72 2724 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2725
Charles MacNeill 7:1add29d51e72 2726 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2727
Charles MacNeill 7:1add29d51e72 2728 if (TuningParameterId ==
Charles MacNeill 7:1add29d51e72 2729 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS)
Charles MacNeill 7:1add29d51e72 2730 return VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 2731
Charles MacNeill 7:1add29d51e72 2732 if (TuningParameterId >= 32768)
Charles MacNeill 7:1add29d51e72 2733 Status = VL53L1_set_tuning_parm(Dev,
Charles MacNeill 7:1add29d51e72 2734 TuningParameterId,
Charles MacNeill 7:1add29d51e72 2735 TuningParameterValue);
Charles MacNeill 7:1add29d51e72 2736 else {
Charles MacNeill 7:1add29d51e72 2737 if (TuningParameterId < VL53L1_TUNING_MAX_TUNABLE_KEY)
Charles MacNeill 7:1add29d51e72 2738 BDTable[TuningParameterId] = TuningParameterValue;
Charles MacNeill 7:1add29d51e72 2739 else
Charles MacNeill 7:1add29d51e72 2740 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 2741 }
Charles MacNeill 7:1add29d51e72 2742
Charles MacNeill 7:1add29d51e72 2743 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2744 return Status;
Charles MacNeill 7:1add29d51e72 2745 }
Charles MacNeill 7:1add29d51e72 2746
Charles MacNeill 7:1add29d51e72 2747 VL53L1_Error VL53L1_GetTuningParameter(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2748 uint16_t TuningParameterId, int32_t *pTuningParameterValue)
Charles MacNeill 7:1add29d51e72 2749 {
Charles MacNeill 7:1add29d51e72 2750 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2751
Charles MacNeill 7:1add29d51e72 2752 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2753
Charles MacNeill 7:1add29d51e72 2754 if (TuningParameterId >= 32768)
Charles MacNeill 7:1add29d51e72 2755 Status = VL53L1_get_tuning_parm(Dev,
Charles MacNeill 7:1add29d51e72 2756 TuningParameterId,
Charles MacNeill 7:1add29d51e72 2757 pTuningParameterValue);
Charles MacNeill 7:1add29d51e72 2758 else {
Charles MacNeill 7:1add29d51e72 2759 if (TuningParameterId < VL53L1_TUNING_MAX_TUNABLE_KEY)
Charles MacNeill 7:1add29d51e72 2760 *pTuningParameterValue = BDTable[TuningParameterId];
Charles MacNeill 7:1add29d51e72 2761 else
Charles MacNeill 7:1add29d51e72 2762 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 2763 }
Charles MacNeill 7:1add29d51e72 2764
Charles MacNeill 7:1add29d51e72 2765 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2766 return Status;
Charles MacNeill 7:1add29d51e72 2767 }
Charles MacNeill 7:1add29d51e72 2768
Charles MacNeill 7:1add29d51e72 2769
Charles MacNeill 7:1add29d51e72 2770 VL53L1_Error VL53L1_PerformRefSpadManagement(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 2771 {
Charles MacNeill 7:1add29d51e72 2772 #ifdef VL53L1_NOCALIB
Charles MacNeill 7:1add29d51e72 2773 VL53L1_Error Status = VL53L1_ERROR_NOT_SUPPORTED;
Charles MacNeill 7:1add29d51e72 2774
Charles MacNeill 7:1add29d51e72 2775 SUPPRESS_UNUSED_WARNING(Dev);
Charles MacNeill 7:1add29d51e72 2776
Charles MacNeill 7:1add29d51e72 2777 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2778 #else
Charles MacNeill 7:1add29d51e72 2779 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2780 VL53L1_Error RawStatus;
Charles MacNeill 7:1add29d51e72 2781 uint8_t dcrbuffer[24];
Charles MacNeill 7:1add29d51e72 2782 uint8_t *commbuf;
Charles MacNeill 7:1add29d51e72 2783 uint8_t numloc[2] = {5, 3};
Charles MacNeill 7:1add29d51e72 2784 VL53L1_LLDriverData_t *pdev;
Charles MacNeill 7:1add29d51e72 2785 VL53L1_customer_nvm_managed_t *pc;
Charles MacNeill 7:1add29d51e72 2786 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 2787
Charles MacNeill 7:1add29d51e72 2788 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2789
Charles MacNeill 7:1add29d51e72 2790 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 2791 pc = &pdev->customer;
Charles MacNeill 7:1add29d51e72 2792
Charles MacNeill 7:1add29d51e72 2793 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 2794 PresetMode = VL53L1DevDataGet(Dev,
Charles MacNeill 7:1add29d51e72 2795 CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 2796 Status = VL53L1_run_ref_spad_char(Dev, &RawStatus);
Charles MacNeill 7:1add29d51e72 2797
Charles MacNeill 7:1add29d51e72 2798 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 2799 Status = VL53L1_SetPresetMode(Dev, PresetMode);
Charles MacNeill 7:1add29d51e72 2800 }
Charles MacNeill 7:1add29d51e72 2801
Charles MacNeill 7:1add29d51e72 2802 if (Status == VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH) {
Charles MacNeill 7:1add29d51e72 2803
Charles MacNeill 7:1add29d51e72 2804 Status = VL53L1_read_nvm_raw_data(Dev,
Charles MacNeill 7:1add29d51e72 2805 (uint8_t)(0xA0 >> 2),
Charles MacNeill 7:1add29d51e72 2806 (uint8_t)(24 >> 2),
Charles MacNeill 7:1add29d51e72 2807 dcrbuffer);
Charles MacNeill 7:1add29d51e72 2808
Charles MacNeill 7:1add29d51e72 2809 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 2810 Status = VL53L1_WriteMulti(Dev,
Charles MacNeill 7:1add29d51e72 2811 VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
Charles MacNeill 7:1add29d51e72 2812 numloc, 2);
Charles MacNeill 7:1add29d51e72 2813
Charles MacNeill 7:1add29d51e72 2814 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 2815 pc->ref_spad_man__num_requested_ref_spads = numloc[0];
Charles MacNeill 7:1add29d51e72 2816 pc->ref_spad_man__ref_location = numloc[1];
Charles MacNeill 7:1add29d51e72 2817 }
Charles MacNeill 7:1add29d51e72 2818
lugandc 18:0696efe39d08 2819 commbuf = &dcrbuffer[16];
Charles MacNeill 7:1add29d51e72 2820
Charles MacNeill 7:1add29d51e72 2821 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 2822 Status = VL53L1_WriteMulti(Dev,
Charles MacNeill 7:1add29d51e72 2823 VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
Charles MacNeill 7:1add29d51e72 2824 commbuf, 6);
Charles MacNeill 7:1add29d51e72 2825
Charles MacNeill 7:1add29d51e72 2826 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 2827 pc->global_config__spad_enables_ref_0 = commbuf[0];
Charles MacNeill 7:1add29d51e72 2828 pc->global_config__spad_enables_ref_1 = commbuf[1];
Charles MacNeill 7:1add29d51e72 2829 pc->global_config__spad_enables_ref_2 = commbuf[2];
Charles MacNeill 7:1add29d51e72 2830 pc->global_config__spad_enables_ref_3 = commbuf[3];
Charles MacNeill 7:1add29d51e72 2831 pc->global_config__spad_enables_ref_4 = commbuf[4];
Charles MacNeill 7:1add29d51e72 2832 pc->global_config__spad_enables_ref_5 = commbuf[5];
Charles MacNeill 7:1add29d51e72 2833 }
Charles MacNeill 7:1add29d51e72 2834
Charles MacNeill 7:1add29d51e72 2835 }
Charles MacNeill 7:1add29d51e72 2836
Charles MacNeill 7:1add29d51e72 2837 #endif
Charles MacNeill 7:1add29d51e72 2838
Charles MacNeill 7:1add29d51e72 2839 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2840 return Status;
Charles MacNeill 7:1add29d51e72 2841 }
Charles MacNeill 7:1add29d51e72 2842
Charles MacNeill 7:1add29d51e72 2843 VL53L1_Error VL53L1_SmudgeCorrectionEnable(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2844 VL53L1_SmudgeCorrectionModes Mode)
Charles MacNeill 7:1add29d51e72 2845 {
Charles MacNeill 7:1add29d51e72 2846 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2847 VL53L1_Error s1 = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2848 VL53L1_Error s2 = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2849 VL53L1_Error s3 = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2850
Charles MacNeill 7:1add29d51e72 2851 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2852
Charles MacNeill 7:1add29d51e72 2853 switch (Mode) {
Charles MacNeill 7:1add29d51e72 2854 case VL53L1_SMUDGE_CORRECTION_NONE:
Charles MacNeill 7:1add29d51e72 2855 s1 = VL53L1_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 7:1add29d51e72 2856 s2 = VL53L1_dynamic_xtalk_correction_apply_disable(Dev);
Charles MacNeill 7:1add29d51e72 2857 s3 = VL53L1_dynamic_xtalk_correction_single_apply_disable(Dev);
Charles MacNeill 7:1add29d51e72 2858 break;
Charles MacNeill 7:1add29d51e72 2859 case VL53L1_SMUDGE_CORRECTION_CONTINUOUS:
Charles MacNeill 7:1add29d51e72 2860 s1 = VL53L1_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 7:1add29d51e72 2861 s2 = VL53L1_dynamic_xtalk_correction_apply_enable(Dev);
Charles MacNeill 7:1add29d51e72 2862 s3 = VL53L1_dynamic_xtalk_correction_single_apply_disable(Dev);
Charles MacNeill 7:1add29d51e72 2863 break;
Charles MacNeill 7:1add29d51e72 2864 case VL53L1_SMUDGE_CORRECTION_SINGLE:
Charles MacNeill 7:1add29d51e72 2865 s1 = VL53L1_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 7:1add29d51e72 2866 s2 = VL53L1_dynamic_xtalk_correction_apply_enable(Dev);
Charles MacNeill 7:1add29d51e72 2867 s3 = VL53L1_dynamic_xtalk_correction_single_apply_enable(Dev);
Charles MacNeill 7:1add29d51e72 2868 break;
Charles MacNeill 7:1add29d51e72 2869 case VL53L1_SMUDGE_CORRECTION_DEBUG:
Charles MacNeill 7:1add29d51e72 2870 s1 = VL53L1_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 7:1add29d51e72 2871 s2 = VL53L1_dynamic_xtalk_correction_apply_disable(Dev);
Charles MacNeill 7:1add29d51e72 2872 s3 = VL53L1_dynamic_xtalk_correction_single_apply_disable(Dev);
Charles MacNeill 7:1add29d51e72 2873 break;
Charles MacNeill 7:1add29d51e72 2874 default:
Charles MacNeill 7:1add29d51e72 2875 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 2876 break;
Charles MacNeill 7:1add29d51e72 2877 }
Charles MacNeill 7:1add29d51e72 2878
Charles MacNeill 7:1add29d51e72 2879 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 2880 Status = s1;
Charles MacNeill 7:1add29d51e72 2881 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 2882 Status = s2;
Charles MacNeill 7:1add29d51e72 2883 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 2884 Status = s3;
Charles MacNeill 7:1add29d51e72 2885 }
Charles MacNeill 7:1add29d51e72 2886
Charles MacNeill 7:1add29d51e72 2887 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2888 return Status;
Charles MacNeill 7:1add29d51e72 2889 }
Charles MacNeill 7:1add29d51e72 2890
Charles MacNeill 7:1add29d51e72 2891 VL53L1_Error VL53L1_SetXTalkCompensationEnable(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2892 uint8_t XTalkCompensationEnable)
Charles MacNeill 7:1add29d51e72 2893 {
Charles MacNeill 7:1add29d51e72 2894 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2895
Charles MacNeill 7:1add29d51e72 2896 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2897
Charles MacNeill 7:1add29d51e72 2898 if (XTalkCompensationEnable == 0)
Charles MacNeill 7:1add29d51e72 2899 Status = VL53L1_disable_xtalk_compensation(Dev);
Charles MacNeill 7:1add29d51e72 2900 else
Charles MacNeill 7:1add29d51e72 2901 Status = VL53L1_enable_xtalk_compensation(Dev);
Charles MacNeill 7:1add29d51e72 2902
Charles MacNeill 7:1add29d51e72 2903 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2904 return Status;
Charles MacNeill 7:1add29d51e72 2905 }
Charles MacNeill 7:1add29d51e72 2906
Charles MacNeill 7:1add29d51e72 2907
Charles MacNeill 7:1add29d51e72 2908 VL53L1_Error VL53L1_GetXTalkCompensationEnable(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2909 uint8_t *pXTalkCompensationEnable)
Charles MacNeill 7:1add29d51e72 2910 {
Charles MacNeill 7:1add29d51e72 2911 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2912
Charles MacNeill 7:1add29d51e72 2913 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2914
Charles MacNeill 7:1add29d51e72 2915 VL53L1_get_xtalk_compensation_enable(
Charles MacNeill 7:1add29d51e72 2916 Dev,
Charles MacNeill 7:1add29d51e72 2917 pXTalkCompensationEnable);
Charles MacNeill 7:1add29d51e72 2918
Charles MacNeill 7:1add29d51e72 2919 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 2920 return Status;
Charles MacNeill 7:1add29d51e72 2921 }
Charles MacNeill 7:1add29d51e72 2922
Charles MacNeill 7:1add29d51e72 2923
Charles MacNeill 7:1add29d51e72 2924 VL53L1_Error VL53L1_PerformXTalkCalibration(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 2925 uint8_t CalibrationOption)
Charles MacNeill 7:1add29d51e72 2926 {
Charles MacNeill 7:1add29d51e72 2927 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 2928 VL53L1_Error UStatus;
Charles MacNeill 7:1add29d51e72 2929 int16_t CalDistanceMm;
Charles MacNeill 7:1add29d51e72 2930 VL53L1_xtalk_calibration_results_t xtalk;
Charles MacNeill 7:1add29d51e72 2931
Charles MacNeill 7:1add29d51e72 2932 VL53L1_CalibrationData_t caldata;
Charles MacNeill 7:1add29d51e72 2933 VL53L1_LLDriverData_t *pLLData;
Charles MacNeill 7:1add29d51e72 2934 int i;
Charles MacNeill 7:1add29d51e72 2935 uint32_t *pPlaneOffsetKcps;
Charles MacNeill 7:1add29d51e72 2936 uint32_t Margin =
Charles MacNeill 7:1add29d51e72 2937 BDTable[VL53L1_TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN];
Charles MacNeill 7:1add29d51e72 2938 uint32_t DefaultOffset =
Charles MacNeill 7:1add29d51e72 2939 BDTable[VL53L1_TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET];
Charles MacNeill 7:1add29d51e72 2940 uint32_t *pLLDataPlaneOffsetKcps;
Charles MacNeill 7:1add29d51e72 2941 uint32_t sum = 0;
Charles MacNeill 7:1add29d51e72 2942 uint8_t binok = 0;
Charles MacNeill 7:1add29d51e72 2943 int32_t merge;
Charles MacNeill 7:1add29d51e72 2944
Charles MacNeill 7:1add29d51e72 2945 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 2946
Charles MacNeill 7:1add29d51e72 2947 pPlaneOffsetKcps =
Charles MacNeill 7:1add29d51e72 2948 &caldata.customer.algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 7:1add29d51e72 2949 pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 2950 pLLDataPlaneOffsetKcps =
Charles MacNeill 7:1add29d51e72 2951 &pLLData->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 7:1add29d51e72 2952 VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, &merge);
Charles MacNeill 7:1add29d51e72 2953
Charles MacNeill 7:1add29d51e72 2954 VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, 0);
Charles MacNeill 7:1add29d51e72 2955 switch (CalibrationOption) {
Charles MacNeill 7:1add29d51e72 2956 case VL53L1_XTALKCALIBRATIONMODE_NO_TARGET:
Charles MacNeill 7:1add29d51e72 2957 Status = VL53L1_run_xtalk_extraction(Dev, &UStatus);
Charles MacNeill 7:1add29d51e72 2958
Charles MacNeill 7:1add29d51e72 2959 if (Status == VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL)
Charles MacNeill 7:1add29d51e72 2960 VL53L1_xtalk_cal_data_init(Dev);
Charles MacNeill 7:1add29d51e72 2961 break;
Charles MacNeill 7:1add29d51e72 2962 case VL53L1_XTALKCALIBRATIONMODE_SINGLE_TARGET:
Charles MacNeill 7:1add29d51e72 2963 Status = SingleTargetXTalkCalibration(Dev);
Charles MacNeill 7:1add29d51e72 2964 break;
Charles MacNeill 7:1add29d51e72 2965 case VL53L1_XTALKCALIBRATIONMODE_FULL_ROI:
Charles MacNeill 7:1add29d51e72 2966 CalDistanceMm = (int16_t)
Charles MacNeill 7:1add29d51e72 2967 BDTable[VL53L1_TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM];
Charles MacNeill 7:1add29d51e72 2968
Charles MacNeill 7:1add29d51e72 2969 VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
Charles MacNeill 7:1add29d51e72 2970 merge);
Charles MacNeill 7:1add29d51e72 2971 Status = VL53L1_run_hist_xtalk_extraction(Dev, CalDistanceMm,
Charles MacNeill 7:1add29d51e72 2972 &UStatus);
Charles MacNeill 7:1add29d51e72 2973
Charles MacNeill 7:1add29d51e72 2974 VL53L1_GetCalibrationData(Dev, &caldata);
Charles MacNeill 7:1add29d51e72 2975 for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++) {
Charles MacNeill 7:1add29d51e72 2976 sum += caldata.xtalkhisto.xtalk_shape.bin_data[i];
Charles MacNeill 7:1add29d51e72 2977 if (caldata.xtalkhisto.xtalk_shape.bin_data[i] > 0)
Charles MacNeill 7:1add29d51e72 2978 binok++;
Charles MacNeill 7:1add29d51e72 2979 }
Charles MacNeill 7:1add29d51e72 2980 if ((UStatus ==
Charles MacNeill 7:1add29d51e72 2981 VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL) ||
Charles MacNeill 7:1add29d51e72 2982 (sum > (1024 + Margin)) || (sum < (1024 - Margin)) ||
Charles MacNeill 7:1add29d51e72 2983 (binok < 3)) {
Charles MacNeill 7:1add29d51e72 2984 *pPlaneOffsetKcps = DefaultOffset;
Charles MacNeill 7:1add29d51e72 2985 *pLLDataPlaneOffsetKcps = DefaultOffset;
Charles MacNeill 7:1add29d51e72 2986 caldata.xtalkhisto.xtalk_shape.bin_data[0] = 307;
Charles MacNeill 7:1add29d51e72 2987 caldata.xtalkhisto.xtalk_shape.bin_data[1] = 410;
Charles MacNeill 7:1add29d51e72 2988 caldata.xtalkhisto.xtalk_shape.bin_data[2] = 410;
Charles MacNeill 7:1add29d51e72 2989 caldata.xtalkhisto.xtalk_shape.bin_data[3] = 307;
Charles MacNeill 7:1add29d51e72 2990 for (i = 4; i < VL53L1_XTALK_HISTO_BINS; i++)
Charles MacNeill 7:1add29d51e72 2991 caldata.xtalkhisto.xtalk_shape.bin_data[i] = 0;
Charles MacNeill 7:1add29d51e72 2992 for (i = 0; i < VL53L1_BIN_REC_SIZE; i++)
Charles MacNeill 7:1add29d51e72 2993 caldata.algo__xtalk_cpo_HistoMerge_kcps[i] =
Charles MacNeill 7:1add29d51e72 2994 DefaultOffset + DefaultOffset * i;
Charles MacNeill 7:1add29d51e72 2995 VL53L1_SetCalibrationData(Dev, &caldata);
Charles MacNeill 7:1add29d51e72 2996 }
Charles MacNeill 7:1add29d51e72 2997
Charles MacNeill 7:1add29d51e72 2998 break;
Charles MacNeill 7:1add29d51e72 2999 default:
Charles MacNeill 7:1add29d51e72 3000 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3001 }
Charles MacNeill 7:1add29d51e72 3002 VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, merge);
Charles MacNeill 7:1add29d51e72 3003
Charles MacNeill 7:1add29d51e72 3004 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3005 Status = VL53L1_get_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 7:1add29d51e72 3006 Status = VL53L1_set_tuning_parm(Dev,
Charles MacNeill 7:1add29d51e72 3007 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS,
Charles MacNeill 7:1add29d51e72 3008 xtalk.algo__crosstalk_compensation_plane_offset_kcps);
Charles MacNeill 7:1add29d51e72 3009 }
Charles MacNeill 7:1add29d51e72 3010
Charles MacNeill 7:1add29d51e72 3011 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3012 return Status;
Charles MacNeill 7:1add29d51e72 3013 }
Charles MacNeill 7:1add29d51e72 3014
Charles MacNeill 7:1add29d51e72 3015 VL53L1_Error VL53L1_SetOffsetCalibrationMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3016 VL53L1_OffsetCalibrationModes OffsetCalibrationMode)
Charles MacNeill 7:1add29d51e72 3017 {
Charles MacNeill 7:1add29d51e72 3018 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3019 VL53L1_OffsetCalibrationMode offset_cal_mode;
Charles MacNeill 7:1add29d51e72 3020
Charles MacNeill 7:1add29d51e72 3021 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3022
lugandc 18:0696efe39d08 3023 offset_cal_mode =
lugandc 18:0696efe39d08 3024 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD;
lugandc 18:0696efe39d08 3025
Charles MacNeill 7:1add29d51e72 3026 if (OffsetCalibrationMode == VL53L1_OFFSETCALIBRATIONMODE_STANDARD) {
Charles MacNeill 7:1add29d51e72 3027 offset_cal_mode =
Charles MacNeill 7:1add29d51e72 3028 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD;
Charles MacNeill 7:1add29d51e72 3029 } else if (OffsetCalibrationMode ==
Charles MacNeill 7:1add29d51e72 3030 VL53L1_OFFSETCALIBRATIONMODE_PRERANGE_ONLY) {
Charles MacNeill 7:1add29d51e72 3031 offset_cal_mode =
Charles MacNeill 7:1add29d51e72 3032 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY;
Charles MacNeill 7:1add29d51e72 3033 } else if (OffsetCalibrationMode ==
Charles MacNeill 7:1add29d51e72 3034 VL53L1_OFFSETCALIBRATIONMODE_MULTI_ZONE) {
Charles MacNeill 7:1add29d51e72 3035 offset_cal_mode =
Charles MacNeill 7:1add29d51e72 3036 VL53L1_OFFSETCALIBRATIONMODE__PER_ZONE;
Charles MacNeill 7:1add29d51e72 3037 } else {
Charles MacNeill 7:1add29d51e72 3038 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3039 }
Charles MacNeill 7:1add29d51e72 3040
Charles MacNeill 7:1add29d51e72 3041 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3042 Status = VL53L1_set_offset_calibration_mode(Dev,
Charles MacNeill 7:1add29d51e72 3043 offset_cal_mode);
Charles MacNeill 7:1add29d51e72 3044
Charles MacNeill 7:1add29d51e72 3045 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3046 return Status;
Charles MacNeill 7:1add29d51e72 3047 }
Charles MacNeill 7:1add29d51e72 3048
Charles MacNeill 7:1add29d51e72 3049 VL53L1_Error VL53L1_SetOffsetCorrectionMode(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3050 VL53L1_OffsetCorrectionModes OffsetCorrectionMode)
Charles MacNeill 7:1add29d51e72 3051 {
Charles MacNeill 7:1add29d51e72 3052 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3053 VL53L1_OffsetCorrectionMode offset_cor_mode;
Charles MacNeill 7:1add29d51e72 3054
Charles MacNeill 7:1add29d51e72 3055 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3056
lugandc 18:0696efe39d08 3057 offset_cor_mode =
lugandc 18:0696efe39d08 3058 VL53L1_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS;
Charles MacNeill 7:1add29d51e72 3059 if (OffsetCorrectionMode == VL53L1_OFFSETCORRECTIONMODE_STANDARD) {
Charles MacNeill 7:1add29d51e72 3060 offset_cor_mode =
Charles MacNeill 7:1add29d51e72 3061 VL53L1_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS;
Charles MacNeill 7:1add29d51e72 3062 } else if (OffsetCorrectionMode ==
Charles MacNeill 7:1add29d51e72 3063 VL53L1_OFFSETCORRECTIONMODE_PERZONE) {
Charles MacNeill 7:1add29d51e72 3064 offset_cor_mode =
Charles MacNeill 7:1add29d51e72 3065 VL53L1_OFFSETCORRECTIONMODE__PER_ZONE_OFFSETS;
Charles MacNeill 7:1add29d51e72 3066 } else if (OffsetCorrectionMode ==
Charles MacNeill 7:1add29d51e72 3067 VL53L1_OFFSETCORRECTIONMODE_PERVCSEL) {
Charles MacNeill 7:1add29d51e72 3068 offset_cor_mode =
Charles MacNeill 7:1add29d51e72 3069 VL53L1_OFFSETCORRECTIONMODE__PER_VCSEL_OFFSETS;
Charles MacNeill 7:1add29d51e72 3070 } else {
Charles MacNeill 7:1add29d51e72 3071 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3072 }
Charles MacNeill 7:1add29d51e72 3073
Charles MacNeill 7:1add29d51e72 3074 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3075 Status = VL53L1_set_offset_correction_mode(Dev,
Charles MacNeill 7:1add29d51e72 3076 offset_cor_mode);
Charles MacNeill 7:1add29d51e72 3077
Charles MacNeill 7:1add29d51e72 3078 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3079 return Status;
Charles MacNeill 7:1add29d51e72 3080 }
Charles MacNeill 7:1add29d51e72 3081
Charles MacNeill 7:1add29d51e72 3082 VL53L1_Error VL53L1_PerformOffsetCalibration(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3083 int32_t CalDistanceMilliMeter, FixPoint1616_t CalReflectancePercent)
Charles MacNeill 7:1add29d51e72 3084 {
Charles MacNeill 7:1add29d51e72 3085 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3086 VL53L1_Error UnfilteredStatus;
Charles MacNeill 7:1add29d51e72 3087 VL53L1_OffsetCalibrationMode offset_cal_mode;
Charles MacNeill 7:1add29d51e72 3088 uint16_t CalReflectancePercent_int;
Charles MacNeill 7:1add29d51e72 3089
Charles MacNeill 7:1add29d51e72 3090 VL53L1_DevicePresetModes device_preset_mode;
Charles MacNeill 7:1add29d51e72 3091 VL53L1_DeviceZonePreset zone_preset;
Charles MacNeill 7:1add29d51e72 3092 VL53L1_zone_config_t zone_cfg;
Charles MacNeill 7:1add29d51e72 3093 int32_t MergeEnabled;
Charles MacNeill 7:1add29d51e72 3094
Charles MacNeill 7:1add29d51e72 3095 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3096
Charles MacNeill 7:1add29d51e72 3097 CalReflectancePercent_int =
Charles MacNeill 7:1add29d51e72 3098 VL53L1_FIXPOINT1616TOFIXPOINT72(CalReflectancePercent);
Charles MacNeill 7:1add29d51e72 3099
lugandc 18:0696efe39d08 3100 Status = VL53L1_get_offset_calibration_mode(Dev,
lugandc 18:0696efe39d08 3101 &offset_cal_mode);
Charles MacNeill 7:1add29d51e72 3102
Charles MacNeill 7:1add29d51e72 3103 if (Status != VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3104 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3105 return Status;
Charles MacNeill 7:1add29d51e72 3106 }
Charles MacNeill 7:1add29d51e72 3107
Charles MacNeill 7:1add29d51e72 3108
Charles MacNeill 7:1add29d51e72 3109 if ((offset_cal_mode ==
Charles MacNeill 7:1add29d51e72 3110 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD) ||
Charles MacNeill 7:1add29d51e72 3111 (offset_cal_mode ==
Charles MacNeill 7:1add29d51e72 3112 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY
Charles MacNeill 7:1add29d51e72 3113 )) {
Charles MacNeill 7:1add29d51e72 3114 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3115 Status = VL53L1_run_offset_calibration(
Charles MacNeill 7:1add29d51e72 3116 Dev,
Charles MacNeill 7:1add29d51e72 3117 (int16_t)CalDistanceMilliMeter,
Charles MacNeill 7:1add29d51e72 3118 CalReflectancePercent_int,
Charles MacNeill 7:1add29d51e72 3119 &UnfilteredStatus);
Charles MacNeill 7:1add29d51e72 3120
Charles MacNeill 7:1add29d51e72 3121 } else if (offset_cal_mode ==
Charles MacNeill 7:1add29d51e72 3122 VL53L1_OFFSETCALIBRATIONMODE__PER_ZONE) {
Charles MacNeill 7:1add29d51e72 3123 VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
Charles MacNeill 7:1add29d51e72 3124 &MergeEnabled);
Charles MacNeill 7:1add29d51e72 3125 VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, 0);
Charles MacNeill 7:1add29d51e72 3126 device_preset_mode =
Charles MacNeill 7:1add29d51e72 3127 VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_LONG_RANGE;
Charles MacNeill 7:1add29d51e72 3128 zone_preset = VL53L1_DEVICEZONEPRESET_CUSTOM;
Charles MacNeill 7:1add29d51e72 3129
Charles MacNeill 7:1add29d51e72 3130 Status = VL53L1_get_zone_config(Dev, &zone_cfg);
Charles MacNeill 7:1add29d51e72 3131 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3132 Status = VL53L1_run_zone_calibration(
Charles MacNeill 7:1add29d51e72 3133 Dev,
Charles MacNeill 7:1add29d51e72 3134 device_preset_mode,
Charles MacNeill 7:1add29d51e72 3135 zone_preset,
Charles MacNeill 7:1add29d51e72 3136 &zone_cfg,
Charles MacNeill 7:1add29d51e72 3137 (int16_t)CalDistanceMilliMeter,
Charles MacNeill 7:1add29d51e72 3138 CalReflectancePercent_int,
Charles MacNeill 7:1add29d51e72 3139 &UnfilteredStatus);
Charles MacNeill 7:1add29d51e72 3140 VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
Charles MacNeill 7:1add29d51e72 3141 MergeEnabled);
Charles MacNeill 7:1add29d51e72 3142
Charles MacNeill 7:1add29d51e72 3143 } else {
Charles MacNeill 7:1add29d51e72 3144 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3145 }
Charles MacNeill 7:1add29d51e72 3146 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3147 return Status;
Charles MacNeill 7:1add29d51e72 3148 }
Charles MacNeill 7:1add29d51e72 3149
Charles MacNeill 7:1add29d51e72 3150 VL53L1_Error VL53L1_PerformOffsetSimpleCalibration(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3151 int32_t CalDistanceMilliMeter)
Charles MacNeill 7:1add29d51e72 3152 {
Charles MacNeill 7:1add29d51e72 3153 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3154 int32_t sum_ranging;
Charles MacNeill 7:1add29d51e72 3155 uint8_t offset_meas;
Charles MacNeill 7:1add29d51e72 3156 int16_t Max, UnderMax, OverMax, Repeat;
Charles MacNeill 7:1add29d51e72 3157 int32_t total_count, inloopcount;
Charles MacNeill 7:1add29d51e72 3158 int32_t IncRounding;
Charles MacNeill 7:1add29d51e72 3159 int16_t meanDistance_mm;
Charles MacNeill 7:1add29d51e72 3160 int16_t offset;
Charles MacNeill 7:1add29d51e72 3161 VL53L1_RangingMeasurementData_t RangingMeasurementData;
Charles MacNeill 7:1add29d51e72 3162 VL53L1_LLDriverData_t *pdev;
Charles MacNeill 7:1add29d51e72 3163 uint8_t goodmeas;
Charles MacNeill 7:1add29d51e72 3164 VL53L1_Error SmudgeStatus = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3165 uint8_t smudge_corr_en;
Charles MacNeill 7:1add29d51e72 3166
Charles MacNeill 7:1add29d51e72 3167 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3168
Charles MacNeill 7:1add29d51e72 3169 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 3170
Charles MacNeill 7:1add29d51e72 3171 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
Charles MacNeill 7:1add29d51e72 3172 SmudgeStatus = VL53L1_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 7:1add29d51e72 3173
Charles MacNeill 7:1add29d51e72 3174 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3175 pdev->customer.mm_config__inner_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3176 pdev->customer.mm_config__outer_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3177 memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 7:1add29d51e72 3178 Repeat = BDTable[VL53L1_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
Charles MacNeill 7:1add29d51e72 3179 Max = BDTable[
Charles MacNeill 7:1add29d51e72 3180 VL53L1_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
Charles MacNeill 7:1add29d51e72 3181 UnderMax = 1 + (Max / 2);
Charles MacNeill 7:1add29d51e72 3182 OverMax = Max + (Max / 2);
Charles MacNeill 7:1add29d51e72 3183 sum_ranging = 0;
Charles MacNeill 7:1add29d51e72 3184 total_count = 0;
Charles MacNeill 7:1add29d51e72 3185
Charles MacNeill 7:1add29d51e72 3186 while ((Repeat > 0) && (Status == VL53L1_ERROR_NONE)) {
Charles MacNeill 7:1add29d51e72 3187 Status = VL53L1_StartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3188
Charles MacNeill 7:1add29d51e72 3189 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3190 VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 3191 VL53L1_GetRangingMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 3192 &RangingMeasurementData);
Charles MacNeill 7:1add29d51e72 3193 VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3194 }
Charles MacNeill 7:1add29d51e72 3195
Charles MacNeill 7:1add29d51e72 3196 inloopcount = 0;
Charles MacNeill 7:1add29d51e72 3197 offset_meas = 0;
Charles MacNeill 7:1add29d51e72 3198 while ((Status == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
Charles MacNeill 7:1add29d51e72 3199 (offset_meas < OverMax)) {
Charles MacNeill 7:1add29d51e72 3200 Status = VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 3201 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3202 Status = VL53L1_GetRangingMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 3203 &RangingMeasurementData);
Charles MacNeill 7:1add29d51e72 3204 goodmeas = (RangingMeasurementData.RangeStatus ==
Charles MacNeill 7:1add29d51e72 3205 VL53L1_RANGESTATUS_RANGE_VALID);
Charles MacNeill 7:1add29d51e72 3206 if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
Charles MacNeill 7:1add29d51e72 3207 sum_ranging = sum_ranging +
Charles MacNeill 7:1add29d51e72 3208 RangingMeasurementData.RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 3209 inloopcount++;
Charles MacNeill 7:1add29d51e72 3210 }
Charles MacNeill 7:1add29d51e72 3211 Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3212 offset_meas++;
Charles MacNeill 7:1add29d51e72 3213 }
Charles MacNeill 7:1add29d51e72 3214 total_count += inloopcount;
Charles MacNeill 7:1add29d51e72 3215
Charles MacNeill 7:1add29d51e72 3216
Charles MacNeill 7:1add29d51e72 3217 if (inloopcount < UnderMax)
Charles MacNeill 7:1add29d51e72 3218 Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
Charles MacNeill 7:1add29d51e72 3219
Charles MacNeill 7:1add29d51e72 3220 VL53L1_StopMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3221
Charles MacNeill 7:1add29d51e72 3222 Repeat--;
Charles MacNeill 7:1add29d51e72 3223
Charles MacNeill 7:1add29d51e72 3224 }
Charles MacNeill 7:1add29d51e72 3225
Charles MacNeill 7:1add29d51e72 3226 if ((SmudgeStatus == VL53L1_ERROR_NONE) && (smudge_corr_en == 1))
Charles MacNeill 7:1add29d51e72 3227 SmudgeStatus = VL53L1_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 7:1add29d51e72 3228
Charles MacNeill 7:1add29d51e72 3229 if ((sum_ranging < 0) ||
Charles MacNeill 7:1add29d51e72 3230 (sum_ranging > ((int32_t) total_count * 0xffff)))
Charles MacNeill 7:1add29d51e72 3231 Status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
Charles MacNeill 7:1add29d51e72 3232
Charles MacNeill 7:1add29d51e72 3233 if ((Status == VL53L1_ERROR_NONE) && (total_count > 0)) {
Charles MacNeill 7:1add29d51e72 3234 IncRounding = total_count / 2;
Charles MacNeill 7:1add29d51e72 3235 meanDistance_mm = (int16_t)((sum_ranging + IncRounding)
Charles MacNeill 7:1add29d51e72 3236 / total_count);
Charles MacNeill 7:1add29d51e72 3237 offset = (int16_t)CalDistanceMilliMeter - meanDistance_mm;
Charles MacNeill 7:1add29d51e72 3238 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3239 pdev->customer.mm_config__inner_offset_mm = offset;
Charles MacNeill 7:1add29d51e72 3240 pdev->customer.mm_config__outer_offset_mm = offset;
Charles MacNeill 7:1add29d51e72 3241
Charles MacNeill 7:1add29d51e72 3242 Status = VL53L1_set_customer_nvm_managed(Dev,
Charles MacNeill 7:1add29d51e72 3243 &(pdev->customer));
Charles MacNeill 7:1add29d51e72 3244 }
Charles MacNeill 7:1add29d51e72 3245
Charles MacNeill 7:1add29d51e72 3246 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3247 return Status;
Charles MacNeill 7:1add29d51e72 3248 }
Charles MacNeill 7:1add29d51e72 3249
Charles MacNeill 7:1add29d51e72 3250 VL53L1_Error VL53L1_PerformOffsetZeroDistanceCalibration(VL53L1_DEV Dev)
Charles MacNeill 7:1add29d51e72 3251 {
Charles MacNeill 7:1add29d51e72 3252 #define START_OFFSET 50
Charles MacNeill 7:1add29d51e72 3253 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3254 int32_t sum_ranging;
Charles MacNeill 7:1add29d51e72 3255 uint8_t offset_meas;
Charles MacNeill 7:1add29d51e72 3256 int16_t Max, UnderMax, OverMax, Repeat;
Charles MacNeill 7:1add29d51e72 3257 int32_t total_count, inloopcount;
Charles MacNeill 7:1add29d51e72 3258 int32_t IncRounding;
Charles MacNeill 7:1add29d51e72 3259 int16_t meanDistance_mm;
Charles MacNeill 7:1add29d51e72 3260 int16_t offset, ZeroDistanceOffset;
Charles MacNeill 7:1add29d51e72 3261 VL53L1_RangingMeasurementData_t RangingMeasurementData;
Charles MacNeill 7:1add29d51e72 3262 VL53L1_LLDriverData_t *pdev;
Charles MacNeill 7:1add29d51e72 3263 uint8_t goodmeas;
Charles MacNeill 7:1add29d51e72 3264 VL53L1_Error SmudgeStatus = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3265 uint8_t smudge_corr_en;
Charles MacNeill 7:1add29d51e72 3266
Charles MacNeill 7:1add29d51e72 3267 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3268
Charles MacNeill 7:1add29d51e72 3269 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 3270 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
Charles MacNeill 7:1add29d51e72 3271 SmudgeStatus = VL53L1_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 7:1add29d51e72 3272 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3273 pdev->customer.mm_config__inner_offset_mm = START_OFFSET;
Charles MacNeill 7:1add29d51e72 3274 pdev->customer.mm_config__outer_offset_mm = START_OFFSET;
Charles MacNeill 7:1add29d51e72 3275 memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 7:1add29d51e72 3276 ZeroDistanceOffset = BDTable[
Charles MacNeill 7:1add29d51e72 3277 VL53L1_TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR];
Charles MacNeill 7:1add29d51e72 3278 Repeat = BDTable[VL53L1_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
Charles MacNeill 7:1add29d51e72 3279 Max = BDTable[
Charles MacNeill 7:1add29d51e72 3280 VL53L1_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
Charles MacNeill 7:1add29d51e72 3281 UnderMax = 1 + (Max / 2);
Charles MacNeill 7:1add29d51e72 3282 OverMax = Max + (Max / 2);
Charles MacNeill 7:1add29d51e72 3283 sum_ranging = 0;
Charles MacNeill 7:1add29d51e72 3284 total_count = 0;
Charles MacNeill 7:1add29d51e72 3285
Charles MacNeill 7:1add29d51e72 3286 while ((Repeat > 0) && (Status == VL53L1_ERROR_NONE)) {
Charles MacNeill 7:1add29d51e72 3287 Status = VL53L1_StartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3288 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3289 VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 3290 VL53L1_GetRangingMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 3291 &RangingMeasurementData);
Charles MacNeill 7:1add29d51e72 3292 VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3293 }
Charles MacNeill 7:1add29d51e72 3294 inloopcount = 0;
Charles MacNeill 7:1add29d51e72 3295 offset_meas = 0;
Charles MacNeill 7:1add29d51e72 3296 while ((Status == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
Charles MacNeill 7:1add29d51e72 3297 (offset_meas < OverMax)) {
Charles MacNeill 7:1add29d51e72 3298 Status = VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 3299 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3300 Status = VL53L1_GetRangingMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 3301 &RangingMeasurementData);
Charles MacNeill 7:1add29d51e72 3302 goodmeas = (RangingMeasurementData.RangeStatus ==
Charles MacNeill 7:1add29d51e72 3303 VL53L1_RANGESTATUS_RANGE_VALID);
Charles MacNeill 7:1add29d51e72 3304 if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
Charles MacNeill 7:1add29d51e72 3305 sum_ranging = sum_ranging +
Charles MacNeill 7:1add29d51e72 3306 RangingMeasurementData.RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 3307 inloopcount++;
Charles MacNeill 7:1add29d51e72 3308 }
Charles MacNeill 7:1add29d51e72 3309 Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3310 offset_meas++;
Charles MacNeill 7:1add29d51e72 3311 }
Charles MacNeill 7:1add29d51e72 3312 total_count += inloopcount;
Charles MacNeill 7:1add29d51e72 3313 if (inloopcount < UnderMax)
Charles MacNeill 7:1add29d51e72 3314 Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
Charles MacNeill 7:1add29d51e72 3315 VL53L1_StopMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3316 Repeat--;
Charles MacNeill 7:1add29d51e72 3317 }
Charles MacNeill 7:1add29d51e72 3318 if ((SmudgeStatus == VL53L1_ERROR_NONE) && (smudge_corr_en == 1))
Charles MacNeill 7:1add29d51e72 3319 SmudgeStatus = VL53L1_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 7:1add29d51e72 3320 if ((sum_ranging < 0) ||
Charles MacNeill 7:1add29d51e72 3321 (sum_ranging > ((int32_t) total_count * 0xffff)))
Charles MacNeill 7:1add29d51e72 3322 Status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
Charles MacNeill 7:1add29d51e72 3323
Charles MacNeill 7:1add29d51e72 3324 if ((Status == VL53L1_ERROR_NONE) && (total_count > 0)) {
Charles MacNeill 7:1add29d51e72 3325 IncRounding = total_count / 2;
Charles MacNeill 7:1add29d51e72 3326 meanDistance_mm = (int16_t)
Charles MacNeill 7:1add29d51e72 3327 ((sum_ranging + IncRounding) / total_count);
Charles MacNeill 7:1add29d51e72 3328 offset = START_OFFSET - meanDistance_mm + ZeroDistanceOffset;
Charles MacNeill 7:1add29d51e72 3329 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3330 pdev->customer.mm_config__inner_offset_mm = offset;
Charles MacNeill 7:1add29d51e72 3331 pdev->customer.mm_config__outer_offset_mm = offset;
Charles MacNeill 7:1add29d51e72 3332 Status = VL53L1_set_customer_nvm_managed(Dev,
Charles MacNeill 7:1add29d51e72 3333 &(pdev->customer));
Charles MacNeill 7:1add29d51e72 3334 }
Charles MacNeill 7:1add29d51e72 3335
Charles MacNeill 7:1add29d51e72 3336 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3337 return Status;
Charles MacNeill 7:1add29d51e72 3338 }
Charles MacNeill 7:1add29d51e72 3339
Charles MacNeill 7:1add29d51e72 3340 VL53L1_Error VL53L1_SetCalibrationData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3341 VL53L1_CalibrationData_t *pCalibrationData)
Charles MacNeill 7:1add29d51e72 3342 {
Charles MacNeill 7:1add29d51e72 3343 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3344 VL53L1_CustomerNvmManaged_t *pC;
Charles MacNeill 7:1add29d51e72 3345 VL53L1_calibration_data_t cal_data;
Charles MacNeill 7:1add29d51e72 3346 uint32_t x, IncomeVersion, CurrentVersion;
Charles MacNeill 7:1add29d51e72 3347 uint8_t CalStopsOn_cal_peak_rate_map = 0;
Charles MacNeill 7:1add29d51e72 3348 VL53L1_xtalk_calibration_results_t xtalk;
Charles MacNeill 7:1add29d51e72 3349 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 3350
Charles MacNeill 7:1add29d51e72 3351 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3352
Charles MacNeill 7:1add29d51e72 3353 cal_data.struct_version = pCalibrationData->struct_version -
Charles MacNeill 7:1add29d51e72 3354 VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
Charles MacNeill 7:1add29d51e72 3355
Charles MacNeill 7:1add29d51e72 3356 IncomeVersion = pCalibrationData->struct_version;
Charles MacNeill 7:1add29d51e72 3357 CurrentVersion = VL53L1_LL_CALIBRATION_DATA_STRUCT_VERSION +
Charles MacNeill 7:1add29d51e72 3358 VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
Charles MacNeill 7:1add29d51e72 3359
Charles MacNeill 7:1add29d51e72 3360 if ((IncomeVersion < CurrentVersion) &&
Charles MacNeill 7:1add29d51e72 3361 ((IncomeVersion & 0xFFFFFF0F) ==
Charles MacNeill 7:1add29d51e72 3362 (CurrentVersion & 0xFFFFFF0F))) {
Charles MacNeill 7:1add29d51e72 3363 cal_data.struct_version =
Charles MacNeill 7:1add29d51e72 3364 VL53L1_LL_CALIBRATION_DATA_STRUCT_VERSION;
Charles MacNeill 7:1add29d51e72 3365 CalStopsOn_cal_peak_rate_map = 1;
Charles MacNeill 7:1add29d51e72 3366
Charles MacNeill 7:1add29d51e72 3367 pdev->tuning_parms.tp_hist_merge = 0;
Charles MacNeill 7:1add29d51e72 3368 }
Charles MacNeill 7:1add29d51e72 3369
Charles MacNeill 7:1add29d51e72 3370
Charles MacNeill 7:1add29d51e72 3371
Charles MacNeill 7:1add29d51e72 3372 memcpy(
Charles MacNeill 7:1add29d51e72 3373 &(cal_data.fmt_dmax_cal),
Charles MacNeill 7:1add29d51e72 3374 &(pCalibrationData->fmt_dmax_cal),
Charles MacNeill 7:1add29d51e72 3375 sizeof(VL53L1_dmax_calibration_data_t));
Charles MacNeill 7:1add29d51e72 3376
Charles MacNeill 7:1add29d51e72 3377
Charles MacNeill 7:1add29d51e72 3378 memcpy(
Charles MacNeill 7:1add29d51e72 3379 &(cal_data.cust_dmax_cal),
Charles MacNeill 7:1add29d51e72 3380 &(pCalibrationData->cust_dmax_cal),
Charles MacNeill 7:1add29d51e72 3381 sizeof(VL53L1_dmax_calibration_data_t));
Charles MacNeill 7:1add29d51e72 3382
Charles MacNeill 7:1add29d51e72 3383
Charles MacNeill 7:1add29d51e72 3384 memcpy(
Charles MacNeill 7:1add29d51e72 3385 &(cal_data.add_off_cal_data),
Charles MacNeill 7:1add29d51e72 3386 &(pCalibrationData->add_off_cal_data),
Charles MacNeill 7:1add29d51e72 3387 sizeof(VL53L1_additional_offset_cal_data_t));
Charles MacNeill 7:1add29d51e72 3388
Charles MacNeill 7:1add29d51e72 3389
Charles MacNeill 7:1add29d51e72 3390 memcpy(
Charles MacNeill 7:1add29d51e72 3391 &(cal_data.optical_centre),
Charles MacNeill 7:1add29d51e72 3392 &(pCalibrationData->optical_centre),
Charles MacNeill 7:1add29d51e72 3393 sizeof(VL53L1_optical_centre_t));
Charles MacNeill 7:1add29d51e72 3394
Charles MacNeill 7:1add29d51e72 3395
Charles MacNeill 7:1add29d51e72 3396 memcpy(
Charles MacNeill 7:1add29d51e72 3397 &(cal_data.xtalkhisto),
Charles MacNeill 7:1add29d51e72 3398 &(pCalibrationData->xtalkhisto),
Charles MacNeill 7:1add29d51e72 3399 sizeof(VL53L1_xtalk_histogram_data_t));
Charles MacNeill 7:1add29d51e72 3400
Charles MacNeill 7:1add29d51e72 3401
Charles MacNeill 7:1add29d51e72 3402 memcpy(
Charles MacNeill 7:1add29d51e72 3403 &(cal_data.gain_cal),
Charles MacNeill 7:1add29d51e72 3404 &(pCalibrationData->gain_cal),
Charles MacNeill 7:1add29d51e72 3405 sizeof(VL53L1_gain_calibration_data_t));
Charles MacNeill 7:1add29d51e72 3406
Charles MacNeill 7:1add29d51e72 3407
Charles MacNeill 7:1add29d51e72 3408 memcpy(
Charles MacNeill 7:1add29d51e72 3409 &(cal_data.cal_peak_rate_map),
Charles MacNeill 7:1add29d51e72 3410 &(pCalibrationData->cal_peak_rate_map),
Charles MacNeill 7:1add29d51e72 3411 sizeof(VL53L1_cal_peak_rate_map_t));
Charles MacNeill 7:1add29d51e72 3412
Charles MacNeill 7:1add29d51e72 3413
Charles MacNeill 7:1add29d51e72 3414 if (!CalStopsOn_cal_peak_rate_map)
Charles MacNeill 7:1add29d51e72 3415 memcpy(
Charles MacNeill 7:1add29d51e72 3416 &(cal_data.per_vcsel_cal_data),
Charles MacNeill 7:1add29d51e72 3417 &(pCalibrationData->per_vcsel_cal_data),
Charles MacNeill 7:1add29d51e72 3418 sizeof(VL53L1_per_vcsel_period_offset_cal_data_t));
Charles MacNeill 7:1add29d51e72 3419 else {
Charles MacNeill 7:1add29d51e72 3420 cal_data.per_vcsel_cal_data.short_a_offset_mm =
Charles MacNeill 7:1add29d51e72 3421 cal_data.per_vcsel_cal_data.short_b_offset_mm =
Charles MacNeill 7:1add29d51e72 3422 cal_data.per_vcsel_cal_data.medium_a_offset_mm =
Charles MacNeill 7:1add29d51e72 3423 cal_data.per_vcsel_cal_data.medium_b_offset_mm =
Charles MacNeill 7:1add29d51e72 3424 cal_data.per_vcsel_cal_data.long_a_offset_mm =
Charles MacNeill 7:1add29d51e72 3425 cal_data.per_vcsel_cal_data.long_b_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3426 }
Charles MacNeill 7:1add29d51e72 3427
Charles MacNeill 7:1add29d51e72 3428 pC = &pCalibrationData->customer;
Charles MacNeill 7:1add29d51e72 3429 x = pC->algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 7:1add29d51e72 3430 cal_data.customer.algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 7:1add29d51e72 3431 (uint16_t)(x&0x0000FFFF);
Charles MacNeill 7:1add29d51e72 3432
Charles MacNeill 7:1add29d51e72 3433 cal_data.customer.global_config__spad_enables_ref_0 =
Charles MacNeill 7:1add29d51e72 3434 pC->global_config__spad_enables_ref_0;
Charles MacNeill 7:1add29d51e72 3435 cal_data.customer.global_config__spad_enables_ref_1 =
Charles MacNeill 7:1add29d51e72 3436 pC->global_config__spad_enables_ref_1;
Charles MacNeill 7:1add29d51e72 3437 cal_data.customer.global_config__spad_enables_ref_2 =
Charles MacNeill 7:1add29d51e72 3438 pC->global_config__spad_enables_ref_2;
Charles MacNeill 7:1add29d51e72 3439 cal_data.customer.global_config__spad_enables_ref_3 =
Charles MacNeill 7:1add29d51e72 3440 pC->global_config__spad_enables_ref_3;
Charles MacNeill 7:1add29d51e72 3441 cal_data.customer.global_config__spad_enables_ref_4 =
Charles MacNeill 7:1add29d51e72 3442 pC->global_config__spad_enables_ref_4;
Charles MacNeill 7:1add29d51e72 3443 cal_data.customer.global_config__spad_enables_ref_5 =
Charles MacNeill 7:1add29d51e72 3444 pC->global_config__spad_enables_ref_5;
Charles MacNeill 7:1add29d51e72 3445 cal_data.customer.global_config__ref_en_start_select =
Charles MacNeill 7:1add29d51e72 3446 pC->global_config__ref_en_start_select;
Charles MacNeill 7:1add29d51e72 3447 cal_data.customer.ref_spad_man__num_requested_ref_spads =
Charles MacNeill 7:1add29d51e72 3448 pC->ref_spad_man__num_requested_ref_spads;
Charles MacNeill 7:1add29d51e72 3449 cal_data.customer.ref_spad_man__ref_location =
Charles MacNeill 7:1add29d51e72 3450 pC->ref_spad_man__ref_location;
Charles MacNeill 7:1add29d51e72 3451 cal_data.customer.algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 7:1add29d51e72 3452 pC->algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 7:1add29d51e72 3453 cal_data.customer.algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 7:1add29d51e72 3454 pC->algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 7:1add29d51e72 3455 cal_data.customer.ref_spad_char__total_rate_target_mcps =
Charles MacNeill 7:1add29d51e72 3456 pC->ref_spad_char__total_rate_target_mcps;
Charles MacNeill 7:1add29d51e72 3457 cal_data.customer.algo__part_to_part_range_offset_mm =
Charles MacNeill 7:1add29d51e72 3458 pC->algo__part_to_part_range_offset_mm;
Charles MacNeill 7:1add29d51e72 3459 cal_data.customer.mm_config__inner_offset_mm =
Charles MacNeill 7:1add29d51e72 3460 pC->mm_config__inner_offset_mm;
Charles MacNeill 7:1add29d51e72 3461 cal_data.customer.mm_config__outer_offset_mm =
Charles MacNeill 7:1add29d51e72 3462 pC->mm_config__outer_offset_mm;
Charles MacNeill 7:1add29d51e72 3463
Charles MacNeill 7:1add29d51e72 3464 Status = VL53L1_set_part_to_part_data(Dev, &cal_data);
Charles MacNeill 7:1add29d51e72 3465 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3466 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 3467
Charles MacNeill 7:1add29d51e72 3468 Status = VL53L1_get_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 7:1add29d51e72 3469
Charles MacNeill 7:1add29d51e72 3470 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3471 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 3472
Charles MacNeill 7:1add29d51e72 3473 xtalk.algo__crosstalk_compensation_plane_offset_kcps = x;
Charles MacNeill 7:1add29d51e72 3474
Charles MacNeill 7:1add29d51e72 3475 Status = VL53L1_set_tuning_parm(Dev,
Charles MacNeill 7:1add29d51e72 3476 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS,
Charles MacNeill 7:1add29d51e72 3477 x);
Charles MacNeill 7:1add29d51e72 3478
Charles MacNeill 7:1add29d51e72 3479
Charles MacNeill 7:1add29d51e72 3480 if (!CalStopsOn_cal_peak_rate_map)
Charles MacNeill 7:1add29d51e72 3481 memcpy(
Charles MacNeill 7:1add29d51e72 3482 &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 7:1add29d51e72 3483 &(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 7:1add29d51e72 3484 sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
Charles MacNeill 7:1add29d51e72 3485 else
Charles MacNeill 7:1add29d51e72 3486 memset(
Charles MacNeill 7:1add29d51e72 3487 &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]), 0,
Charles MacNeill 7:1add29d51e72 3488 sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
Charles MacNeill 7:1add29d51e72 3489
Charles MacNeill 7:1add29d51e72 3490 Status = VL53L1_set_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 7:1add29d51e72 3491
Charles MacNeill 7:1add29d51e72 3492 ENDFUNC:
Charles MacNeill 7:1add29d51e72 3493 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3494 return Status;
Charles MacNeill 7:1add29d51e72 3495
Charles MacNeill 7:1add29d51e72 3496 }
Charles MacNeill 7:1add29d51e72 3497
Charles MacNeill 7:1add29d51e72 3498 VL53L1_Error VL53L1_GetCalibrationData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3499 VL53L1_CalibrationData_t *pCalibrationData)
Charles MacNeill 7:1add29d51e72 3500 {
Charles MacNeill 7:1add29d51e72 3501 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3502 VL53L1_calibration_data_t cal_data;
Charles MacNeill 7:1add29d51e72 3503 VL53L1_CustomerNvmManaged_t *pC;
Charles MacNeill 7:1add29d51e72 3504 VL53L1_customer_nvm_managed_t *pC2;
Charles MacNeill 7:1add29d51e72 3505 VL53L1_xtalk_calibration_results_t xtalk;
Charles MacNeill 7:1add29d51e72 3506 uint32_t tmp;
Charles MacNeill 7:1add29d51e72 3507 VL53L1_PresetModes PresetMode;
Charles MacNeill 7:1add29d51e72 3508
Charles MacNeill 7:1add29d51e72 3509 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3510
Charles MacNeill 7:1add29d51e72 3511
Charles MacNeill 7:1add29d51e72 3512 Status = VL53L1_get_part_to_part_data(Dev, &cal_data);
Charles MacNeill 7:1add29d51e72 3513
Charles MacNeill 7:1add29d51e72 3514 pCalibrationData->struct_version = cal_data.struct_version +
Charles MacNeill 7:1add29d51e72 3515 VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
Charles MacNeill 7:1add29d51e72 3516
Charles MacNeill 7:1add29d51e72 3517
Charles MacNeill 7:1add29d51e72 3518 memcpy(
Charles MacNeill 7:1add29d51e72 3519 &(pCalibrationData->fmt_dmax_cal),
Charles MacNeill 7:1add29d51e72 3520 &(cal_data.fmt_dmax_cal),
Charles MacNeill 7:1add29d51e72 3521 sizeof(VL53L1_dmax_calibration_data_t));
Charles MacNeill 7:1add29d51e72 3522
Charles MacNeill 7:1add29d51e72 3523
Charles MacNeill 7:1add29d51e72 3524 memcpy(
Charles MacNeill 7:1add29d51e72 3525 &(pCalibrationData->cust_dmax_cal),
Charles MacNeill 7:1add29d51e72 3526 &(cal_data.cust_dmax_cal),
Charles MacNeill 7:1add29d51e72 3527 sizeof(VL53L1_dmax_calibration_data_t));
Charles MacNeill 7:1add29d51e72 3528
Charles MacNeill 7:1add29d51e72 3529
Charles MacNeill 7:1add29d51e72 3530 memcpy(
Charles MacNeill 7:1add29d51e72 3531 &(pCalibrationData->add_off_cal_data),
Charles MacNeill 7:1add29d51e72 3532 &(cal_data.add_off_cal_data),
Charles MacNeill 7:1add29d51e72 3533 sizeof(VL53L1_additional_offset_cal_data_t));
Charles MacNeill 7:1add29d51e72 3534
Charles MacNeill 7:1add29d51e72 3535
Charles MacNeill 7:1add29d51e72 3536 memcpy(
Charles MacNeill 7:1add29d51e72 3537 &(pCalibrationData->optical_centre),
Charles MacNeill 7:1add29d51e72 3538 &(cal_data.optical_centre),
Charles MacNeill 7:1add29d51e72 3539 sizeof(VL53L1_optical_centre_t));
Charles MacNeill 7:1add29d51e72 3540
Charles MacNeill 7:1add29d51e72 3541
Charles MacNeill 7:1add29d51e72 3542 memcpy(
Charles MacNeill 7:1add29d51e72 3543 &(pCalibrationData->xtalkhisto),
Charles MacNeill 7:1add29d51e72 3544 &(cal_data.xtalkhisto),
Charles MacNeill 7:1add29d51e72 3545 sizeof(VL53L1_xtalk_histogram_data_t));
Charles MacNeill 7:1add29d51e72 3546
Charles MacNeill 7:1add29d51e72 3547 memcpy(
Charles MacNeill 7:1add29d51e72 3548 &(pCalibrationData->gain_cal),
Charles MacNeill 7:1add29d51e72 3549 &(cal_data.gain_cal),
Charles MacNeill 7:1add29d51e72 3550 sizeof(VL53L1_gain_calibration_data_t));
Charles MacNeill 7:1add29d51e72 3551
Charles MacNeill 7:1add29d51e72 3552
Charles MacNeill 7:1add29d51e72 3553 memcpy(
Charles MacNeill 7:1add29d51e72 3554 &(pCalibrationData->cal_peak_rate_map),
Charles MacNeill 7:1add29d51e72 3555 &(cal_data.cal_peak_rate_map),
Charles MacNeill 7:1add29d51e72 3556 sizeof(VL53L1_cal_peak_rate_map_t));
Charles MacNeill 7:1add29d51e72 3557
Charles MacNeill 7:1add29d51e72 3558
Charles MacNeill 7:1add29d51e72 3559 memcpy(
Charles MacNeill 7:1add29d51e72 3560 &(pCalibrationData->per_vcsel_cal_data),
Charles MacNeill 7:1add29d51e72 3561 &(cal_data.per_vcsel_cal_data),
Charles MacNeill 7:1add29d51e72 3562 sizeof(VL53L1_per_vcsel_period_offset_cal_data_t));
Charles MacNeill 7:1add29d51e72 3563
Charles MacNeill 7:1add29d51e72 3564 pC = &pCalibrationData->customer;
Charles MacNeill 7:1add29d51e72 3565 pC2 = &cal_data.customer;
Charles MacNeill 7:1add29d51e72 3566 pC->global_config__spad_enables_ref_0 =
Charles MacNeill 7:1add29d51e72 3567 pC2->global_config__spad_enables_ref_0;
Charles MacNeill 7:1add29d51e72 3568 pC->global_config__spad_enables_ref_1 =
Charles MacNeill 7:1add29d51e72 3569 pC2->global_config__spad_enables_ref_1;
Charles MacNeill 7:1add29d51e72 3570 pC->global_config__spad_enables_ref_2 =
Charles MacNeill 7:1add29d51e72 3571 pC2->global_config__spad_enables_ref_2;
Charles MacNeill 7:1add29d51e72 3572 pC->global_config__spad_enables_ref_3 =
Charles MacNeill 7:1add29d51e72 3573 pC2->global_config__spad_enables_ref_3;
Charles MacNeill 7:1add29d51e72 3574 pC->global_config__spad_enables_ref_4 =
Charles MacNeill 7:1add29d51e72 3575 pC2->global_config__spad_enables_ref_4;
Charles MacNeill 7:1add29d51e72 3576 pC->global_config__spad_enables_ref_5 =
Charles MacNeill 7:1add29d51e72 3577 pC2->global_config__spad_enables_ref_5;
Charles MacNeill 7:1add29d51e72 3578 pC->global_config__ref_en_start_select =
Charles MacNeill 7:1add29d51e72 3579 pC2->global_config__ref_en_start_select;
Charles MacNeill 7:1add29d51e72 3580 pC->ref_spad_man__num_requested_ref_spads =
Charles MacNeill 7:1add29d51e72 3581 pC2->ref_spad_man__num_requested_ref_spads;
Charles MacNeill 7:1add29d51e72 3582 pC->ref_spad_man__ref_location =
Charles MacNeill 7:1add29d51e72 3583 pC2->ref_spad_man__ref_location;
Charles MacNeill 7:1add29d51e72 3584 pC->algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 7:1add29d51e72 3585 pC2->algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 7:1add29d51e72 3586 pC->algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 7:1add29d51e72 3587 pC2->algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 7:1add29d51e72 3588 pC->ref_spad_char__total_rate_target_mcps =
Charles MacNeill 7:1add29d51e72 3589 pC2->ref_spad_char__total_rate_target_mcps;
Charles MacNeill 7:1add29d51e72 3590 pC->algo__part_to_part_range_offset_mm =
Charles MacNeill 7:1add29d51e72 3591 pC2->algo__part_to_part_range_offset_mm;
Charles MacNeill 7:1add29d51e72 3592 pC->mm_config__inner_offset_mm =
Charles MacNeill 7:1add29d51e72 3593 pC2->mm_config__inner_offset_mm;
Charles MacNeill 7:1add29d51e72 3594 pC->mm_config__outer_offset_mm =
Charles MacNeill 7:1add29d51e72 3595 pC2->mm_config__outer_offset_mm;
Charles MacNeill 7:1add29d51e72 3596
Charles MacNeill 7:1add29d51e72 3597 pC->algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 7:1add29d51e72 3598 (uint32_t)(
Charles MacNeill 7:1add29d51e72 3599 pC2->algo__crosstalk_compensation_plane_offset_kcps);
Charles MacNeill 7:1add29d51e72 3600
Charles MacNeill 7:1add29d51e72 3601 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
Charles MacNeill 7:1add29d51e72 3602
Charles MacNeill 7:1add29d51e72 3603 if ((PresetMode == VL53L1_PRESETMODE_RANGING) ||
Charles MacNeill 7:1add29d51e72 3604 (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) ||
Charles MacNeill 7:1add29d51e72 3605 (PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE)
Charles MacNeill 7:1add29d51e72 3606 ) {
Charles MacNeill 7:1add29d51e72 3607
Charles MacNeill 7:1add29d51e72 3608 Status = VL53L1_get_current_xtalk_settings(Dev, &xtalk);
Charles MacNeill 7:1add29d51e72 3609
Charles MacNeill 7:1add29d51e72 3610 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3611 goto ENDFUNC;
Charles MacNeill 7:1add29d51e72 3612
Charles MacNeill 7:1add29d51e72 3613 tmp = xtalk.algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 7:1add29d51e72 3614 pC->algo__crosstalk_compensation_plane_offset_kcps = tmp;
Charles MacNeill 7:1add29d51e72 3615 tmp = xtalk.algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 7:1add29d51e72 3616 pC->algo__crosstalk_compensation_x_plane_gradient_kcps = tmp;
Charles MacNeill 7:1add29d51e72 3617 tmp = xtalk.algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 7:1add29d51e72 3618 pC->algo__crosstalk_compensation_y_plane_gradient_kcps = tmp;
Charles MacNeill 7:1add29d51e72 3619
Charles MacNeill 7:1add29d51e72 3620 memcpy(&(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 7:1add29d51e72 3621 &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]),
Charles MacNeill 7:1add29d51e72 3622 sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
Charles MacNeill 7:1add29d51e72 3623 }
Charles MacNeill 7:1add29d51e72 3624 ENDFUNC:
Charles MacNeill 7:1add29d51e72 3625 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3626 return Status;
Charles MacNeill 7:1add29d51e72 3627 }
Charles MacNeill 7:1add29d51e72 3628
Charles MacNeill 7:1add29d51e72 3629
Charles MacNeill 7:1add29d51e72 3630 VL53L1_Error VL53L1_SetZoneCalibrationData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3631 VL53L1_ZoneCalibrationData_t *pZoneCalibrationData)
Charles MacNeill 7:1add29d51e72 3632 {
Charles MacNeill 7:1add29d51e72 3633 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3634
Charles MacNeill 7:1add29d51e72 3635 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3636
Charles MacNeill 7:1add29d51e72 3637 Status = VL53L1_set_zone_calibration_data(Dev, pZoneCalibrationData);
Charles MacNeill 7:1add29d51e72 3638
Charles MacNeill 7:1add29d51e72 3639 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3640 return Status;
Charles MacNeill 7:1add29d51e72 3641
Charles MacNeill 7:1add29d51e72 3642 }
Charles MacNeill 7:1add29d51e72 3643
Charles MacNeill 7:1add29d51e72 3644 VL53L1_Error VL53L1_GetZoneCalibrationData(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3645 VL53L1_ZoneCalibrationData_t *pZoneCalibrationData)
Charles MacNeill 7:1add29d51e72 3646 {
Charles MacNeill 7:1add29d51e72 3647 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3648
Charles MacNeill 7:1add29d51e72 3649 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3650
Charles MacNeill 7:1add29d51e72 3651 Status = VL53L1_get_zone_calibration_data(Dev, pZoneCalibrationData);
Charles MacNeill 7:1add29d51e72 3652
Charles MacNeill 7:1add29d51e72 3653 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3654 return Status;
Charles MacNeill 7:1add29d51e72 3655 }
Charles MacNeill 7:1add29d51e72 3656
Charles MacNeill 7:1add29d51e72 3657 VL53L1_Error VL53L1_GetOpticalCenter(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3658 FixPoint1616_t *pOpticalCenterX,
Charles MacNeill 7:1add29d51e72 3659 FixPoint1616_t *pOpticalCenterY)
Charles MacNeill 7:1add29d51e72 3660 {
Charles MacNeill 7:1add29d51e72 3661 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3662 VL53L1_calibration_data_t CalibrationData;
Charles MacNeill 7:1add29d51e72 3663
Charles MacNeill 7:1add29d51e72 3664 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3665
Charles MacNeill 7:1add29d51e72 3666 *pOpticalCenterX = 0;
Charles MacNeill 7:1add29d51e72 3667 *pOpticalCenterY = 0;
Charles MacNeill 7:1add29d51e72 3668 Status = VL53L1_get_part_to_part_data(Dev, &CalibrationData);
Charles MacNeill 7:1add29d51e72 3669 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3670 *pOpticalCenterX = VL53L1_FIXPOINT44TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 3671 CalibrationData.optical_centre.x_centre);
Charles MacNeill 7:1add29d51e72 3672 *pOpticalCenterY = VL53L1_FIXPOINT44TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 3673 CalibrationData.optical_centre.y_centre);
Charles MacNeill 7:1add29d51e72 3674 }
Charles MacNeill 7:1add29d51e72 3675
Charles MacNeill 7:1add29d51e72 3676 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3677 return Status;
Charles MacNeill 7:1add29d51e72 3678 }
Charles MacNeill 7:1add29d51e72 3679
Charles MacNeill 7:1add29d51e72 3680
Charles MacNeill 7:1add29d51e72 3681
Charles MacNeill 7:1add29d51e72 3682
Charles MacNeill 7:1add29d51e72 3683
Charles MacNeill 7:1add29d51e72 3684
Charles MacNeill 7:1add29d51e72 3685 VL53L1_Error VL53L1_SetThresholdConfig(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3686 VL53L1_DetectionConfig_t *pConfig)
Charles MacNeill 7:1add29d51e72 3687 {
Charles MacNeill 7:1add29d51e72 3688 #define BADTHRESBOUNDS(T) \
Charles MacNeill 7:1add29d51e72 3689 (((T.CrossMode == VL53L1_THRESHOLD_OUT_OF_WINDOW) || \
Charles MacNeill 7:1add29d51e72 3690 (T.CrossMode == VL53L1_THRESHOLD_IN_WINDOW)) && (T.Low > T.High))
Charles MacNeill 7:1add29d51e72 3691
Charles MacNeill 7:1add29d51e72 3692 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3693 VL53L1_GPIO_interrupt_config_t Cfg;
Charles MacNeill 7:1add29d51e72 3694 uint16_t g;
Charles MacNeill 7:1add29d51e72 3695 FixPoint1616_t gain, high1616, low1616;
Charles MacNeill 7:1add29d51e72 3696 VL53L1_LLDriverData_t *pdev;
Charles MacNeill 7:1add29d51e72 3697
Charles MacNeill 7:1add29d51e72 3698 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3699
Charles MacNeill 7:1add29d51e72 3700 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 3701
Charles MacNeill 7:1add29d51e72 3702 Status = VL53L1_get_GPIO_interrupt_config(Dev, &Cfg);
Charles MacNeill 7:1add29d51e72 3703 if (Status != VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3704 return Status;
Charles MacNeill 7:1add29d51e72 3705
Charles MacNeill 7:1add29d51e72 3706 if (pConfig->DetectionMode == VL53L1_DETECTION_NORMAL_RUN) {
Charles MacNeill 7:1add29d51e72 3707 Cfg.intr_new_measure_ready = 1;
Charles MacNeill 7:1add29d51e72 3708 Status = VL53L1_set_GPIO_interrupt_config_struct(Dev,
Charles MacNeill 7:1add29d51e72 3709 Cfg);
Charles MacNeill 7:1add29d51e72 3710 } else {
Charles MacNeill 7:1add29d51e72 3711 if (BADTHRESBOUNDS(pConfig->Distance))
Charles MacNeill 7:1add29d51e72 3712 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3713 if ((Status == VL53L1_ERROR_NONE) &&
Charles MacNeill 7:1add29d51e72 3714 (BADTHRESBOUNDS(pConfig->Rate)))
Charles MacNeill 7:1add29d51e72 3715 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3716 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3717 Cfg.intr_new_measure_ready = 0;
Charles MacNeill 7:1add29d51e72 3718 Cfg.intr_no_target = pConfig->IntrNoTarget;
Charles MacNeill 7:1add29d51e72 3719
Charles MacNeill 7:1add29d51e72 3720 g = pdev->gain_cal.standard_ranging_gain_factor;
Charles MacNeill 7:1add29d51e72 3721 if (g != 0) {
Charles MacNeill 7:1add29d51e72 3722
Charles MacNeill 7:1add29d51e72 3723 gain = (FixPoint1616_t) ((uint32_t)g << 5);
Charles MacNeill 7:1add29d51e72 3724 high1616 = (FixPoint1616_t) ((uint32_t)
Charles MacNeill 7:1add29d51e72 3725 pConfig->Distance.High << 16);
Charles MacNeill 7:1add29d51e72 3726 low1616 = (FixPoint1616_t) ((uint32_t)
Charles MacNeill 7:1add29d51e72 3727 pConfig->Distance.Low << 16);
Charles MacNeill 7:1add29d51e72 3728
Charles MacNeill 7:1add29d51e72 3729 high1616 = (high1616 + 32768) / gain;
Charles MacNeill 7:1add29d51e72 3730 low1616 = (low1616 + 32768) / gain;
Charles MacNeill 7:1add29d51e72 3731 Cfg.threshold_distance_high = (uint16_t)
Charles MacNeill 7:1add29d51e72 3732 (high1616 & 0xFFFF);
Charles MacNeill 7:1add29d51e72 3733 Cfg.threshold_distance_low = (uint16_t)
Charles MacNeill 7:1add29d51e72 3734 (low1616 & 0xFFFF);
Charles MacNeill 7:1add29d51e72 3735 }
Charles MacNeill 7:1add29d51e72 3736 Cfg.threshold_rate_high =
Charles MacNeill 7:1add29d51e72 3737 VL53L1_FIXPOINT1616TOFIXPOINT97(
Charles MacNeill 7:1add29d51e72 3738 pConfig->Rate.High);
Charles MacNeill 7:1add29d51e72 3739 Cfg.threshold_rate_low =
Charles MacNeill 7:1add29d51e72 3740 VL53L1_FIXPOINT1616TOFIXPOINT97(
Charles MacNeill 7:1add29d51e72 3741 pConfig->Rate.Low);
Charles MacNeill 7:1add29d51e72 3742
Charles MacNeill 7:1add29d51e72 3743 Cfg.intr_mode_distance = ConvertModeToLLD(
Charles MacNeill 7:1add29d51e72 3744 &Status,
Charles MacNeill 7:1add29d51e72 3745 pConfig->Distance.CrossMode);
Charles MacNeill 7:1add29d51e72 3746 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3747 Cfg.intr_mode_rate = ConvertModeToLLD(
Charles MacNeill 7:1add29d51e72 3748 &Status,
Charles MacNeill 7:1add29d51e72 3749 pConfig->Rate.CrossMode);
Charles MacNeill 7:1add29d51e72 3750 }
Charles MacNeill 7:1add29d51e72 3751
Charles MacNeill 7:1add29d51e72 3752
Charles MacNeill 7:1add29d51e72 3753 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3754 Cfg.intr_combined_mode = 1;
Charles MacNeill 7:1add29d51e72 3755 switch (pConfig->DetectionMode) {
Charles MacNeill 7:1add29d51e72 3756 case VL53L1_DETECTION_DISTANCE_ONLY:
Charles MacNeill 7:1add29d51e72 3757 Cfg.threshold_rate_high = 0;
Charles MacNeill 7:1add29d51e72 3758 Cfg.threshold_rate_low = 0;
Charles MacNeill 7:1add29d51e72 3759 break;
Charles MacNeill 7:1add29d51e72 3760 case VL53L1_DETECTION_RATE_ONLY:
Charles MacNeill 7:1add29d51e72 3761 Cfg.threshold_distance_high = 0;
Charles MacNeill 7:1add29d51e72 3762 Cfg.threshold_distance_low = 0;
Charles MacNeill 7:1add29d51e72 3763 break;
Charles MacNeill 7:1add29d51e72 3764 case VL53L1_DETECTION_DISTANCE_OR_RATE:
Charles MacNeill 7:1add29d51e72 3765
Charles MacNeill 7:1add29d51e72 3766 break;
Charles MacNeill 7:1add29d51e72 3767 case VL53L1_DETECTION_DISTANCE_AND_RATE:
Charles MacNeill 7:1add29d51e72 3768 Cfg.intr_combined_mode = 0;
Charles MacNeill 7:1add29d51e72 3769 break;
Charles MacNeill 7:1add29d51e72 3770 default:
Charles MacNeill 7:1add29d51e72 3771 Status = VL53L1_ERROR_INVALID_PARAMS;
Charles MacNeill 7:1add29d51e72 3772 }
Charles MacNeill 7:1add29d51e72 3773 }
Charles MacNeill 7:1add29d51e72 3774
Charles MacNeill 7:1add29d51e72 3775 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3776 Status =
Charles MacNeill 7:1add29d51e72 3777 VL53L1_set_GPIO_interrupt_config_struct(Dev, Cfg);
Charles MacNeill 7:1add29d51e72 3778
Charles MacNeill 7:1add29d51e72 3779 }
Charles MacNeill 7:1add29d51e72 3780
Charles MacNeill 7:1add29d51e72 3781 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3782 return Status;
Charles MacNeill 7:1add29d51e72 3783 }
Charles MacNeill 7:1add29d51e72 3784
Charles MacNeill 7:1add29d51e72 3785
Charles MacNeill 7:1add29d51e72 3786 VL53L1_Error VL53L1_GetThresholdConfig(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3787 VL53L1_DetectionConfig_t *pConfig)
Charles MacNeill 7:1add29d51e72 3788 {
Charles MacNeill 7:1add29d51e72 3789 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3790 VL53L1_GPIO_interrupt_config_t Cfg;
Charles MacNeill 7:1add29d51e72 3791
Charles MacNeill 7:1add29d51e72 3792 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3793
Charles MacNeill 7:1add29d51e72 3794 Status = VL53L1_get_GPIO_interrupt_config(Dev, &Cfg);
Charles MacNeill 7:1add29d51e72 3795
Charles MacNeill 7:1add29d51e72 3796 if (Status != VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3797 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3798 return Status;
Charles MacNeill 7:1add29d51e72 3799 }
Charles MacNeill 7:1add29d51e72 3800
Charles MacNeill 7:1add29d51e72 3801 pConfig->IntrNoTarget = Cfg.intr_no_target;
Charles MacNeill 7:1add29d51e72 3802 pConfig->Distance.High = Cfg.threshold_distance_high;
Charles MacNeill 7:1add29d51e72 3803 pConfig->Distance.Low = Cfg.threshold_distance_low;
Charles MacNeill 7:1add29d51e72 3804 pConfig->Rate.High =
Charles MacNeill 7:1add29d51e72 3805 VL53L1_FIXPOINT97TOFIXPOINT1616(
Charles MacNeill 7:1add29d51e72 3806 Cfg.threshold_rate_high);
Charles MacNeill 7:1add29d51e72 3807 pConfig->Rate.Low =
Charles MacNeill 7:1add29d51e72 3808 VL53L1_FIXPOINT97TOFIXPOINT1616(Cfg.threshold_rate_low);
Charles MacNeill 7:1add29d51e72 3809 pConfig->Distance.CrossMode =
Charles MacNeill 7:1add29d51e72 3810 ConvertModeFromLLD(&Status, Cfg.intr_mode_distance);
Charles MacNeill 7:1add29d51e72 3811 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3812 pConfig->Rate.CrossMode =
Charles MacNeill 7:1add29d51e72 3813 ConvertModeFromLLD(&Status, Cfg.intr_mode_rate);
Charles MacNeill 7:1add29d51e72 3814
Charles MacNeill 7:1add29d51e72 3815 if (Cfg.intr_new_measure_ready == 1) {
Charles MacNeill 7:1add29d51e72 3816 pConfig->DetectionMode = VL53L1_DETECTION_NORMAL_RUN;
Charles MacNeill 7:1add29d51e72 3817 } else {
Charles MacNeill 7:1add29d51e72 3818
Charles MacNeill 7:1add29d51e72 3819 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3820 if (Cfg.intr_combined_mode == 0)
Charles MacNeill 7:1add29d51e72 3821 pConfig->DetectionMode =
Charles MacNeill 7:1add29d51e72 3822 VL53L1_DETECTION_DISTANCE_AND_RATE;
Charles MacNeill 7:1add29d51e72 3823 else {
Charles MacNeill 7:1add29d51e72 3824 if ((Cfg.threshold_distance_high == 0) &&
Charles MacNeill 7:1add29d51e72 3825 (Cfg.threshold_distance_low == 0))
Charles MacNeill 7:1add29d51e72 3826 pConfig->DetectionMode =
Charles MacNeill 7:1add29d51e72 3827 VL53L1_DETECTION_RATE_ONLY;
Charles MacNeill 7:1add29d51e72 3828 else if ((Cfg.threshold_rate_high == 0) &&
Charles MacNeill 7:1add29d51e72 3829 (Cfg.threshold_rate_low == 0))
Charles MacNeill 7:1add29d51e72 3830 pConfig->DetectionMode =
Charles MacNeill 7:1add29d51e72 3831 VL53L1_DETECTION_DISTANCE_ONLY;
Charles MacNeill 7:1add29d51e72 3832 else
Charles MacNeill 7:1add29d51e72 3833 pConfig->DetectionMode =
Charles MacNeill 7:1add29d51e72 3834 VL53L1_DETECTION_DISTANCE_OR_RATE;
Charles MacNeill 7:1add29d51e72 3835 }
Charles MacNeill 7:1add29d51e72 3836 }
Charles MacNeill 7:1add29d51e72 3837 }
Charles MacNeill 7:1add29d51e72 3838
Charles MacNeill 7:1add29d51e72 3839 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3840 return Status;
Charles MacNeill 7:1add29d51e72 3841 }
Charles MacNeill 7:1add29d51e72 3842
Charles MacNeill 7:1add29d51e72 3843
Charles MacNeill 7:1add29d51e72 3844
Charles MacNeill 7:1add29d51e72 3845
Charles MacNeill 7:1add29d51e72 3846 VL53L1_Error VL53L1_PerformOffsetPerVcselCalibration(VL53L1_DEV Dev,
Charles MacNeill 7:1add29d51e72 3847 int32_t CalDistanceMilliMeter)
Charles MacNeill 7:1add29d51e72 3848 {
Charles MacNeill 7:1add29d51e72 3849 VL53L1_Error Status = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3850 int32_t sum_ranging_range_A, sum_ranging_range_B;
Charles MacNeill 7:1add29d51e72 3851 uint8_t offset_meas_range_A, offset_meas_range_B;
Charles MacNeill 7:1add29d51e72 3852 int16_t Max, UnderMax, OverMax, Repeat;
Charles MacNeill 7:1add29d51e72 3853 int32_t inloopcount;
Charles MacNeill 7:1add29d51e72 3854 int32_t IncRounding;
Charles MacNeill 7:1add29d51e72 3855 int16_t meanDistance_mm;
Charles MacNeill 7:1add29d51e72 3856 VL53L1_RangingMeasurementData_t RangingMeasurementData;
Charles MacNeill 7:1add29d51e72 3857 VL53L1_LLDriverData_t *pdev;
Charles MacNeill 7:1add29d51e72 3858 uint8_t goodmeas;
Charles MacNeill 7:1add29d51e72 3859 VL53L1_PresetModes currentMode;
Charles MacNeill 7:1add29d51e72 3860 VL53L1_DistanceModes currentDist;
Charles MacNeill 7:1add29d51e72 3861 VL53L1_DistanceModes DistMode[3] = {VL53L1_DISTANCEMODE_SHORT,
Charles MacNeill 7:1add29d51e72 3862 VL53L1_DISTANCEMODE_MEDIUM, VL53L1_DISTANCEMODE_LONG};
Charles MacNeill 7:1add29d51e72 3863 int16_t offsetA[3];
Charles MacNeill 7:1add29d51e72 3864 int16_t offsetB[3];
Charles MacNeill 7:1add29d51e72 3865
Charles MacNeill 7:1add29d51e72 3866 VL53L1_Error SmudgeStatus = VL53L1_ERROR_NONE;
Charles MacNeill 7:1add29d51e72 3867 uint8_t smudge_corr_en, isc;
Charles MacNeill 7:1add29d51e72 3868
Charles MacNeill 7:1add29d51e72 3869 LOG_FUNCTION_START("");
Charles MacNeill 7:1add29d51e72 3870
Charles MacNeill 7:1add29d51e72 3871 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Charles MacNeill 7:1add29d51e72 3872
Charles MacNeill 7:1add29d51e72 3873 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
Charles MacNeill 7:1add29d51e72 3874 SmudgeStatus = VL53L1_dynamic_xtalk_correction_disable(Dev);
Charles MacNeill 7:1add29d51e72 3875
Charles MacNeill 7:1add29d51e72 3876 pdev->customer.algo__part_to_part_range_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3877 pdev->customer.mm_config__inner_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3878 pdev->customer.mm_config__outer_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3879 pdev->customer.mm_config__outer_offset_mm = 0;
Charles MacNeill 7:1add29d51e72 3880 memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data));
Charles MacNeill 7:1add29d51e72 3881
Charles MacNeill 7:1add29d51e72 3882 Repeat = 0;
Charles MacNeill 7:1add29d51e72 3883 Max = 2 * BDTable[
Charles MacNeill 7:1add29d51e72 3884 VL53L1_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
Charles MacNeill 7:1add29d51e72 3885 UnderMax = 1 + (Max / 2);
Charles MacNeill 7:1add29d51e72 3886 OverMax = Max + (Max / 2);
Charles MacNeill 7:1add29d51e72 3887
Charles MacNeill 7:1add29d51e72 3888 Status = VL53L1_GetPresetMode(Dev, &currentMode);
Charles MacNeill 7:1add29d51e72 3889 Status = VL53L1_GetDistanceMode(Dev, &currentDist);
Charles MacNeill 7:1add29d51e72 3890
Charles MacNeill 7:1add29d51e72 3891 while ((Repeat < 3) && (Status == VL53L1_ERROR_NONE)) {
Charles MacNeill 7:1add29d51e72 3892 Status = VL53L1_SetDistanceMode(Dev, DistMode[Repeat]);
Charles MacNeill 7:1add29d51e72 3893 Status = VL53L1_StartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3894
Charles MacNeill 7:1add29d51e72 3895 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3896 VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 3897 VL53L1_GetRangingMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 3898 &RangingMeasurementData);
Charles MacNeill 7:1add29d51e72 3899 VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3900 }
Charles MacNeill 7:1add29d51e72 3901
Charles MacNeill 7:1add29d51e72 3902 inloopcount = 0;
Charles MacNeill 7:1add29d51e72 3903 offset_meas_range_A = 0;
Charles MacNeill 7:1add29d51e72 3904 sum_ranging_range_A = 0;
Charles MacNeill 7:1add29d51e72 3905 offset_meas_range_B = 0;
Charles MacNeill 7:1add29d51e72 3906 sum_ranging_range_B = 0;
Charles MacNeill 7:1add29d51e72 3907 while ((Status == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
Charles MacNeill 7:1add29d51e72 3908 (inloopcount < OverMax)) {
Charles MacNeill 7:1add29d51e72 3909 Status = VL53L1_WaitMeasurementDataReady(Dev);
Charles MacNeill 7:1add29d51e72 3910 if (Status == VL53L1_ERROR_NONE)
Charles MacNeill 7:1add29d51e72 3911 Status = VL53L1_GetRangingMeasurementData(Dev,
Charles MacNeill 7:1add29d51e72 3912 &RangingMeasurementData);
Charles MacNeill 7:1add29d51e72 3913 goodmeas = (RangingMeasurementData.RangeStatus ==
Charles MacNeill 7:1add29d51e72 3914 VL53L1_RANGESTATUS_RANGE_VALID);
Charles MacNeill 7:1add29d51e72 3915 isc = pdev->ll_state.cfg_internal_stream_count;
Charles MacNeill 7:1add29d51e72 3916 if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
Charles MacNeill 7:1add29d51e72 3917 if (isc & 0x01) {
Charles MacNeill 7:1add29d51e72 3918 sum_ranging_range_A +=
Charles MacNeill 7:1add29d51e72 3919 RangingMeasurementData.RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 3920 offset_meas_range_A++;
Charles MacNeill 7:1add29d51e72 3921 } else {
Charles MacNeill 7:1add29d51e72 3922 sum_ranging_range_B +=
Charles MacNeill 7:1add29d51e72 3923 RangingMeasurementData.RangeMilliMeter;
Charles MacNeill 7:1add29d51e72 3924 offset_meas_range_B++;
Charles MacNeill 7:1add29d51e72 3925 }
Charles MacNeill 7:1add29d51e72 3926 inloopcount = offset_meas_range_A +
Charles MacNeill 7:1add29d51e72 3927 offset_meas_range_B;
Charles MacNeill 7:1add29d51e72 3928 }
Charles MacNeill 7:1add29d51e72 3929 Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3930 }
Charles MacNeill 7:1add29d51e72 3931
Charles MacNeill 7:1add29d51e72 3932
Charles MacNeill 7:1add29d51e72 3933 if (inloopcount < UnderMax)
Charles MacNeill 7:1add29d51e72 3934 Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
Charles MacNeill 7:1add29d51e72 3935
Charles MacNeill 7:1add29d51e72 3936 VL53L1_StopMeasurement(Dev);
Charles MacNeill 7:1add29d51e72 3937
Charles MacNeill 7:1add29d51e72 3938
Charles MacNeill 7:1add29d51e72 3939 if ((sum_ranging_range_A < 0) ||
Charles MacNeill 7:1add29d51e72 3940 (sum_ranging_range_B < 0) ||
Charles MacNeill 7:1add29d51e72 3941 (sum_ranging_range_A >
Charles MacNeill 7:1add29d51e72 3942 ((int32_t) offset_meas_range_A * 0xffff)) ||
Charles MacNeill 7:1add29d51e72 3943 (sum_ranging_range_B >
Charles MacNeill 7:1add29d51e72 3944 ((int32_t) offset_meas_range_B * 0xffff))) {
Charles MacNeill 7:1add29d51e72 3945 Status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
Charles MacNeill 7:1add29d51e72 3946 }
Charles MacNeill 7:1add29d51e72 3947
Charles MacNeill 7:1add29d51e72 3948 if ((Status == VL53L1_ERROR_NONE) &&
Charles MacNeill 7:1add29d51e72 3949 (offset_meas_range_A > 0)) {
Charles MacNeill 7:1add29d51e72 3950 IncRounding = offset_meas_range_A / 2;
Charles MacNeill 7:1add29d51e72 3951 meanDistance_mm = (int16_t)
Charles MacNeill 7:1add29d51e72 3952 ((sum_ranging_range_A + IncRounding)
Charles MacNeill 7:1add29d51e72 3953 / offset_meas_range_A);
Charles MacNeill 7:1add29d51e72 3954 offsetA[Repeat] = (int16_t)
Charles MacNeill 7:1add29d51e72 3955 CalDistanceMilliMeter - meanDistance_mm;
Charles MacNeill 7:1add29d51e72 3956 }
Charles MacNeill 7:1add29d51e72 3957
Charles MacNeill 7:1add29d51e72 3958 if ((Status == VL53L1_ERROR_NONE) &&
Charles MacNeill 7:1add29d51e72 3959 (offset_meas_range_B > 0)) {
Charles MacNeill 7:1add29d51e72 3960 IncRounding = offset_meas_range_B / 2;
Charles MacNeill 7:1add29d51e72 3961 meanDistance_mm = (int16_t)
Charles MacNeill 7:1add29d51e72 3962 ((sum_ranging_range_B + IncRounding)
Charles MacNeill 7:1add29d51e72 3963 / offset_meas_range_B);
Charles MacNeill 7:1add29d51e72 3964 offsetB[Repeat] = (int16_t)
Charles MacNeill 7:1add29d51e72 3965 CalDistanceMilliMeter - meanDistance_mm;
Charles MacNeill 7:1add29d51e72 3966 }
Charles MacNeill 7:1add29d51e72 3967 Repeat++;
Charles MacNeill 7:1add29d51e72 3968 }
Charles MacNeill 7:1add29d51e72 3969
Charles MacNeill 7:1add29d51e72 3970 if ((SmudgeStatus == VL53L1_ERROR_NONE) && (smudge_corr_en == 1))
Charles MacNeill 7:1add29d51e72 3971 SmudgeStatus = VL53L1_dynamic_xtalk_correction_enable(Dev);
Charles MacNeill 7:1add29d51e72 3972
Charles MacNeill 7:1add29d51e72 3973 if (Status == VL53L1_ERROR_NONE) {
Charles MacNeill 7:1add29d51e72 3974 pdev->per_vcsel_cal_data.short_a_offset_mm = offsetA[0];
Charles MacNeill 7:1add29d51e72 3975 pdev->per_vcsel_cal_data.short_b_offset_mm = offsetB[0];
Charles MacNeill 7:1add29d51e72 3976 pdev->per_vcsel_cal_data.medium_a_offset_mm = offsetA[1];
Charles MacNeill 7:1add29d51e72 3977 pdev->per_vcsel_cal_data.medium_b_offset_mm = offsetB[1];
Charles MacNeill 7:1add29d51e72 3978 pdev->per_vcsel_cal_data.long_a_offset_mm = offsetA[2];
Charles MacNeill 7:1add29d51e72 3979 pdev->per_vcsel_cal_data.long_b_offset_mm = offsetB[2];
Charles MacNeill 7:1add29d51e72 3980 }
Charles MacNeill 7:1add29d51e72 3981
Charles MacNeill 7:1add29d51e72 3982 VL53L1_SetPresetMode(Dev, currentMode);
Charles MacNeill 7:1add29d51e72 3983 VL53L1_SetDistanceMode(Dev, currentDist);
Charles MacNeill 7:1add29d51e72 3984
Charles MacNeill 7:1add29d51e72 3985 LOG_FUNCTION_END(Status);
Charles MacNeill 7:1add29d51e72 3986 return Status;
Charles MacNeill 7:1add29d51e72 3987 }
Charles MacNeill 7:1add29d51e72 3988
Charles MacNeill 7:1add29d51e72 3989