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