ST Expansion SW Team / VL53L3CX_mbed

Rename library

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging

modules/vl53lx_core.c@5:89031b2f5316, 2021-07-14 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 5:89031b2f5316 1
Charles MacNeill 5:89031b2f5316 2 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
Charles MacNeill 5:89031b2f5316 3 /******************************************************************************
Charles MacNeill 5:89031b2f5316 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 5:89031b2f5316 5
Charles MacNeill 5:89031b2f5316 6 This file is part of VL53LX and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either GPL-2.0+
Charles MacNeill 5:89031b2f5316 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 5:89031b2f5316 9 ******************************************************************************
Charles MacNeill 5:89031b2f5316 10 */
Charles MacNeill 5:89031b2f5316 11
Charles MacNeill 5:89031b2f5316 12
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14
Charles MacNeill 5:89031b2f5316 15 #include "vl53lx_platform.h"
Charles MacNeill 5:89031b2f5316 16 #include "vl53lx_ll_def.h"
Charles MacNeill 5:89031b2f5316 17 #include "vl53lx_ll_device.h"
Charles MacNeill 5:89031b2f5316 18 #include "vl53lx_register_map.h"
Charles MacNeill 5:89031b2f5316 19 #include "vl53lx_register_funcs.h"
Charles MacNeill 5:89031b2f5316 20 #include "vl53lx_register_settings.h"
Charles MacNeill 5:89031b2f5316 21 #include "vl53lx_hist_structs.h"
Charles MacNeill 5:89031b2f5316 22 #include "vl53lx_api_preset_modes.h"
Charles MacNeill 5:89031b2f5316 23 #include "vl53lx_core.h"
Charles MacNeill 5:89031b2f5316 24 #include "vl53lx_tuning_parm_defaults.h"
Charles MacNeill 5:89031b2f5316 25
Charles MacNeill 5:89031b2f5316 26
Charles MacNeill 5:89031b2f5316 27
Charles MacNeill 5:89031b2f5316 28 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 29 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 30 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 31 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 32 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 33 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_CORE, \
Charles MacNeill 5:89031b2f5316 34 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 35
Charles MacNeill 5:89031b2f5316 36 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 37 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_CORE, \
Charles MacNeill 5:89031b2f5316 38 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 39
Charles MacNeill 5:89031b2f5316 40
Charles MacNeill 5:89031b2f5316 41 void VL53LX_init_version(
Charles MacNeill 5:89031b2f5316 42 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 43 {
Charles MacNeill 5:89031b2f5316 44
Charles MacNeill 5:89031b2f5316 45
Charles MacNeill 5:89031b2f5316 46 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 47
Charles MacNeill 5:89031b2f5316 48 pdev->version.ll_major = VL53LX_LL_API_IMPLEMENTATION_VER_MAJOR;
Charles MacNeill 5:89031b2f5316 49 pdev->version.ll_minor = VL53LX_LL_API_IMPLEMENTATION_VER_MINOR;
Charles MacNeill 5:89031b2f5316 50 pdev->version.ll_build = VL53LX_LL_API_IMPLEMENTATION_VER_SUB;
Charles MacNeill 5:89031b2f5316 51 pdev->version.ll_revision = VL53LX_LL_API_IMPLEMENTATION_VER_REVISION;
Charles MacNeill 5:89031b2f5316 52 }
Charles MacNeill 5:89031b2f5316 53
Charles MacNeill 5:89031b2f5316 54
Charles MacNeill 5:89031b2f5316 55 void VL53LX_init_ll_driver_state(
Charles MacNeill 5:89031b2f5316 56 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 57 VL53LX_DeviceState device_state)
Charles MacNeill 5:89031b2f5316 58 {
Charles MacNeill 5:89031b2f5316 59
Charles MacNeill 5:89031b2f5316 60
Charles MacNeill 5:89031b2f5316 61 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 62 VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state);
Charles MacNeill 5:89031b2f5316 63
Charles MacNeill 5:89031b2f5316 64 pstate->cfg_device_state = device_state;
Charles MacNeill 5:89031b2f5316 65 pstate->cfg_stream_count = 0;
Charles MacNeill 5:89031b2f5316 66 pstate->cfg_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 67 pstate->cfg_timing_status = 0;
Charles MacNeill 5:89031b2f5316 68 pstate->cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 69
Charles MacNeill 5:89031b2f5316 70 pstate->rd_device_state = device_state;
Charles MacNeill 5:89031b2f5316 71 pstate->rd_stream_count = 0;
Charles MacNeill 5:89031b2f5316 72 pstate->rd_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 73 pstate->rd_timing_status = 0;
Charles MacNeill 5:89031b2f5316 74 pstate->rd_zone_id = 0;
Charles MacNeill 5:89031b2f5316 75
Charles MacNeill 5:89031b2f5316 76 }
Charles MacNeill 5:89031b2f5316 77
Charles MacNeill 5:89031b2f5316 78
Charles MacNeill 5:89031b2f5316 79 VL53LX_Error VL53LX_update_ll_driver_rd_state(
Charles MacNeill 5:89031b2f5316 80 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 81 {
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83
Charles MacNeill 5:89031b2f5316 84 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 85 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 86 VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state);
Charles MacNeill 5:89031b2f5316 87
Charles MacNeill 5:89031b2f5316 88
Charles MacNeill 5:89031b2f5316 89
Charles MacNeill 5:89031b2f5316 90 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 91
Charles MacNeill 5:89031b2f5316 92
Charles MacNeill 5:89031b2f5316 93
Charles MacNeill 5:89031b2f5316 94 if ((pdev->sys_ctrl.system__mode_start &
Charles MacNeill 5:89031b2f5316 95 VL53LX_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
Charles MacNeill 5:89031b2f5316 96
Charles MacNeill 5:89031b2f5316 97 pstate->rd_device_state = VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 98 pstate->rd_stream_count = 0;
Charles MacNeill 5:89031b2f5316 99 pstate->rd_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 100 pstate->rd_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 101 pstate->rd_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 102 pstate->rd_timing_status = 0;
Charles MacNeill 5:89031b2f5316 103 pstate->rd_zone_id = 0;
Charles MacNeill 5:89031b2f5316 104
Charles MacNeill 5:89031b2f5316 105 } else {
Charles MacNeill 5:89031b2f5316 106
Charles MacNeill 5:89031b2f5316 107
Charles MacNeill 5:89031b2f5316 108
Charles MacNeill 5:89031b2f5316 109 if (pstate->rd_stream_count == 0xFF)
Charles MacNeill 5:89031b2f5316 110 pstate->rd_stream_count = 0x80;
Charles MacNeill 5:89031b2f5316 111 else
Charles MacNeill 5:89031b2f5316 112 pstate->rd_stream_count++;
Charles MacNeill 5:89031b2f5316 113
Charles MacNeill 5:89031b2f5316 114
Charles MacNeill 5:89031b2f5316 115 status = VL53LX_update_internal_stream_counters(Dev,
Charles MacNeill 5:89031b2f5316 116 pstate->rd_stream_count,
Charles MacNeill 5:89031b2f5316 117 &(pstate->rd_internal_stream_count),
Charles MacNeill 5:89031b2f5316 118 &(pstate->rd_internal_stream_count_val));
Charles MacNeill 5:89031b2f5316 119
Charles MacNeill 5:89031b2f5316 120
Charles MacNeill 5:89031b2f5316 121
Charles MacNeill 5:89031b2f5316 122 pstate->rd_gph_id ^= VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 123
Charles MacNeill 5:89031b2f5316 124
Charles MacNeill 5:89031b2f5316 125
Charles MacNeill 5:89031b2f5316 126 switch (pstate->rd_device_state) {
Charles MacNeill 5:89031b2f5316 127
Charles MacNeill 5:89031b2f5316 128 case VL53LX_DEVICESTATE_SW_STANDBY:
Charles MacNeill 5:89031b2f5316 129
Charles MacNeill 5:89031b2f5316 130 if ((pdev->dyn_cfg.system__grouped_parameter_hold &
Charles MacNeill 5:89031b2f5316 131 VL53LX_GROUPEDPARAMETERHOLD_ID_MASK) > 0) {
Charles MacNeill 5:89031b2f5316 132 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 133 VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC;
Charles MacNeill 5:89031b2f5316 134 } else {
Charles MacNeill 5:89031b2f5316 135 if (pstate->rd_zone_id >=
Charles MacNeill 5:89031b2f5316 136 pdev->zone_cfg.active_zones)
Charles MacNeill 5:89031b2f5316 137 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 138 VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA;
Charles MacNeill 5:89031b2f5316 139 else
Charles MacNeill 5:89031b2f5316 140 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 141 VL53LX_DEVICESTATE_RANGING_GATHER_DATA;
Charles MacNeill 5:89031b2f5316 142 }
Charles MacNeill 5:89031b2f5316 143
Charles MacNeill 5:89031b2f5316 144 pstate->rd_stream_count = 0;
Charles MacNeill 5:89031b2f5316 145 pstate->rd_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 146 pstate->rd_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 147 pstate->rd_timing_status = 0;
Charles MacNeill 5:89031b2f5316 148 pstate->rd_zone_id = 0;
Charles MacNeill 5:89031b2f5316 149
Charles MacNeill 5:89031b2f5316 150 break;
Charles MacNeill 5:89031b2f5316 151
Charles MacNeill 5:89031b2f5316 152 case VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC:
Charles MacNeill 5:89031b2f5316 153 pstate->rd_stream_count = 0;
Charles MacNeill 5:89031b2f5316 154 pstate->rd_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 155 pstate->rd_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 156 pstate->rd_zone_id = 0;
Charles MacNeill 5:89031b2f5316 157 if (pstate->rd_zone_id >=
Charles MacNeill 5:89031b2f5316 158 pdev->zone_cfg.active_zones)
Charles MacNeill 5:89031b2f5316 159 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 160 VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA;
Charles MacNeill 5:89031b2f5316 161 else
Charles MacNeill 5:89031b2f5316 162 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 163 VL53LX_DEVICESTATE_RANGING_GATHER_DATA;
Charles MacNeill 5:89031b2f5316 164
Charles MacNeill 5:89031b2f5316 165 break;
Charles MacNeill 5:89031b2f5316 166
Charles MacNeill 5:89031b2f5316 167 case VL53LX_DEVICESTATE_RANGING_GATHER_DATA:
Charles MacNeill 5:89031b2f5316 168 pstate->rd_zone_id++;
Charles MacNeill 5:89031b2f5316 169 if (pstate->rd_zone_id >=
Charles MacNeill 5:89031b2f5316 170 pdev->zone_cfg.active_zones)
Charles MacNeill 5:89031b2f5316 171 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 172 VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA;
Charles MacNeill 5:89031b2f5316 173 else
Charles MacNeill 5:89031b2f5316 174 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 175 VL53LX_DEVICESTATE_RANGING_GATHER_DATA;
Charles MacNeill 5:89031b2f5316 176
Charles MacNeill 5:89031b2f5316 177 break;
Charles MacNeill 5:89031b2f5316 178
Charles MacNeill 5:89031b2f5316 179 case VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA:
Charles MacNeill 5:89031b2f5316 180 pstate->rd_zone_id = 0;
Charles MacNeill 5:89031b2f5316 181 pstate->rd_timing_status ^= 0x01;
Charles MacNeill 5:89031b2f5316 182
Charles MacNeill 5:89031b2f5316 183 if (pstate->rd_zone_id >=
Charles MacNeill 5:89031b2f5316 184 pdev->zone_cfg.active_zones)
Charles MacNeill 5:89031b2f5316 185 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 186 VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA;
Charles MacNeill 5:89031b2f5316 187 else
Charles MacNeill 5:89031b2f5316 188 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 189 VL53LX_DEVICESTATE_RANGING_GATHER_DATA;
Charles MacNeill 5:89031b2f5316 190 break;
Charles MacNeill 5:89031b2f5316 191
Charles MacNeill 5:89031b2f5316 192 default:
Charles MacNeill 5:89031b2f5316 193 pstate->rd_device_state =
Charles MacNeill 5:89031b2f5316 194 VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 195 pstate->rd_stream_count = 0;
Charles MacNeill 5:89031b2f5316 196 pstate->rd_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 197 pstate->rd_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 198 pstate->rd_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 199 pstate->rd_timing_status = 0;
Charles MacNeill 5:89031b2f5316 200 pstate->rd_zone_id = 0;
Charles MacNeill 5:89031b2f5316 201 break;
Charles MacNeill 5:89031b2f5316 202 }
Charles MacNeill 5:89031b2f5316 203 }
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 208
Charles MacNeill 5:89031b2f5316 209 return status;
Charles MacNeill 5:89031b2f5316 210 }
Charles MacNeill 5:89031b2f5316 211
Charles MacNeill 5:89031b2f5316 212
Charles MacNeill 5:89031b2f5316 213 VL53LX_Error VL53LX_check_ll_driver_rd_state(
Charles MacNeill 5:89031b2f5316 214 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 215 {
Charles MacNeill 5:89031b2f5316 216
Charles MacNeill 5:89031b2f5316 217
Charles MacNeill 5:89031b2f5316 218 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 219 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 220 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 221 VL53LX_LLDriverResults_t *pres =
Charles MacNeill 5:89031b2f5316 222 VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 223
Charles MacNeill 5:89031b2f5316 224 VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state);
Charles MacNeill 5:89031b2f5316 225 VL53LX_system_results_t *psys_results = &(pdev->sys_results);
Charles MacNeill 5:89031b2f5316 226 VL53LX_histogram_bin_data_t *phist_data = &(pdev->hist_data);
Charles MacNeill 5:89031b2f5316 227 VL53LX_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs);
Charles MacNeill 5:89031b2f5316 228
Charles MacNeill 5:89031b2f5316 229 uint8_t device_range_status = 0;
Charles MacNeill 5:89031b2f5316 230 uint8_t device_stream_count = 0;
Charles MacNeill 5:89031b2f5316 231 uint8_t device_gph_id = 0;
Charles MacNeill 5:89031b2f5316 232 uint8_t histogram_mode = 0;
Charles MacNeill 5:89031b2f5316 233 uint8_t expected_stream_count = 0;
Charles MacNeill 5:89031b2f5316 234 uint8_t expected_gph_id = 0;
Charles MacNeill 5:89031b2f5316 235
Charles MacNeill 5:89031b2f5316 236 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 237
Charles MacNeill 5:89031b2f5316 238
Charles MacNeill 5:89031b2f5316 239
Charles MacNeill 5:89031b2f5316 240 device_range_status =
Charles MacNeill 5:89031b2f5316 241 psys_results->result__range_status &
Charles MacNeill 5:89031b2f5316 242 VL53LX_RANGE_STATUS__RANGE_STATUS_MASK;
Charles MacNeill 5:89031b2f5316 243
Charles MacNeill 5:89031b2f5316 244 device_stream_count = psys_results->result__stream_count;
Charles MacNeill 5:89031b2f5316 245
Charles MacNeill 5:89031b2f5316 246
Charles MacNeill 5:89031b2f5316 247
Charles MacNeill 5:89031b2f5316 248 histogram_mode =
Charles MacNeill 5:89031b2f5316 249 (pdev->sys_ctrl.system__mode_start &
Charles MacNeill 5:89031b2f5316 250 VL53LX_DEVICESCHEDULERMODE_HISTOGRAM) ==
Charles MacNeill 5:89031b2f5316 251 VL53LX_DEVICESCHEDULERMODE_HISTOGRAM;
Charles MacNeill 5:89031b2f5316 252
Charles MacNeill 5:89031b2f5316 253
Charles MacNeill 5:89031b2f5316 254 device_gph_id = (psys_results->result__interrupt_status &
Charles MacNeill 5:89031b2f5316 255 VL53LX_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
Charles MacNeill 5:89031b2f5316 256
Charles MacNeill 5:89031b2f5316 257 if (histogram_mode)
Charles MacNeill 5:89031b2f5316 258 device_gph_id = (phist_data->result__interrupt_status &
Charles MacNeill 5:89031b2f5316 259 VL53LX_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
Charles MacNeill 5:89031b2f5316 260
Charles MacNeill 5:89031b2f5316 261
Charles MacNeill 5:89031b2f5316 262
Charles MacNeill 5:89031b2f5316 263 if (!((pdev->sys_ctrl.system__mode_start &
Charles MacNeill 5:89031b2f5316 264 VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK) ==
Charles MacNeill 5:89031b2f5316 265 VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK))
Charles MacNeill 5:89031b2f5316 266 goto ENDFUNC;
Charles MacNeill 5:89031b2f5316 267
Charles MacNeill 5:89031b2f5316 268
Charles MacNeill 5:89031b2f5316 269
Charles MacNeill 5:89031b2f5316 270 if (pstate->rd_device_state ==
Charles MacNeill 5:89031b2f5316 271 VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC) {
Charles MacNeill 5:89031b2f5316 272
Charles MacNeill 5:89031b2f5316 273 if (histogram_mode == 0) {
Charles MacNeill 5:89031b2f5316 274 if (device_range_status !=
Charles MacNeill 5:89031b2f5316 275 VL53LX_DEVICEERROR_GPHSTREAMCOUNT0READY)
Charles MacNeill 5:89031b2f5316 276 status =
Charles MacNeill 5:89031b2f5316 277 VL53LX_ERROR_GPH_SYNC_CHECK_FAIL;
Charles MacNeill 5:89031b2f5316 278
Charles MacNeill 5:89031b2f5316 279 }
Charles MacNeill 5:89031b2f5316 280 } else {
Charles MacNeill 5:89031b2f5316 281 if (pstate->rd_stream_count != device_stream_count)
Charles MacNeill 5:89031b2f5316 282 status = VL53LX_ERROR_STREAM_COUNT_CHECK_FAIL;
Charles MacNeill 5:89031b2f5316 283
Charles MacNeill 5:89031b2f5316 284
Charles MacNeill 5:89031b2f5316 285 if (pstate->rd_gph_id != device_gph_id)
Charles MacNeill 5:89031b2f5316 286 status = VL53LX_ERROR_GPH_ID_CHECK_FAIL;
Charles MacNeill 5:89031b2f5316 287
Charles MacNeill 5:89031b2f5316 288
Charles MacNeill 5:89031b2f5316 289
Charles MacNeill 5:89031b2f5316 290
Charles MacNeill 5:89031b2f5316 291 expected_stream_count =
Charles MacNeill 5:89031b2f5316 292 pZ->VL53LX_p_003[pstate->rd_zone_id].expected_stream_count;
Charles MacNeill 5:89031b2f5316 293 expected_gph_id =
Charles MacNeill 5:89031b2f5316 294 pZ->VL53LX_p_003[pstate->rd_zone_id].expected_gph_id;
Charles MacNeill 5:89031b2f5316 295
Charles MacNeill 5:89031b2f5316 296
Charles MacNeill 5:89031b2f5316 297
Charles MacNeill 5:89031b2f5316 298 if (expected_stream_count != device_stream_count) {
Charles MacNeill 5:89031b2f5316 299
Charles MacNeill 5:89031b2f5316 300
Charles MacNeill 5:89031b2f5316 301 if (!((pdev->zone_cfg.active_zones == 0) &&
Charles MacNeill 5:89031b2f5316 302 (device_stream_count == 255)))
Charles MacNeill 5:89031b2f5316 303 status =
Charles MacNeill 5:89031b2f5316 304 VL53LX_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL;
Charles MacNeill 5:89031b2f5316 305
Charles MacNeill 5:89031b2f5316 306
Charles MacNeill 5:89031b2f5316 307 }
Charles MacNeill 5:89031b2f5316 308
Charles MacNeill 5:89031b2f5316 309
Charles MacNeill 5:89031b2f5316 310
Charles MacNeill 5:89031b2f5316 311 if (expected_gph_id != device_gph_id)
Charles MacNeill 5:89031b2f5316 312 status = VL53LX_ERROR_ZONE_GPH_ID_CHECK_FAIL;
Charles MacNeill 5:89031b2f5316 313
Charles MacNeill 5:89031b2f5316 314 }
Charles MacNeill 5:89031b2f5316 315
Charles MacNeill 5:89031b2f5316 316
Charles MacNeill 5:89031b2f5316 317
Charles MacNeill 5:89031b2f5316 318 ENDFUNC:
Charles MacNeill 5:89031b2f5316 319 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 320 return status;
Charles MacNeill 5:89031b2f5316 321 }
Charles MacNeill 5:89031b2f5316 322
Charles MacNeill 5:89031b2f5316 323
Charles MacNeill 5:89031b2f5316 324 VL53LX_Error VL53LX_update_ll_driver_cfg_state(
Charles MacNeill 5:89031b2f5316 325 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 326 {
Charles MacNeill 5:89031b2f5316 327
Charles MacNeill 5:89031b2f5316 328
Charles MacNeill 5:89031b2f5316 329 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 330 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 331 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 332 VL53LX_LLDriverResults_t *pres =
Charles MacNeill 5:89031b2f5316 333 VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 334
Charles MacNeill 5:89031b2f5316 335 VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state);
Charles MacNeill 5:89031b2f5316 336 VL53LX_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs);
Charles MacNeill 5:89031b2f5316 337
Charles MacNeill 5:89031b2f5316 338 uint8_t prev_cfg_zone_id;
Charles MacNeill 5:89031b2f5316 339 uint8_t prev_cfg_gph_id;
Charles MacNeill 5:89031b2f5316 340 uint8_t prev_cfg_stream_count;
Charles MacNeill 5:89031b2f5316 341
Charles MacNeill 5:89031b2f5316 342 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 343
Charles MacNeill 5:89031b2f5316 344
Charles MacNeill 5:89031b2f5316 345
Charles MacNeill 5:89031b2f5316 346
Charles MacNeill 5:89031b2f5316 347
Charles MacNeill 5:89031b2f5316 348 if ((pdev->sys_ctrl.system__mode_start &
Charles MacNeill 5:89031b2f5316 349 VL53LX_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
Charles MacNeill 5:89031b2f5316 350
Charles MacNeill 5:89031b2f5316 351 pstate->cfg_device_state = VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 352 pstate->cfg_stream_count = 0;
Charles MacNeill 5:89031b2f5316 353 pstate->cfg_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 354 pstate->cfg_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 355 pstate->cfg_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 356 pstate->cfg_timing_status = 0;
Charles MacNeill 5:89031b2f5316 357 pstate->cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 358 prev_cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 359 prev_cfg_gph_id = 0;
Charles MacNeill 5:89031b2f5316 360 prev_cfg_stream_count = 0;
Charles MacNeill 5:89031b2f5316 361
Charles MacNeill 5:89031b2f5316 362 } else {
Charles MacNeill 5:89031b2f5316 363
Charles MacNeill 5:89031b2f5316 364 prev_cfg_gph_id = pstate->cfg_gph_id;
Charles MacNeill 5:89031b2f5316 365 prev_cfg_zone_id = pstate->cfg_zone_id;
Charles MacNeill 5:89031b2f5316 366 prev_cfg_stream_count = pstate->cfg_stream_count;
Charles MacNeill 5:89031b2f5316 367
Charles MacNeill 5:89031b2f5316 368
Charles MacNeill 5:89031b2f5316 369
Charles MacNeill 5:89031b2f5316 370 if (pstate->cfg_stream_count == 0xFF)
Charles MacNeill 5:89031b2f5316 371 pstate->cfg_stream_count = 0x80;
Charles MacNeill 5:89031b2f5316 372 else
Charles MacNeill 5:89031b2f5316 373 pstate->cfg_stream_count++;
Charles MacNeill 5:89031b2f5316 374
Charles MacNeill 5:89031b2f5316 375
Charles MacNeill 5:89031b2f5316 376 status = VL53LX_update_internal_stream_counters(
Charles MacNeill 5:89031b2f5316 377 Dev,
Charles MacNeill 5:89031b2f5316 378 pstate->cfg_stream_count,
Charles MacNeill 5:89031b2f5316 379 &(pstate->cfg_internal_stream_count),
Charles MacNeill 5:89031b2f5316 380 &(pstate->cfg_internal_stream_count_val));
Charles MacNeill 5:89031b2f5316 381
Charles MacNeill 5:89031b2f5316 382
Charles MacNeill 5:89031b2f5316 383
Charles MacNeill 5:89031b2f5316 384 pstate->cfg_gph_id ^= VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 385
Charles MacNeill 5:89031b2f5316 386
Charles MacNeill 5:89031b2f5316 387
Charles MacNeill 5:89031b2f5316 388 switch (pstate->cfg_device_state) {
Charles MacNeill 5:89031b2f5316 389
Charles MacNeill 5:89031b2f5316 390 case VL53LX_DEVICESTATE_SW_STANDBY:
Charles MacNeill 5:89031b2f5316 391 pstate->cfg_zone_id = 1;
Charles MacNeill 5:89031b2f5316 392 if (pstate->cfg_zone_id >
Charles MacNeill 5:89031b2f5316 393 pdev->zone_cfg.active_zones) {
Charles MacNeill 5:89031b2f5316 394 pstate->cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 395 pstate->cfg_timing_status ^= 0x01;
Charles MacNeill 5:89031b2f5316 396 }
Charles MacNeill 5:89031b2f5316 397 pstate->cfg_stream_count = 1;
Charles MacNeill 5:89031b2f5316 398
Charles MacNeill 5:89031b2f5316 399 if (pdev->gen_cfg.global_config__stream_divider == 0) {
Charles MacNeill 5:89031b2f5316 400 pstate->cfg_internal_stream_count = 1;
Charles MacNeill 5:89031b2f5316 401 pstate->cfg_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 402 } else {
Charles MacNeill 5:89031b2f5316 403 pstate->cfg_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 404 pstate->cfg_internal_stream_count_val = 1;
Charles MacNeill 5:89031b2f5316 405 }
Charles MacNeill 5:89031b2f5316 406 pstate->cfg_device_state =
Charles MacNeill 5:89031b2f5316 407 VL53LX_DEVICESTATE_RANGING_DSS_AUTO;
Charles MacNeill 5:89031b2f5316 408 break;
Charles MacNeill 5:89031b2f5316 409
Charles MacNeill 5:89031b2f5316 410 case VL53LX_DEVICESTATE_RANGING_DSS_AUTO:
Charles MacNeill 5:89031b2f5316 411 pstate->cfg_zone_id++;
Charles MacNeill 5:89031b2f5316 412 if (pstate->cfg_zone_id >
Charles MacNeill 5:89031b2f5316 413 pdev->zone_cfg.active_zones) {
Charles MacNeill 5:89031b2f5316 414
Charles MacNeill 5:89031b2f5316 415 pstate->cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 416 pstate->cfg_timing_status ^= 0x01;
Charles MacNeill 5:89031b2f5316 417
Charles MacNeill 5:89031b2f5316 418
Charles MacNeill 5:89031b2f5316 419
Charles MacNeill 5:89031b2f5316 420
Charles MacNeill 5:89031b2f5316 421 if (pdev->zone_cfg.active_zones > 0) {
Charles MacNeill 5:89031b2f5316 422 pstate->cfg_device_state =
Charles MacNeill 5:89031b2f5316 423 VL53LX_DEVICESTATE_RANGING_DSS_MANUAL;
Charles MacNeill 5:89031b2f5316 424 }
Charles MacNeill 5:89031b2f5316 425 }
Charles MacNeill 5:89031b2f5316 426 break;
Charles MacNeill 5:89031b2f5316 427
Charles MacNeill 5:89031b2f5316 428 case VL53LX_DEVICESTATE_RANGING_DSS_MANUAL:
Charles MacNeill 5:89031b2f5316 429 pstate->cfg_zone_id++;
Charles MacNeill 5:89031b2f5316 430 if (pstate->cfg_zone_id >
Charles MacNeill 5:89031b2f5316 431 pdev->zone_cfg.active_zones) {
Charles MacNeill 5:89031b2f5316 432 pstate->cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 433 pstate->cfg_timing_status ^= 0x01;
Charles MacNeill 5:89031b2f5316 434 }
Charles MacNeill 5:89031b2f5316 435 break;
Charles MacNeill 5:89031b2f5316 436
Charles MacNeill 5:89031b2f5316 437 default:
Charles MacNeill 5:89031b2f5316 438 pstate->cfg_device_state =
Charles MacNeill 5:89031b2f5316 439 VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 440 pstate->cfg_stream_count = 0;
Charles MacNeill 5:89031b2f5316 441 pstate->cfg_internal_stream_count = 0;
Charles MacNeill 5:89031b2f5316 442 pstate->cfg_internal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 443 pstate->cfg_gph_id =
Charles MacNeill 5:89031b2f5316 444 VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 445 pstate->cfg_timing_status = 0;
Charles MacNeill 5:89031b2f5316 446 pstate->cfg_zone_id = 0;
Charles MacNeill 5:89031b2f5316 447 break;
Charles MacNeill 5:89031b2f5316 448 }
Charles MacNeill 5:89031b2f5316 449 }
Charles MacNeill 5:89031b2f5316 450
Charles MacNeill 5:89031b2f5316 451
Charles MacNeill 5:89031b2f5316 452 if (pdev->zone_cfg.active_zones == 0) {
Charles MacNeill 5:89031b2f5316 453
Charles MacNeill 5:89031b2f5316 454 pZ->VL53LX_p_003[prev_cfg_zone_id].expected_stream_count
Charles MacNeill 5:89031b2f5316 455 = prev_cfg_stream_count - 1;
Charles MacNeill 5:89031b2f5316 456
Charles MacNeill 5:89031b2f5316 457 pZ->VL53LX_p_003[pstate->rd_zone_id].expected_gph_id =
Charles MacNeill 5:89031b2f5316 458 prev_cfg_gph_id ^ VL53LX_GROUPEDPARAMETERHOLD_ID_MASK;
Charles MacNeill 5:89031b2f5316 459 } else {
Charles MacNeill 5:89031b2f5316 460 pZ->VL53LX_p_003[prev_cfg_zone_id].expected_stream_count
Charles MacNeill 5:89031b2f5316 461 = prev_cfg_stream_count;
Charles MacNeill 5:89031b2f5316 462 pZ->VL53LX_p_003[prev_cfg_zone_id].expected_gph_id =
Charles MacNeill 5:89031b2f5316 463 prev_cfg_gph_id;
Charles MacNeill 5:89031b2f5316 464 }
Charles MacNeill 5:89031b2f5316 465
Charles MacNeill 5:89031b2f5316 466
Charles MacNeill 5:89031b2f5316 467
Charles MacNeill 5:89031b2f5316 468 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 469
Charles MacNeill 5:89031b2f5316 470 return status;
Charles MacNeill 5:89031b2f5316 471 }
Charles MacNeill 5:89031b2f5316 472
Charles MacNeill 5:89031b2f5316 473
Charles MacNeill 5:89031b2f5316 474 void VL53LX_copy_rtn_good_spads_to_buffer(
Charles MacNeill 5:89031b2f5316 475 VL53LX_nvm_copy_data_t *pdata,
Charles MacNeill 5:89031b2f5316 476 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 477 {
Charles MacNeill 5:89031b2f5316 478
Charles MacNeill 5:89031b2f5316 479
Charles MacNeill 5:89031b2f5316 480 *(pbuffer + 0) = pdata->global_config__spad_enables_rtn_0;
Charles MacNeill 5:89031b2f5316 481 *(pbuffer + 1) = pdata->global_config__spad_enables_rtn_1;
Charles MacNeill 5:89031b2f5316 482 *(pbuffer + 2) = pdata->global_config__spad_enables_rtn_2;
Charles MacNeill 5:89031b2f5316 483 *(pbuffer + 3) = pdata->global_config__spad_enables_rtn_3;
Charles MacNeill 5:89031b2f5316 484 *(pbuffer + 4) = pdata->global_config__spad_enables_rtn_4;
Charles MacNeill 5:89031b2f5316 485 *(pbuffer + 5) = pdata->global_config__spad_enables_rtn_5;
Charles MacNeill 5:89031b2f5316 486 *(pbuffer + 6) = pdata->global_config__spad_enables_rtn_6;
Charles MacNeill 5:89031b2f5316 487 *(pbuffer + 7) = pdata->global_config__spad_enables_rtn_7;
Charles MacNeill 5:89031b2f5316 488 *(pbuffer + 8) = pdata->global_config__spad_enables_rtn_8;
Charles MacNeill 5:89031b2f5316 489 *(pbuffer + 9) = pdata->global_config__spad_enables_rtn_9;
Charles MacNeill 5:89031b2f5316 490 *(pbuffer + 10) = pdata->global_config__spad_enables_rtn_10;
Charles MacNeill 5:89031b2f5316 491 *(pbuffer + 11) = pdata->global_config__spad_enables_rtn_11;
Charles MacNeill 5:89031b2f5316 492 *(pbuffer + 12) = pdata->global_config__spad_enables_rtn_12;
Charles MacNeill 5:89031b2f5316 493 *(pbuffer + 13) = pdata->global_config__spad_enables_rtn_13;
Charles MacNeill 5:89031b2f5316 494 *(pbuffer + 14) = pdata->global_config__spad_enables_rtn_14;
Charles MacNeill 5:89031b2f5316 495 *(pbuffer + 15) = pdata->global_config__spad_enables_rtn_15;
Charles MacNeill 5:89031b2f5316 496 *(pbuffer + 16) = pdata->global_config__spad_enables_rtn_16;
Charles MacNeill 5:89031b2f5316 497 *(pbuffer + 17) = pdata->global_config__spad_enables_rtn_17;
Charles MacNeill 5:89031b2f5316 498 *(pbuffer + 18) = pdata->global_config__spad_enables_rtn_18;
Charles MacNeill 5:89031b2f5316 499 *(pbuffer + 19) = pdata->global_config__spad_enables_rtn_19;
Charles MacNeill 5:89031b2f5316 500 *(pbuffer + 20) = pdata->global_config__spad_enables_rtn_20;
Charles MacNeill 5:89031b2f5316 501 *(pbuffer + 21) = pdata->global_config__spad_enables_rtn_21;
Charles MacNeill 5:89031b2f5316 502 *(pbuffer + 22) = pdata->global_config__spad_enables_rtn_22;
Charles MacNeill 5:89031b2f5316 503 *(pbuffer + 23) = pdata->global_config__spad_enables_rtn_23;
Charles MacNeill 5:89031b2f5316 504 *(pbuffer + 24) = pdata->global_config__spad_enables_rtn_24;
Charles MacNeill 5:89031b2f5316 505 *(pbuffer + 25) = pdata->global_config__spad_enables_rtn_25;
Charles MacNeill 5:89031b2f5316 506 *(pbuffer + 26) = pdata->global_config__spad_enables_rtn_26;
Charles MacNeill 5:89031b2f5316 507 *(pbuffer + 27) = pdata->global_config__spad_enables_rtn_27;
Charles MacNeill 5:89031b2f5316 508 *(pbuffer + 28) = pdata->global_config__spad_enables_rtn_28;
Charles MacNeill 5:89031b2f5316 509 *(pbuffer + 29) = pdata->global_config__spad_enables_rtn_29;
Charles MacNeill 5:89031b2f5316 510 *(pbuffer + 30) = pdata->global_config__spad_enables_rtn_30;
Charles MacNeill 5:89031b2f5316 511 *(pbuffer + 31) = pdata->global_config__spad_enables_rtn_31;
Charles MacNeill 5:89031b2f5316 512 }
Charles MacNeill 5:89031b2f5316 513
Charles MacNeill 5:89031b2f5316 514
Charles MacNeill 5:89031b2f5316 515 void VL53LX_init_system_results(
Charles MacNeill 5:89031b2f5316 516 VL53LX_system_results_t *pdata)
Charles MacNeill 5:89031b2f5316 517 {
Charles MacNeill 5:89031b2f5316 518
Charles MacNeill 5:89031b2f5316 519
Charles MacNeill 5:89031b2f5316 520 pdata->result__interrupt_status = 0xFF;
Charles MacNeill 5:89031b2f5316 521 pdata->result__range_status = 0xFF;
Charles MacNeill 5:89031b2f5316 522 pdata->result__report_status = 0xFF;
Charles MacNeill 5:89031b2f5316 523 pdata->result__stream_count = 0xFF;
Charles MacNeill 5:89031b2f5316 524
Charles MacNeill 5:89031b2f5316 525 pdata->result__dss_actual_effective_spads_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 526 pdata->result__peak_signal_count_rate_mcps_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 527 pdata->result__ambient_count_rate_mcps_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 528 pdata->result__sigma_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 529 pdata->result__phase_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 530 pdata->result__final_crosstalk_corrected_range_mm_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 531 pdata->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 =
Charles MacNeill 5:89031b2f5316 532 0xFFFF;
Charles MacNeill 5:89031b2f5316 533 pdata->result__mm_inner_actual_effective_spads_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 534 pdata->result__mm_outer_actual_effective_spads_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 535 pdata->result__avg_signal_count_rate_mcps_sd0 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 536
Charles MacNeill 5:89031b2f5316 537 pdata->result__dss_actual_effective_spads_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 538 pdata->result__peak_signal_count_rate_mcps_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 539 pdata->result__ambient_count_rate_mcps_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 540 pdata->result__sigma_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 541 pdata->result__phase_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 542 pdata->result__final_crosstalk_corrected_range_mm_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 543 pdata->result__spare_0_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 544 pdata->result__spare_1_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 545 pdata->result__spare_2_sd1 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 546 pdata->result__spare_3_sd1 = 0xFF;
Charles MacNeill 5:89031b2f5316 547
Charles MacNeill 5:89031b2f5316 548 }
Charles MacNeill 5:89031b2f5316 549
Charles MacNeill 5:89031b2f5316 550
Charles MacNeill 5:89031b2f5316 551 void V53L1_init_zone_results_structure(
Charles MacNeill 5:89031b2f5316 552 uint8_t active_zones,
Charles MacNeill 5:89031b2f5316 553 VL53LX_zone_results_t *pdata)
Charles MacNeill 5:89031b2f5316 554 {
Charles MacNeill 5:89031b2f5316 555
Charles MacNeill 5:89031b2f5316 556
Charles MacNeill 5:89031b2f5316 557
Charles MacNeill 5:89031b2f5316 558 uint8_t z = 0;
Charles MacNeill 5:89031b2f5316 559 VL53LX_zone_objects_t *pobjects;
Charles MacNeill 5:89031b2f5316 560
Charles MacNeill 5:89031b2f5316 561 pdata->max_zones = VL53LX_MAX_USER_ZONES;
Charles MacNeill 5:89031b2f5316 562 pdata->active_zones = active_zones;
Charles MacNeill 5:89031b2f5316 563
Charles MacNeill 5:89031b2f5316 564 for (z = 0; z < pdata->max_zones; z++) {
Charles MacNeill 5:89031b2f5316 565 pobjects = &(pdata->VL53LX_p_003[z]);
Charles MacNeill 5:89031b2f5316 566 pobjects->cfg_device_state = VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 567 pobjects->rd_device_state = VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 568 pobjects->max_objects = VL53LX_MAX_RANGE_RESULTS;
Charles MacNeill 5:89031b2f5316 569 pobjects->active_objects = 0;
Charles MacNeill 5:89031b2f5316 570 }
Charles MacNeill 5:89031b2f5316 571 }
Charles MacNeill 5:89031b2f5316 572
Charles MacNeill 5:89031b2f5316 573 void V53L1_init_zone_dss_configs(
Charles MacNeill 5:89031b2f5316 574 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 575 {
Charles MacNeill 5:89031b2f5316 576
Charles MacNeill 5:89031b2f5316 577
Charles MacNeill 5:89031b2f5316 578
Charles MacNeill 5:89031b2f5316 579 VL53LX_LLDriverResults_t *pres =
Charles MacNeill 5:89031b2f5316 580 VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 581 uint8_t z = 0;
Charles MacNeill 5:89031b2f5316 582 uint8_t max_zones = VL53LX_MAX_USER_ZONES;
Charles MacNeill 5:89031b2f5316 583 VL53LX_zone_private_dyn_cfgs_t *pdata = &(pres->zone_dyn_cfgs);
Charles MacNeill 5:89031b2f5316 584
Charles MacNeill 5:89031b2f5316 585 for (z = 0; z < max_zones; z++) {
Charles MacNeill 5:89031b2f5316 586 pdata->VL53LX_p_003[z].dss_mode =
Charles MacNeill 5:89031b2f5316 587 VL53LX_DSS_CONTROL__MODE_TARGET_RATE;
Charles MacNeill 5:89031b2f5316 588 pdata->VL53LX_p_003[z].dss_requested_effective_spad_count = 0;
Charles MacNeill 5:89031b2f5316 589 }
Charles MacNeill 5:89031b2f5316 590 }
Charles MacNeill 5:89031b2f5316 591
Charles MacNeill 5:89031b2f5316 592
Charles MacNeill 5:89031b2f5316 593 void VL53LX_init_histogram_config_structure(
Charles MacNeill 5:89031b2f5316 594 uint8_t even_bin0,
Charles MacNeill 5:89031b2f5316 595 uint8_t even_bin1,
Charles MacNeill 5:89031b2f5316 596 uint8_t even_bin2,
Charles MacNeill 5:89031b2f5316 597 uint8_t even_bin3,
Charles MacNeill 5:89031b2f5316 598 uint8_t even_bin4,
Charles MacNeill 5:89031b2f5316 599 uint8_t even_bin5,
Charles MacNeill 5:89031b2f5316 600 uint8_t odd_bin0,
Charles MacNeill 5:89031b2f5316 601 uint8_t odd_bin1,
Charles MacNeill 5:89031b2f5316 602 uint8_t odd_bin2,
Charles MacNeill 5:89031b2f5316 603 uint8_t odd_bin3,
Charles MacNeill 5:89031b2f5316 604 uint8_t odd_bin4,
Charles MacNeill 5:89031b2f5316 605 uint8_t odd_bin5,
Charles MacNeill 5:89031b2f5316 606 VL53LX_histogram_config_t *pdata)
Charles MacNeill 5:89031b2f5316 607 {
Charles MacNeill 5:89031b2f5316 608
Charles MacNeill 5:89031b2f5316 609
Charles MacNeill 5:89031b2f5316 610 pdata->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 611 (even_bin1 << 4) + even_bin0;
Charles MacNeill 5:89031b2f5316 612 pdata->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 613 (even_bin3 << 4) + even_bin2;
Charles MacNeill 5:89031b2f5316 614 pdata->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 615 (even_bin5 << 4) + even_bin4;
Charles MacNeill 5:89031b2f5316 616
Charles MacNeill 5:89031b2f5316 617 pdata->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 618 (odd_bin1 << 4) + odd_bin0;
Charles MacNeill 5:89031b2f5316 619 pdata->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 620 (odd_bin3 << 4) + odd_bin2;
Charles MacNeill 5:89031b2f5316 621 pdata->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 622 (odd_bin5 << 4) + odd_bin4;
Charles MacNeill 5:89031b2f5316 623
Charles MacNeill 5:89031b2f5316 624 pdata->histogram_config__mid_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 625 pdata->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 626 pdata->histogram_config__mid_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 627 pdata->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 628 pdata->histogram_config__mid_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 629 pdata->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 630
Charles MacNeill 5:89031b2f5316 631 pdata->histogram_config__mid_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 632 pdata->histogram_config__low_amb_odd_bin_0_1;
Charles MacNeill 5:89031b2f5316 633 pdata->histogram_config__mid_amb_odd_bin_2 = odd_bin2;
Charles MacNeill 5:89031b2f5316 634 pdata->histogram_config__mid_amb_odd_bin_3_4 =
Charles MacNeill 5:89031b2f5316 635 (odd_bin4 << 4) + odd_bin3;
Charles MacNeill 5:89031b2f5316 636 pdata->histogram_config__mid_amb_odd_bin_5 = odd_bin5;
Charles MacNeill 5:89031b2f5316 637
Charles MacNeill 5:89031b2f5316 638 pdata->histogram_config__user_bin_offset = 0x00;
Charles MacNeill 5:89031b2f5316 639
Charles MacNeill 5:89031b2f5316 640 pdata->histogram_config__high_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 641 pdata->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 642 pdata->histogram_config__high_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 643 pdata->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 644 pdata->histogram_config__high_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 645 pdata->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 646
Charles MacNeill 5:89031b2f5316 647 pdata->histogram_config__high_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 648 pdata->histogram_config__low_amb_odd_bin_0_1;
Charles MacNeill 5:89031b2f5316 649 pdata->histogram_config__high_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 650 pdata->histogram_config__low_amb_odd_bin_2_3;
Charles MacNeill 5:89031b2f5316 651 pdata->histogram_config__high_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 652 pdata->histogram_config__low_amb_odd_bin_4_5;
Charles MacNeill 5:89031b2f5316 653
Charles MacNeill 5:89031b2f5316 654
Charles MacNeill 5:89031b2f5316 655
Charles MacNeill 5:89031b2f5316 656 pdata->histogram_config__amb_thresh_low = 0xFFFF;
Charles MacNeill 5:89031b2f5316 657 pdata->histogram_config__amb_thresh_high = 0xFFFF;
Charles MacNeill 5:89031b2f5316 658
Charles MacNeill 5:89031b2f5316 659
Charles MacNeill 5:89031b2f5316 660
Charles MacNeill 5:89031b2f5316 661 pdata->histogram_config__spad_array_selection = 0x00;
Charles MacNeill 5:89031b2f5316 662
Charles MacNeill 5:89031b2f5316 663 }
Charles MacNeill 5:89031b2f5316 664
Charles MacNeill 5:89031b2f5316 665 void VL53LX_init_histogram_multizone_config_structure(
Charles MacNeill 5:89031b2f5316 666 uint8_t even_bin0,
Charles MacNeill 5:89031b2f5316 667 uint8_t even_bin1,
Charles MacNeill 5:89031b2f5316 668 uint8_t even_bin2,
Charles MacNeill 5:89031b2f5316 669 uint8_t even_bin3,
Charles MacNeill 5:89031b2f5316 670 uint8_t even_bin4,
Charles MacNeill 5:89031b2f5316 671 uint8_t even_bin5,
Charles MacNeill 5:89031b2f5316 672 uint8_t odd_bin0,
Charles MacNeill 5:89031b2f5316 673 uint8_t odd_bin1,
Charles MacNeill 5:89031b2f5316 674 uint8_t odd_bin2,
Charles MacNeill 5:89031b2f5316 675 uint8_t odd_bin3,
Charles MacNeill 5:89031b2f5316 676 uint8_t odd_bin4,
Charles MacNeill 5:89031b2f5316 677 uint8_t odd_bin5,
Charles MacNeill 5:89031b2f5316 678 VL53LX_histogram_config_t *pdata)
Charles MacNeill 5:89031b2f5316 679 {
Charles MacNeill 5:89031b2f5316 680
Charles MacNeill 5:89031b2f5316 681
Charles MacNeill 5:89031b2f5316 682 pdata->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 683 (even_bin1 << 4) + even_bin0;
Charles MacNeill 5:89031b2f5316 684 pdata->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 685 (even_bin3 << 4) + even_bin2;
Charles MacNeill 5:89031b2f5316 686 pdata->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 687 (even_bin5 << 4) + even_bin4;
Charles MacNeill 5:89031b2f5316 688
Charles MacNeill 5:89031b2f5316 689 pdata->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 690 pdata->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 691 pdata->histogram_config__low_amb_odd_bin_2_3
Charles MacNeill 5:89031b2f5316 692 = pdata->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 693 pdata->histogram_config__low_amb_odd_bin_4_5
Charles MacNeill 5:89031b2f5316 694 = pdata->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 695
Charles MacNeill 5:89031b2f5316 696 pdata->histogram_config__mid_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 697 pdata->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 698 pdata->histogram_config__mid_amb_even_bin_2_3
Charles MacNeill 5:89031b2f5316 699 = pdata->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 700 pdata->histogram_config__mid_amb_even_bin_4_5
Charles MacNeill 5:89031b2f5316 701 = pdata->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 702
Charles MacNeill 5:89031b2f5316 703 pdata->histogram_config__mid_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 704 = pdata->histogram_config__low_amb_odd_bin_0_1;
Charles MacNeill 5:89031b2f5316 705 pdata->histogram_config__mid_amb_odd_bin_2 = odd_bin2;
Charles MacNeill 5:89031b2f5316 706 pdata->histogram_config__mid_amb_odd_bin_3_4 =
Charles MacNeill 5:89031b2f5316 707 (odd_bin4 << 4) + odd_bin3;
Charles MacNeill 5:89031b2f5316 708 pdata->histogram_config__mid_amb_odd_bin_5 = odd_bin5;
Charles MacNeill 5:89031b2f5316 709
Charles MacNeill 5:89031b2f5316 710 pdata->histogram_config__user_bin_offset = 0x00;
Charles MacNeill 5:89031b2f5316 711
Charles MacNeill 5:89031b2f5316 712 pdata->histogram_config__high_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 713 (odd_bin1 << 4) + odd_bin0;
Charles MacNeill 5:89031b2f5316 714 pdata->histogram_config__high_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 715 (odd_bin3 << 4) + odd_bin2;
Charles MacNeill 5:89031b2f5316 716 pdata->histogram_config__high_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 717 (odd_bin5 << 4) + odd_bin4;
Charles MacNeill 5:89031b2f5316 718
Charles MacNeill 5:89031b2f5316 719 pdata->histogram_config__high_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 720 = pdata->histogram_config__high_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 721 pdata->histogram_config__high_amb_odd_bin_2_3
Charles MacNeill 5:89031b2f5316 722 = pdata->histogram_config__high_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 723 pdata->histogram_config__high_amb_odd_bin_4_5
Charles MacNeill 5:89031b2f5316 724 = pdata->histogram_config__high_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 725
Charles MacNeill 5:89031b2f5316 726
Charles MacNeill 5:89031b2f5316 727
Charles MacNeill 5:89031b2f5316 728 pdata->histogram_config__amb_thresh_low = 0xFFFF;
Charles MacNeill 5:89031b2f5316 729 pdata->histogram_config__amb_thresh_high = 0xFFFF;
Charles MacNeill 5:89031b2f5316 730
Charles MacNeill 5:89031b2f5316 731
Charles MacNeill 5:89031b2f5316 732
Charles MacNeill 5:89031b2f5316 733 pdata->histogram_config__spad_array_selection = 0x00;
Charles MacNeill 5:89031b2f5316 734 }
Charles MacNeill 5:89031b2f5316 735
Charles MacNeill 5:89031b2f5316 736
Charles MacNeill 5:89031b2f5316 737 void VL53LX_init_xtalk_bin_data_struct(
Charles MacNeill 5:89031b2f5316 738 uint32_t bin_value,
Charles MacNeill 5:89031b2f5316 739 uint16_t VL53LX_p_021,
Charles MacNeill 5:89031b2f5316 740 VL53LX_xtalk_histogram_shape_t *pdata)
Charles MacNeill 5:89031b2f5316 741 {
Charles MacNeill 5:89031b2f5316 742
Charles MacNeill 5:89031b2f5316 743
Charles MacNeill 5:89031b2f5316 744
Charles MacNeill 5:89031b2f5316 745 uint16_t i = 0;
Charles MacNeill 5:89031b2f5316 746
Charles MacNeill 5:89031b2f5316 747 pdata->zone_id = 0;
Charles MacNeill 5:89031b2f5316 748 pdata->time_stamp = 0;
Charles MacNeill 5:89031b2f5316 749
Charles MacNeill 5:89031b2f5316 750 pdata->VL53LX_p_019 = 0;
Charles MacNeill 5:89031b2f5316 751 pdata->VL53LX_p_020 = VL53LX_XTALK_HISTO_BINS;
Charles MacNeill 5:89031b2f5316 752 pdata->VL53LX_p_021 = (uint8_t)VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 753
Charles MacNeill 5:89031b2f5316 754 pdata->phasecal_result__reference_phase = 0;
Charles MacNeill 5:89031b2f5316 755 pdata->phasecal_result__vcsel_start = 0;
Charles MacNeill 5:89031b2f5316 756 pdata->cal_config__vcsel_start = 0;
Charles MacNeill 5:89031b2f5316 757
Charles MacNeill 5:89031b2f5316 758 pdata->vcsel_width = 0;
Charles MacNeill 5:89031b2f5316 759 pdata->VL53LX_p_015 = 0;
Charles MacNeill 5:89031b2f5316 760
Charles MacNeill 5:89031b2f5316 761 pdata->zero_distance_phase = 0;
Charles MacNeill 5:89031b2f5316 762
Charles MacNeill 5:89031b2f5316 763 for (i = 0; i < VL53LX_XTALK_HISTO_BINS; i++) {
Charles MacNeill 5:89031b2f5316 764 if (i < VL53LX_p_021)
Charles MacNeill 5:89031b2f5316 765 pdata->bin_data[i] = bin_value;
Charles MacNeill 5:89031b2f5316 766 else
Charles MacNeill 5:89031b2f5316 767 pdata->bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 768 }
Charles MacNeill 5:89031b2f5316 769 }
Charles MacNeill 5:89031b2f5316 770
Charles MacNeill 5:89031b2f5316 771
Charles MacNeill 5:89031b2f5316 772 void VL53LX_i2c_encode_uint16_t(
Charles MacNeill 5:89031b2f5316 773 uint16_t ip_value,
Charles MacNeill 5:89031b2f5316 774 uint16_t count,
Charles MacNeill 5:89031b2f5316 775 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 776 {
Charles MacNeill 5:89031b2f5316 777
Charles MacNeill 5:89031b2f5316 778
Charles MacNeill 5:89031b2f5316 779 uint16_t i = 0;
Charles MacNeill 5:89031b2f5316 780 uint16_t VL53LX_p_003 = 0;
Charles MacNeill 5:89031b2f5316 781
Charles MacNeill 5:89031b2f5316 782 VL53LX_p_003 = ip_value;
Charles MacNeill 5:89031b2f5316 783
Charles MacNeill 5:89031b2f5316 784 for (i = 0; i < count; i++) {
Charles MacNeill 5:89031b2f5316 785 pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF);
Charles MacNeill 5:89031b2f5316 786 VL53LX_p_003 = VL53LX_p_003 >> 8;
Charles MacNeill 5:89031b2f5316 787 }
Charles MacNeill 5:89031b2f5316 788 }
Charles MacNeill 5:89031b2f5316 789
Charles MacNeill 5:89031b2f5316 790 uint16_t VL53LX_i2c_decode_uint16_t(
Charles MacNeill 5:89031b2f5316 791 uint16_t count,
Charles MacNeill 5:89031b2f5316 792 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 793 {
Charles MacNeill 5:89031b2f5316 794
Charles MacNeill 5:89031b2f5316 795
Charles MacNeill 5:89031b2f5316 796 uint16_t value = 0x00;
Charles MacNeill 5:89031b2f5316 797
Charles MacNeill 5:89031b2f5316 798 while (count-- > 0)
Charles MacNeill 5:89031b2f5316 799 value = (value << 8) | (uint16_t)*pbuffer++;
Charles MacNeill 5:89031b2f5316 800
Charles MacNeill 5:89031b2f5316 801 return value;
Charles MacNeill 5:89031b2f5316 802 }
Charles MacNeill 5:89031b2f5316 803
Charles MacNeill 5:89031b2f5316 804
Charles MacNeill 5:89031b2f5316 805 void VL53LX_i2c_encode_int16_t(
Charles MacNeill 5:89031b2f5316 806 int16_t ip_value,
Charles MacNeill 5:89031b2f5316 807 uint16_t count,
Charles MacNeill 5:89031b2f5316 808 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 809 {
Charles MacNeill 5:89031b2f5316 810
Charles MacNeill 5:89031b2f5316 811
Charles MacNeill 5:89031b2f5316 812 uint16_t i = 0;
Charles MacNeill 5:89031b2f5316 813 int16_t VL53LX_p_003 = 0;
Charles MacNeill 5:89031b2f5316 814
Charles MacNeill 5:89031b2f5316 815 VL53LX_p_003 = ip_value;
Charles MacNeill 5:89031b2f5316 816
Charles MacNeill 5:89031b2f5316 817 for (i = 0; i < count; i++) {
Charles MacNeill 5:89031b2f5316 818 pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF);
Charles MacNeill 5:89031b2f5316 819 VL53LX_p_003 = VL53LX_p_003 >> 8;
Charles MacNeill 5:89031b2f5316 820 }
Charles MacNeill 5:89031b2f5316 821 }
Charles MacNeill 5:89031b2f5316 822
Charles MacNeill 5:89031b2f5316 823 int16_t VL53LX_i2c_decode_int16_t(
Charles MacNeill 5:89031b2f5316 824 uint16_t count,
Charles MacNeill 5:89031b2f5316 825 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 826 {
Charles MacNeill 5:89031b2f5316 827
Charles MacNeill 5:89031b2f5316 828
Charles MacNeill 5:89031b2f5316 829 int16_t value = 0x00;
Charles MacNeill 5:89031b2f5316 830
Charles MacNeill 5:89031b2f5316 831
Charles MacNeill 5:89031b2f5316 832 if (*pbuffer >= 0x80)
Charles MacNeill 5:89031b2f5316 833 value = 0xFFFF;
Charles MacNeill 5:89031b2f5316 834
Charles MacNeill 5:89031b2f5316 835 while (count-- > 0)
Charles MacNeill 5:89031b2f5316 836 value = (value << 8) | (int16_t)*pbuffer++;
Charles MacNeill 5:89031b2f5316 837
Charles MacNeill 5:89031b2f5316 838 return value;
Charles MacNeill 5:89031b2f5316 839 }
Charles MacNeill 5:89031b2f5316 840
Charles MacNeill 5:89031b2f5316 841 void VL53LX_i2c_encode_uint32_t(
Charles MacNeill 5:89031b2f5316 842 uint32_t ip_value,
Charles MacNeill 5:89031b2f5316 843 uint16_t count,
Charles MacNeill 5:89031b2f5316 844 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 845 {
Charles MacNeill 5:89031b2f5316 846
Charles MacNeill 5:89031b2f5316 847
Charles MacNeill 5:89031b2f5316 848 uint16_t i = 0;
Charles MacNeill 5:89031b2f5316 849 uint32_t VL53LX_p_003 = 0;
Charles MacNeill 5:89031b2f5316 850
Charles MacNeill 5:89031b2f5316 851 VL53LX_p_003 = ip_value;
Charles MacNeill 5:89031b2f5316 852
Charles MacNeill 5:89031b2f5316 853 for (i = 0; i < count; i++) {
Charles MacNeill 5:89031b2f5316 854 pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF);
Charles MacNeill 5:89031b2f5316 855 VL53LX_p_003 = VL53LX_p_003 >> 8;
Charles MacNeill 5:89031b2f5316 856 }
Charles MacNeill 5:89031b2f5316 857 }
Charles MacNeill 5:89031b2f5316 858
Charles MacNeill 5:89031b2f5316 859 uint32_t VL53LX_i2c_decode_uint32_t(
Charles MacNeill 5:89031b2f5316 860 uint16_t count,
Charles MacNeill 5:89031b2f5316 861 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 862 {
Charles MacNeill 5:89031b2f5316 863
Charles MacNeill 5:89031b2f5316 864
Charles MacNeill 5:89031b2f5316 865 uint32_t value = 0x00;
Charles MacNeill 5:89031b2f5316 866
Charles MacNeill 5:89031b2f5316 867 while (count-- > 0)
Charles MacNeill 5:89031b2f5316 868 value = (value << 8) | (uint32_t)*pbuffer++;
Charles MacNeill 5:89031b2f5316 869
Charles MacNeill 5:89031b2f5316 870 return value;
Charles MacNeill 5:89031b2f5316 871 }
Charles MacNeill 5:89031b2f5316 872
Charles MacNeill 5:89031b2f5316 873
Charles MacNeill 5:89031b2f5316 874 uint32_t VL53LX_i2c_decode_with_mask(
Charles MacNeill 5:89031b2f5316 875 uint16_t count,
Charles MacNeill 5:89031b2f5316 876 uint8_t *pbuffer,
Charles MacNeill 5:89031b2f5316 877 uint32_t bit_mask,
Charles MacNeill 5:89031b2f5316 878 uint32_t down_shift,
Charles MacNeill 5:89031b2f5316 879 uint32_t offset)
Charles MacNeill 5:89031b2f5316 880 {
Charles MacNeill 5:89031b2f5316 881
Charles MacNeill 5:89031b2f5316 882
Charles MacNeill 5:89031b2f5316 883 uint32_t value = 0x00;
Charles MacNeill 5:89031b2f5316 884
Charles MacNeill 5:89031b2f5316 885
Charles MacNeill 5:89031b2f5316 886 while (count-- > 0)
Charles MacNeill 5:89031b2f5316 887 value = (value << 8) | (uint32_t)*pbuffer++;
Charles MacNeill 5:89031b2f5316 888
Charles MacNeill 5:89031b2f5316 889
Charles MacNeill 5:89031b2f5316 890 value = value & bit_mask;
Charles MacNeill 5:89031b2f5316 891 if (down_shift > 0)
Charles MacNeill 5:89031b2f5316 892 value = value >> down_shift;
Charles MacNeill 5:89031b2f5316 893
Charles MacNeill 5:89031b2f5316 894
Charles MacNeill 5:89031b2f5316 895 value = value + offset;
Charles MacNeill 5:89031b2f5316 896
Charles MacNeill 5:89031b2f5316 897 return value;
Charles MacNeill 5:89031b2f5316 898 }
Charles MacNeill 5:89031b2f5316 899
Charles MacNeill 5:89031b2f5316 900
Charles MacNeill 5:89031b2f5316 901 void VL53LX_i2c_encode_int32_t(
Charles MacNeill 5:89031b2f5316 902 int32_t ip_value,
Charles MacNeill 5:89031b2f5316 903 uint16_t count,
Charles MacNeill 5:89031b2f5316 904 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 905 {
Charles MacNeill 5:89031b2f5316 906
Charles MacNeill 5:89031b2f5316 907
Charles MacNeill 5:89031b2f5316 908 uint16_t i = 0;
Charles MacNeill 5:89031b2f5316 909 int32_t VL53LX_p_003 = 0;
Charles MacNeill 5:89031b2f5316 910
Charles MacNeill 5:89031b2f5316 911 VL53LX_p_003 = ip_value;
Charles MacNeill 5:89031b2f5316 912
Charles MacNeill 5:89031b2f5316 913 for (i = 0; i < count; i++) {
Charles MacNeill 5:89031b2f5316 914 pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF);
Charles MacNeill 5:89031b2f5316 915 VL53LX_p_003 = VL53LX_p_003 >> 8;
Charles MacNeill 5:89031b2f5316 916 }
Charles MacNeill 5:89031b2f5316 917 }
Charles MacNeill 5:89031b2f5316 918
Charles MacNeill 5:89031b2f5316 919 int32_t VL53LX_i2c_decode_int32_t(
Charles MacNeill 5:89031b2f5316 920 uint16_t count,
Charles MacNeill 5:89031b2f5316 921 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 922 {
Charles MacNeill 5:89031b2f5316 923
Charles MacNeill 5:89031b2f5316 924
Charles MacNeill 5:89031b2f5316 925 int32_t value = 0x00;
Charles MacNeill 5:89031b2f5316 926
Charles MacNeill 5:89031b2f5316 927
Charles MacNeill 5:89031b2f5316 928 if (*pbuffer >= 0x80)
Charles MacNeill 5:89031b2f5316 929 value = 0xFFFFFFFF;
Charles MacNeill 5:89031b2f5316 930
Charles MacNeill 5:89031b2f5316 931 while (count-- > 0)
Charles MacNeill 5:89031b2f5316 932 value = (value << 8) | (int32_t)*pbuffer++;
Charles MacNeill 5:89031b2f5316 933
Charles MacNeill 5:89031b2f5316 934 return value;
Charles MacNeill 5:89031b2f5316 935 }
Charles MacNeill 5:89031b2f5316 936
Charles MacNeill 5:89031b2f5316 937
Charles MacNeill 5:89031b2f5316 938 VL53LX_Error VL53LX_start_test(
Charles MacNeill 5:89031b2f5316 939 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 940 uint8_t test_mode__ctrl)
Charles MacNeill 5:89031b2f5316 941 {
Charles MacNeill 5:89031b2f5316 942
Charles MacNeill 5:89031b2f5316 943
Charles MacNeill 5:89031b2f5316 944 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 945
Charles MacNeill 5:89031b2f5316 946 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 947
Charles MacNeill 5:89031b2f5316 948 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 949 status = VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 950 Dev,
Charles MacNeill 5:89031b2f5316 951 VL53LX_TEST_MODE__CTRL,
Charles MacNeill 5:89031b2f5316 952 test_mode__ctrl);
Charles MacNeill 5:89031b2f5316 953 }
Charles MacNeill 5:89031b2f5316 954
Charles MacNeill 5:89031b2f5316 955 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 956
Charles MacNeill 5:89031b2f5316 957 return status;
Charles MacNeill 5:89031b2f5316 958 }
Charles MacNeill 5:89031b2f5316 959
Charles MacNeill 5:89031b2f5316 960
Charles MacNeill 5:89031b2f5316 961 VL53LX_Error VL53LX_set_firmware_enable_register(
Charles MacNeill 5:89031b2f5316 962 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 963 uint8_t value)
Charles MacNeill 5:89031b2f5316 964 {
Charles MacNeill 5:89031b2f5316 965
Charles MacNeill 5:89031b2f5316 966
Charles MacNeill 5:89031b2f5316 967 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 968 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 969
Charles MacNeill 5:89031b2f5316 970 pdev->sys_ctrl.firmware__enable = value;
Charles MacNeill 5:89031b2f5316 971
Charles MacNeill 5:89031b2f5316 972 status = VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 973 Dev,
Charles MacNeill 5:89031b2f5316 974 VL53LX_FIRMWARE__ENABLE,
Charles MacNeill 5:89031b2f5316 975 pdev->sys_ctrl.firmware__enable);
Charles MacNeill 5:89031b2f5316 976
Charles MacNeill 5:89031b2f5316 977 return status;
Charles MacNeill 5:89031b2f5316 978 }
Charles MacNeill 5:89031b2f5316 979
Charles MacNeill 5:89031b2f5316 980 VL53LX_Error VL53LX_enable_firmware(
Charles MacNeill 5:89031b2f5316 981 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 982 {
Charles MacNeill 5:89031b2f5316 983
Charles MacNeill 5:89031b2f5316 984
Charles MacNeill 5:89031b2f5316 985 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 986
Charles MacNeill 5:89031b2f5316 987 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 988
Charles MacNeill 5:89031b2f5316 989 status = VL53LX_set_firmware_enable_register(Dev, 0x01);
Charles MacNeill 5:89031b2f5316 990
Charles MacNeill 5:89031b2f5316 991 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 992
Charles MacNeill 5:89031b2f5316 993 return status;
Charles MacNeill 5:89031b2f5316 994 }
Charles MacNeill 5:89031b2f5316 995
Charles MacNeill 5:89031b2f5316 996
Charles MacNeill 5:89031b2f5316 997 VL53LX_Error VL53LX_disable_firmware(
Charles MacNeill 5:89031b2f5316 998 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 999 {
Charles MacNeill 5:89031b2f5316 1000
Charles MacNeill 5:89031b2f5316 1001
Charles MacNeill 5:89031b2f5316 1002 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1003
Charles MacNeill 5:89031b2f5316 1004 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1005
Charles MacNeill 5:89031b2f5316 1006 status = VL53LX_set_firmware_enable_register(Dev, 0x00);
Charles MacNeill 5:89031b2f5316 1007
Charles MacNeill 5:89031b2f5316 1008 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 1009
Charles MacNeill 5:89031b2f5316 1010 return status;
Charles MacNeill 5:89031b2f5316 1011 }
Charles MacNeill 5:89031b2f5316 1012
Charles MacNeill 5:89031b2f5316 1013
Charles MacNeill 5:89031b2f5316 1014 VL53LX_Error VL53LX_set_powerforce_register(
Charles MacNeill 5:89031b2f5316 1015 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1016 uint8_t value)
Charles MacNeill 5:89031b2f5316 1017 {
Charles MacNeill 5:89031b2f5316 1018
Charles MacNeill 5:89031b2f5316 1019
Charles MacNeill 5:89031b2f5316 1020 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1021 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1022
Charles MacNeill 5:89031b2f5316 1023 pdev->sys_ctrl.power_management__go1_power_force = value;
Charles MacNeill 5:89031b2f5316 1024
Charles MacNeill 5:89031b2f5316 1025 status = VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 1026 Dev,
Charles MacNeill 5:89031b2f5316 1027 VL53LX_POWER_MANAGEMENT__GO1_POWER_FORCE,
Charles MacNeill 5:89031b2f5316 1028 pdev->sys_ctrl.power_management__go1_power_force);
Charles MacNeill 5:89031b2f5316 1029
Charles MacNeill 5:89031b2f5316 1030 return status;
Charles MacNeill 5:89031b2f5316 1031 }
Charles MacNeill 5:89031b2f5316 1032
Charles MacNeill 5:89031b2f5316 1033
Charles MacNeill 5:89031b2f5316 1034 VL53LX_Error VL53LX_enable_powerforce(
Charles MacNeill 5:89031b2f5316 1035 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1036 {
Charles MacNeill 5:89031b2f5316 1037
Charles MacNeill 5:89031b2f5316 1038
Charles MacNeill 5:89031b2f5316 1039 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1040
Charles MacNeill 5:89031b2f5316 1041 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1042
Charles MacNeill 5:89031b2f5316 1043 status = VL53LX_set_powerforce_register(Dev, 0x01);
Charles MacNeill 5:89031b2f5316 1044
Charles MacNeill 5:89031b2f5316 1045 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 1046
Charles MacNeill 5:89031b2f5316 1047 return status;
Charles MacNeill 5:89031b2f5316 1048 }
Charles MacNeill 5:89031b2f5316 1049
Charles MacNeill 5:89031b2f5316 1050
Charles MacNeill 5:89031b2f5316 1051 VL53LX_Error VL53LX_disable_powerforce(
Charles MacNeill 5:89031b2f5316 1052 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1053 {
Charles MacNeill 5:89031b2f5316 1054
Charles MacNeill 5:89031b2f5316 1055
Charles MacNeill 5:89031b2f5316 1056 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1057
Charles MacNeill 5:89031b2f5316 1058 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1059
Charles MacNeill 5:89031b2f5316 1060 status = VL53LX_set_powerforce_register(Dev, 0x00);
Charles MacNeill 5:89031b2f5316 1061
Charles MacNeill 5:89031b2f5316 1062 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 1063
Charles MacNeill 5:89031b2f5316 1064 return status;
Charles MacNeill 5:89031b2f5316 1065 }
Charles MacNeill 5:89031b2f5316 1066
Charles MacNeill 5:89031b2f5316 1067
Charles MacNeill 5:89031b2f5316 1068 VL53LX_Error VL53LX_clear_interrupt(
Charles MacNeill 5:89031b2f5316 1069 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1070 {
Charles MacNeill 5:89031b2f5316 1071
Charles MacNeill 5:89031b2f5316 1072
Charles MacNeill 5:89031b2f5316 1073 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1074 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1075
Charles MacNeill 5:89031b2f5316 1076 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1077
Charles MacNeill 5:89031b2f5316 1078 pdev->sys_ctrl.system__interrupt_clear = VL53LX_CLEAR_RANGE_INT;
Charles MacNeill 5:89031b2f5316 1079
Charles MacNeill 5:89031b2f5316 1080 status = VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 1081 Dev,
Charles MacNeill 5:89031b2f5316 1082 VL53LX_SYSTEM__INTERRUPT_CLEAR,
Charles MacNeill 5:89031b2f5316 1083 pdev->sys_ctrl.system__interrupt_clear);
Charles MacNeill 5:89031b2f5316 1084
Charles MacNeill 5:89031b2f5316 1085 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 1086
Charles MacNeill 5:89031b2f5316 1087 return status;
Charles MacNeill 5:89031b2f5316 1088 }
Charles MacNeill 5:89031b2f5316 1089
Charles MacNeill 5:89031b2f5316 1090
Charles MacNeill 5:89031b2f5316 1091 VL53LX_Error VL53LX_force_shadow_stream_count_to_zero(
Charles MacNeill 5:89031b2f5316 1092 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 1093 {
Charles MacNeill 5:89031b2f5316 1094
Charles MacNeill 5:89031b2f5316 1095
Charles MacNeill 5:89031b2f5316 1096 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1097
Charles MacNeill 5:89031b2f5316 1098 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1099 status = VL53LX_disable_firmware(Dev);
Charles MacNeill 5:89031b2f5316 1100
Charles MacNeill 5:89031b2f5316 1101 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 1102 status = VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 1103 Dev,
Charles MacNeill 5:89031b2f5316 1104 VL53LX_SHADOW_RESULT__STREAM_COUNT,
Charles MacNeill 5:89031b2f5316 1105 0x00);
Charles MacNeill 5:89031b2f5316 1106 }
Charles MacNeill 5:89031b2f5316 1107
Charles MacNeill 5:89031b2f5316 1108 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1109 status = VL53LX_enable_firmware(Dev);
Charles MacNeill 5:89031b2f5316 1110
Charles MacNeill 5:89031b2f5316 1111 return status;
Charles MacNeill 5:89031b2f5316 1112 }
Charles MacNeill 5:89031b2f5316 1113
Charles MacNeill 5:89031b2f5316 1114
Charles MacNeill 5:89031b2f5316 1115 uint32_t VL53LX_calc_macro_period_us(
Charles MacNeill 5:89031b2f5316 1116 uint16_t fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 1117 uint8_t VL53LX_p_005)
Charles MacNeill 5:89031b2f5316 1118 {
Charles MacNeill 5:89031b2f5316 1119
Charles MacNeill 5:89031b2f5316 1120
Charles MacNeill 5:89031b2f5316 1121 uint32_t pll_period_us = 0;
Charles MacNeill 5:89031b2f5316 1122 uint8_t VL53LX_p_030 = 0;
Charles MacNeill 5:89031b2f5316 1123 uint32_t macro_period_us = 0;
Charles MacNeill 5:89031b2f5316 1124
Charles MacNeill 5:89031b2f5316 1125 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1126
Charles MacNeill 5:89031b2f5316 1127
Charles MacNeill 5:89031b2f5316 1128
Charles MacNeill 5:89031b2f5316 1129 pll_period_us = VL53LX_calc_pll_period_us(fast_osc_frequency);
Charles MacNeill 5:89031b2f5316 1130
Charles MacNeill 5:89031b2f5316 1131
Charles MacNeill 5:89031b2f5316 1132
Charles MacNeill 5:89031b2f5316 1133 VL53LX_p_030 = VL53LX_decode_vcsel_period(VL53LX_p_005);
Charles MacNeill 5:89031b2f5316 1134
Charles MacNeill 5:89031b2f5316 1135
Charles MacNeill 5:89031b2f5316 1136
Charles MacNeill 5:89031b2f5316 1137 macro_period_us =
Charles MacNeill 5:89031b2f5316 1138 (uint32_t)VL53LX_MACRO_PERIOD_VCSEL_PERIODS *
Charles MacNeill 5:89031b2f5316 1139 pll_period_us;
Charles MacNeill 5:89031b2f5316 1140 macro_period_us = macro_period_us >> 6;
Charles MacNeill 5:89031b2f5316 1141
Charles MacNeill 5:89031b2f5316 1142 macro_period_us = macro_period_us * (uint32_t)VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 1143 macro_period_us = macro_period_us >> 6;
Charles MacNeill 5:89031b2f5316 1144
Charles MacNeill 5:89031b2f5316 1145
Charles MacNeill 5:89031b2f5316 1146
Charles MacNeill 5:89031b2f5316 1147 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1148
Charles MacNeill 5:89031b2f5316 1149 return macro_period_us;
Charles MacNeill 5:89031b2f5316 1150 }
Charles MacNeill 5:89031b2f5316 1151
Charles MacNeill 5:89031b2f5316 1152
Charles MacNeill 5:89031b2f5316 1153 uint16_t VL53LX_calc_range_ignore_threshold(
Charles MacNeill 5:89031b2f5316 1154 uint32_t central_rate,
Charles MacNeill 5:89031b2f5316 1155 int16_t x_gradient,
Charles MacNeill 5:89031b2f5316 1156 int16_t y_gradient,
Charles MacNeill 5:89031b2f5316 1157 uint8_t rate_mult)
Charles MacNeill 5:89031b2f5316 1158 {
Charles MacNeill 5:89031b2f5316 1159
Charles MacNeill 5:89031b2f5316 1160
Charles MacNeill 5:89031b2f5316 1161 int32_t range_ignore_thresh_int = 0;
Charles MacNeill 5:89031b2f5316 1162 uint16_t range_ignore_thresh_kcps = 0;
Charles MacNeill 5:89031b2f5316 1163 int32_t central_rate_int = 0;
Charles MacNeill 5:89031b2f5316 1164 int16_t x_gradient_int = 0;
Charles MacNeill 5:89031b2f5316 1165 int16_t y_gradient_int = 0;
Charles MacNeill 5:89031b2f5316 1166
Charles MacNeill 5:89031b2f5316 1167 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1168
Charles MacNeill 5:89031b2f5316 1169
Charles MacNeill 5:89031b2f5316 1170
Charles MacNeill 5:89031b2f5316 1171 central_rate_int = ((int32_t)central_rate * (1 << 4)) / (1000);
Charles MacNeill 5:89031b2f5316 1172
Charles MacNeill 5:89031b2f5316 1173 if (x_gradient < 0)
Charles MacNeill 5:89031b2f5316 1174 x_gradient_int = x_gradient * -1;
Charles MacNeill 5:89031b2f5316 1175
Charles MacNeill 5:89031b2f5316 1176 if (y_gradient < 0)
Charles MacNeill 5:89031b2f5316 1177 y_gradient_int = y_gradient * -1;
Charles MacNeill 5:89031b2f5316 1178
Charles MacNeill 5:89031b2f5316 1179
Charles MacNeill 5:89031b2f5316 1180
Charles MacNeill 5:89031b2f5316 1181
Charles MacNeill 5:89031b2f5316 1182
Charles MacNeill 5:89031b2f5316 1183 range_ignore_thresh_int = (8 * x_gradient_int * 4) +
Charles MacNeill 5:89031b2f5316 1184 (8 * y_gradient_int * 4);
Charles MacNeill 5:89031b2f5316 1185
Charles MacNeill 5:89031b2f5316 1186
Charles MacNeill 5:89031b2f5316 1187
Charles MacNeill 5:89031b2f5316 1188 range_ignore_thresh_int = range_ignore_thresh_int / 1000;
Charles MacNeill 5:89031b2f5316 1189
Charles MacNeill 5:89031b2f5316 1190
Charles MacNeill 5:89031b2f5316 1191
Charles MacNeill 5:89031b2f5316 1192 range_ignore_thresh_int = range_ignore_thresh_int + central_rate_int;
Charles MacNeill 5:89031b2f5316 1193
Charles MacNeill 5:89031b2f5316 1194
Charles MacNeill 5:89031b2f5316 1195
Charles MacNeill 5:89031b2f5316 1196 range_ignore_thresh_int = (int32_t)rate_mult * range_ignore_thresh_int;
Charles MacNeill 5:89031b2f5316 1197
Charles MacNeill 5:89031b2f5316 1198 range_ignore_thresh_int = (range_ignore_thresh_int + (1<<4)) / (1<<5);
Charles MacNeill 5:89031b2f5316 1199
Charles MacNeill 5:89031b2f5316 1200
Charles MacNeill 5:89031b2f5316 1201
Charles MacNeill 5:89031b2f5316 1202 if (range_ignore_thresh_int > 0xFFFF)
Charles MacNeill 5:89031b2f5316 1203 range_ignore_thresh_kcps = 0xFFFF;
Charles MacNeill 5:89031b2f5316 1204 else
Charles MacNeill 5:89031b2f5316 1205 range_ignore_thresh_kcps = (uint16_t)range_ignore_thresh_int;
Charles MacNeill 5:89031b2f5316 1206
Charles MacNeill 5:89031b2f5316 1207
Charles MacNeill 5:89031b2f5316 1208
Charles MacNeill 5:89031b2f5316 1209 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1210
Charles MacNeill 5:89031b2f5316 1211 return range_ignore_thresh_kcps;
Charles MacNeill 5:89031b2f5316 1212 }
Charles MacNeill 5:89031b2f5316 1213
Charles MacNeill 5:89031b2f5316 1214
Charles MacNeill 5:89031b2f5316 1215 uint32_t VL53LX_calc_timeout_mclks(
Charles MacNeill 5:89031b2f5316 1216 uint32_t timeout_us,
Charles MacNeill 5:89031b2f5316 1217 uint32_t macro_period_us)
Charles MacNeill 5:89031b2f5316 1218 {
Charles MacNeill 5:89031b2f5316 1219
Charles MacNeill 5:89031b2f5316 1220
Charles MacNeill 5:89031b2f5316 1221 uint32_t timeout_mclks = 0;
Charles MacNeill 5:89031b2f5316 1222
Charles MacNeill 5:89031b2f5316 1223 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1224
Charles MacNeill 5:89031b2f5316 1225 timeout_mclks =
Charles MacNeill 5:89031b2f5316 1226 ((timeout_us << 12) + (macro_period_us>>1)) /
Charles MacNeill 5:89031b2f5316 1227 macro_period_us;
Charles MacNeill 5:89031b2f5316 1228
Charles MacNeill 5:89031b2f5316 1229 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1230
Charles MacNeill 5:89031b2f5316 1231 return timeout_mclks;
Charles MacNeill 5:89031b2f5316 1232 }
Charles MacNeill 5:89031b2f5316 1233
Charles MacNeill 5:89031b2f5316 1234
Charles MacNeill 5:89031b2f5316 1235 uint16_t VL53LX_calc_encoded_timeout(
Charles MacNeill 5:89031b2f5316 1236 uint32_t timeout_us,
Charles MacNeill 5:89031b2f5316 1237 uint32_t macro_period_us)
Charles MacNeill 5:89031b2f5316 1238 {
Charles MacNeill 5:89031b2f5316 1239
Charles MacNeill 5:89031b2f5316 1240
Charles MacNeill 5:89031b2f5316 1241 uint32_t timeout_mclks = 0;
Charles MacNeill 5:89031b2f5316 1242 uint16_t timeout_encoded = 0;
Charles MacNeill 5:89031b2f5316 1243
Charles MacNeill 5:89031b2f5316 1244 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1245
Charles MacNeill 5:89031b2f5316 1246 timeout_mclks =
Charles MacNeill 5:89031b2f5316 1247 VL53LX_calc_timeout_mclks(timeout_us, macro_period_us);
Charles MacNeill 5:89031b2f5316 1248
Charles MacNeill 5:89031b2f5316 1249 timeout_encoded =
Charles MacNeill 5:89031b2f5316 1250 VL53LX_encode_timeout(timeout_mclks);
Charles MacNeill 5:89031b2f5316 1251
Charles MacNeill 5:89031b2f5316 1252
Charles MacNeill 5:89031b2f5316 1253
Charles MacNeill 5:89031b2f5316 1254 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1255
Charles MacNeill 5:89031b2f5316 1256 return timeout_encoded;
Charles MacNeill 5:89031b2f5316 1257 }
Charles MacNeill 5:89031b2f5316 1258
Charles MacNeill 5:89031b2f5316 1259
Charles MacNeill 5:89031b2f5316 1260 uint32_t VL53LX_calc_timeout_us(
Charles MacNeill 5:89031b2f5316 1261 uint32_t timeout_mclks,
Charles MacNeill 5:89031b2f5316 1262 uint32_t macro_period_us)
Charles MacNeill 5:89031b2f5316 1263 {
Charles MacNeill 5:89031b2f5316 1264
Charles MacNeill 5:89031b2f5316 1265
Charles MacNeill 5:89031b2f5316 1266 uint32_t timeout_us = 0;
Charles MacNeill 5:89031b2f5316 1267 uint64_t tmp = 0;
Charles MacNeill 5:89031b2f5316 1268
Charles MacNeill 5:89031b2f5316 1269 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1270
Charles MacNeill 5:89031b2f5316 1271 tmp = (uint64_t)timeout_mclks * (uint64_t)macro_period_us;
Charles MacNeill 5:89031b2f5316 1272 tmp += 0x00800;
Charles MacNeill 5:89031b2f5316 1273 tmp = tmp >> 12;
Charles MacNeill 5:89031b2f5316 1274
Charles MacNeill 5:89031b2f5316 1275 timeout_us = (uint32_t)tmp;
Charles MacNeill 5:89031b2f5316 1276
Charles MacNeill 5:89031b2f5316 1277
Charles MacNeill 5:89031b2f5316 1278
Charles MacNeill 5:89031b2f5316 1279 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1280
Charles MacNeill 5:89031b2f5316 1281 return timeout_us;
Charles MacNeill 5:89031b2f5316 1282 }
Charles MacNeill 5:89031b2f5316 1283
Charles MacNeill 5:89031b2f5316 1284 uint32_t VL53LX_calc_crosstalk_plane_offset_with_margin(
Charles MacNeill 5:89031b2f5316 1285 uint32_t plane_offset_kcps,
Charles MacNeill 5:89031b2f5316 1286 int16_t margin_offset_kcps)
Charles MacNeill 5:89031b2f5316 1287 {
Charles MacNeill 5:89031b2f5316 1288 uint32_t plane_offset_with_margin = 0;
Charles MacNeill 5:89031b2f5316 1289 int32_t plane_offset_kcps_temp = 0;
Charles MacNeill 5:89031b2f5316 1290
Charles MacNeill 5:89031b2f5316 1291 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1292
Charles MacNeill 5:89031b2f5316 1293 plane_offset_kcps_temp =
Charles MacNeill 5:89031b2f5316 1294 (int32_t)plane_offset_kcps +
Charles MacNeill 5:89031b2f5316 1295 (int32_t)margin_offset_kcps;
Charles MacNeill 5:89031b2f5316 1296
Charles MacNeill 5:89031b2f5316 1297 if (plane_offset_kcps_temp < 0)
Charles MacNeill 5:89031b2f5316 1298 plane_offset_kcps_temp = 0;
Charles MacNeill 5:89031b2f5316 1299 else
Charles MacNeill 5:89031b2f5316 1300 if (plane_offset_kcps_temp > 0x3FFFF)
Charles MacNeill 5:89031b2f5316 1301 plane_offset_kcps_temp = 0x3FFFF;
Charles MacNeill 5:89031b2f5316 1302
Charles MacNeill 5:89031b2f5316 1303 plane_offset_with_margin = (uint32_t) plane_offset_kcps_temp;
Charles MacNeill 5:89031b2f5316 1304
Charles MacNeill 5:89031b2f5316 1305 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1306
Charles MacNeill 5:89031b2f5316 1307 return plane_offset_with_margin;
Charles MacNeill 5:89031b2f5316 1308
Charles MacNeill 5:89031b2f5316 1309 }
Charles MacNeill 5:89031b2f5316 1310
Charles MacNeill 5:89031b2f5316 1311 uint32_t VL53LX_calc_decoded_timeout_us(
Charles MacNeill 5:89031b2f5316 1312 uint16_t timeout_encoded,
Charles MacNeill 5:89031b2f5316 1313 uint32_t macro_period_us)
Charles MacNeill 5:89031b2f5316 1314 {
Charles MacNeill 5:89031b2f5316 1315
Charles MacNeill 5:89031b2f5316 1316
Charles MacNeill 5:89031b2f5316 1317 uint32_t timeout_mclks = 0;
Charles MacNeill 5:89031b2f5316 1318 uint32_t timeout_us = 0;
Charles MacNeill 5:89031b2f5316 1319
Charles MacNeill 5:89031b2f5316 1320 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1321
Charles MacNeill 5:89031b2f5316 1322 timeout_mclks =
Charles MacNeill 5:89031b2f5316 1323 VL53LX_decode_timeout(timeout_encoded);
Charles MacNeill 5:89031b2f5316 1324
Charles MacNeill 5:89031b2f5316 1325 timeout_us =
Charles MacNeill 5:89031b2f5316 1326 VL53LX_calc_timeout_us(timeout_mclks, macro_period_us);
Charles MacNeill 5:89031b2f5316 1327
Charles MacNeill 5:89031b2f5316 1328 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1329
Charles MacNeill 5:89031b2f5316 1330 return timeout_us;
Charles MacNeill 5:89031b2f5316 1331 }
Charles MacNeill 5:89031b2f5316 1332
Charles MacNeill 5:89031b2f5316 1333
Charles MacNeill 5:89031b2f5316 1334 uint16_t VL53LX_encode_timeout(uint32_t timeout_mclks)
Charles MacNeill 5:89031b2f5316 1335 {
Charles MacNeill 5:89031b2f5316 1336
Charles MacNeill 5:89031b2f5316 1337
Charles MacNeill 5:89031b2f5316 1338 uint16_t encoded_timeout = 0;
Charles MacNeill 5:89031b2f5316 1339 uint32_t ls_byte = 0;
Charles MacNeill 5:89031b2f5316 1340 uint16_t ms_byte = 0;
Charles MacNeill 5:89031b2f5316 1341
Charles MacNeill 5:89031b2f5316 1342 if (timeout_mclks > 0) {
Charles MacNeill 5:89031b2f5316 1343 ls_byte = timeout_mclks - 1;
Charles MacNeill 5:89031b2f5316 1344
Charles MacNeill 5:89031b2f5316 1345 while ((ls_byte & 0xFFFFFF00) > 0) {
Charles MacNeill 5:89031b2f5316 1346 ls_byte = ls_byte >> 1;
Charles MacNeill 5:89031b2f5316 1347 ms_byte++;
Charles MacNeill 5:89031b2f5316 1348 }
Charles MacNeill 5:89031b2f5316 1349
Charles MacNeill 5:89031b2f5316 1350 encoded_timeout = (ms_byte << 8)
Charles MacNeill 5:89031b2f5316 1351 + (uint16_t) (ls_byte & 0x000000FF);
Charles MacNeill 5:89031b2f5316 1352 }
Charles MacNeill 5:89031b2f5316 1353
Charles MacNeill 5:89031b2f5316 1354 return encoded_timeout;
Charles MacNeill 5:89031b2f5316 1355 }
Charles MacNeill 5:89031b2f5316 1356
Charles MacNeill 5:89031b2f5316 1357
Charles MacNeill 5:89031b2f5316 1358 uint32_t VL53LX_decode_timeout(uint16_t encoded_timeout)
Charles MacNeill 5:89031b2f5316 1359 {
Charles MacNeill 5:89031b2f5316 1360
Charles MacNeill 5:89031b2f5316 1361
Charles MacNeill 5:89031b2f5316 1362 uint32_t timeout_macro_clks = 0;
Charles MacNeill 5:89031b2f5316 1363
Charles MacNeill 5:89031b2f5316 1364 timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
Charles MacNeill 5:89031b2f5316 1365 << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1;
Charles MacNeill 5:89031b2f5316 1366
Charles MacNeill 5:89031b2f5316 1367 return timeout_macro_clks;
Charles MacNeill 5:89031b2f5316 1368 }
Charles MacNeill 5:89031b2f5316 1369
Charles MacNeill 5:89031b2f5316 1370
Charles MacNeill 5:89031b2f5316 1371 VL53LX_Error VL53LX_calc_timeout_register_values(
Charles MacNeill 5:89031b2f5316 1372 uint32_t phasecal_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1373 uint32_t mm_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1374 uint32_t range_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1375 uint16_t fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 1376 VL53LX_general_config_t *pgeneral,
Charles MacNeill 5:89031b2f5316 1377 VL53LX_timing_config_t *ptiming)
Charles MacNeill 5:89031b2f5316 1378 {
Charles MacNeill 5:89031b2f5316 1379
Charles MacNeill 5:89031b2f5316 1380
Charles MacNeill 5:89031b2f5316 1381 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1382
Charles MacNeill 5:89031b2f5316 1383 uint32_t macro_period_us = 0;
Charles MacNeill 5:89031b2f5316 1384 uint32_t timeout_mclks = 0;
Charles MacNeill 5:89031b2f5316 1385 uint16_t timeout_encoded = 0;
Charles MacNeill 5:89031b2f5316 1386
Charles MacNeill 5:89031b2f5316 1387 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1388
Charles MacNeill 5:89031b2f5316 1389 if (fast_osc_frequency == 0) {
Charles MacNeill 5:89031b2f5316 1390 status = VL53LX_ERROR_DIVISION_BY_ZERO;
Charles MacNeill 5:89031b2f5316 1391 } else {
Charles MacNeill 5:89031b2f5316 1392
Charles MacNeill 5:89031b2f5316 1393 macro_period_us =
Charles MacNeill 5:89031b2f5316 1394 VL53LX_calc_macro_period_us(
Charles MacNeill 5:89031b2f5316 1395 fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 1396 ptiming->range_config__vcsel_period_a);
Charles MacNeill 5:89031b2f5316 1397
Charles MacNeill 5:89031b2f5316 1398
Charles MacNeill 5:89031b2f5316 1399 timeout_mclks =
Charles MacNeill 5:89031b2f5316 1400 VL53LX_calc_timeout_mclks(
Charles MacNeill 5:89031b2f5316 1401 phasecal_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1402 macro_period_us);
Charles MacNeill 5:89031b2f5316 1403
Charles MacNeill 5:89031b2f5316 1404
Charles MacNeill 5:89031b2f5316 1405 if (timeout_mclks > 0xFF)
Charles MacNeill 5:89031b2f5316 1406 timeout_mclks = 0xFF;
Charles MacNeill 5:89031b2f5316 1407
Charles MacNeill 5:89031b2f5316 1408 pgeneral->phasecal_config__timeout_macrop =
Charles MacNeill 5:89031b2f5316 1409 (uint8_t)timeout_mclks;
Charles MacNeill 5:89031b2f5316 1410
Charles MacNeill 5:89031b2f5316 1411
Charles MacNeill 5:89031b2f5316 1412 timeout_encoded =
Charles MacNeill 5:89031b2f5316 1413 VL53LX_calc_encoded_timeout(
Charles MacNeill 5:89031b2f5316 1414 mm_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1415 macro_period_us);
Charles MacNeill 5:89031b2f5316 1416
Charles MacNeill 5:89031b2f5316 1417 ptiming->mm_config__timeout_macrop_a_hi =
Charles MacNeill 5:89031b2f5316 1418 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
Charles MacNeill 5:89031b2f5316 1419 ptiming->mm_config__timeout_macrop_a_lo =
Charles MacNeill 5:89031b2f5316 1420 (uint8_t) (timeout_encoded & 0x00FF);
Charles MacNeill 5:89031b2f5316 1421
Charles MacNeill 5:89031b2f5316 1422
Charles MacNeill 5:89031b2f5316 1423 timeout_encoded =
Charles MacNeill 5:89031b2f5316 1424 VL53LX_calc_encoded_timeout(
Charles MacNeill 5:89031b2f5316 1425 range_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1426 macro_period_us);
Charles MacNeill 5:89031b2f5316 1427
Charles MacNeill 5:89031b2f5316 1428 ptiming->range_config__timeout_macrop_a_hi =
Charles MacNeill 5:89031b2f5316 1429 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
Charles MacNeill 5:89031b2f5316 1430 ptiming->range_config__timeout_macrop_a_lo =
Charles MacNeill 5:89031b2f5316 1431 (uint8_t) (timeout_encoded & 0x00FF);
Charles MacNeill 5:89031b2f5316 1432
Charles MacNeill 5:89031b2f5316 1433
Charles MacNeill 5:89031b2f5316 1434 macro_period_us =
Charles MacNeill 5:89031b2f5316 1435 VL53LX_calc_macro_period_us(
Charles MacNeill 5:89031b2f5316 1436 fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 1437 ptiming->range_config__vcsel_period_b);
Charles MacNeill 5:89031b2f5316 1438
Charles MacNeill 5:89031b2f5316 1439
Charles MacNeill 5:89031b2f5316 1440 timeout_encoded =
Charles MacNeill 5:89031b2f5316 1441 VL53LX_calc_encoded_timeout(
Charles MacNeill 5:89031b2f5316 1442 mm_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1443 macro_period_us);
Charles MacNeill 5:89031b2f5316 1444
Charles MacNeill 5:89031b2f5316 1445 ptiming->mm_config__timeout_macrop_b_hi =
Charles MacNeill 5:89031b2f5316 1446 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
Charles MacNeill 5:89031b2f5316 1447 ptiming->mm_config__timeout_macrop_b_lo =
Charles MacNeill 5:89031b2f5316 1448 (uint8_t) (timeout_encoded & 0x00FF);
Charles MacNeill 5:89031b2f5316 1449
Charles MacNeill 5:89031b2f5316 1450
Charles MacNeill 5:89031b2f5316 1451 timeout_encoded = VL53LX_calc_encoded_timeout(
Charles MacNeill 5:89031b2f5316 1452 range_config_timeout_us,
Charles MacNeill 5:89031b2f5316 1453 macro_period_us);
Charles MacNeill 5:89031b2f5316 1454
Charles MacNeill 5:89031b2f5316 1455 ptiming->range_config__timeout_macrop_b_hi =
Charles MacNeill 5:89031b2f5316 1456 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
Charles MacNeill 5:89031b2f5316 1457 ptiming->range_config__timeout_macrop_b_lo =
Charles MacNeill 5:89031b2f5316 1458 (uint8_t) (timeout_encoded & 0x00FF);
Charles MacNeill 5:89031b2f5316 1459 }
Charles MacNeill 5:89031b2f5316 1460
Charles MacNeill 5:89031b2f5316 1461 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1462
Charles MacNeill 5:89031b2f5316 1463 return status;
Charles MacNeill 5:89031b2f5316 1464
Charles MacNeill 5:89031b2f5316 1465 }
Charles MacNeill 5:89031b2f5316 1466
Charles MacNeill 5:89031b2f5316 1467
Charles MacNeill 5:89031b2f5316 1468 uint8_t VL53LX_encode_vcsel_period(uint8_t VL53LX_p_030)
Charles MacNeill 5:89031b2f5316 1469 {
Charles MacNeill 5:89031b2f5316 1470
Charles MacNeill 5:89031b2f5316 1471
Charles MacNeill 5:89031b2f5316 1472 uint8_t vcsel_period_reg = 0;
Charles MacNeill 5:89031b2f5316 1473
Charles MacNeill 5:89031b2f5316 1474 vcsel_period_reg = (VL53LX_p_030 >> 1) - 1;
Charles MacNeill 5:89031b2f5316 1475
Charles MacNeill 5:89031b2f5316 1476 return vcsel_period_reg;
Charles MacNeill 5:89031b2f5316 1477 }
Charles MacNeill 5:89031b2f5316 1478
Charles MacNeill 5:89031b2f5316 1479
Charles MacNeill 5:89031b2f5316 1480 uint32_t VL53LX_decode_unsigned_integer(
Charles MacNeill 5:89031b2f5316 1481 uint8_t *pbuffer,
Charles MacNeill 5:89031b2f5316 1482 uint8_t no_of_bytes)
Charles MacNeill 5:89031b2f5316 1483 {
Charles MacNeill 5:89031b2f5316 1484
Charles MacNeill 5:89031b2f5316 1485
Charles MacNeill 5:89031b2f5316 1486 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 1487 uint32_t decoded_value = 0;
Charles MacNeill 5:89031b2f5316 1488
Charles MacNeill 5:89031b2f5316 1489 for (i = 0; i < no_of_bytes; i++)
Charles MacNeill 5:89031b2f5316 1490 decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i];
Charles MacNeill 5:89031b2f5316 1491
Charles MacNeill 5:89031b2f5316 1492 return decoded_value;
Charles MacNeill 5:89031b2f5316 1493 }
Charles MacNeill 5:89031b2f5316 1494
Charles MacNeill 5:89031b2f5316 1495
Charles MacNeill 5:89031b2f5316 1496 void VL53LX_encode_unsigned_integer(
Charles MacNeill 5:89031b2f5316 1497 uint32_t ip_value,
Charles MacNeill 5:89031b2f5316 1498 uint8_t no_of_bytes,
Charles MacNeill 5:89031b2f5316 1499 uint8_t *pbuffer)
Charles MacNeill 5:89031b2f5316 1500 {
Charles MacNeill 5:89031b2f5316 1501
Charles MacNeill 5:89031b2f5316 1502
Charles MacNeill 5:89031b2f5316 1503 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 1504 uint32_t VL53LX_p_003 = 0;
Charles MacNeill 5:89031b2f5316 1505
Charles MacNeill 5:89031b2f5316 1506 VL53LX_p_003 = ip_value;
Charles MacNeill 5:89031b2f5316 1507 for (i = 0; i < no_of_bytes; i++) {
Charles MacNeill 5:89031b2f5316 1508 pbuffer[no_of_bytes-i-1] = VL53LX_p_003 & 0x00FF;
Charles MacNeill 5:89031b2f5316 1509 VL53LX_p_003 = VL53LX_p_003 >> 8;
Charles MacNeill 5:89031b2f5316 1510 }
Charles MacNeill 5:89031b2f5316 1511 }
Charles MacNeill 5:89031b2f5316 1512
Charles MacNeill 5:89031b2f5316 1513
Charles MacNeill 5:89031b2f5316 1514 VL53LX_Error VL53LX_hist_copy_and_scale_ambient_info(
Charles MacNeill 5:89031b2f5316 1515 VL53LX_zone_hist_info_t *pidata,
Charles MacNeill 5:89031b2f5316 1516 VL53LX_histogram_bin_data_t *podata)
Charles MacNeill 5:89031b2f5316 1517 {
Charles MacNeill 5:89031b2f5316 1518
Charles MacNeill 5:89031b2f5316 1519
Charles MacNeill 5:89031b2f5316 1520 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1521
Charles MacNeill 5:89031b2f5316 1522 int64_t evts = 0;
Charles MacNeill 5:89031b2f5316 1523 int64_t tmpi = 0;
Charles MacNeill 5:89031b2f5316 1524 int64_t tmpo = 0;
Charles MacNeill 5:89031b2f5316 1525
Charles MacNeill 5:89031b2f5316 1526 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1527
Charles MacNeill 5:89031b2f5316 1528
Charles MacNeill 5:89031b2f5316 1529 if (pidata->result__dss_actual_effective_spads == 0) {
Charles MacNeill 5:89031b2f5316 1530 status = VL53LX_ERROR_DIVISION_BY_ZERO;
Charles MacNeill 5:89031b2f5316 1531 } else {
Charles MacNeill 5:89031b2f5316 1532 if (pidata->number_of_ambient_bins > 0 &&
Charles MacNeill 5:89031b2f5316 1533 podata->number_of_ambient_bins == 0) {
Charles MacNeill 5:89031b2f5316 1534
Charles MacNeill 5:89031b2f5316 1535
Charles MacNeill 5:89031b2f5316 1536
Charles MacNeill 5:89031b2f5316 1537 tmpo = 1 + (int64_t)podata->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 1538 tmpo *=
Charles MacNeill 5:89031b2f5316 1539 (int64_t)podata->result__dss_actual_effective_spads;
Charles MacNeill 5:89031b2f5316 1540
Charles MacNeill 5:89031b2f5316 1541 tmpi = 1 + (int64_t)pidata->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 1542 tmpi *=
Charles MacNeill 5:89031b2f5316 1543 (int64_t)pidata->result__dss_actual_effective_spads;
Charles MacNeill 5:89031b2f5316 1544
Charles MacNeill 5:89031b2f5316 1545 evts = tmpo *
Charles MacNeill 5:89031b2f5316 1546 (int64_t)pidata->ambient_events_sum;
Charles MacNeill 5:89031b2f5316 1547 evts += (tmpi/2);
Charles MacNeill 5:89031b2f5316 1548
Charles MacNeill 5:89031b2f5316 1549
Charles MacNeill 5:89031b2f5316 1550 if (tmpi != 0)
Charles MacNeill 5:89031b2f5316 1551 evts = do_division_s(evts, tmpi);
Charles MacNeill 5:89031b2f5316 1552
Charles MacNeill 5:89031b2f5316 1553 podata->ambient_events_sum = (int32_t)evts;
Charles MacNeill 5:89031b2f5316 1554
Charles MacNeill 5:89031b2f5316 1555
Charles MacNeill 5:89031b2f5316 1556
Charles MacNeill 5:89031b2f5316 1557 podata->VL53LX_p_028 =
Charles MacNeill 5:89031b2f5316 1558 podata->ambient_events_sum;
Charles MacNeill 5:89031b2f5316 1559 podata->VL53LX_p_028 +=
Charles MacNeill 5:89031b2f5316 1560 ((int32_t)pidata->number_of_ambient_bins / 2);
Charles MacNeill 5:89031b2f5316 1561 podata->VL53LX_p_028 /=
Charles MacNeill 5:89031b2f5316 1562 (int32_t)pidata->number_of_ambient_bins;
Charles MacNeill 5:89031b2f5316 1563 }
Charles MacNeill 5:89031b2f5316 1564 }
Charles MacNeill 5:89031b2f5316 1565
Charles MacNeill 5:89031b2f5316 1566 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1567
Charles MacNeill 5:89031b2f5316 1568 return status;
Charles MacNeill 5:89031b2f5316 1569 }
Charles MacNeill 5:89031b2f5316 1570
Charles MacNeill 5:89031b2f5316 1571
Charles MacNeill 5:89031b2f5316 1572 void VL53LX_hist_get_bin_sequence_config(
Charles MacNeill 5:89031b2f5316 1573 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1574 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 1575 {
Charles MacNeill 5:89031b2f5316 1576
Charles MacNeill 5:89031b2f5316 1577
Charles MacNeill 5:89031b2f5316 1578 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1579
Charles MacNeill 5:89031b2f5316 1580 int32_t amb_thresh_low = 0;
Charles MacNeill 5:89031b2f5316 1581 int32_t amb_thresh_high = 0;
Charles MacNeill 5:89031b2f5316 1582
Charles MacNeill 5:89031b2f5316 1583 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 1584
Charles MacNeill 5:89031b2f5316 1585 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1586
Charles MacNeill 5:89031b2f5316 1587
Charles MacNeill 5:89031b2f5316 1588
Charles MacNeill 5:89031b2f5316 1589 amb_thresh_low = 1024 *
Charles MacNeill 5:89031b2f5316 1590 (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_low;
Charles MacNeill 5:89031b2f5316 1591 amb_thresh_high = 1024 *
Charles MacNeill 5:89031b2f5316 1592 (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_high;
Charles MacNeill 5:89031b2f5316 1593
Charles MacNeill 5:89031b2f5316 1594
Charles MacNeill 5:89031b2f5316 1595
Charles MacNeill 5:89031b2f5316 1596 if ((pdev->ll_state.rd_stream_count & 0x01) == 0) {
Charles MacNeill 5:89031b2f5316 1597
Charles MacNeill 5:89031b2f5316 1598 pdata->bin_seq[5] =
Charles MacNeill 5:89031b2f5316 1599 pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 >> 4;
Charles MacNeill 5:89031b2f5316 1600 pdata->bin_seq[4] =
Charles MacNeill 5:89031b2f5316 1601 pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 & 0x0F;
Charles MacNeill 5:89031b2f5316 1602 pdata->bin_seq[3] =
Charles MacNeill 5:89031b2f5316 1603 pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 >> 4;
Charles MacNeill 5:89031b2f5316 1604 pdata->bin_seq[2] =
Charles MacNeill 5:89031b2f5316 1605 pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 & 0x0F;
Charles MacNeill 5:89031b2f5316 1606 pdata->bin_seq[1] =
Charles MacNeill 5:89031b2f5316 1607 pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 >> 4;
Charles MacNeill 5:89031b2f5316 1608 pdata->bin_seq[0] =
Charles MacNeill 5:89031b2f5316 1609 pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 & 0x0F;
Charles MacNeill 5:89031b2f5316 1610
Charles MacNeill 5:89031b2f5316 1611 if (pdata->ambient_events_sum > amb_thresh_high) {
Charles MacNeill 5:89031b2f5316 1612 pdata->bin_seq[5] =
Charles MacNeill 5:89031b2f5316 1613 pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5
Charles MacNeill 5:89031b2f5316 1614 >> 4;
Charles MacNeill 5:89031b2f5316 1615 pdata->bin_seq[4] =
Charles MacNeill 5:89031b2f5316 1616 pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5
Charles MacNeill 5:89031b2f5316 1617 & 0x0F;
Charles MacNeill 5:89031b2f5316 1618 pdata->bin_seq[3] =
Charles MacNeill 5:89031b2f5316 1619 pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3
Charles MacNeill 5:89031b2f5316 1620 >> 4;
Charles MacNeill 5:89031b2f5316 1621 pdata->bin_seq[2] =
Charles MacNeill 5:89031b2f5316 1622 pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3
Charles MacNeill 5:89031b2f5316 1623 & 0x0F;
Charles MacNeill 5:89031b2f5316 1624 pdata->bin_seq[1] =
Charles MacNeill 5:89031b2f5316 1625 pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1
Charles MacNeill 5:89031b2f5316 1626 >> 4;
Charles MacNeill 5:89031b2f5316 1627 pdata->bin_seq[0] =
Charles MacNeill 5:89031b2f5316 1628 pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1
Charles MacNeill 5:89031b2f5316 1629 & 0x0F;
Charles MacNeill 5:89031b2f5316 1630 }
Charles MacNeill 5:89031b2f5316 1631
Charles MacNeill 5:89031b2f5316 1632 if (pdata->ambient_events_sum < amb_thresh_low) {
Charles MacNeill 5:89031b2f5316 1633 pdata->bin_seq[5] =
Charles MacNeill 5:89031b2f5316 1634 pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5
Charles MacNeill 5:89031b2f5316 1635 >> 4;
Charles MacNeill 5:89031b2f5316 1636 pdata->bin_seq[4] =
Charles MacNeill 5:89031b2f5316 1637 pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5
Charles MacNeill 5:89031b2f5316 1638 & 0x0F;
Charles MacNeill 5:89031b2f5316 1639 pdata->bin_seq[3] =
Charles MacNeill 5:89031b2f5316 1640 pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3
Charles MacNeill 5:89031b2f5316 1641 >> 4;
Charles MacNeill 5:89031b2f5316 1642 pdata->bin_seq[2] =
Charles MacNeill 5:89031b2f5316 1643 pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3
Charles MacNeill 5:89031b2f5316 1644 & 0x0F;
Charles MacNeill 5:89031b2f5316 1645 pdata->bin_seq[1] =
Charles MacNeill 5:89031b2f5316 1646 pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1
Charles MacNeill 5:89031b2f5316 1647 >> 4;
Charles MacNeill 5:89031b2f5316 1648 pdata->bin_seq[0] =
Charles MacNeill 5:89031b2f5316 1649 pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1
Charles MacNeill 5:89031b2f5316 1650 & 0x0F;
Charles MacNeill 5:89031b2f5316 1651 }
Charles MacNeill 5:89031b2f5316 1652
Charles MacNeill 5:89031b2f5316 1653 } else {
Charles MacNeill 5:89031b2f5316 1654 pdata->bin_seq[5] =
Charles MacNeill 5:89031b2f5316 1655 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_5
Charles MacNeill 5:89031b2f5316 1656 & 0x0F;
Charles MacNeill 5:89031b2f5316 1657 pdata->bin_seq[4] =
Charles MacNeill 5:89031b2f5316 1658 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4
Charles MacNeill 5:89031b2f5316 1659 & 0x0F;
Charles MacNeill 5:89031b2f5316 1660 pdata->bin_seq[3] =
Charles MacNeill 5:89031b2f5316 1661 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4
Charles MacNeill 5:89031b2f5316 1662 >> 4;
Charles MacNeill 5:89031b2f5316 1663 pdata->bin_seq[2] =
Charles MacNeill 5:89031b2f5316 1664 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_2 &
Charles MacNeill 5:89031b2f5316 1665 0x0F;
Charles MacNeill 5:89031b2f5316 1666 pdata->bin_seq[1] =
Charles MacNeill 5:89031b2f5316 1667 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 1668 >> 4;
Charles MacNeill 5:89031b2f5316 1669 pdata->bin_seq[0] =
Charles MacNeill 5:89031b2f5316 1670 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 1671 & 0x0F;
Charles MacNeill 5:89031b2f5316 1672
Charles MacNeill 5:89031b2f5316 1673 if (pdata->ambient_events_sum > amb_thresh_high) {
Charles MacNeill 5:89031b2f5316 1674 pdata->bin_seq[5] =
Charles MacNeill 5:89031b2f5316 1675 pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5
Charles MacNeill 5:89031b2f5316 1676 >> 4;
Charles MacNeill 5:89031b2f5316 1677 pdata->bin_seq[4] =
Charles MacNeill 5:89031b2f5316 1678 pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5
Charles MacNeill 5:89031b2f5316 1679 & 0x0F;
Charles MacNeill 5:89031b2f5316 1680 pdata->bin_seq[3] =
Charles MacNeill 5:89031b2f5316 1681 pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3
Charles MacNeill 5:89031b2f5316 1682 >> 4;
Charles MacNeill 5:89031b2f5316 1683 pdata->bin_seq[2] =
Charles MacNeill 5:89031b2f5316 1684 pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3
Charles MacNeill 5:89031b2f5316 1685 & 0x0F;
Charles MacNeill 5:89031b2f5316 1686 pdata->bin_seq[1] =
Charles MacNeill 5:89031b2f5316 1687 pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 1688 >> 4;
Charles MacNeill 5:89031b2f5316 1689 pdata->bin_seq[0] =
Charles MacNeill 5:89031b2f5316 1690 pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 1691 & 0x0F;
Charles MacNeill 5:89031b2f5316 1692 }
Charles MacNeill 5:89031b2f5316 1693
Charles MacNeill 5:89031b2f5316 1694 if (pdata->ambient_events_sum < amb_thresh_low) {
Charles MacNeill 5:89031b2f5316 1695 pdata->bin_seq[5] =
Charles MacNeill 5:89031b2f5316 1696 pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5
Charles MacNeill 5:89031b2f5316 1697 >> 4;
Charles MacNeill 5:89031b2f5316 1698 pdata->bin_seq[4] =
Charles MacNeill 5:89031b2f5316 1699 pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5
Charles MacNeill 5:89031b2f5316 1700 & 0x0F;
Charles MacNeill 5:89031b2f5316 1701 pdata->bin_seq[3] =
Charles MacNeill 5:89031b2f5316 1702 pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3
Charles MacNeill 5:89031b2f5316 1703 >> 4;
Charles MacNeill 5:89031b2f5316 1704 pdata->bin_seq[2] =
Charles MacNeill 5:89031b2f5316 1705 pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3
Charles MacNeill 5:89031b2f5316 1706 & 0x0F;
Charles MacNeill 5:89031b2f5316 1707 pdata->bin_seq[1] =
Charles MacNeill 5:89031b2f5316 1708 pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 1709 >> 4;
Charles MacNeill 5:89031b2f5316 1710 pdata->bin_seq[0] =
Charles MacNeill 5:89031b2f5316 1711 pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1
Charles MacNeill 5:89031b2f5316 1712 & 0x0F;
Charles MacNeill 5:89031b2f5316 1713 }
Charles MacNeill 5:89031b2f5316 1714 }
Charles MacNeill 5:89031b2f5316 1715
Charles MacNeill 5:89031b2f5316 1716
Charles MacNeill 5:89031b2f5316 1717
Charles MacNeill 5:89031b2f5316 1718 for (i = 0; i < VL53LX_MAX_BIN_SEQUENCE_LENGTH; i++)
Charles MacNeill 5:89031b2f5316 1719 pdata->bin_rep[i] = 1;
Charles MacNeill 5:89031b2f5316 1720
Charles MacNeill 5:89031b2f5316 1721 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 1722
Charles MacNeill 5:89031b2f5316 1723 }
Charles MacNeill 5:89031b2f5316 1724
Charles MacNeill 5:89031b2f5316 1725
Charles MacNeill 5:89031b2f5316 1726 VL53LX_Error VL53LX_hist_phase_consistency_check(
Charles MacNeill 5:89031b2f5316 1727 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 1728 VL53LX_zone_hist_info_t *phist_prev,
Charles MacNeill 5:89031b2f5316 1729 VL53LX_zone_objects_t *prange_prev,
Charles MacNeill 5:89031b2f5316 1730 VL53LX_range_results_t *prange_curr)
Charles MacNeill 5:89031b2f5316 1731 {
Charles MacNeill 5:89031b2f5316 1732
Charles MacNeill 5:89031b2f5316 1733
Charles MacNeill 5:89031b2f5316 1734
Charles MacNeill 5:89031b2f5316 1735 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1736 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 1737 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 1738
Charles MacNeill 5:89031b2f5316 1739 uint8_t lc = 0;
Charles MacNeill 5:89031b2f5316 1740 uint8_t p = 0;
Charles MacNeill 5:89031b2f5316 1741
Charles MacNeill 5:89031b2f5316 1742 uint16_t phase_delta = 0;
Charles MacNeill 5:89031b2f5316 1743 uint16_t phase_tolerance = 0;
Charles MacNeill 5:89031b2f5316 1744
Charles MacNeill 5:89031b2f5316 1745 int32_t events_delta = 0;
Charles MacNeill 5:89031b2f5316 1746 int32_t events_tolerance = 0;
Charles MacNeill 5:89031b2f5316 1747
Charles MacNeill 5:89031b2f5316 1748
Charles MacNeill 5:89031b2f5316 1749 uint8_t event_sigma;
Charles MacNeill 5:89031b2f5316 1750 uint16_t event_min_spad_count;
Charles MacNeill 5:89031b2f5316 1751 uint16_t min_max_tolerance;
Charles MacNeill 5:89031b2f5316 1752 uint8_t pht;
Charles MacNeill 5:89031b2f5316 1753
Charles MacNeill 5:89031b2f5316 1754 VL53LX_DeviceError range_status = 0;
Charles MacNeill 5:89031b2f5316 1755
Charles MacNeill 5:89031b2f5316 1756 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1757
Charles MacNeill 5:89031b2f5316 1758 event_sigma =
Charles MacNeill 5:89031b2f5316 1759 pdev->histpostprocess.algo__consistency_check__event_sigma;
Charles MacNeill 5:89031b2f5316 1760 event_min_spad_count =
Charles MacNeill 5:89031b2f5316 1761 pdev->histpostprocess.algo__consistency_check__event_min_spad_count;
Charles MacNeill 5:89031b2f5316 1762 min_max_tolerance =
Charles MacNeill 5:89031b2f5316 1763 pdev->histpostprocess.algo__consistency_check__min_max_tolerance;
Charles MacNeill 5:89031b2f5316 1764
Charles MacNeill 5:89031b2f5316 1765
Charles MacNeill 5:89031b2f5316 1766 pht = pdev->histpostprocess.algo__consistency_check__phase_tolerance;
Charles MacNeill 5:89031b2f5316 1767 phase_tolerance = (uint16_t)pht;
Charles MacNeill 5:89031b2f5316 1768 phase_tolerance = phase_tolerance << 8;
Charles MacNeill 5:89031b2f5316 1769
Charles MacNeill 5:89031b2f5316 1770
Charles MacNeill 5:89031b2f5316 1771
Charles MacNeill 5:89031b2f5316 1772 if (prange_prev->rd_device_state !=
Charles MacNeill 5:89031b2f5316 1773 VL53LX_DEVICESTATE_RANGING_GATHER_DATA &&
Charles MacNeill 5:89031b2f5316 1774 prange_prev->rd_device_state !=
Charles MacNeill 5:89031b2f5316 1775 VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA)
Charles MacNeill 5:89031b2f5316 1776 return status;
Charles MacNeill 5:89031b2f5316 1777
Charles MacNeill 5:89031b2f5316 1778
Charles MacNeill 5:89031b2f5316 1779
Charles MacNeill 5:89031b2f5316 1780 if (phase_tolerance == 0)
Charles MacNeill 5:89031b2f5316 1781 return status;
Charles MacNeill 5:89031b2f5316 1782
Charles MacNeill 5:89031b2f5316 1783 for (lc = 0; lc < prange_curr->active_results; lc++) {
Charles MacNeill 5:89031b2f5316 1784
Charles MacNeill 5:89031b2f5316 1785 if (!((prange_curr->VL53LX_p_003[lc].range_status ==
Charles MacNeill 5:89031b2f5316 1786 VL53LX_DEVICEERROR_RANGECOMPLETE) ||
Charles MacNeill 5:89031b2f5316 1787 (prange_curr->VL53LX_p_003[lc].range_status ==
Charles MacNeill 5:89031b2f5316 1788 VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK)))
Charles MacNeill 5:89031b2f5316 1789 continue;
Charles MacNeill 5:89031b2f5316 1790
Charles MacNeill 5:89031b2f5316 1791
Charles MacNeill 5:89031b2f5316 1792
Charles MacNeill 5:89031b2f5316 1793
Charles MacNeill 5:89031b2f5316 1794
Charles MacNeill 5:89031b2f5316 1795
Charles MacNeill 5:89031b2f5316 1796 if (prange_prev->active_objects == 0)
Charles MacNeill 5:89031b2f5316 1797 prange_curr->VL53LX_p_003[lc].range_status =
Charles MacNeill 5:89031b2f5316 1798 VL53LX_DEVICEERROR_PREV_RANGE_NO_TARGETS;
Charles MacNeill 5:89031b2f5316 1799 else
Charles MacNeill 5:89031b2f5316 1800 prange_curr->VL53LX_p_003[lc].range_status =
Charles MacNeill 5:89031b2f5316 1801 VL53LX_DEVICEERROR_PHASECONSISTENCY;
Charles MacNeill 5:89031b2f5316 1802
Charles MacNeill 5:89031b2f5316 1803
Charles MacNeill 5:89031b2f5316 1804
Charles MacNeill 5:89031b2f5316 1805
Charles MacNeill 5:89031b2f5316 1806
Charles MacNeill 5:89031b2f5316 1807 for (p = 0; p < prange_prev->active_objects; p++) {
Charles MacNeill 5:89031b2f5316 1808
Charles MacNeill 5:89031b2f5316 1809 if (prange_curr->VL53LX_p_003[lc].VL53LX_p_011 >
Charles MacNeill 5:89031b2f5316 1810 prange_prev->VL53LX_p_003[p].VL53LX_p_011) {
Charles MacNeill 5:89031b2f5316 1811 phase_delta =
Charles MacNeill 5:89031b2f5316 1812 prange_curr->VL53LX_p_003[lc].VL53LX_p_011 -
Charles MacNeill 5:89031b2f5316 1813 prange_prev->VL53LX_p_003[p].VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 1814 } else {
Charles MacNeill 5:89031b2f5316 1815 phase_delta =
Charles MacNeill 5:89031b2f5316 1816 prange_prev->VL53LX_p_003[p].VL53LX_p_011 -
Charles MacNeill 5:89031b2f5316 1817 prange_curr->VL53LX_p_003[lc].VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 1818 }
Charles MacNeill 5:89031b2f5316 1819
Charles MacNeill 5:89031b2f5316 1820 if (phase_delta < phase_tolerance) {
Charles MacNeill 5:89031b2f5316 1821
Charles MacNeill 5:89031b2f5316 1822
Charles MacNeill 5:89031b2f5316 1823
Charles MacNeill 5:89031b2f5316 1824
Charles MacNeill 5:89031b2f5316 1825
Charles MacNeill 5:89031b2f5316 1826 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 1827 status =
Charles MacNeill 5:89031b2f5316 1828 VL53LX_hist_events_consistency_check(
Charles MacNeill 5:89031b2f5316 1829 event_sigma,
Charles MacNeill 5:89031b2f5316 1830 event_min_spad_count,
Charles MacNeill 5:89031b2f5316 1831 phist_prev,
Charles MacNeill 5:89031b2f5316 1832 &(prange_prev->VL53LX_p_003[p]),
Charles MacNeill 5:89031b2f5316 1833 &(prange_curr->VL53LX_p_003[lc]),
Charles MacNeill 5:89031b2f5316 1834 &events_tolerance,
Charles MacNeill 5:89031b2f5316 1835 &events_delta,
Charles MacNeill 5:89031b2f5316 1836 &range_status);
Charles MacNeill 5:89031b2f5316 1837
Charles MacNeill 5:89031b2f5316 1838
Charles MacNeill 5:89031b2f5316 1839
Charles MacNeill 5:89031b2f5316 1840
Charles MacNeill 5:89031b2f5316 1841 if (status == VL53LX_ERROR_NONE &&
Charles MacNeill 5:89031b2f5316 1842 range_status ==
Charles MacNeill 5:89031b2f5316 1843 VL53LX_DEVICEERROR_RANGECOMPLETE)
Charles MacNeill 5:89031b2f5316 1844 status =
Charles MacNeill 5:89031b2f5316 1845 VL53LX_hist_merged_pulse_check(
Charles MacNeill 5:89031b2f5316 1846 min_max_tolerance,
Charles MacNeill 5:89031b2f5316 1847 &(prange_curr->VL53LX_p_003[lc]),
Charles MacNeill 5:89031b2f5316 1848 &range_status);
Charles MacNeill 5:89031b2f5316 1849
Charles MacNeill 5:89031b2f5316 1850 prange_curr->VL53LX_p_003[lc].range_status =
Charles MacNeill 5:89031b2f5316 1851 range_status;
Charles MacNeill 5:89031b2f5316 1852 }
Charles MacNeill 5:89031b2f5316 1853 }
Charles MacNeill 5:89031b2f5316 1854
Charles MacNeill 5:89031b2f5316 1855 }
Charles MacNeill 5:89031b2f5316 1856
Charles MacNeill 5:89031b2f5316 1857 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 1858
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_hist_events_consistency_check(
Charles MacNeill 5:89031b2f5316 1865 uint8_t event_sigma,
Charles MacNeill 5:89031b2f5316 1866 uint16_t min_effective_spad_count,
Charles MacNeill 5:89031b2f5316 1867 VL53LX_zone_hist_info_t *phist_prev,
Charles MacNeill 5:89031b2f5316 1868 VL53LX_object_data_t *prange_prev,
Charles MacNeill 5:89031b2f5316 1869 VL53LX_range_data_t *prange_curr,
Charles MacNeill 5:89031b2f5316 1870 int32_t *pevents_tolerance,
Charles MacNeill 5:89031b2f5316 1871 int32_t *pevents_delta,
Charles MacNeill 5:89031b2f5316 1872 VL53LX_DeviceError *prange_status)
Charles MacNeill 5:89031b2f5316 1873 {
Charles MacNeill 5:89031b2f5316 1874
Charles MacNeill 5:89031b2f5316 1875
Charles MacNeill 5:89031b2f5316 1876
Charles MacNeill 5:89031b2f5316 1877 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1878
Charles MacNeill 5:89031b2f5316 1879 int64_t tmpp = 0;
Charles MacNeill 5:89031b2f5316 1880 int64_t tmpc = 0;
Charles MacNeill 5:89031b2f5316 1881 int64_t events_scaler = 0;
Charles MacNeill 5:89031b2f5316 1882 int64_t events_scaler_sq = 0;
Charles MacNeill 5:89031b2f5316 1883 int64_t c_signal_events = 0;
Charles MacNeill 5:89031b2f5316 1884 int64_t c_sig_noise_sq = 0;
Charles MacNeill 5:89031b2f5316 1885 int64_t c_amb_noise_sq = 0;
Charles MacNeill 5:89031b2f5316 1886 int64_t p_amb_noise_sq = 0;
Charles MacNeill 5:89031b2f5316 1887
Charles MacNeill 5:89031b2f5316 1888 int32_t p_signal_events = 0;
Charles MacNeill 5:89031b2f5316 1889 uint32_t noise_sq_sum = 0;
Charles MacNeill 5:89031b2f5316 1890
Charles MacNeill 5:89031b2f5316 1891
Charles MacNeill 5:89031b2f5316 1892
Charles MacNeill 5:89031b2f5316 1893 if (event_sigma == 0) {
Charles MacNeill 5:89031b2f5316 1894 *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE;
Charles MacNeill 5:89031b2f5316 1895 return status;
Charles MacNeill 5:89031b2f5316 1896 }
Charles MacNeill 5:89031b2f5316 1897
Charles MacNeill 5:89031b2f5316 1898
Charles MacNeill 5:89031b2f5316 1899
Charles MacNeill 5:89031b2f5316 1900 tmpp = 1 + (int64_t)phist_prev->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 1901 tmpp *= (int64_t)phist_prev->result__dss_actual_effective_spads;
Charles MacNeill 5:89031b2f5316 1902
Charles MacNeill 5:89031b2f5316 1903
Charles MacNeill 5:89031b2f5316 1904
Charles MacNeill 5:89031b2f5316 1905 tmpc = 1 + (int64_t)prange_curr->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 1906 tmpc *= (int64_t)prange_curr->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 1907
Charles MacNeill 5:89031b2f5316 1908
Charles MacNeill 5:89031b2f5316 1909
Charles MacNeill 5:89031b2f5316 1910 events_scaler = tmpp * 4096;
Charles MacNeill 5:89031b2f5316 1911 if (tmpc != 0) {
Charles MacNeill 5:89031b2f5316 1912 events_scaler += (tmpc/2);
Charles MacNeill 5:89031b2f5316 1913 events_scaler = do_division_s(events_scaler, tmpc);
Charles MacNeill 5:89031b2f5316 1914 }
Charles MacNeill 5:89031b2f5316 1915
Charles MacNeill 5:89031b2f5316 1916 events_scaler_sq = events_scaler * events_scaler;
Charles MacNeill 5:89031b2f5316 1917 events_scaler_sq += 2048;
Charles MacNeill 5:89031b2f5316 1918 events_scaler_sq /= 4096;
Charles MacNeill 5:89031b2f5316 1919
Charles MacNeill 5:89031b2f5316 1920
Charles MacNeill 5:89031b2f5316 1921
Charles MacNeill 5:89031b2f5316 1922 c_signal_events = (int64_t)prange_curr->VL53LX_p_017;
Charles MacNeill 5:89031b2f5316 1923 c_signal_events -= (int64_t)prange_curr->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 1924 c_signal_events *= (int64_t)events_scaler;
Charles MacNeill 5:89031b2f5316 1925 c_signal_events += 2048;
Charles MacNeill 5:89031b2f5316 1926 c_signal_events /= 4096;
Charles MacNeill 5:89031b2f5316 1927
Charles MacNeill 5:89031b2f5316 1928 c_sig_noise_sq = (int64_t)events_scaler_sq;
Charles MacNeill 5:89031b2f5316 1929 c_sig_noise_sq *= (int64_t)prange_curr->VL53LX_p_017;
Charles MacNeill 5:89031b2f5316 1930 c_sig_noise_sq += 2048;
Charles MacNeill 5:89031b2f5316 1931 c_sig_noise_sq /= 4096;
Charles MacNeill 5:89031b2f5316 1932
Charles MacNeill 5:89031b2f5316 1933 c_amb_noise_sq = (int64_t)events_scaler_sq;
Charles MacNeill 5:89031b2f5316 1934 c_amb_noise_sq *= (int64_t)prange_curr->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 1935 c_amb_noise_sq += 2048;
Charles MacNeill 5:89031b2f5316 1936 c_amb_noise_sq /= 4096;
Charles MacNeill 5:89031b2f5316 1937
Charles MacNeill 5:89031b2f5316 1938
Charles MacNeill 5:89031b2f5316 1939 c_amb_noise_sq += 2;
Charles MacNeill 5:89031b2f5316 1940 c_amb_noise_sq /= 4;
Charles MacNeill 5:89031b2f5316 1941
Charles MacNeill 5:89031b2f5316 1942
Charles MacNeill 5:89031b2f5316 1943
Charles MacNeill 5:89031b2f5316 1944 p_amb_noise_sq =
Charles MacNeill 5:89031b2f5316 1945 (int64_t)prange_prev->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 1946
Charles MacNeill 5:89031b2f5316 1947
Charles MacNeill 5:89031b2f5316 1948 p_amb_noise_sq += 2;
Charles MacNeill 5:89031b2f5316 1949 p_amb_noise_sq /= 4;
Charles MacNeill 5:89031b2f5316 1950
Charles MacNeill 5:89031b2f5316 1951 noise_sq_sum =
Charles MacNeill 5:89031b2f5316 1952 (uint32_t)prange_prev->VL53LX_p_017 +
Charles MacNeill 5:89031b2f5316 1953 (uint32_t)c_sig_noise_sq +
Charles MacNeill 5:89031b2f5316 1954 (uint32_t)p_amb_noise_sq +
Charles MacNeill 5:89031b2f5316 1955 (uint32_t)c_amb_noise_sq;
Charles MacNeill 5:89031b2f5316 1956
Charles MacNeill 5:89031b2f5316 1957 *pevents_tolerance =
Charles MacNeill 5:89031b2f5316 1958 (int32_t)VL53LX_isqrt(noise_sq_sum * 16);
Charles MacNeill 5:89031b2f5316 1959
Charles MacNeill 5:89031b2f5316 1960 *pevents_tolerance *= (int32_t)event_sigma;
Charles MacNeill 5:89031b2f5316 1961 *pevents_tolerance += 32;
Charles MacNeill 5:89031b2f5316 1962 *pevents_tolerance /= 64;
Charles MacNeill 5:89031b2f5316 1963
Charles MacNeill 5:89031b2f5316 1964 p_signal_events = (int32_t)prange_prev->VL53LX_p_017;
Charles MacNeill 5:89031b2f5316 1965 p_signal_events -= (int32_t)prange_prev->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 1966
Charles MacNeill 5:89031b2f5316 1967 if ((int32_t)c_signal_events > p_signal_events)
Charles MacNeill 5:89031b2f5316 1968 *pevents_delta =
Charles MacNeill 5:89031b2f5316 1969 (int32_t)c_signal_events - p_signal_events;
Charles MacNeill 5:89031b2f5316 1970 else
Charles MacNeill 5:89031b2f5316 1971 *pevents_delta =
Charles MacNeill 5:89031b2f5316 1972 p_signal_events - (int32_t)c_signal_events;
Charles MacNeill 5:89031b2f5316 1973
Charles MacNeill 5:89031b2f5316 1974 if (*pevents_delta > *pevents_tolerance &&
Charles MacNeill 5:89031b2f5316 1975 prange_curr->VL53LX_p_004 > min_effective_spad_count)
Charles MacNeill 5:89031b2f5316 1976 *prange_status = VL53LX_DEVICEERROR_EVENTCONSISTENCY;
Charles MacNeill 5:89031b2f5316 1977 else
Charles MacNeill 5:89031b2f5316 1978 *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE;
Charles MacNeill 5:89031b2f5316 1979
Charles MacNeill 5:89031b2f5316 1980
Charles MacNeill 5:89031b2f5316 1981
Charles MacNeill 5:89031b2f5316 1982
Charles MacNeill 5:89031b2f5316 1983
Charles MacNeill 5:89031b2f5316 1984 return status;
Charles MacNeill 5:89031b2f5316 1985 }
Charles MacNeill 5:89031b2f5316 1986
Charles MacNeill 5:89031b2f5316 1987
Charles MacNeill 5:89031b2f5316 1988
Charles MacNeill 5:89031b2f5316 1989
Charles MacNeill 5:89031b2f5316 1990 VL53LX_Error VL53LX_hist_merged_pulse_check(
Charles MacNeill 5:89031b2f5316 1991 int16_t min_max_tolerance_mm,
Charles MacNeill 5:89031b2f5316 1992 VL53LX_range_data_t *pdata,
Charles MacNeill 5:89031b2f5316 1993 VL53LX_DeviceError *prange_status)
Charles MacNeill 5:89031b2f5316 1994 {
Charles MacNeill 5:89031b2f5316 1995
Charles MacNeill 5:89031b2f5316 1996
Charles MacNeill 5:89031b2f5316 1997 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1998 int16_t delta_mm = 0;
Charles MacNeill 5:89031b2f5316 1999
Charles MacNeill 5:89031b2f5316 2000 if (pdata->max_range_mm > pdata->min_range_mm)
Charles MacNeill 5:89031b2f5316 2001 delta_mm =
Charles MacNeill 5:89031b2f5316 2002 pdata->max_range_mm - pdata->min_range_mm;
Charles MacNeill 5:89031b2f5316 2003 else
Charles MacNeill 5:89031b2f5316 2004 delta_mm =
Charles MacNeill 5:89031b2f5316 2005 pdata->min_range_mm - pdata->max_range_mm;
Charles MacNeill 5:89031b2f5316 2006
Charles MacNeill 5:89031b2f5316 2007 if (min_max_tolerance_mm > 0 &&
Charles MacNeill 5:89031b2f5316 2008 delta_mm > min_max_tolerance_mm)
Charles MacNeill 5:89031b2f5316 2009 *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE;
Charles MacNeill 5:89031b2f5316 2010 else
Charles MacNeill 5:89031b2f5316 2011 *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE;
Charles MacNeill 5:89031b2f5316 2012
Charles MacNeill 5:89031b2f5316 2013 return status;
Charles MacNeill 5:89031b2f5316 2014 }
Charles MacNeill 5:89031b2f5316 2015
Charles MacNeill 5:89031b2f5316 2016
Charles MacNeill 5:89031b2f5316 2017
Charles MacNeill 5:89031b2f5316 2018
Charles MacNeill 5:89031b2f5316 2019 VL53LX_Error VL53LX_hist_xmonitor_consistency_check(
Charles MacNeill 5:89031b2f5316 2020 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 2021 VL53LX_zone_hist_info_t *phist_prev,
Charles MacNeill 5:89031b2f5316 2022 VL53LX_zone_objects_t *prange_prev,
Charles MacNeill 5:89031b2f5316 2023 VL53LX_range_data_t *prange_curr)
Charles MacNeill 5:89031b2f5316 2024 {
Charles MacNeill 5:89031b2f5316 2025
Charles MacNeill 5:89031b2f5316 2026
Charles MacNeill 5:89031b2f5316 2027 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2028 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 2029 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 2030
Charles MacNeill 5:89031b2f5316 2031 int32_t events_delta = 0;
Charles MacNeill 5:89031b2f5316 2032 int32_t events_tolerance = 0;
Charles MacNeill 5:89031b2f5316 2033 uint8_t event_sigma;
Charles MacNeill 5:89031b2f5316 2034 uint16_t min_spad_count;
Charles MacNeill 5:89031b2f5316 2035
Charles MacNeill 5:89031b2f5316 2036 event_sigma = pdev->histpostprocess.algo__crosstalk_detect_event_sigma;
Charles MacNeill 5:89031b2f5316 2037 min_spad_count =
Charles MacNeill 5:89031b2f5316 2038 pdev->histpostprocess.algo__consistency_check__event_min_spad_count;
Charles MacNeill 5:89031b2f5316 2039
Charles MacNeill 5:89031b2f5316 2040 if (prange_curr->range_status == VL53LX_DEVICEERROR_RANGECOMPLETE ||
Charles MacNeill 5:89031b2f5316 2041 prange_curr->range_status ==
Charles MacNeill 5:89031b2f5316 2042 VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ||
Charles MacNeill 5:89031b2f5316 2043 prange_curr->range_status ==
Charles MacNeill 5:89031b2f5316 2044 VL53LX_DEVICEERROR_EVENTCONSISTENCY) {
Charles MacNeill 5:89031b2f5316 2045
Charles MacNeill 5:89031b2f5316 2046 if (prange_prev->xmonitor.range_status ==
Charles MacNeill 5:89031b2f5316 2047 VL53LX_DEVICEERROR_RANGECOMPLETE ||
Charles MacNeill 5:89031b2f5316 2048 prange_prev->xmonitor.range_status ==
Charles MacNeill 5:89031b2f5316 2049 VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ||
Charles MacNeill 5:89031b2f5316 2050 prange_prev->xmonitor.range_status ==
Charles MacNeill 5:89031b2f5316 2051 VL53LX_DEVICEERROR_EVENTCONSISTENCY) {
Charles MacNeill 5:89031b2f5316 2052
Charles MacNeill 5:89031b2f5316 2053 prange_curr->range_status =
Charles MacNeill 5:89031b2f5316 2054 VL53LX_DEVICEERROR_RANGECOMPLETE;
Charles MacNeill 5:89031b2f5316 2055
Charles MacNeill 5:89031b2f5316 2056 status =
Charles MacNeill 5:89031b2f5316 2057 VL53LX_hist_events_consistency_check(
Charles MacNeill 5:89031b2f5316 2058 event_sigma,
Charles MacNeill 5:89031b2f5316 2059 min_spad_count,
Charles MacNeill 5:89031b2f5316 2060 phist_prev,
Charles MacNeill 5:89031b2f5316 2061 &(prange_prev->xmonitor),
Charles MacNeill 5:89031b2f5316 2062 prange_curr,
Charles MacNeill 5:89031b2f5316 2063 &events_tolerance,
Charles MacNeill 5:89031b2f5316 2064 &events_delta,
Charles MacNeill 5:89031b2f5316 2065 &(prange_curr->range_status));
Charles MacNeill 5:89031b2f5316 2066
Charles MacNeill 5:89031b2f5316 2067 }
Charles MacNeill 5:89031b2f5316 2068 }
Charles MacNeill 5:89031b2f5316 2069
Charles MacNeill 5:89031b2f5316 2070 return status;
Charles MacNeill 5:89031b2f5316 2071 }
Charles MacNeill 5:89031b2f5316 2072
Charles MacNeill 5:89031b2f5316 2073
Charles MacNeill 5:89031b2f5316 2074
Charles MacNeill 5:89031b2f5316 2075
Charles MacNeill 5:89031b2f5316 2076 VL53LX_Error VL53LX_hist_wrap_dmax(
Charles MacNeill 5:89031b2f5316 2077 VL53LX_hist_post_process_config_t *phistpostprocess,
Charles MacNeill 5:89031b2f5316 2078 VL53LX_histogram_bin_data_t *pcurrent,
Charles MacNeill 5:89031b2f5316 2079 int16_t *pwrap_dmax_mm)
Charles MacNeill 5:89031b2f5316 2080 {
Charles MacNeill 5:89031b2f5316 2081
Charles MacNeill 5:89031b2f5316 2082
Charles MacNeill 5:89031b2f5316 2083
Charles MacNeill 5:89031b2f5316 2084 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2085
Charles MacNeill 5:89031b2f5316 2086 uint32_t pll_period_mm = 0;
Charles MacNeill 5:89031b2f5316 2087 uint32_t wrap_dmax_phase = 0;
Charles MacNeill 5:89031b2f5316 2088 uint32_t range_mm = 0;
Charles MacNeill 5:89031b2f5316 2089
Charles MacNeill 5:89031b2f5316 2090 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2091
Charles MacNeill 5:89031b2f5316 2092 *pwrap_dmax_mm = 0;
Charles MacNeill 5:89031b2f5316 2093
Charles MacNeill 5:89031b2f5316 2094
Charles MacNeill 5:89031b2f5316 2095 if (pcurrent->VL53LX_p_015 != 0) {
Charles MacNeill 5:89031b2f5316 2096
Charles MacNeill 5:89031b2f5316 2097
Charles MacNeill 5:89031b2f5316 2098
Charles MacNeill 5:89031b2f5316 2099 pll_period_mm =
Charles MacNeill 5:89031b2f5316 2100 VL53LX_calc_pll_period_mm(
Charles MacNeill 5:89031b2f5316 2101 pcurrent->VL53LX_p_015);
Charles MacNeill 5:89031b2f5316 2102
Charles MacNeill 5:89031b2f5316 2103
Charles MacNeill 5:89031b2f5316 2104
Charles MacNeill 5:89031b2f5316 2105 wrap_dmax_phase =
Charles MacNeill 5:89031b2f5316 2106 (uint32_t)phistpostprocess->valid_phase_high << 8;
Charles MacNeill 5:89031b2f5316 2107
Charles MacNeill 5:89031b2f5316 2108
Charles MacNeill 5:89031b2f5316 2109
Charles MacNeill 5:89031b2f5316 2110 range_mm = wrap_dmax_phase * pll_period_mm;
Charles MacNeill 5:89031b2f5316 2111 range_mm = (range_mm + (1<<14)) >> 15;
Charles MacNeill 5:89031b2f5316 2112
Charles MacNeill 5:89031b2f5316 2113 *pwrap_dmax_mm = (int16_t)range_mm;
Charles MacNeill 5:89031b2f5316 2114 }
Charles MacNeill 5:89031b2f5316 2115
Charles MacNeill 5:89031b2f5316 2116 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 2117
Charles MacNeill 5:89031b2f5316 2118 return status;
Charles MacNeill 5:89031b2f5316 2119 }
Charles MacNeill 5:89031b2f5316 2120
Charles MacNeill 5:89031b2f5316 2121
Charles MacNeill 5:89031b2f5316 2122 void VL53LX_hist_combine_mm1_mm2_offsets(
Charles MacNeill 5:89031b2f5316 2123 int16_t mm1_offset_mm,
Charles MacNeill 5:89031b2f5316 2124 int16_t mm2_offset_mm,
Charles MacNeill 5:89031b2f5316 2125 uint8_t encoded_mm_roi_centre,
Charles MacNeill 5:89031b2f5316 2126 uint8_t encoded_mm_roi_size,
Charles MacNeill 5:89031b2f5316 2127 uint8_t encoded_zone_centre,
Charles MacNeill 5:89031b2f5316 2128 uint8_t encoded_zone_size,
Charles MacNeill 5:89031b2f5316 2129 VL53LX_additional_offset_cal_data_t *pcal_data,
Charles MacNeill 5:89031b2f5316 2130 uint8_t *pgood_spads,
Charles MacNeill 5:89031b2f5316 2131 uint16_t aperture_attenuation,
Charles MacNeill 5:89031b2f5316 2132 int16_t *prange_offset_mm)
Charles MacNeill 5:89031b2f5316 2133 {
Charles MacNeill 5:89031b2f5316 2134
Charles MacNeill 5:89031b2f5316 2135
Charles MacNeill 5:89031b2f5316 2136
Charles MacNeill 5:89031b2f5316 2137 uint16_t max_mm_inner_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2138 uint16_t max_mm_outer_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2139 uint16_t mm_inner_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2140 uint16_t mm_outer_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2141
Charles MacNeill 5:89031b2f5316 2142 uint32_t scaled_mm1_peak_rate_mcps = 0;
Charles MacNeill 5:89031b2f5316 2143 uint32_t scaled_mm2_peak_rate_mcps = 0;
Charles MacNeill 5:89031b2f5316 2144
Charles MacNeill 5:89031b2f5316 2145 int32_t tmp0 = 0;
Charles MacNeill 5:89031b2f5316 2146 int32_t tmp1 = 0;
Charles MacNeill 5:89031b2f5316 2147
Charles MacNeill 5:89031b2f5316 2148
Charles MacNeill 5:89031b2f5316 2149
Charles MacNeill 5:89031b2f5316 2150 VL53LX_calc_mm_effective_spads(
Charles MacNeill 5:89031b2f5316 2151 encoded_mm_roi_centre,
Charles MacNeill 5:89031b2f5316 2152 encoded_mm_roi_size,
Charles MacNeill 5:89031b2f5316 2153 0xC7,
Charles MacNeill 5:89031b2f5316 2154 0xFF,
Charles MacNeill 5:89031b2f5316 2155 pgood_spads,
Charles MacNeill 5:89031b2f5316 2156 aperture_attenuation,
Charles MacNeill 5:89031b2f5316 2157 &max_mm_inner_effective_spads,
Charles MacNeill 5:89031b2f5316 2158 &max_mm_outer_effective_spads);
Charles MacNeill 5:89031b2f5316 2159
Charles MacNeill 5:89031b2f5316 2160 if ((max_mm_inner_effective_spads == 0) ||
Charles MacNeill 5:89031b2f5316 2161 (max_mm_outer_effective_spads == 0))
Charles MacNeill 5:89031b2f5316 2162 goto FAIL;
Charles MacNeill 5:89031b2f5316 2163
Charles MacNeill 5:89031b2f5316 2164
Charles MacNeill 5:89031b2f5316 2165 VL53LX_calc_mm_effective_spads(
Charles MacNeill 5:89031b2f5316 2166 encoded_mm_roi_centre,
Charles MacNeill 5:89031b2f5316 2167 encoded_mm_roi_size,
Charles MacNeill 5:89031b2f5316 2168 encoded_zone_centre,
Charles MacNeill 5:89031b2f5316 2169 encoded_zone_size,
Charles MacNeill 5:89031b2f5316 2170 pgood_spads,
Charles MacNeill 5:89031b2f5316 2171 aperture_attenuation,
Charles MacNeill 5:89031b2f5316 2172 &mm_inner_effective_spads,
Charles MacNeill 5:89031b2f5316 2173 &mm_outer_effective_spads);
Charles MacNeill 5:89031b2f5316 2174
Charles MacNeill 5:89031b2f5316 2175
Charles MacNeill 5:89031b2f5316 2176
Charles MacNeill 5:89031b2f5316 2177 scaled_mm1_peak_rate_mcps =
Charles MacNeill 5:89031b2f5316 2178 (uint32_t)pcal_data->result__mm_inner_peak_signal_count_rtn_mcps;
Charles MacNeill 5:89031b2f5316 2179 scaled_mm1_peak_rate_mcps *= (uint32_t)mm_inner_effective_spads;
Charles MacNeill 5:89031b2f5316 2180 scaled_mm1_peak_rate_mcps /= (uint32_t)max_mm_inner_effective_spads;
Charles MacNeill 5:89031b2f5316 2181
Charles MacNeill 5:89031b2f5316 2182 scaled_mm2_peak_rate_mcps =
Charles MacNeill 5:89031b2f5316 2183 (uint32_t)pcal_data->result__mm_outer_peak_signal_count_rtn_mcps;
Charles MacNeill 5:89031b2f5316 2184 scaled_mm2_peak_rate_mcps *= (uint32_t)mm_outer_effective_spads;
Charles MacNeill 5:89031b2f5316 2185 scaled_mm2_peak_rate_mcps /= (uint32_t)max_mm_outer_effective_spads;
Charles MacNeill 5:89031b2f5316 2186
Charles MacNeill 5:89031b2f5316 2187
Charles MacNeill 5:89031b2f5316 2188
Charles MacNeill 5:89031b2f5316 2189 tmp0 = ((int32_t)mm1_offset_mm * (int32_t)scaled_mm1_peak_rate_mcps);
Charles MacNeill 5:89031b2f5316 2190 tmp0 += ((int32_t)mm2_offset_mm * (int32_t)scaled_mm2_peak_rate_mcps);
Charles MacNeill 5:89031b2f5316 2191
Charles MacNeill 5:89031b2f5316 2192 tmp1 = (int32_t)scaled_mm1_peak_rate_mcps +
Charles MacNeill 5:89031b2f5316 2193 (int32_t)scaled_mm2_peak_rate_mcps;
Charles MacNeill 5:89031b2f5316 2194
Charles MacNeill 5:89031b2f5316 2195
Charles MacNeill 5:89031b2f5316 2196
Charles MacNeill 5:89031b2f5316 2197 if (tmp1 != 0)
Charles MacNeill 5:89031b2f5316 2198 tmp0 = (tmp0 * 4) / tmp1;
Charles MacNeill 5:89031b2f5316 2199 FAIL:
Charles MacNeill 5:89031b2f5316 2200 *prange_offset_mm = (int16_t)tmp0;
Charles MacNeill 5:89031b2f5316 2201
Charles MacNeill 5:89031b2f5316 2202 }
Charles MacNeill 5:89031b2f5316 2203
Charles MacNeill 5:89031b2f5316 2204
Charles MacNeill 5:89031b2f5316 2205 VL53LX_Error VL53LX_hist_xtalk_extract_calc_window(
Charles MacNeill 5:89031b2f5316 2206 int16_t target_distance_mm,
Charles MacNeill 5:89031b2f5316 2207 uint16_t target_width_oversize,
Charles MacNeill 5:89031b2f5316 2208 VL53LX_histogram_bin_data_t *phist_bins,
Charles MacNeill 5:89031b2f5316 2209 VL53LX_hist_xtalk_extract_data_t *pxtalk_data)
Charles MacNeill 5:89031b2f5316 2210 {
Charles MacNeill 5:89031b2f5316 2211
Charles MacNeill 5:89031b2f5316 2212
Charles MacNeill 5:89031b2f5316 2213 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2214
Charles MacNeill 5:89031b2f5316 2215 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2216
Charles MacNeill 5:89031b2f5316 2217
Charles MacNeill 5:89031b2f5316 2218 pxtalk_data->pll_period_mm =
Charles MacNeill 5:89031b2f5316 2219 VL53LX_calc_pll_period_mm(phist_bins->VL53LX_p_015);
Charles MacNeill 5:89031b2f5316 2220 if (pxtalk_data->pll_period_mm == 0)
Charles MacNeill 5:89031b2f5316 2221 pxtalk_data->pll_period_mm = 1;
Charles MacNeill 5:89031b2f5316 2222
Charles MacNeill 5:89031b2f5316 2223
Charles MacNeill 5:89031b2f5316 2224 pxtalk_data->xtalk_width_phase =
Charles MacNeill 5:89031b2f5316 2225 (int32_t)phist_bins->vcsel_width * 128;
Charles MacNeill 5:89031b2f5316 2226 pxtalk_data->target_width_phase =
Charles MacNeill 5:89031b2f5316 2227 pxtalk_data->xtalk_width_phase +
Charles MacNeill 5:89031b2f5316 2228 (int32_t)target_width_oversize * 128;
Charles MacNeill 5:89031b2f5316 2229
Charles MacNeill 5:89031b2f5316 2230
Charles MacNeill 5:89031b2f5316 2231
Charles MacNeill 5:89031b2f5316 2232 pxtalk_data->xtalk_start_phase =
Charles MacNeill 5:89031b2f5316 2233 (int32_t)phist_bins->zero_distance_phase -
Charles MacNeill 5:89031b2f5316 2234 (pxtalk_data->xtalk_width_phase / 2);
Charles MacNeill 5:89031b2f5316 2235 pxtalk_data->xtalk_end_phase =
Charles MacNeill 5:89031b2f5316 2236 (int32_t)pxtalk_data->xtalk_start_phase +
Charles MacNeill 5:89031b2f5316 2237 pxtalk_data->xtalk_width_phase;
Charles MacNeill 5:89031b2f5316 2238
Charles MacNeill 5:89031b2f5316 2239 if (pxtalk_data->xtalk_start_phase < 0)
Charles MacNeill 5:89031b2f5316 2240 pxtalk_data->xtalk_start_phase = 0;
Charles MacNeill 5:89031b2f5316 2241
Charles MacNeill 5:89031b2f5316 2242
Charles MacNeill 5:89031b2f5316 2243
Charles MacNeill 5:89031b2f5316 2244
Charles MacNeill 5:89031b2f5316 2245 pxtalk_data->VL53LX_p_012 =
Charles MacNeill 5:89031b2f5316 2246 (uint8_t)(pxtalk_data->xtalk_start_phase / 2048);
Charles MacNeill 5:89031b2f5316 2247
Charles MacNeill 5:89031b2f5316 2248
Charles MacNeill 5:89031b2f5316 2249 pxtalk_data->VL53LX_p_013 =
Charles MacNeill 5:89031b2f5316 2250 (uint8_t)((pxtalk_data->xtalk_end_phase + 2047) / 2048);
Charles MacNeill 5:89031b2f5316 2251
Charles MacNeill 5:89031b2f5316 2252
Charles MacNeill 5:89031b2f5316 2253
Charles MacNeill 5:89031b2f5316 2254 pxtalk_data->target_start_phase =
Charles MacNeill 5:89031b2f5316 2255 (int32_t)target_distance_mm * 2048 * 16;
Charles MacNeill 5:89031b2f5316 2256 pxtalk_data->target_start_phase +=
Charles MacNeill 5:89031b2f5316 2257 ((int32_t)pxtalk_data->pll_period_mm / 2);
Charles MacNeill 5:89031b2f5316 2258 pxtalk_data->target_start_phase /= (int32_t)pxtalk_data->pll_period_mm;
Charles MacNeill 5:89031b2f5316 2259 pxtalk_data->target_start_phase +=
Charles MacNeill 5:89031b2f5316 2260 (int32_t)phist_bins->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 2261
Charles MacNeill 5:89031b2f5316 2262
Charles MacNeill 5:89031b2f5316 2263
Charles MacNeill 5:89031b2f5316 2264 pxtalk_data->target_start_phase -=
Charles MacNeill 5:89031b2f5316 2265 (pxtalk_data->target_width_phase / 2);
Charles MacNeill 5:89031b2f5316 2266 pxtalk_data->target_end_phase =
Charles MacNeill 5:89031b2f5316 2267 (int32_t)pxtalk_data->target_start_phase +
Charles MacNeill 5:89031b2f5316 2268 pxtalk_data->target_width_phase;
Charles MacNeill 5:89031b2f5316 2269
Charles MacNeill 5:89031b2f5316 2270 if (pxtalk_data->target_start_phase < 0)
Charles MacNeill 5:89031b2f5316 2271 pxtalk_data->target_start_phase = 0;
Charles MacNeill 5:89031b2f5316 2272
Charles MacNeill 5:89031b2f5316 2273
Charles MacNeill 5:89031b2f5316 2274 pxtalk_data->target_start =
Charles MacNeill 5:89031b2f5316 2275 (uint8_t)(pxtalk_data->target_start_phase / 2048);
Charles MacNeill 5:89031b2f5316 2276
Charles MacNeill 5:89031b2f5316 2277
Charles MacNeill 5:89031b2f5316 2278 if (pxtalk_data->VL53LX_p_013 > (pxtalk_data->target_start-1))
Charles MacNeill 5:89031b2f5316 2279 pxtalk_data->VL53LX_p_013 = pxtalk_data->target_start-1;
Charles MacNeill 5:89031b2f5316 2280
Charles MacNeill 5:89031b2f5316 2281
Charles MacNeill 5:89031b2f5316 2282 pxtalk_data->effective_width =
Charles MacNeill 5:89031b2f5316 2283 (2048 * ((int32_t)pxtalk_data->VL53LX_p_013+1));
Charles MacNeill 5:89031b2f5316 2284 pxtalk_data->effective_width -= pxtalk_data->xtalk_start_phase;
Charles MacNeill 5:89031b2f5316 2285
Charles MacNeill 5:89031b2f5316 2286
Charles MacNeill 5:89031b2f5316 2287 if (pxtalk_data->effective_width > pxtalk_data->xtalk_width_phase)
Charles MacNeill 5:89031b2f5316 2288 pxtalk_data->effective_width = pxtalk_data->xtalk_width_phase;
Charles MacNeill 5:89031b2f5316 2289
Charles MacNeill 5:89031b2f5316 2290 if (pxtalk_data->effective_width < 1)
Charles MacNeill 5:89031b2f5316 2291 pxtalk_data->effective_width = 1;
Charles MacNeill 5:89031b2f5316 2292
Charles MacNeill 5:89031b2f5316 2293
Charles MacNeill 5:89031b2f5316 2294 pxtalk_data->event_scaler = pxtalk_data->xtalk_width_phase * 1000;
Charles MacNeill 5:89031b2f5316 2295 pxtalk_data->event_scaler += (pxtalk_data->effective_width / 2);
Charles MacNeill 5:89031b2f5316 2296 pxtalk_data->event_scaler /= pxtalk_data->effective_width;
Charles MacNeill 5:89031b2f5316 2297
Charles MacNeill 5:89031b2f5316 2298
Charles MacNeill 5:89031b2f5316 2299 if (pxtalk_data->event_scaler < 1000)
Charles MacNeill 5:89031b2f5316 2300 pxtalk_data->event_scaler = 1000;
Charles MacNeill 5:89031b2f5316 2301
Charles MacNeill 5:89031b2f5316 2302 if (pxtalk_data->event_scaler > 4000)
Charles MacNeill 5:89031b2f5316 2303 pxtalk_data->event_scaler = 4000;
Charles MacNeill 5:89031b2f5316 2304
Charles MacNeill 5:89031b2f5316 2305
Charles MacNeill 5:89031b2f5316 2306 pxtalk_data->event_scaler_sum += pxtalk_data->event_scaler;
Charles MacNeill 5:89031b2f5316 2307
Charles MacNeill 5:89031b2f5316 2308
Charles MacNeill 5:89031b2f5316 2309 pxtalk_data->peak_duration_us_sum +=
Charles MacNeill 5:89031b2f5316 2310 (uint32_t)phist_bins->peak_duration_us;
Charles MacNeill 5:89031b2f5316 2311
Charles MacNeill 5:89031b2f5316 2312
Charles MacNeill 5:89031b2f5316 2313 pxtalk_data->effective_spad_count_sum +=
Charles MacNeill 5:89031b2f5316 2314 (uint32_t)phist_bins->result__dss_actual_effective_spads;
Charles MacNeill 5:89031b2f5316 2315
Charles MacNeill 5:89031b2f5316 2316
Charles MacNeill 5:89031b2f5316 2317 pxtalk_data->zero_distance_phase_sum +=
Charles MacNeill 5:89031b2f5316 2318 (uint32_t)phist_bins->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 2319
Charles MacNeill 5:89031b2f5316 2320 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 2321
Charles MacNeill 5:89031b2f5316 2322 return status;
Charles MacNeill 5:89031b2f5316 2323 }
Charles MacNeill 5:89031b2f5316 2324
Charles MacNeill 5:89031b2f5316 2325
Charles MacNeill 5:89031b2f5316 2326 VL53LX_Error VL53LX_hist_xtalk_extract_calc_event_sums(
Charles MacNeill 5:89031b2f5316 2327 VL53LX_histogram_bin_data_t *phist_bins,
Charles MacNeill 5:89031b2f5316 2328 VL53LX_hist_xtalk_extract_data_t *pxtalk_data)
Charles MacNeill 5:89031b2f5316 2329 {
Charles MacNeill 5:89031b2f5316 2330
Charles MacNeill 5:89031b2f5316 2331
Charles MacNeill 5:89031b2f5316 2332
Charles MacNeill 5:89031b2f5316 2333 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2334
Charles MacNeill 5:89031b2f5316 2335 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 2336 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 2337
Charles MacNeill 5:89031b2f5316 2338 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2339
Charles MacNeill 5:89031b2f5316 2340
Charles MacNeill 5:89031b2f5316 2341
Charles MacNeill 5:89031b2f5316 2342 for (lb = pxtalk_data->VL53LX_p_012;
Charles MacNeill 5:89031b2f5316 2343 lb <= pxtalk_data->VL53LX_p_013;
Charles MacNeill 5:89031b2f5316 2344 lb++) {
Charles MacNeill 5:89031b2f5316 2345
Charles MacNeill 5:89031b2f5316 2346
Charles MacNeill 5:89031b2f5316 2347 i = (lb + phist_bins->number_of_ambient_bins +
Charles MacNeill 5:89031b2f5316 2348 phist_bins->VL53LX_p_021) %
Charles MacNeill 5:89031b2f5316 2349 phist_bins->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 2350
Charles MacNeill 5:89031b2f5316 2351
Charles MacNeill 5:89031b2f5316 2352 pxtalk_data->signal_events_sum += phist_bins->bin_data[i];
Charles MacNeill 5:89031b2f5316 2353 pxtalk_data->signal_events_sum -=
Charles MacNeill 5:89031b2f5316 2354 phist_bins->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 2355 }
Charles MacNeill 5:89031b2f5316 2356
Charles MacNeill 5:89031b2f5316 2357
Charles MacNeill 5:89031b2f5316 2358
Charles MacNeill 5:89031b2f5316 2359 for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS &&
Charles MacNeill 5:89031b2f5316 2360 lb < phist_bins->VL53LX_p_021; lb++) {
Charles MacNeill 5:89031b2f5316 2361
Charles MacNeill 5:89031b2f5316 2362
Charles MacNeill 5:89031b2f5316 2363 i = (lb + phist_bins->number_of_ambient_bins +
Charles MacNeill 5:89031b2f5316 2364 phist_bins->VL53LX_p_021) %
Charles MacNeill 5:89031b2f5316 2365 phist_bins->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 2366
Charles MacNeill 5:89031b2f5316 2367
Charles MacNeill 5:89031b2f5316 2368 pxtalk_data->bin_data_sums[lb] += phist_bins->bin_data[i];
Charles MacNeill 5:89031b2f5316 2369 pxtalk_data->bin_data_sums[lb] -=
Charles MacNeill 5:89031b2f5316 2370 phist_bins->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 2371 }
Charles MacNeill 5:89031b2f5316 2372
Charles MacNeill 5:89031b2f5316 2373 pxtalk_data->sample_count += 1;
Charles MacNeill 5:89031b2f5316 2374
Charles MacNeill 5:89031b2f5316 2375 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 2376
Charles MacNeill 5:89031b2f5316 2377 return status;
Charles MacNeill 5:89031b2f5316 2378 }
Charles MacNeill 5:89031b2f5316 2379
Charles MacNeill 5:89031b2f5316 2380
Charles MacNeill 5:89031b2f5316 2381 VL53LX_Error VL53LX_hist_xtalk_extract_calc_rate_per_spad(
Charles MacNeill 5:89031b2f5316 2382 VL53LX_hist_xtalk_extract_data_t *pxtalk_data)
Charles MacNeill 5:89031b2f5316 2383 {
Charles MacNeill 5:89031b2f5316 2384
Charles MacNeill 5:89031b2f5316 2385
Charles MacNeill 5:89031b2f5316 2386
Charles MacNeill 5:89031b2f5316 2387 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2388
Charles MacNeill 5:89031b2f5316 2389 uint64_t tmp64_0 = 0;
Charles MacNeill 5:89031b2f5316 2390 uint64_t tmp64_1 = 0;
Charles MacNeill 5:89031b2f5316 2391 uint64_t xtalk_per_spad = 0;
Charles MacNeill 5:89031b2f5316 2392
Charles MacNeill 5:89031b2f5316 2393 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2394
Charles MacNeill 5:89031b2f5316 2395
Charles MacNeill 5:89031b2f5316 2396
Charles MacNeill 5:89031b2f5316 2397
Charles MacNeill 5:89031b2f5316 2398
Charles MacNeill 5:89031b2f5316 2399
Charles MacNeill 5:89031b2f5316 2400 if (pxtalk_data->signal_events_sum > 0) {
Charles MacNeill 5:89031b2f5316 2401 tmp64_0 =
Charles MacNeill 5:89031b2f5316 2402 ((uint64_t)pxtalk_data->signal_events_sum *
Charles MacNeill 5:89031b2f5316 2403 (uint64_t)pxtalk_data->sample_count *
Charles MacNeill 5:89031b2f5316 2404 (uint64_t)pxtalk_data->event_scaler_avg * 256U) << 9U;
Charles MacNeill 5:89031b2f5316 2405 tmp64_1 =
Charles MacNeill 5:89031b2f5316 2406 (uint64_t)pxtalk_data->effective_spad_count_sum *
Charles MacNeill 5:89031b2f5316 2407 (uint64_t)pxtalk_data->peak_duration_us_sum;
Charles MacNeill 5:89031b2f5316 2408
Charles MacNeill 5:89031b2f5316 2409
Charles MacNeill 5:89031b2f5316 2410
Charles MacNeill 5:89031b2f5316 2411 if (tmp64_1 > 0U) {
Charles MacNeill 5:89031b2f5316 2412
Charles MacNeill 5:89031b2f5316 2413 tmp64_0 = tmp64_0 + (tmp64_1 >> 1U);
Charles MacNeill 5:89031b2f5316 2414 xtalk_per_spad = do_division_u(tmp64_0, tmp64_1);
Charles MacNeill 5:89031b2f5316 2415 } else {
Charles MacNeill 5:89031b2f5316 2416 xtalk_per_spad = (uint64_t)tmp64_0;
Charles MacNeill 5:89031b2f5316 2417 }
Charles MacNeill 5:89031b2f5316 2418
Charles MacNeill 5:89031b2f5316 2419 } else {
Charles MacNeill 5:89031b2f5316 2420 status = VL53LX_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL;
Charles MacNeill 5:89031b2f5316 2421 }
Charles MacNeill 5:89031b2f5316 2422
Charles MacNeill 5:89031b2f5316 2423 pxtalk_data->xtalk_rate_kcps_per_spad = (uint32_t)xtalk_per_spad;
Charles MacNeill 5:89031b2f5316 2424
Charles MacNeill 5:89031b2f5316 2425 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 2426
Charles MacNeill 5:89031b2f5316 2427 return status;
Charles MacNeill 5:89031b2f5316 2428 }
Charles MacNeill 5:89031b2f5316 2429
Charles MacNeill 5:89031b2f5316 2430
Charles MacNeill 5:89031b2f5316 2431 VL53LX_Error VL53LX_hist_xtalk_extract_calc_shape(
Charles MacNeill 5:89031b2f5316 2432 VL53LX_hist_xtalk_extract_data_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 2433 VL53LX_xtalk_histogram_shape_t *pxtalk_shape)
Charles MacNeill 5:89031b2f5316 2434 {
Charles MacNeill 5:89031b2f5316 2435
Charles MacNeill 5:89031b2f5316 2436
Charles MacNeill 5:89031b2f5316 2437 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2438
Charles MacNeill 5:89031b2f5316 2439 int32_t lb = 0;
Charles MacNeill 5:89031b2f5316 2440 uint64_t total_events = 0U;
Charles MacNeill 5:89031b2f5316 2441 uint64_t tmp64_0 = 0U;
Charles MacNeill 5:89031b2f5316 2442 int32_t remaining_area = 1024;
Charles MacNeill 5:89031b2f5316 2443
Charles MacNeill 5:89031b2f5316 2444 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2445
Charles MacNeill 5:89031b2f5316 2446
Charles MacNeill 5:89031b2f5316 2447
Charles MacNeill 5:89031b2f5316 2448 pxtalk_shape->VL53LX_p_019 = 0;
Charles MacNeill 5:89031b2f5316 2449 pxtalk_shape->VL53LX_p_020 = VL53LX_XTALK_HISTO_BINS;
Charles MacNeill 5:89031b2f5316 2450 pxtalk_shape->VL53LX_p_021 = VL53LX_XTALK_HISTO_BINS;
Charles MacNeill 5:89031b2f5316 2451
Charles MacNeill 5:89031b2f5316 2452 pxtalk_shape->zero_distance_phase =
Charles MacNeill 5:89031b2f5316 2453 (uint16_t)pxtalk_data->zero_distance_phase_avg;
Charles MacNeill 5:89031b2f5316 2454 pxtalk_shape->phasecal_result__reference_phase =
Charles MacNeill 5:89031b2f5316 2455 (uint16_t)pxtalk_data->zero_distance_phase_avg + (3*2048);
Charles MacNeill 5:89031b2f5316 2456
Charles MacNeill 5:89031b2f5316 2457
Charles MacNeill 5:89031b2f5316 2458
Charles MacNeill 5:89031b2f5316 2459 if (pxtalk_data->signal_events_sum > 0)
Charles MacNeill 5:89031b2f5316 2460 total_events =
Charles MacNeill 5:89031b2f5316 2461 (uint64_t)pxtalk_data->signal_events_sum *
Charles MacNeill 5:89031b2f5316 2462 (uint64_t)pxtalk_data->event_scaler_avg;
Charles MacNeill 5:89031b2f5316 2463 else
Charles MacNeill 5:89031b2f5316 2464 total_events = 1;
Charles MacNeill 5:89031b2f5316 2465 if (total_events == 0)
Charles MacNeill 5:89031b2f5316 2466 total_events = 1;
Charles MacNeill 5:89031b2f5316 2467
Charles MacNeill 5:89031b2f5316 2468
Charles MacNeill 5:89031b2f5316 2469 remaining_area = 1024;
Charles MacNeill 5:89031b2f5316 2470 pxtalk_data->max_shape_value = 0;
Charles MacNeill 5:89031b2f5316 2471
Charles MacNeill 5:89031b2f5316 2472 for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS; lb++) {
Charles MacNeill 5:89031b2f5316 2473
Charles MacNeill 5:89031b2f5316 2474 if ((lb < (int32_t)pxtalk_data->VL53LX_p_012 ||
Charles MacNeill 5:89031b2f5316 2475 lb > (int32_t)pxtalk_data->VL53LX_p_013) ||
Charles MacNeill 5:89031b2f5316 2476 pxtalk_data->bin_data_sums[lb] < 0) {
Charles MacNeill 5:89031b2f5316 2477
Charles MacNeill 5:89031b2f5316 2478
Charles MacNeill 5:89031b2f5316 2479 if (remaining_area > 0 && remaining_area < 1024) {
Charles MacNeill 5:89031b2f5316 2480 if (remaining_area >
Charles MacNeill 5:89031b2f5316 2481 pxtalk_data->max_shape_value) {
Charles MacNeill 5:89031b2f5316 2482 pxtalk_shape->bin_data[lb] =
Charles MacNeill 5:89031b2f5316 2483 (uint32_t)pxtalk_data->max_shape_value;
Charles MacNeill 5:89031b2f5316 2484 remaining_area -=
Charles MacNeill 5:89031b2f5316 2485 pxtalk_data->max_shape_value;
Charles MacNeill 5:89031b2f5316 2486 } else {
Charles MacNeill 5:89031b2f5316 2487 pxtalk_shape->bin_data[lb] =
Charles MacNeill 5:89031b2f5316 2488 (uint32_t)remaining_area;
Charles MacNeill 5:89031b2f5316 2489 remaining_area = 0;
Charles MacNeill 5:89031b2f5316 2490 }
Charles MacNeill 5:89031b2f5316 2491 } else {
Charles MacNeill 5:89031b2f5316 2492 pxtalk_shape->bin_data[lb] = 0;
Charles MacNeill 5:89031b2f5316 2493 }
Charles MacNeill 5:89031b2f5316 2494
Charles MacNeill 5:89031b2f5316 2495 } else {
Charles MacNeill 5:89031b2f5316 2496
Charles MacNeill 5:89031b2f5316 2497 tmp64_0 =
Charles MacNeill 5:89031b2f5316 2498 (uint64_t)pxtalk_data->bin_data_sums[lb]
Charles MacNeill 5:89031b2f5316 2499 * 1024U * 1000U;
Charles MacNeill 5:89031b2f5316 2500 tmp64_0 += (total_events >> 1);
Charles MacNeill 5:89031b2f5316 2501 tmp64_0 = do_division_u(tmp64_0, total_events);
Charles MacNeill 5:89031b2f5316 2502 if (tmp64_0 > 0xFFFFU)
Charles MacNeill 5:89031b2f5316 2503 tmp64_0 = 0xFFFFU;
Charles MacNeill 5:89031b2f5316 2504
Charles MacNeill 5:89031b2f5316 2505 pxtalk_shape->bin_data[lb] = (uint32_t)tmp64_0;
Charles MacNeill 5:89031b2f5316 2506
Charles MacNeill 5:89031b2f5316 2507
Charles MacNeill 5:89031b2f5316 2508 if ((int32_t)pxtalk_shape->bin_data[lb] >
Charles MacNeill 5:89031b2f5316 2509 pxtalk_data->max_shape_value)
Charles MacNeill 5:89031b2f5316 2510 pxtalk_data->max_shape_value =
Charles MacNeill 5:89031b2f5316 2511 (int32_t)pxtalk_shape->bin_data[lb];
Charles MacNeill 5:89031b2f5316 2512
Charles MacNeill 5:89031b2f5316 2513 remaining_area -= (int32_t)pxtalk_shape->bin_data[lb];
Charles MacNeill 5:89031b2f5316 2514 }
Charles MacNeill 5:89031b2f5316 2515 }
Charles MacNeill 5:89031b2f5316 2516
Charles MacNeill 5:89031b2f5316 2517 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 2518
Charles MacNeill 5:89031b2f5316 2519 return status;
Charles MacNeill 5:89031b2f5316 2520 }
Charles MacNeill 5:89031b2f5316 2521
Charles MacNeill 5:89031b2f5316 2522
Charles MacNeill 5:89031b2f5316 2523 VL53LX_Error VL53LX_hist_xtalk_shape_model(
Charles MacNeill 5:89031b2f5316 2524 uint16_t events_per_bin,
Charles MacNeill 5:89031b2f5316 2525 uint16_t pulse_centre,
Charles MacNeill 5:89031b2f5316 2526 uint16_t pulse_width,
Charles MacNeill 5:89031b2f5316 2527 VL53LX_xtalk_histogram_shape_t *pxtalk_shape)
Charles MacNeill 5:89031b2f5316 2528 {
Charles MacNeill 5:89031b2f5316 2529
Charles MacNeill 5:89031b2f5316 2530
Charles MacNeill 5:89031b2f5316 2531 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 2532
Charles MacNeill 5:89031b2f5316 2533 uint32_t phase_start = 0;
Charles MacNeill 5:89031b2f5316 2534 uint32_t phase_stop = 0;
Charles MacNeill 5:89031b2f5316 2535 uint32_t phase_bin = 0;
Charles MacNeill 5:89031b2f5316 2536
Charles MacNeill 5:89031b2f5316 2537 uint32_t bin_start = 0;
Charles MacNeill 5:89031b2f5316 2538 uint32_t bin_stop = 0;
Charles MacNeill 5:89031b2f5316 2539
Charles MacNeill 5:89031b2f5316 2540 uint32_t lb = 0;
Charles MacNeill 5:89031b2f5316 2541 uint16_t VL53LX_p_018 = 0;
Charles MacNeill 5:89031b2f5316 2542
Charles MacNeill 5:89031b2f5316 2543 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2544
Charles MacNeill 5:89031b2f5316 2545
Charles MacNeill 5:89031b2f5316 2546
Charles MacNeill 5:89031b2f5316 2547 pxtalk_shape->VL53LX_p_019 = 0;
Charles MacNeill 5:89031b2f5316 2548 pxtalk_shape->VL53LX_p_020 = VL53LX_XTALK_HISTO_BINS;
Charles MacNeill 5:89031b2f5316 2549 pxtalk_shape->VL53LX_p_021 = VL53LX_XTALK_HISTO_BINS;
Charles MacNeill 5:89031b2f5316 2550
Charles MacNeill 5:89031b2f5316 2551 pxtalk_shape->zero_distance_phase = pulse_centre;
Charles MacNeill 5:89031b2f5316 2552 pxtalk_shape->phasecal_result__reference_phase =
Charles MacNeill 5:89031b2f5316 2553 pulse_centre + (3*2048);
Charles MacNeill 5:89031b2f5316 2554
Charles MacNeill 5:89031b2f5316 2555
Charles MacNeill 5:89031b2f5316 2556 if (pulse_centre > (pulse_width >> 1))
Charles MacNeill 5:89031b2f5316 2557 phase_start = (uint32_t)pulse_centre -
Charles MacNeill 5:89031b2f5316 2558 ((uint32_t)pulse_width >> 1);
Charles MacNeill 5:89031b2f5316 2559 else
Charles MacNeill 5:89031b2f5316 2560 phase_start = 0;
Charles MacNeill 5:89031b2f5316 2561
Charles MacNeill 5:89031b2f5316 2562 phase_stop = (uint32_t)pulse_centre +
Charles MacNeill 5:89031b2f5316 2563 ((uint32_t)pulse_width >> 1);
Charles MacNeill 5:89031b2f5316 2564
Charles MacNeill 5:89031b2f5316 2565
Charles MacNeill 5:89031b2f5316 2566 bin_start = (phase_start / 2048);
Charles MacNeill 5:89031b2f5316 2567 bin_stop = (phase_stop / 2048);
Charles MacNeill 5:89031b2f5316 2568
Charles MacNeill 5:89031b2f5316 2569 for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS; lb++) {
Charles MacNeill 5:89031b2f5316 2570 VL53LX_p_018 = 0;
Charles MacNeill 5:89031b2f5316 2571
Charles MacNeill 5:89031b2f5316 2572
Charles MacNeill 5:89031b2f5316 2573 if (lb == bin_start && lb == bin_stop) {
Charles MacNeill 5:89031b2f5316 2574 VL53LX_p_018 =
Charles MacNeill 5:89031b2f5316 2575 VL53LX_hist_xtalk_shape_model_interp(
Charles MacNeill 5:89031b2f5316 2576 events_per_bin,
Charles MacNeill 5:89031b2f5316 2577 phase_stop - phase_start);
Charles MacNeill 5:89031b2f5316 2578
Charles MacNeill 5:89031b2f5316 2579 } else if (lb > bin_start && lb < bin_stop) {
Charles MacNeill 5:89031b2f5316 2580
Charles MacNeill 5:89031b2f5316 2581
Charles MacNeill 5:89031b2f5316 2582 VL53LX_p_018 = events_per_bin;
Charles MacNeill 5:89031b2f5316 2583
Charles MacNeill 5:89031b2f5316 2584 } else if (lb == bin_start) {
Charles MacNeill 5:89031b2f5316 2585
Charles MacNeill 5:89031b2f5316 2586
Charles MacNeill 5:89031b2f5316 2587 phase_bin = (lb + 1) * 2048;
Charles MacNeill 5:89031b2f5316 2588 VL53LX_p_018 =
Charles MacNeill 5:89031b2f5316 2589 VL53LX_hist_xtalk_shape_model_interp(
Charles MacNeill 5:89031b2f5316 2590 events_per_bin,
Charles MacNeill 5:89031b2f5316 2591 (phase_bin - phase_start));
Charles MacNeill 5:89031b2f5316 2592
Charles MacNeill 5:89031b2f5316 2593 } else if (lb == bin_stop) {
Charles MacNeill 5:89031b2f5316 2594
Charles MacNeill 5:89031b2f5316 2595
Charles MacNeill 5:89031b2f5316 2596 phase_bin = lb * 2048;
Charles MacNeill 5:89031b2f5316 2597 VL53LX_p_018 =
Charles MacNeill 5:89031b2f5316 2598 VL53LX_hist_xtalk_shape_model_interp(
Charles MacNeill 5:89031b2f5316 2599 events_per_bin,
Charles MacNeill 5:89031b2f5316 2600 (phase_stop - phase_bin));
Charles MacNeill 5:89031b2f5316 2601 }
Charles MacNeill 5:89031b2f5316 2602
Charles MacNeill 5:89031b2f5316 2603 pxtalk_shape->bin_data[lb] = VL53LX_p_018;
Charles MacNeill 5:89031b2f5316 2604 }
Charles MacNeill 5:89031b2f5316 2605
Charles MacNeill 5:89031b2f5316 2606 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 2607
Charles MacNeill 5:89031b2f5316 2608 return status;
Charles MacNeill 5:89031b2f5316 2609 }
Charles MacNeill 5:89031b2f5316 2610
Charles MacNeill 5:89031b2f5316 2611
Charles MacNeill 5:89031b2f5316 2612 uint16_t VL53LX_hist_xtalk_shape_model_interp(
Charles MacNeill 5:89031b2f5316 2613 uint16_t events_per_bin,
Charles MacNeill 5:89031b2f5316 2614 uint32_t phase_delta)
Charles MacNeill 5:89031b2f5316 2615 {
Charles MacNeill 5:89031b2f5316 2616
Charles MacNeill 5:89031b2f5316 2617
Charles MacNeill 5:89031b2f5316 2618 uint32_t VL53LX_p_018 = 0;
Charles MacNeill 5:89031b2f5316 2619
Charles MacNeill 5:89031b2f5316 2620 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2621
Charles MacNeill 5:89031b2f5316 2622
Charles MacNeill 5:89031b2f5316 2623 VL53LX_p_018 = (uint32_t)events_per_bin * phase_delta;
Charles MacNeill 5:89031b2f5316 2624 VL53LX_p_018 += 1024;
Charles MacNeill 5:89031b2f5316 2625 VL53LX_p_018 /= 2048;
Charles MacNeill 5:89031b2f5316 2626
Charles MacNeill 5:89031b2f5316 2627
Charles MacNeill 5:89031b2f5316 2628 if (VL53LX_p_018 > 0xFFFFU)
Charles MacNeill 5:89031b2f5316 2629 VL53LX_p_018 = 0xFFFFU;
Charles MacNeill 5:89031b2f5316 2630
Charles MacNeill 5:89031b2f5316 2631 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 2632
Charles MacNeill 5:89031b2f5316 2633 return (uint16_t)VL53LX_p_018;
Charles MacNeill 5:89031b2f5316 2634 }
Charles MacNeill 5:89031b2f5316 2635
Charles MacNeill 5:89031b2f5316 2636
Charles MacNeill 5:89031b2f5316 2637 void VL53LX_spad_number_to_byte_bit_index(
Charles MacNeill 5:89031b2f5316 2638 uint8_t spad_number,
Charles MacNeill 5:89031b2f5316 2639 uint8_t *pbyte_index,
Charles MacNeill 5:89031b2f5316 2640 uint8_t *pbit_index,
Charles MacNeill 5:89031b2f5316 2641 uint8_t *pbit_mask)
Charles MacNeill 5:89031b2f5316 2642 {
Charles MacNeill 5:89031b2f5316 2643
Charles MacNeill 5:89031b2f5316 2644
Charles MacNeill 5:89031b2f5316 2645
Charles MacNeill 5:89031b2f5316 2646 *pbyte_index = spad_number >> 3;
Charles MacNeill 5:89031b2f5316 2647 *pbit_index = spad_number & 0x07;
Charles MacNeill 5:89031b2f5316 2648 *pbit_mask = 0x01 << *pbit_index;
Charles MacNeill 5:89031b2f5316 2649
Charles MacNeill 5:89031b2f5316 2650 }
Charles MacNeill 5:89031b2f5316 2651
Charles MacNeill 5:89031b2f5316 2652
Charles MacNeill 5:89031b2f5316 2653 void VL53LX_encode_row_col(
Charles MacNeill 5:89031b2f5316 2654 uint8_t row,
Charles MacNeill 5:89031b2f5316 2655 uint8_t col,
Charles MacNeill 5:89031b2f5316 2656 uint8_t *pspad_number)
Charles MacNeill 5:89031b2f5316 2657 {
Charles MacNeill 5:89031b2f5316 2658
Charles MacNeill 5:89031b2f5316 2659
Charles MacNeill 5:89031b2f5316 2660 if (row > 7)
Charles MacNeill 5:89031b2f5316 2661 *pspad_number = 128 + (col << 3) + (15-row);
Charles MacNeill 5:89031b2f5316 2662 else
Charles MacNeill 5:89031b2f5316 2663 *pspad_number = ((15-col) << 3) + row;
Charles MacNeill 5:89031b2f5316 2664
Charles MacNeill 5:89031b2f5316 2665 }
Charles MacNeill 5:89031b2f5316 2666
Charles MacNeill 5:89031b2f5316 2667
Charles MacNeill 5:89031b2f5316 2668 void VL53LX_decode_zone_size(
Charles MacNeill 5:89031b2f5316 2669 uint8_t encoded_xy_size,
Charles MacNeill 5:89031b2f5316 2670 uint8_t *pwidth,
Charles MacNeill 5:89031b2f5316 2671 uint8_t *pheight)
Charles MacNeill 5:89031b2f5316 2672 {
Charles MacNeill 5:89031b2f5316 2673
Charles MacNeill 5:89031b2f5316 2674
Charles MacNeill 5:89031b2f5316 2675
Charles MacNeill 5:89031b2f5316 2676 *pheight = encoded_xy_size >> 4;
Charles MacNeill 5:89031b2f5316 2677 *pwidth = encoded_xy_size & 0x0F;
Charles MacNeill 5:89031b2f5316 2678
Charles MacNeill 5:89031b2f5316 2679 }
Charles MacNeill 5:89031b2f5316 2680
Charles MacNeill 5:89031b2f5316 2681
Charles MacNeill 5:89031b2f5316 2682 void VL53LX_encode_zone_size(
Charles MacNeill 5:89031b2f5316 2683 uint8_t width,
Charles MacNeill 5:89031b2f5316 2684 uint8_t height,
Charles MacNeill 5:89031b2f5316 2685 uint8_t *pencoded_xy_size)
Charles MacNeill 5:89031b2f5316 2686 {
Charles MacNeill 5:89031b2f5316 2687
Charles MacNeill 5:89031b2f5316 2688
Charles MacNeill 5:89031b2f5316 2689 *pencoded_xy_size = (height << 4) + width;
Charles MacNeill 5:89031b2f5316 2690
Charles MacNeill 5:89031b2f5316 2691 }
Charles MacNeill 5:89031b2f5316 2692
Charles MacNeill 5:89031b2f5316 2693
Charles MacNeill 5:89031b2f5316 2694 void VL53LX_decode_zone_limits(
Charles MacNeill 5:89031b2f5316 2695 uint8_t encoded_xy_centre,
Charles MacNeill 5:89031b2f5316 2696 uint8_t encoded_xy_size,
Charles MacNeill 5:89031b2f5316 2697 int16_t *px_ll,
Charles MacNeill 5:89031b2f5316 2698 int16_t *py_ll,
Charles MacNeill 5:89031b2f5316 2699 int16_t *px_ur,
Charles MacNeill 5:89031b2f5316 2700 int16_t *py_ur)
Charles MacNeill 5:89031b2f5316 2701 {
Charles MacNeill 5:89031b2f5316 2702
Charles MacNeill 5:89031b2f5316 2703
Charles MacNeill 5:89031b2f5316 2704
Charles MacNeill 5:89031b2f5316 2705 uint8_t x_centre = 0;
Charles MacNeill 5:89031b2f5316 2706 uint8_t y_centre = 0;
Charles MacNeill 5:89031b2f5316 2707 uint8_t width = 0;
Charles MacNeill 5:89031b2f5316 2708 uint8_t height = 0;
Charles MacNeill 5:89031b2f5316 2709
Charles MacNeill 5:89031b2f5316 2710
Charles MacNeill 5:89031b2f5316 2711
Charles MacNeill 5:89031b2f5316 2712 VL53LX_decode_row_col(
Charles MacNeill 5:89031b2f5316 2713 encoded_xy_centre,
Charles MacNeill 5:89031b2f5316 2714 &y_centre,
Charles MacNeill 5:89031b2f5316 2715 &x_centre);
Charles MacNeill 5:89031b2f5316 2716
Charles MacNeill 5:89031b2f5316 2717 VL53LX_decode_zone_size(
Charles MacNeill 5:89031b2f5316 2718 encoded_xy_size,
Charles MacNeill 5:89031b2f5316 2719 &width,
Charles MacNeill 5:89031b2f5316 2720 &height);
Charles MacNeill 5:89031b2f5316 2721
Charles MacNeill 5:89031b2f5316 2722
Charles MacNeill 5:89031b2f5316 2723
Charles MacNeill 5:89031b2f5316 2724 *px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2;
Charles MacNeill 5:89031b2f5316 2725 if (*px_ll < 0)
Charles MacNeill 5:89031b2f5316 2726 *px_ll = 0;
Charles MacNeill 5:89031b2f5316 2727
Charles MacNeill 5:89031b2f5316 2728 *px_ur = *px_ll + (int16_t)width;
Charles MacNeill 5:89031b2f5316 2729 if (*px_ur > (VL53LX_SPAD_ARRAY_WIDTH-1))
Charles MacNeill 5:89031b2f5316 2730 *px_ur = VL53LX_SPAD_ARRAY_WIDTH-1;
Charles MacNeill 5:89031b2f5316 2731
Charles MacNeill 5:89031b2f5316 2732 *py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2;
Charles MacNeill 5:89031b2f5316 2733 if (*py_ll < 0)
Charles MacNeill 5:89031b2f5316 2734 *py_ll = 0;
Charles MacNeill 5:89031b2f5316 2735
Charles MacNeill 5:89031b2f5316 2736 *py_ur = *py_ll + (int16_t)height;
Charles MacNeill 5:89031b2f5316 2737 if (*py_ur > (VL53LX_SPAD_ARRAY_HEIGHT-1))
Charles MacNeill 5:89031b2f5316 2738 *py_ur = VL53LX_SPAD_ARRAY_HEIGHT-1;
Charles MacNeill 5:89031b2f5316 2739 }
Charles MacNeill 5:89031b2f5316 2740
Charles MacNeill 5:89031b2f5316 2741
Charles MacNeill 5:89031b2f5316 2742 uint8_t VL53LX_is_aperture_location(
Charles MacNeill 5:89031b2f5316 2743 uint8_t row,
Charles MacNeill 5:89031b2f5316 2744 uint8_t col)
Charles MacNeill 5:89031b2f5316 2745 {
Charles MacNeill 5:89031b2f5316 2746
Charles MacNeill 5:89031b2f5316 2747
Charles MacNeill 5:89031b2f5316 2748 uint8_t is_aperture = 0;
Charles MacNeill 5:89031b2f5316 2749 uint8_t mod_row = row % 4;
Charles MacNeill 5:89031b2f5316 2750 uint8_t mod_col = col % 4;
Charles MacNeill 5:89031b2f5316 2751
Charles MacNeill 5:89031b2f5316 2752 if (mod_row == 0 && mod_col == 2)
Charles MacNeill 5:89031b2f5316 2753 is_aperture = 1;
Charles MacNeill 5:89031b2f5316 2754
Charles MacNeill 5:89031b2f5316 2755 if (mod_row == 2 && mod_col == 0)
Charles MacNeill 5:89031b2f5316 2756 is_aperture = 1;
Charles MacNeill 5:89031b2f5316 2757
Charles MacNeill 5:89031b2f5316 2758 return is_aperture;
Charles MacNeill 5:89031b2f5316 2759 }
Charles MacNeill 5:89031b2f5316 2760
Charles MacNeill 5:89031b2f5316 2761
Charles MacNeill 5:89031b2f5316 2762 void VL53LX_calc_max_effective_spads(
Charles MacNeill 5:89031b2f5316 2763 uint8_t encoded_zone_centre,
Charles MacNeill 5:89031b2f5316 2764 uint8_t encoded_zone_size,
Charles MacNeill 5:89031b2f5316 2765 uint8_t *pgood_spads,
Charles MacNeill 5:89031b2f5316 2766 uint16_t aperture_attenuation,
Charles MacNeill 5:89031b2f5316 2767 uint16_t *pmax_effective_spads)
Charles MacNeill 5:89031b2f5316 2768 {
Charles MacNeill 5:89031b2f5316 2769
Charles MacNeill 5:89031b2f5316 2770
Charles MacNeill 5:89031b2f5316 2771
Charles MacNeill 5:89031b2f5316 2772 int16_t x = 0;
Charles MacNeill 5:89031b2f5316 2773 int16_t y = 0;
Charles MacNeill 5:89031b2f5316 2774
Charles MacNeill 5:89031b2f5316 2775 int16_t zone_x_ll = 0;
Charles MacNeill 5:89031b2f5316 2776 int16_t zone_y_ll = 0;
Charles MacNeill 5:89031b2f5316 2777 int16_t zone_x_ur = 0;
Charles MacNeill 5:89031b2f5316 2778 int16_t zone_y_ur = 0;
Charles MacNeill 5:89031b2f5316 2779
Charles MacNeill 5:89031b2f5316 2780 uint8_t spad_number = 0;
Charles MacNeill 5:89031b2f5316 2781 uint8_t byte_index = 0;
Charles MacNeill 5:89031b2f5316 2782 uint8_t bit_index = 0;
Charles MacNeill 5:89031b2f5316 2783 uint8_t bit_mask = 0;
Charles MacNeill 5:89031b2f5316 2784
Charles MacNeill 5:89031b2f5316 2785 uint8_t is_aperture = 0;
Charles MacNeill 5:89031b2f5316 2786
Charles MacNeill 5:89031b2f5316 2787
Charles MacNeill 5:89031b2f5316 2788
Charles MacNeill 5:89031b2f5316 2789 VL53LX_decode_zone_limits(
Charles MacNeill 5:89031b2f5316 2790 encoded_zone_centre,
Charles MacNeill 5:89031b2f5316 2791 encoded_zone_size,
Charles MacNeill 5:89031b2f5316 2792 &zone_x_ll,
Charles MacNeill 5:89031b2f5316 2793 &zone_y_ll,
Charles MacNeill 5:89031b2f5316 2794 &zone_x_ur,
Charles MacNeill 5:89031b2f5316 2795 &zone_y_ur);
Charles MacNeill 5:89031b2f5316 2796
Charles MacNeill 5:89031b2f5316 2797
Charles MacNeill 5:89031b2f5316 2798
Charles MacNeill 5:89031b2f5316 2799 *pmax_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2800
Charles MacNeill 5:89031b2f5316 2801 for (y = zone_y_ll; y <= zone_y_ur; y++) {
Charles MacNeill 5:89031b2f5316 2802 for (x = zone_x_ll; x <= zone_x_ur; x++) {
Charles MacNeill 5:89031b2f5316 2803
Charles MacNeill 5:89031b2f5316 2804
Charles MacNeill 5:89031b2f5316 2805
Charles MacNeill 5:89031b2f5316 2806 VL53LX_encode_row_col(
Charles MacNeill 5:89031b2f5316 2807 (uint8_t)y,
Charles MacNeill 5:89031b2f5316 2808 (uint8_t)x,
Charles MacNeill 5:89031b2f5316 2809 &spad_number);
Charles MacNeill 5:89031b2f5316 2810
Charles MacNeill 5:89031b2f5316 2811
Charles MacNeill 5:89031b2f5316 2812
Charles MacNeill 5:89031b2f5316 2813 VL53LX_spad_number_to_byte_bit_index(
Charles MacNeill 5:89031b2f5316 2814 spad_number,
Charles MacNeill 5:89031b2f5316 2815 &byte_index,
Charles MacNeill 5:89031b2f5316 2816 &bit_index,
Charles MacNeill 5:89031b2f5316 2817 &bit_mask);
Charles MacNeill 5:89031b2f5316 2818
Charles MacNeill 5:89031b2f5316 2819
Charles MacNeill 5:89031b2f5316 2820
Charles MacNeill 5:89031b2f5316 2821 if ((pgood_spads[byte_index] & bit_mask) > 0) {
Charles MacNeill 5:89031b2f5316 2822
Charles MacNeill 5:89031b2f5316 2823
Charles MacNeill 5:89031b2f5316 2824 is_aperture = VL53LX_is_aperture_location(
Charles MacNeill 5:89031b2f5316 2825 (uint8_t)y,
Charles MacNeill 5:89031b2f5316 2826 (uint8_t)x);
Charles MacNeill 5:89031b2f5316 2827
Charles MacNeill 5:89031b2f5316 2828 if (is_aperture > 0)
Charles MacNeill 5:89031b2f5316 2829 *pmax_effective_spads +=
Charles MacNeill 5:89031b2f5316 2830 aperture_attenuation;
Charles MacNeill 5:89031b2f5316 2831 else
Charles MacNeill 5:89031b2f5316 2832 *pmax_effective_spads += 0x0100;
Charles MacNeill 5:89031b2f5316 2833
Charles MacNeill 5:89031b2f5316 2834 }
Charles MacNeill 5:89031b2f5316 2835 }
Charles MacNeill 5:89031b2f5316 2836 }
Charles MacNeill 5:89031b2f5316 2837 }
Charles MacNeill 5:89031b2f5316 2838
Charles MacNeill 5:89031b2f5316 2839
Charles MacNeill 5:89031b2f5316 2840 void VL53LX_calc_mm_effective_spads(
Charles MacNeill 5:89031b2f5316 2841 uint8_t encoded_mm_roi_centre,
Charles MacNeill 5:89031b2f5316 2842 uint8_t encoded_mm_roi_size,
Charles MacNeill 5:89031b2f5316 2843 uint8_t encoded_zone_centre,
Charles MacNeill 5:89031b2f5316 2844 uint8_t encoded_zone_size,
Charles MacNeill 5:89031b2f5316 2845 uint8_t *pgood_spads,
Charles MacNeill 5:89031b2f5316 2846 uint16_t aperture_attenuation,
Charles MacNeill 5:89031b2f5316 2847 uint16_t *pmm_inner_effective_spads,
Charles MacNeill 5:89031b2f5316 2848 uint16_t *pmm_outer_effective_spads)
Charles MacNeill 5:89031b2f5316 2849 {
Charles MacNeill 5:89031b2f5316 2850
Charles MacNeill 5:89031b2f5316 2851
Charles MacNeill 5:89031b2f5316 2852
Charles MacNeill 5:89031b2f5316 2853 int16_t x = 0;
Charles MacNeill 5:89031b2f5316 2854 int16_t y = 0;
Charles MacNeill 5:89031b2f5316 2855
Charles MacNeill 5:89031b2f5316 2856 int16_t mm_x_ll = 0;
Charles MacNeill 5:89031b2f5316 2857 int16_t mm_y_ll = 0;
Charles MacNeill 5:89031b2f5316 2858 int16_t mm_x_ur = 0;
Charles MacNeill 5:89031b2f5316 2859 int16_t mm_y_ur = 0;
Charles MacNeill 5:89031b2f5316 2860
Charles MacNeill 5:89031b2f5316 2861 int16_t zone_x_ll = 0;
Charles MacNeill 5:89031b2f5316 2862 int16_t zone_y_ll = 0;
Charles MacNeill 5:89031b2f5316 2863 int16_t zone_x_ur = 0;
Charles MacNeill 5:89031b2f5316 2864 int16_t zone_y_ur = 0;
Charles MacNeill 5:89031b2f5316 2865
Charles MacNeill 5:89031b2f5316 2866 uint8_t spad_number = 0;
Charles MacNeill 5:89031b2f5316 2867 uint8_t byte_index = 0;
Charles MacNeill 5:89031b2f5316 2868 uint8_t bit_index = 0;
Charles MacNeill 5:89031b2f5316 2869 uint8_t bit_mask = 0;
Charles MacNeill 5:89031b2f5316 2870
Charles MacNeill 5:89031b2f5316 2871 uint8_t is_aperture = 0;
Charles MacNeill 5:89031b2f5316 2872 uint16_t spad_attenuation = 0;
Charles MacNeill 5:89031b2f5316 2873
Charles MacNeill 5:89031b2f5316 2874
Charles MacNeill 5:89031b2f5316 2875
Charles MacNeill 5:89031b2f5316 2876 VL53LX_decode_zone_limits(
Charles MacNeill 5:89031b2f5316 2877 encoded_mm_roi_centre,
Charles MacNeill 5:89031b2f5316 2878 encoded_mm_roi_size,
Charles MacNeill 5:89031b2f5316 2879 &mm_x_ll,
Charles MacNeill 5:89031b2f5316 2880 &mm_y_ll,
Charles MacNeill 5:89031b2f5316 2881 &mm_x_ur,
Charles MacNeill 5:89031b2f5316 2882 &mm_y_ur);
Charles MacNeill 5:89031b2f5316 2883
Charles MacNeill 5:89031b2f5316 2884 VL53LX_decode_zone_limits(
Charles MacNeill 5:89031b2f5316 2885 encoded_zone_centre,
Charles MacNeill 5:89031b2f5316 2886 encoded_zone_size,
Charles MacNeill 5:89031b2f5316 2887 &zone_x_ll,
Charles MacNeill 5:89031b2f5316 2888 &zone_y_ll,
Charles MacNeill 5:89031b2f5316 2889 &zone_x_ur,
Charles MacNeill 5:89031b2f5316 2890 &zone_y_ur);
Charles MacNeill 5:89031b2f5316 2891
Charles MacNeill 5:89031b2f5316 2892
Charles MacNeill 5:89031b2f5316 2893
Charles MacNeill 5:89031b2f5316 2894 *pmm_inner_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2895 *pmm_outer_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 2896
Charles MacNeill 5:89031b2f5316 2897 for (y = zone_y_ll; y <= zone_y_ur; y++) {
Charles MacNeill 5:89031b2f5316 2898 for (x = zone_x_ll; x <= zone_x_ur; x++) {
Charles MacNeill 5:89031b2f5316 2899
Charles MacNeill 5:89031b2f5316 2900
Charles MacNeill 5:89031b2f5316 2901
Charles MacNeill 5:89031b2f5316 2902 VL53LX_encode_row_col(
Charles MacNeill 5:89031b2f5316 2903 (uint8_t)y,
Charles MacNeill 5:89031b2f5316 2904 (uint8_t)x,
Charles MacNeill 5:89031b2f5316 2905 &spad_number);
Charles MacNeill 5:89031b2f5316 2906
Charles MacNeill 5:89031b2f5316 2907
Charles MacNeill 5:89031b2f5316 2908
Charles MacNeill 5:89031b2f5316 2909 VL53LX_spad_number_to_byte_bit_index(
Charles MacNeill 5:89031b2f5316 2910 spad_number,
Charles MacNeill 5:89031b2f5316 2911 &byte_index,
Charles MacNeill 5:89031b2f5316 2912 &bit_index,
Charles MacNeill 5:89031b2f5316 2913 &bit_mask);
Charles MacNeill 5:89031b2f5316 2914
Charles MacNeill 5:89031b2f5316 2915
Charles MacNeill 5:89031b2f5316 2916
Charles MacNeill 5:89031b2f5316 2917 if ((pgood_spads[byte_index] & bit_mask) > 0) {
Charles MacNeill 5:89031b2f5316 2918
Charles MacNeill 5:89031b2f5316 2919
Charles MacNeill 5:89031b2f5316 2920 is_aperture = VL53LX_is_aperture_location(
Charles MacNeill 5:89031b2f5316 2921 (uint8_t)y,
Charles MacNeill 5:89031b2f5316 2922 (uint8_t)x);
Charles MacNeill 5:89031b2f5316 2923
Charles MacNeill 5:89031b2f5316 2924 if (is_aperture > 0)
Charles MacNeill 5:89031b2f5316 2925 spad_attenuation = aperture_attenuation;
Charles MacNeill 5:89031b2f5316 2926 else
Charles MacNeill 5:89031b2f5316 2927 spad_attenuation = 0x0100;
Charles MacNeill 5:89031b2f5316 2928
Charles MacNeill 5:89031b2f5316 2929
Charles MacNeill 5:89031b2f5316 2930
Charles MacNeill 5:89031b2f5316 2931 if (x >= mm_x_ll && x <= mm_x_ur &&
Charles MacNeill 5:89031b2f5316 2932 y >= mm_y_ll && y <= mm_y_ur)
Charles MacNeill 5:89031b2f5316 2933 *pmm_inner_effective_spads +=
Charles MacNeill 5:89031b2f5316 2934 spad_attenuation;
Charles MacNeill 5:89031b2f5316 2935 else
Charles MacNeill 5:89031b2f5316 2936 *pmm_outer_effective_spads +=
Charles MacNeill 5:89031b2f5316 2937 spad_attenuation;
Charles MacNeill 5:89031b2f5316 2938 }
Charles MacNeill 5:89031b2f5316 2939 }
Charles MacNeill 5:89031b2f5316 2940 }
Charles MacNeill 5:89031b2f5316 2941 }
Charles MacNeill 5:89031b2f5316 2942
Charles MacNeill 5:89031b2f5316 2943
Charles MacNeill 5:89031b2f5316 2944 void VL53LX_hist_copy_results_to_sys_and_core(
Charles MacNeill 5:89031b2f5316 2945 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 2946 VL53LX_range_results_t *phist,
Charles MacNeill 5:89031b2f5316 2947 VL53LX_system_results_t *psys,
Charles MacNeill 5:89031b2f5316 2948 VL53LX_core_results_t *pcore)
Charles MacNeill 5:89031b2f5316 2949 {
Charles MacNeill 5:89031b2f5316 2950
Charles MacNeill 5:89031b2f5316 2951
Charles MacNeill 5:89031b2f5316 2952 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 2953
Charles MacNeill 5:89031b2f5316 2954 VL53LX_range_data_t *pdata;
Charles MacNeill 5:89031b2f5316 2955
Charles MacNeill 5:89031b2f5316 2956 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 2957
Charles MacNeill 5:89031b2f5316 2958
Charles MacNeill 5:89031b2f5316 2959
Charles MacNeill 5:89031b2f5316 2960 VL53LX_init_system_results(psys);
Charles MacNeill 5:89031b2f5316 2961
Charles MacNeill 5:89031b2f5316 2962
Charles MacNeill 5:89031b2f5316 2963
Charles MacNeill 5:89031b2f5316 2964 psys->result__interrupt_status = pbins->result__interrupt_status;
Charles MacNeill 5:89031b2f5316 2965 psys->result__range_status = phist->active_results;
Charles MacNeill 5:89031b2f5316 2966 psys->result__report_status = pbins->result__report_status;
Charles MacNeill 5:89031b2f5316 2967 psys->result__stream_count = pbins->result__stream_count;
Charles MacNeill 5:89031b2f5316 2968
Charles MacNeill 5:89031b2f5316 2969 pdata = &(phist->VL53LX_p_003[0]);
Charles MacNeill 5:89031b2f5316 2970
Charles MacNeill 5:89031b2f5316 2971 for (i = 0; i < phist->active_results; i++) {
Charles MacNeill 5:89031b2f5316 2972
Charles MacNeill 5:89031b2f5316 2973 switch (i) {
Charles MacNeill 5:89031b2f5316 2974 case 0:
Charles MacNeill 5:89031b2f5316 2975 psys->result__dss_actual_effective_spads_sd0 =
Charles MacNeill 5:89031b2f5316 2976 pdata->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 2977 psys->result__peak_signal_count_rate_mcps_sd0 =
Charles MacNeill 5:89031b2f5316 2978 pdata->peak_signal_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 2979 psys->result__avg_signal_count_rate_mcps_sd0 =
Charles MacNeill 5:89031b2f5316 2980 pdata->avg_signal_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 2981 psys->result__ambient_count_rate_mcps_sd0 =
Charles MacNeill 5:89031b2f5316 2982 pdata->ambient_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 2983
Charles MacNeill 5:89031b2f5316 2984 psys->result__sigma_sd0 = pdata->VL53LX_p_002;
Charles MacNeill 5:89031b2f5316 2985 psys->result__phase_sd0 = pdata->VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 2986
Charles MacNeill 5:89031b2f5316 2987 psys->result__final_crosstalk_corrected_range_mm_sd0 =
Charles MacNeill 5:89031b2f5316 2988 (uint16_t)pdata->median_range_mm;
Charles MacNeill 5:89031b2f5316 2989
Charles MacNeill 5:89031b2f5316 2990 psys->result__phase_sd1 = pdata->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 2991
Charles MacNeill 5:89031b2f5316 2992 pcore->result_core__ranging_total_events_sd0 =
Charles MacNeill 5:89031b2f5316 2993 pdata->VL53LX_p_017;
Charles MacNeill 5:89031b2f5316 2994 pcore->result_core__signal_total_events_sd0 =
Charles MacNeill 5:89031b2f5316 2995 pdata->VL53LX_p_010;
Charles MacNeill 5:89031b2f5316 2996 pcore->result_core__total_periods_elapsed_sd0 =
Charles MacNeill 5:89031b2f5316 2997 pdata->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 2998 pcore->result_core__ambient_window_events_sd0 =
Charles MacNeill 5:89031b2f5316 2999 pdata->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 3000
Charles MacNeill 5:89031b2f5316 3001 break;
Charles MacNeill 5:89031b2f5316 3002 case 1:
Charles MacNeill 5:89031b2f5316 3003 psys->result__dss_actual_effective_spads_sd1 =
Charles MacNeill 5:89031b2f5316 3004 pdata->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 3005 psys->result__peak_signal_count_rate_mcps_sd1 =
Charles MacNeill 5:89031b2f5316 3006 pdata->peak_signal_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 3007 psys->result__ambient_count_rate_mcps_sd1 =
Charles MacNeill 5:89031b2f5316 3008 pdata->ambient_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 3009
Charles MacNeill 5:89031b2f5316 3010 psys->result__sigma_sd1 = pdata->VL53LX_p_002;
Charles MacNeill 5:89031b2f5316 3011 psys->result__phase_sd1 = pdata->VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 3012
Charles MacNeill 5:89031b2f5316 3013 psys->result__final_crosstalk_corrected_range_mm_sd1 =
Charles MacNeill 5:89031b2f5316 3014 (uint16_t)pdata->median_range_mm;
Charles MacNeill 5:89031b2f5316 3015
Charles MacNeill 5:89031b2f5316 3016 pcore->result_core__ranging_total_events_sd1 =
Charles MacNeill 5:89031b2f5316 3017 pdata->VL53LX_p_017;
Charles MacNeill 5:89031b2f5316 3018 pcore->result_core__signal_total_events_sd1 =
Charles MacNeill 5:89031b2f5316 3019 pdata->VL53LX_p_010;
Charles MacNeill 5:89031b2f5316 3020 pcore->result_core__total_periods_elapsed_sd1 =
Charles MacNeill 5:89031b2f5316 3021 pdata->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 3022 pcore->result_core__ambient_window_events_sd1 =
Charles MacNeill 5:89031b2f5316 3023 pdata->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 3024 break;
Charles MacNeill 5:89031b2f5316 3025 }
Charles MacNeill 5:89031b2f5316 3026
Charles MacNeill 5:89031b2f5316 3027 pdata++;
Charles MacNeill 5:89031b2f5316 3028 }
Charles MacNeill 5:89031b2f5316 3029
Charles MacNeill 5:89031b2f5316 3030 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 3031
Charles MacNeill 5:89031b2f5316 3032 }
Charles MacNeill 5:89031b2f5316 3033
Charles MacNeill 5:89031b2f5316 3034
Charles MacNeill 5:89031b2f5316 3035 VL53LX_Error VL53LX_sum_histogram_data(
Charles MacNeill 5:89031b2f5316 3036 VL53LX_histogram_bin_data_t *phist_input,
Charles MacNeill 5:89031b2f5316 3037 VL53LX_histogram_bin_data_t *phist_output)
Charles MacNeill 5:89031b2f5316 3038 {
Charles MacNeill 5:89031b2f5316 3039
Charles MacNeill 5:89031b2f5316 3040
Charles MacNeill 5:89031b2f5316 3041 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3042
Charles MacNeill 5:89031b2f5316 3043 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 3044 uint8_t smallest_bin_num = 0;
Charles MacNeill 5:89031b2f5316 3045
Charles MacNeill 5:89031b2f5316 3046 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3047
Charles MacNeill 5:89031b2f5316 3048
Charles MacNeill 5:89031b2f5316 3049
Charles MacNeill 5:89031b2f5316 3050 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 3051 if (phist_output->VL53LX_p_021 >=
Charles MacNeill 5:89031b2f5316 3052 phist_input->VL53LX_p_021)
Charles MacNeill 5:89031b2f5316 3053 smallest_bin_num = phist_input->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 3054 else
Charles MacNeill 5:89031b2f5316 3055 smallest_bin_num = phist_output->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 3056 }
Charles MacNeill 5:89031b2f5316 3057
Charles MacNeill 5:89031b2f5316 3058
Charles MacNeill 5:89031b2f5316 3059
Charles MacNeill 5:89031b2f5316 3060
Charles MacNeill 5:89031b2f5316 3061
Charles MacNeill 5:89031b2f5316 3062 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3063 for (i = 0; i < smallest_bin_num; i++)
Charles MacNeill 5:89031b2f5316 3064
Charles MacNeill 5:89031b2f5316 3065 phist_output->bin_data[i] += phist_input->bin_data[i];
Charles MacNeill 5:89031b2f5316 3066
Charles MacNeill 5:89031b2f5316 3067 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3068 phist_output->VL53LX_p_028 +=
Charles MacNeill 5:89031b2f5316 3069 phist_input->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 3070
Charles MacNeill 5:89031b2f5316 3071
Charles MacNeill 5:89031b2f5316 3072 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3073
Charles MacNeill 5:89031b2f5316 3074 return status;
Charles MacNeill 5:89031b2f5316 3075 }
Charles MacNeill 5:89031b2f5316 3076
Charles MacNeill 5:89031b2f5316 3077
Charles MacNeill 5:89031b2f5316 3078 VL53LX_Error VL53LX_avg_histogram_data(
Charles MacNeill 5:89031b2f5316 3079 uint8_t no_of_samples,
Charles MacNeill 5:89031b2f5316 3080 VL53LX_histogram_bin_data_t *phist_sum,
Charles MacNeill 5:89031b2f5316 3081 VL53LX_histogram_bin_data_t *phist_avg)
Charles MacNeill 5:89031b2f5316 3082 {
Charles MacNeill 5:89031b2f5316 3083
Charles MacNeill 5:89031b2f5316 3084
Charles MacNeill 5:89031b2f5316 3085 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3086
Charles MacNeill 5:89031b2f5316 3087 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 3088
Charles MacNeill 5:89031b2f5316 3089 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3090
Charles MacNeill 5:89031b2f5316 3091
Charles MacNeill 5:89031b2f5316 3092
Charles MacNeill 5:89031b2f5316 3093
Charles MacNeill 5:89031b2f5316 3094
Charles MacNeill 5:89031b2f5316 3095 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 3096 for (i = 0; i < phist_sum->VL53LX_p_021; i++) {
Charles MacNeill 5:89031b2f5316 3097
Charles MacNeill 5:89031b2f5316 3098
Charles MacNeill 5:89031b2f5316 3099
Charles MacNeill 5:89031b2f5316 3100 if (no_of_samples > 0)
Charles MacNeill 5:89031b2f5316 3101 phist_avg->bin_data[i] =
Charles MacNeill 5:89031b2f5316 3102 phist_sum->bin_data[i] /
Charles MacNeill 5:89031b2f5316 3103 (int32_t)no_of_samples;
Charles MacNeill 5:89031b2f5316 3104 else
Charles MacNeill 5:89031b2f5316 3105 phist_avg->bin_data[i] = phist_sum->bin_data[i];
Charles MacNeill 5:89031b2f5316 3106 }
Charles MacNeill 5:89031b2f5316 3107 }
Charles MacNeill 5:89031b2f5316 3108
Charles MacNeill 5:89031b2f5316 3109 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 3110 if (no_of_samples > 0)
Charles MacNeill 5:89031b2f5316 3111 phist_avg->VL53LX_p_028 =
Charles MacNeill 5:89031b2f5316 3112 phist_sum->VL53LX_p_028 /
Charles MacNeill 5:89031b2f5316 3113 (int32_t)no_of_samples;
Charles MacNeill 5:89031b2f5316 3114 else
Charles MacNeill 5:89031b2f5316 3115 phist_avg->VL53LX_p_028 =
Charles MacNeill 5:89031b2f5316 3116 phist_sum->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 3117 }
Charles MacNeill 5:89031b2f5316 3118
Charles MacNeill 5:89031b2f5316 3119 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3120
Charles MacNeill 5:89031b2f5316 3121 return status;
Charles MacNeill 5:89031b2f5316 3122 }
Charles MacNeill 5:89031b2f5316 3123
Charles MacNeill 5:89031b2f5316 3124
Charles MacNeill 5:89031b2f5316 3125 VL53LX_Error VL53LX_save_cfg_data(
Charles MacNeill 5:89031b2f5316 3126 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 3127 {
Charles MacNeill 5:89031b2f5316 3128
Charles MacNeill 5:89031b2f5316 3129
Charles MacNeill 5:89031b2f5316 3130 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3131
Charles MacNeill 5:89031b2f5316 3132 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 3133 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3134 VL53LX_LLDriverResults_t *pres =
Charles MacNeill 5:89031b2f5316 3135 VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 3136
Charles MacNeill 5:89031b2f5316 3137 VL53LX_zone_private_dyn_cfg_t *pzone_dyn_cfg;
Charles MacNeill 5:89031b2f5316 3138 VL53LX_dynamic_config_t *pdynamic = &(pdev->dyn_cfg);
Charles MacNeill 5:89031b2f5316 3139
Charles MacNeill 5:89031b2f5316 3140 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3141
Charles MacNeill 5:89031b2f5316 3142 pzone_dyn_cfg =
Charles MacNeill 5:89031b2f5316 3143 &(pres->zone_dyn_cfgs.VL53LX_p_003[pdev->ll_state.cfg_zone_id]);
Charles MacNeill 5:89031b2f5316 3144
Charles MacNeill 5:89031b2f5316 3145 pzone_dyn_cfg->expected_stream_count =
Charles MacNeill 5:89031b2f5316 3146 pdev->ll_state.cfg_stream_count;
Charles MacNeill 5:89031b2f5316 3147
Charles MacNeill 5:89031b2f5316 3148 pzone_dyn_cfg->expected_gph_id =
Charles MacNeill 5:89031b2f5316 3149 pdev->ll_state.cfg_gph_id;
Charles MacNeill 5:89031b2f5316 3150
Charles MacNeill 5:89031b2f5316 3151 pzone_dyn_cfg->roi_config__user_roi_centre_spad =
Charles MacNeill 5:89031b2f5316 3152 pdynamic->roi_config__user_roi_centre_spad;
Charles MacNeill 5:89031b2f5316 3153
Charles MacNeill 5:89031b2f5316 3154 pzone_dyn_cfg->roi_config__user_roi_requested_global_xy_size =
Charles MacNeill 5:89031b2f5316 3155 pdynamic->roi_config__user_roi_requested_global_xy_size;
Charles MacNeill 5:89031b2f5316 3156
Charles MacNeill 5:89031b2f5316 3157 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3158
Charles MacNeill 5:89031b2f5316 3159 return status;
Charles MacNeill 5:89031b2f5316 3160 }
Charles MacNeill 5:89031b2f5316 3161
Charles MacNeill 5:89031b2f5316 3162
Charles MacNeill 5:89031b2f5316 3163 VL53LX_Error VL53LX_dynamic_zone_update(
Charles MacNeill 5:89031b2f5316 3164 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3165 VL53LX_range_results_t *presults)
Charles MacNeill 5:89031b2f5316 3166 {
Charles MacNeill 5:89031b2f5316 3167
Charles MacNeill 5:89031b2f5316 3168
Charles MacNeill 5:89031b2f5316 3169 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3170
Charles MacNeill 5:89031b2f5316 3171 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 3172 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3173 VL53LX_LLDriverResults_t *pres =
Charles MacNeill 5:89031b2f5316 3174 VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 3175 VL53LX_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs);
Charles MacNeill 5:89031b2f5316 3176
Charles MacNeill 5:89031b2f5316 3177 uint8_t zone_id = pdev->ll_state.rd_zone_id;
Charles MacNeill 5:89031b2f5316 3178 uint8_t i;
Charles MacNeill 5:89031b2f5316 3179 uint16_t max_total_rate_per_spads;
Charles MacNeill 5:89031b2f5316 3180 uint16_t target_rate =
Charles MacNeill 5:89031b2f5316 3181 pdev->stat_cfg.dss_config__target_total_rate_mcps;
Charles MacNeill 5:89031b2f5316 3182 uint32_t temp = 0xFFFF;
Charles MacNeill 5:89031b2f5316 3183 #ifdef VL53LX_LOG_ENABLE
Charles MacNeill 5:89031b2f5316 3184 uint16_t eff_spad_cnt =
Charles MacNeill 5:89031b2f5316 3185 pZ->VL53LX_p_003[zone_id].dss_requested_effective_spad_count;
Charles MacNeill 5:89031b2f5316 3186 #endif
Charles MacNeill 5:89031b2f5316 3187
Charles MacNeill 5:89031b2f5316 3188 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3189
Charles MacNeill 5:89031b2f5316 3190 pZ->VL53LX_p_003[zone_id].dss_requested_effective_spad_count = 0;
Charles MacNeill 5:89031b2f5316 3191
Charles MacNeill 5:89031b2f5316 3192 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3193 " DYNZONEUPDATE: peak signal count rate mcps:");
Charles MacNeill 5:89031b2f5316 3194
Charles MacNeill 5:89031b2f5316 3195 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3196 "%u actual effective spads: %u\n",
Charles MacNeill 5:89031b2f5316 3197 presults->VL53LX_p_003[0].peak_signal_count_rate_mcps,
Charles MacNeill 5:89031b2f5316 3198 presults->VL53LX_p_003[0].VL53LX_p_004);
Charles MacNeill 5:89031b2f5316 3199
Charles MacNeill 5:89031b2f5316 3200 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3201 " DYNZONEUPDATE: active results: %u\n",
Charles MacNeill 5:89031b2f5316 3202 presults->active_results);
Charles MacNeill 5:89031b2f5316 3203
Charles MacNeill 5:89031b2f5316 3204 max_total_rate_per_spads =
Charles MacNeill 5:89031b2f5316 3205 presults->VL53LX_p_003[0].total_rate_per_spad_mcps;
Charles MacNeill 5:89031b2f5316 3206
Charles MacNeill 5:89031b2f5316 3207 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3208 " DYNZONEUPDATE: max total rate per spad at start: %u\n",
Charles MacNeill 5:89031b2f5316 3209 max_total_rate_per_spads);
Charles MacNeill 5:89031b2f5316 3210
Charles MacNeill 5:89031b2f5316 3211 for (i = 1; i < presults->active_results; i++) {
Charles MacNeill 5:89031b2f5316 3212 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3213 " DYNZONEUPDATE: zone total rate per spad: zone_id: %u,",
Charles MacNeill 5:89031b2f5316 3214 i);
Charles MacNeill 5:89031b2f5316 3215
Charles MacNeill 5:89031b2f5316 3216 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3217 "total rate per spad: %u\n",
Charles MacNeill 5:89031b2f5316 3218 presults->VL53LX_p_003[i].total_rate_per_spad_mcps);
Charles MacNeill 5:89031b2f5316 3219
Charles MacNeill 5:89031b2f5316 3220 if (presults->VL53LX_p_003[i].total_rate_per_spad_mcps >
Charles MacNeill 5:89031b2f5316 3221 max_total_rate_per_spads)
Charles MacNeill 5:89031b2f5316 3222 max_total_rate_per_spads =
Charles MacNeill 5:89031b2f5316 3223 presults->VL53LX_p_003[i].total_rate_per_spad_mcps;
Charles MacNeill 5:89031b2f5316 3224
Charles MacNeill 5:89031b2f5316 3225 }
Charles MacNeill 5:89031b2f5316 3226
Charles MacNeill 5:89031b2f5316 3227 if (max_total_rate_per_spads == 0) {
Charles MacNeill 5:89031b2f5316 3228
Charles MacNeill 5:89031b2f5316 3229 temp = 0xFFFF;
Charles MacNeill 5:89031b2f5316 3230 } else {
Charles MacNeill 5:89031b2f5316 3231
Charles MacNeill 5:89031b2f5316 3232 temp = target_rate << 14;
Charles MacNeill 5:89031b2f5316 3233 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3234 " DYNZONEUPDATE: 1: temp: %u\n",
Charles MacNeill 5:89031b2f5316 3235 temp);
Charles MacNeill 5:89031b2f5316 3236
Charles MacNeill 5:89031b2f5316 3237
Charles MacNeill 5:89031b2f5316 3238 temp = temp / max_total_rate_per_spads;
Charles MacNeill 5:89031b2f5316 3239
Charles MacNeill 5:89031b2f5316 3240 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3241 " DYNZONEUPDATE: 2: temp: %u\n",
Charles MacNeill 5:89031b2f5316 3242 temp);
Charles MacNeill 5:89031b2f5316 3243
Charles MacNeill 5:89031b2f5316 3244
Charles MacNeill 5:89031b2f5316 3245 if (temp > 0xFFFF)
Charles MacNeill 5:89031b2f5316 3246 temp = 0xFFFF;
Charles MacNeill 5:89031b2f5316 3247
Charles MacNeill 5:89031b2f5316 3248 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3249 " DYNZONEUPDATE: 3: temp: %u\n",
Charles MacNeill 5:89031b2f5316 3250 temp);
Charles MacNeill 5:89031b2f5316 3251 }
Charles MacNeill 5:89031b2f5316 3252
Charles MacNeill 5:89031b2f5316 3253 pZ->VL53LX_p_003[zone_id].dss_requested_effective_spad_count =
Charles MacNeill 5:89031b2f5316 3254 (uint16_t)temp;
Charles MacNeill 5:89031b2f5316 3255
Charles MacNeill 5:89031b2f5316 3256 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3257 " DYNZONEUPDATE: zone_id: %u, target_rate: %u,",
Charles MacNeill 5:89031b2f5316 3258 zone_id,
Charles MacNeill 5:89031b2f5316 3259 target_rate);
Charles MacNeill 5:89031b2f5316 3260
Charles MacNeill 5:89031b2f5316 3261 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3262 "max_total_rate_per_spads: %u, requested_spads: %u\n",
Charles MacNeill 5:89031b2f5316 3263 max_total_rate_per_spads,
Charles MacNeill 5:89031b2f5316 3264 eff_spad_cnt);
Charles MacNeill 5:89031b2f5316 3265
Charles MacNeill 5:89031b2f5316 3266 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3267
Charles MacNeill 5:89031b2f5316 3268 return status;
Charles MacNeill 5:89031b2f5316 3269 }
Charles MacNeill 5:89031b2f5316 3270
Charles MacNeill 5:89031b2f5316 3271 VL53LX_Error VL53LX_multizone_hist_bins_update(
Charles MacNeill 5:89031b2f5316 3272 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 3273 {
Charles MacNeill 5:89031b2f5316 3274
Charles MacNeill 5:89031b2f5316 3275
Charles MacNeill 5:89031b2f5316 3276 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3277
Charles MacNeill 5:89031b2f5316 3278 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3279 VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state);
Charles MacNeill 5:89031b2f5316 3280 VL53LX_zone_config_t *pzone_cfg = &(pdev->zone_cfg);
Charles MacNeill 5:89031b2f5316 3281 VL53LX_histogram_config_t *phist_cfg = &(pdev->hist_cfg);
Charles MacNeill 5:89031b2f5316 3282 VL53LX_histogram_config_t *pmulti_hist =
Charles MacNeill 5:89031b2f5316 3283 &(pzone_cfg->multizone_hist_cfg);
Charles MacNeill 5:89031b2f5316 3284
Charles MacNeill 5:89031b2f5316 3285 uint8_t next_range_is_odd_timing = (pstate->cfg_stream_count) % 2;
Charles MacNeill 5:89031b2f5316 3286
Charles MacNeill 5:89031b2f5316 3287 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3288
Charles MacNeill 5:89031b2f5316 3289
Charles MacNeill 5:89031b2f5316 3290 if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] ==
Charles MacNeill 5:89031b2f5316 3291 VL53LX_ZONECONFIG_BINCONFIG__LOWAMB) {
Charles MacNeill 5:89031b2f5316 3292 if (!next_range_is_odd_timing) {
Charles MacNeill 5:89031b2f5316 3293 trace_print (VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3294 " HISTBINCONFIGUPDATE: Setting LOWAMB EVEN timing\n");
Charles MacNeill 5:89031b2f5316 3295 phist_cfg->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3296 pmulti_hist->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3297 phist_cfg->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3298 pmulti_hist->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3299 phist_cfg->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3300 pmulti_hist->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3301 }
Charles MacNeill 5:89031b2f5316 3302
Charles MacNeill 5:89031b2f5316 3303 if (next_range_is_odd_timing) {
Charles MacNeill 5:89031b2f5316 3304 trace_print (VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3305 " HISTBINCONFIGUPDATE: Setting LOWAMB ODD timing\n");
Charles MacNeill 5:89031b2f5316 3306 phist_cfg->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3307 pmulti_hist->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3308 phist_cfg->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3309 pmulti_hist->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3310 phist_cfg->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3311 pmulti_hist->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3312 }
Charles MacNeill 5:89031b2f5316 3313 } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] ==
Charles MacNeill 5:89031b2f5316 3314 VL53LX_ZONECONFIG_BINCONFIG__MIDAMB) {
Charles MacNeill 5:89031b2f5316 3315 trace_print (VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3316 " HISTBINCONFIGUPDATE: Setting MIDAMB timing\n");
Charles MacNeill 5:89031b2f5316 3317 if (!next_range_is_odd_timing) {
Charles MacNeill 5:89031b2f5316 3318 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3319 " HISTBINCONFIGUPDATE: Setting MIDAMB EVEN timing\n");
Charles MacNeill 5:89031b2f5316 3320 phist_cfg->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3321 pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3322 phist_cfg->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3323 pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3324 phist_cfg->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3325 pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3326 }
Charles MacNeill 5:89031b2f5316 3327
Charles MacNeill 5:89031b2f5316 3328 if (next_range_is_odd_timing) {
Charles MacNeill 5:89031b2f5316 3329 trace_print (VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3330 " HISTBINCONFIGUPDATE: Setting MIDAMB ODD timing\n");
Charles MacNeill 5:89031b2f5316 3331 phist_cfg->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3332 pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3333 phist_cfg->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3334 pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3335 phist_cfg->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3336 pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3337 }
Charles MacNeill 5:89031b2f5316 3338 } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] ==
Charles MacNeill 5:89031b2f5316 3339 VL53LX_ZONECONFIG_BINCONFIG__HIGHAMB) {
Charles MacNeill 5:89031b2f5316 3340 if (!next_range_is_odd_timing) {
Charles MacNeill 5:89031b2f5316 3341 trace_print (VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3342 " HISTBINCONFIGUPDATE: Setting HIGHAMB EVEN timing\n"
Charles MacNeill 5:89031b2f5316 3343 );
Charles MacNeill 5:89031b2f5316 3344 phist_cfg->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3345 pmulti_hist->histogram_config__high_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3346 phist_cfg->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3347 pmulti_hist->histogram_config__high_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3348 phist_cfg->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3349 pmulti_hist->histogram_config__high_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3350 }
Charles MacNeill 5:89031b2f5316 3351
Charles MacNeill 5:89031b2f5316 3352 if (next_range_is_odd_timing) {
Charles MacNeill 5:89031b2f5316 3353 trace_print (VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3354 " HISTBINCONFIGUPDATE: Setting HIGHAMB ODD timing\n");
Charles MacNeill 5:89031b2f5316 3355 phist_cfg->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3356 pmulti_hist->histogram_config__high_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3357 phist_cfg->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3358 pmulti_hist->histogram_config__high_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3359 phist_cfg->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3360 pmulti_hist->histogram_config__high_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3361 }
Charles MacNeill 5:89031b2f5316 3362 }
Charles MacNeill 5:89031b2f5316 3363
Charles MacNeill 5:89031b2f5316 3364
Charles MacNeill 5:89031b2f5316 3365
Charles MacNeill 5:89031b2f5316 3366 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 3367 VL53LX_copy_hist_bins_to_static_cfg(
Charles MacNeill 5:89031b2f5316 3368 phist_cfg,
Charles MacNeill 5:89031b2f5316 3369 &(pdev->stat_cfg),
Charles MacNeill 5:89031b2f5316 3370 &(pdev->tim_cfg));
Charles MacNeill 5:89031b2f5316 3371 }
Charles MacNeill 5:89031b2f5316 3372
Charles MacNeill 5:89031b2f5316 3373 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3374
Charles MacNeill 5:89031b2f5316 3375 return status;
Charles MacNeill 5:89031b2f5316 3376 }
Charles MacNeill 5:89031b2f5316 3377
Charles MacNeill 5:89031b2f5316 3378
Charles MacNeill 5:89031b2f5316 3379
Charles MacNeill 5:89031b2f5316 3380 VL53LX_Error VL53LX_update_internal_stream_counters(
Charles MacNeill 5:89031b2f5316 3381 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3382 uint8_t external_stream_count,
Charles MacNeill 5:89031b2f5316 3383 uint8_t *pinternal_stream_count,
Charles MacNeill 5:89031b2f5316 3384 uint8_t *pinternal_stream_count_val)
Charles MacNeill 5:89031b2f5316 3385 {
Charles MacNeill 5:89031b2f5316 3386
Charles MacNeill 5:89031b2f5316 3387 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3388 uint8_t stream_divider;
Charles MacNeill 5:89031b2f5316 3389
Charles MacNeill 5:89031b2f5316 3390 VL53LX_LLDriverData_t *pdev =
Charles MacNeill 5:89031b2f5316 3391 VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3392
Charles MacNeill 5:89031b2f5316 3393 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3394
Charles MacNeill 5:89031b2f5316 3395 stream_divider = pdev->gen_cfg.global_config__stream_divider;
Charles MacNeill 5:89031b2f5316 3396
Charles MacNeill 5:89031b2f5316 3397 if (stream_divider == 0) {
Charles MacNeill 5:89031b2f5316 3398
Charles MacNeill 5:89031b2f5316 3399
Charles MacNeill 5:89031b2f5316 3400 *pinternal_stream_count = external_stream_count;
Charles MacNeill 5:89031b2f5316 3401
Charles MacNeill 5:89031b2f5316 3402 } else if (*pinternal_stream_count_val == (stream_divider-1)) {
Charles MacNeill 5:89031b2f5316 3403
Charles MacNeill 5:89031b2f5316 3404
Charles MacNeill 5:89031b2f5316 3405 if (*pinternal_stream_count == 0xFF)
Charles MacNeill 5:89031b2f5316 3406 *pinternal_stream_count = 0x80;
Charles MacNeill 5:89031b2f5316 3407 else
Charles MacNeill 5:89031b2f5316 3408 *pinternal_stream_count = *pinternal_stream_count + 1;
Charles MacNeill 5:89031b2f5316 3409
Charles MacNeill 5:89031b2f5316 3410
Charles MacNeill 5:89031b2f5316 3411 *pinternal_stream_count_val = 0;
Charles MacNeill 5:89031b2f5316 3412
Charles MacNeill 5:89031b2f5316 3413 } else {
Charles MacNeill 5:89031b2f5316 3414
Charles MacNeill 5:89031b2f5316 3415
Charles MacNeill 5:89031b2f5316 3416 *pinternal_stream_count_val = *pinternal_stream_count_val + 1;
Charles MacNeill 5:89031b2f5316 3417 }
Charles MacNeill 5:89031b2f5316 3418
Charles MacNeill 5:89031b2f5316 3419 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3420 "UPDINTSTREAMCOUNT internal_steam_count: %d,",
Charles MacNeill 5:89031b2f5316 3421 *pinternal_stream_count);
Charles MacNeill 5:89031b2f5316 3422
Charles MacNeill 5:89031b2f5316 3423 trace_print(VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 3424 "internal_stream_count_val: %d, divider: %d\n",
Charles MacNeill 5:89031b2f5316 3425 *pinternal_stream_count_val,
Charles MacNeill 5:89031b2f5316 3426 stream_divider);
Charles MacNeill 5:89031b2f5316 3427
Charles MacNeill 5:89031b2f5316 3428 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3429
Charles MacNeill 5:89031b2f5316 3430 return status;
Charles MacNeill 5:89031b2f5316 3431 }
Charles MacNeill 5:89031b2f5316 3432
Charles MacNeill 5:89031b2f5316 3433
Charles MacNeill 5:89031b2f5316 3434
Charles MacNeill 5:89031b2f5316 3435 VL53LX_Error VL53LX_set_histogram_multizone_initial_bin_config(
Charles MacNeill 5:89031b2f5316 3436 VL53LX_zone_config_t *pzone_cfg,
Charles MacNeill 5:89031b2f5316 3437 VL53LX_histogram_config_t *phist_cfg,
Charles MacNeill 5:89031b2f5316 3438 VL53LX_histogram_config_t *pmulti_hist)
Charles MacNeill 5:89031b2f5316 3439 {
Charles MacNeill 5:89031b2f5316 3440 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3441
Charles MacNeill 5:89031b2f5316 3442 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3443
Charles MacNeill 5:89031b2f5316 3444
Charles MacNeill 5:89031b2f5316 3445 if (pzone_cfg->bin_config[0] ==
Charles MacNeill 5:89031b2f5316 3446 VL53LX_ZONECONFIG_BINCONFIG__LOWAMB) {
Charles MacNeill 5:89031b2f5316 3447 phist_cfg->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3448 pmulti_hist->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3449 phist_cfg->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3450 pmulti_hist->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3451 phist_cfg->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3452 pmulti_hist->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3453
Charles MacNeill 5:89031b2f5316 3454 phist_cfg->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3455 pmulti_hist->histogram_config__low_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3456 phist_cfg->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3457 pmulti_hist->histogram_config__low_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3458 phist_cfg->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3459 pmulti_hist->histogram_config__low_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3460 } else if (pzone_cfg->bin_config[0] ==
Charles MacNeill 5:89031b2f5316 3461 VL53LX_ZONECONFIG_BINCONFIG__MIDAMB) {
Charles MacNeill 5:89031b2f5316 3462 phist_cfg->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3463 pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3464 phist_cfg->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3465 pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3466 phist_cfg->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3467 pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3468
Charles MacNeill 5:89031b2f5316 3469 phist_cfg->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3470 pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3471 phist_cfg->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3472 pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3473 phist_cfg->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3474 pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3475 } else if (pzone_cfg->bin_config[0] ==
Charles MacNeill 5:89031b2f5316 3476 VL53LX_ZONECONFIG_BINCONFIG__HIGHAMB) {
Charles MacNeill 5:89031b2f5316 3477 phist_cfg->histogram_config__low_amb_even_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3478 pmulti_hist->histogram_config__high_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3479 phist_cfg->histogram_config__low_amb_even_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3480 pmulti_hist->histogram_config__high_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3481 phist_cfg->histogram_config__low_amb_even_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3482 pmulti_hist->histogram_config__high_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3483 phist_cfg->histogram_config__low_amb_odd_bin_0_1 =
Charles MacNeill 5:89031b2f5316 3484 pmulti_hist->histogram_config__high_amb_even_bin_0_1;
Charles MacNeill 5:89031b2f5316 3485 phist_cfg->histogram_config__low_amb_odd_bin_2_3 =
Charles MacNeill 5:89031b2f5316 3486 pmulti_hist->histogram_config__high_amb_even_bin_2_3;
Charles MacNeill 5:89031b2f5316 3487 phist_cfg->histogram_config__low_amb_odd_bin_4_5 =
Charles MacNeill 5:89031b2f5316 3488 pmulti_hist->histogram_config__high_amb_even_bin_4_5;
Charles MacNeill 5:89031b2f5316 3489 }
Charles MacNeill 5:89031b2f5316 3490
Charles MacNeill 5:89031b2f5316 3491 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3492 return status;
Charles MacNeill 5:89031b2f5316 3493 }
Charles MacNeill 5:89031b2f5316 3494
Charles MacNeill 5:89031b2f5316 3495
Charles MacNeill 5:89031b2f5316 3496
Charles MacNeill 5:89031b2f5316 3497 uint8_t VL53LX_encode_GPIO_interrupt_config(
Charles MacNeill 5:89031b2f5316 3498 VL53LX_GPIO_interrupt_config_t *pintconf)
Charles MacNeill 5:89031b2f5316 3499 {
Charles MacNeill 5:89031b2f5316 3500 uint8_t system__interrupt_config;
Charles MacNeill 5:89031b2f5316 3501
Charles MacNeill 5:89031b2f5316 3502 system__interrupt_config = pintconf->intr_mode_distance;
Charles MacNeill 5:89031b2f5316 3503 system__interrupt_config |= ((pintconf->intr_mode_rate) << 2);
Charles MacNeill 5:89031b2f5316 3504 system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5);
Charles MacNeill 5:89031b2f5316 3505 system__interrupt_config |= ((pintconf->intr_no_target) << 6);
Charles MacNeill 5:89031b2f5316 3506 system__interrupt_config |= ((pintconf->intr_combined_mode) << 7);
Charles MacNeill 5:89031b2f5316 3507
Charles MacNeill 5:89031b2f5316 3508 return system__interrupt_config;
Charles MacNeill 5:89031b2f5316 3509 }
Charles MacNeill 5:89031b2f5316 3510
Charles MacNeill 5:89031b2f5316 3511
Charles MacNeill 5:89031b2f5316 3512
Charles MacNeill 5:89031b2f5316 3513 VL53LX_GPIO_interrupt_config_t VL53LX_decode_GPIO_interrupt_config(
Charles MacNeill 5:89031b2f5316 3514 uint8_t system__interrupt_config)
Charles MacNeill 5:89031b2f5316 3515 {
Charles MacNeill 5:89031b2f5316 3516 VL53LX_GPIO_interrupt_config_t intconf;
Charles MacNeill 5:89031b2f5316 3517
Charles MacNeill 5:89031b2f5316 3518 intconf.intr_mode_distance = system__interrupt_config & 0x03;
Charles MacNeill 5:89031b2f5316 3519 intconf.intr_mode_rate = (system__interrupt_config >> 2) & 0x03;
Charles MacNeill 5:89031b2f5316 3520 intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01;
Charles MacNeill 5:89031b2f5316 3521 intconf.intr_no_target = (system__interrupt_config >> 6) & 0x01;
Charles MacNeill 5:89031b2f5316 3522 intconf.intr_combined_mode = (system__interrupt_config >> 7) & 0x01;
Charles MacNeill 5:89031b2f5316 3523
Charles MacNeill 5:89031b2f5316 3524
Charles MacNeill 5:89031b2f5316 3525 intconf.threshold_rate_low = 0;
Charles MacNeill 5:89031b2f5316 3526 intconf.threshold_rate_high = 0;
Charles MacNeill 5:89031b2f5316 3527 intconf.threshold_distance_low = 0;
Charles MacNeill 5:89031b2f5316 3528 intconf.threshold_distance_high = 0;
Charles MacNeill 5:89031b2f5316 3529
Charles MacNeill 5:89031b2f5316 3530 return intconf;
Charles MacNeill 5:89031b2f5316 3531 }
Charles MacNeill 5:89031b2f5316 3532
Charles MacNeill 5:89031b2f5316 3533
Charles MacNeill 5:89031b2f5316 3534
Charles MacNeill 5:89031b2f5316 3535 VL53LX_Error VL53LX_set_GPIO_distance_threshold(
Charles MacNeill 5:89031b2f5316 3536 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3537 uint16_t threshold_high,
Charles MacNeill 5:89031b2f5316 3538 uint16_t threshold_low)
Charles MacNeill 5:89031b2f5316 3539 {
Charles MacNeill 5:89031b2f5316 3540 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3541
Charles MacNeill 5:89031b2f5316 3542 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3543
Charles MacNeill 5:89031b2f5316 3544 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3545
Charles MacNeill 5:89031b2f5316 3546 pdev->dyn_cfg.system__thresh_high = threshold_high;
Charles MacNeill 5:89031b2f5316 3547 pdev->dyn_cfg.system__thresh_low = threshold_low;
Charles MacNeill 5:89031b2f5316 3548
Charles MacNeill 5:89031b2f5316 3549 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3550 return status;
Charles MacNeill 5:89031b2f5316 3551 }
Charles MacNeill 5:89031b2f5316 3552
Charles MacNeill 5:89031b2f5316 3553
Charles MacNeill 5:89031b2f5316 3554
Charles MacNeill 5:89031b2f5316 3555 VL53LX_Error VL53LX_set_GPIO_rate_threshold(
Charles MacNeill 5:89031b2f5316 3556 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3557 uint16_t threshold_high,
Charles MacNeill 5:89031b2f5316 3558 uint16_t threshold_low)
Charles MacNeill 5:89031b2f5316 3559 {
Charles MacNeill 5:89031b2f5316 3560 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3561
Charles MacNeill 5:89031b2f5316 3562 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3563
Charles MacNeill 5:89031b2f5316 3564 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3565
Charles MacNeill 5:89031b2f5316 3566 pdev->gen_cfg.system__thresh_rate_high = threshold_high;
Charles MacNeill 5:89031b2f5316 3567 pdev->gen_cfg.system__thresh_rate_low = threshold_low;
Charles MacNeill 5:89031b2f5316 3568
Charles MacNeill 5:89031b2f5316 3569 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3570 return status;
Charles MacNeill 5:89031b2f5316 3571 }
Charles MacNeill 5:89031b2f5316 3572
Charles MacNeill 5:89031b2f5316 3573
Charles MacNeill 5:89031b2f5316 3574
Charles MacNeill 5:89031b2f5316 3575 VL53LX_Error VL53LX_set_GPIO_thresholds_from_struct(
Charles MacNeill 5:89031b2f5316 3576 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3577 VL53LX_GPIO_interrupt_config_t *pintconf)
Charles MacNeill 5:89031b2f5316 3578 {
Charles MacNeill 5:89031b2f5316 3579 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3580
Charles MacNeill 5:89031b2f5316 3581 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3582
Charles MacNeill 5:89031b2f5316 3583 status = VL53LX_set_GPIO_distance_threshold(
Charles MacNeill 5:89031b2f5316 3584 Dev,
Charles MacNeill 5:89031b2f5316 3585 pintconf->threshold_distance_high,
Charles MacNeill 5:89031b2f5316 3586 pintconf->threshold_distance_low);
Charles MacNeill 5:89031b2f5316 3587
Charles MacNeill 5:89031b2f5316 3588 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 3589 status =
Charles MacNeill 5:89031b2f5316 3590 VL53LX_set_GPIO_rate_threshold(
Charles MacNeill 5:89031b2f5316 3591 Dev,
Charles MacNeill 5:89031b2f5316 3592 pintconf->threshold_rate_high,
Charles MacNeill 5:89031b2f5316 3593 pintconf->threshold_rate_low);
Charles MacNeill 5:89031b2f5316 3594 }
Charles MacNeill 5:89031b2f5316 3595
Charles MacNeill 5:89031b2f5316 3596 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3597 return status;
Charles MacNeill 5:89031b2f5316 3598 }
Charles MacNeill 5:89031b2f5316 3599
Charles MacNeill 5:89031b2f5316 3600
Charles MacNeill 5:89031b2f5316 3601 VL53LX_Error VL53LX_set_ref_spad_char_config(
Charles MacNeill 5:89031b2f5316 3602 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3603 uint8_t vcsel_period_a,
Charles MacNeill 5:89031b2f5316 3604 uint32_t phasecal_timeout_us,
Charles MacNeill 5:89031b2f5316 3605 uint16_t total_rate_target_mcps,
Charles MacNeill 5:89031b2f5316 3606 uint16_t max_count_rate_rtn_limit_mcps,
Charles MacNeill 5:89031b2f5316 3607 uint16_t min_count_rate_rtn_limit_mcps,
Charles MacNeill 5:89031b2f5316 3608 uint16_t fast_osc_frequency)
Charles MacNeill 5:89031b2f5316 3609 {
Charles MacNeill 5:89031b2f5316 3610
Charles MacNeill 5:89031b2f5316 3611
Charles MacNeill 5:89031b2f5316 3612 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3613 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3614
Charles MacNeill 5:89031b2f5316 3615 uint8_t buffer[2];
Charles MacNeill 5:89031b2f5316 3616
Charles MacNeill 5:89031b2f5316 3617 uint32_t macro_period_us = 0;
Charles MacNeill 5:89031b2f5316 3618 uint32_t timeout_mclks = 0;
Charles MacNeill 5:89031b2f5316 3619
Charles MacNeill 5:89031b2f5316 3620 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3621
Charles MacNeill 5:89031b2f5316 3622
Charles MacNeill 5:89031b2f5316 3623 macro_period_us =
Charles MacNeill 5:89031b2f5316 3624 VL53LX_calc_macro_period_us(
Charles MacNeill 5:89031b2f5316 3625 fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 3626 vcsel_period_a);
Charles MacNeill 5:89031b2f5316 3627 if (macro_period_us == 0)
Charles MacNeill 5:89031b2f5316 3628 macro_period_us = 1;
Charles MacNeill 5:89031b2f5316 3629
Charles MacNeill 5:89031b2f5316 3630
Charles MacNeill 5:89031b2f5316 3631 timeout_mclks = phasecal_timeout_us << 12;
Charles MacNeill 5:89031b2f5316 3632 timeout_mclks = timeout_mclks + (macro_period_us>>1);
Charles MacNeill 5:89031b2f5316 3633 timeout_mclks = timeout_mclks / macro_period_us;
Charles MacNeill 5:89031b2f5316 3634
Charles MacNeill 5:89031b2f5316 3635 if (timeout_mclks > 0xFF)
Charles MacNeill 5:89031b2f5316 3636 pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF;
Charles MacNeill 5:89031b2f5316 3637 else
Charles MacNeill 5:89031b2f5316 3638 pdev->gen_cfg.phasecal_config__timeout_macrop =
Charles MacNeill 5:89031b2f5316 3639 (uint8_t)timeout_mclks;
Charles MacNeill 5:89031b2f5316 3640
Charles MacNeill 5:89031b2f5316 3641 pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a;
Charles MacNeill 5:89031b2f5316 3642
Charles MacNeill 5:89031b2f5316 3643
Charles MacNeill 5:89031b2f5316 3644
Charles MacNeill 5:89031b2f5316 3645 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3646 status =
Charles MacNeill 5:89031b2f5316 3647 VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 3648 Dev,
Charles MacNeill 5:89031b2f5316 3649 VL53LX_PHASECAL_CONFIG__TIMEOUT_MACROP,
Charles MacNeill 5:89031b2f5316 3650 pdev->gen_cfg.phasecal_config__timeout_macrop);
Charles MacNeill 5:89031b2f5316 3651
Charles MacNeill 5:89031b2f5316 3652 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3653 status =
Charles MacNeill 5:89031b2f5316 3654 VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 3655 Dev,
Charles MacNeill 5:89031b2f5316 3656 VL53LX_RANGE_CONFIG__VCSEL_PERIOD_A,
Charles MacNeill 5:89031b2f5316 3657 pdev->tim_cfg.range_config__vcsel_period_a);
Charles MacNeill 5:89031b2f5316 3658
Charles MacNeill 5:89031b2f5316 3659
Charles MacNeill 5:89031b2f5316 3660
Charles MacNeill 5:89031b2f5316 3661 buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a;
Charles MacNeill 5:89031b2f5316 3662 buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a;
Charles MacNeill 5:89031b2f5316 3663
Charles MacNeill 5:89031b2f5316 3664 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3665 status =
Charles MacNeill 5:89031b2f5316 3666 VL53LX_WriteMulti(
Charles MacNeill 5:89031b2f5316 3667 Dev,
Charles MacNeill 5:89031b2f5316 3668 VL53LX_SD_CONFIG__WOI_SD0,
Charles MacNeill 5:89031b2f5316 3669 buffer,
Charles MacNeill 5:89031b2f5316 3670 2);
Charles MacNeill 5:89031b2f5316 3671
Charles MacNeill 5:89031b2f5316 3672
Charles MacNeill 5:89031b2f5316 3673
Charles MacNeill 5:89031b2f5316 3674 pdev->customer.ref_spad_char__total_rate_target_mcps =
Charles MacNeill 5:89031b2f5316 3675 total_rate_target_mcps;
Charles MacNeill 5:89031b2f5316 3676
Charles MacNeill 5:89031b2f5316 3677 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3678 status =
Charles MacNeill 5:89031b2f5316 3679 VL53LX_WrWord(
Charles MacNeill 5:89031b2f5316 3680 Dev,
Charles MacNeill 5:89031b2f5316 3681 VL53LX_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS,
Charles MacNeill 5:89031b2f5316 3682 total_rate_target_mcps);
Charles MacNeill 5:89031b2f5316 3683
Charles MacNeill 5:89031b2f5316 3684 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3685 status =
Charles MacNeill 5:89031b2f5316 3686 VL53LX_WrWord(
Charles MacNeill 5:89031b2f5316 3687 Dev,
Charles MacNeill 5:89031b2f5316 3688 VL53LX_RANGE_CONFIG__SIGMA_THRESH,
Charles MacNeill 5:89031b2f5316 3689 max_count_rate_rtn_limit_mcps);
Charles MacNeill 5:89031b2f5316 3690
Charles MacNeill 5:89031b2f5316 3691 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3692 status =
Charles MacNeill 5:89031b2f5316 3693 VL53LX_WrWord(
Charles MacNeill 5:89031b2f5316 3694 Dev,
Charles MacNeill 5:89031b2f5316 3695 VL53LX_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,
Charles MacNeill 5:89031b2f5316 3696 min_count_rate_rtn_limit_mcps);
Charles MacNeill 5:89031b2f5316 3697
Charles MacNeill 5:89031b2f5316 3698 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3699
Charles MacNeill 5:89031b2f5316 3700 return status;
Charles MacNeill 5:89031b2f5316 3701 }
Charles MacNeill 5:89031b2f5316 3702
Charles MacNeill 5:89031b2f5316 3703
Charles MacNeill 5:89031b2f5316 3704 VL53LX_Error VL53LX_set_ssc_config(
Charles MacNeill 5:89031b2f5316 3705 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3706 VL53LX_ssc_config_t *pssc_cfg,
Charles MacNeill 5:89031b2f5316 3707 uint16_t fast_osc_frequency)
Charles MacNeill 5:89031b2f5316 3708 {
Charles MacNeill 5:89031b2f5316 3709
Charles MacNeill 5:89031b2f5316 3710
Charles MacNeill 5:89031b2f5316 3711 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3712 uint8_t buffer[5];
Charles MacNeill 5:89031b2f5316 3713
Charles MacNeill 5:89031b2f5316 3714 uint32_t macro_period_us = 0;
Charles MacNeill 5:89031b2f5316 3715 uint16_t timeout_encoded = 0;
Charles MacNeill 5:89031b2f5316 3716
Charles MacNeill 5:89031b2f5316 3717 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3718
Charles MacNeill 5:89031b2f5316 3719
Charles MacNeill 5:89031b2f5316 3720 macro_period_us =
Charles MacNeill 5:89031b2f5316 3721 VL53LX_calc_macro_period_us(
Charles MacNeill 5:89031b2f5316 3722 fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 3723 pssc_cfg->VL53LX_p_005);
Charles MacNeill 5:89031b2f5316 3724
Charles MacNeill 5:89031b2f5316 3725
Charles MacNeill 5:89031b2f5316 3726 timeout_encoded =
Charles MacNeill 5:89031b2f5316 3727 VL53LX_calc_encoded_timeout(
Charles MacNeill 5:89031b2f5316 3728 pssc_cfg->timeout_us,
Charles MacNeill 5:89031b2f5316 3729 macro_period_us);
Charles MacNeill 5:89031b2f5316 3730
Charles MacNeill 5:89031b2f5316 3731
Charles MacNeill 5:89031b2f5316 3732
Charles MacNeill 5:89031b2f5316 3733 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3734 status =
Charles MacNeill 5:89031b2f5316 3735 VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 3736 Dev,
Charles MacNeill 5:89031b2f5316 3737 VL53LX_CAL_CONFIG__VCSEL_START,
Charles MacNeill 5:89031b2f5316 3738 pssc_cfg->vcsel_start);
Charles MacNeill 5:89031b2f5316 3739
Charles MacNeill 5:89031b2f5316 3740 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3741 status =
Charles MacNeill 5:89031b2f5316 3742 VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 3743 Dev,
Charles MacNeill 5:89031b2f5316 3744 VL53LX_GLOBAL_CONFIG__VCSEL_WIDTH,
Charles MacNeill 5:89031b2f5316 3745 pssc_cfg->vcsel_width);
Charles MacNeill 5:89031b2f5316 3746
Charles MacNeill 5:89031b2f5316 3747
Charles MacNeill 5:89031b2f5316 3748
Charles MacNeill 5:89031b2f5316 3749 buffer[0] = (uint8_t)((timeout_encoded & 0x0000FF00) >> 8);
Charles MacNeill 5:89031b2f5316 3750 buffer[1] = (uint8_t) (timeout_encoded & 0x000000FF);
Charles MacNeill 5:89031b2f5316 3751 buffer[2] = pssc_cfg->VL53LX_p_005;
Charles MacNeill 5:89031b2f5316 3752 buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8);
Charles MacNeill 5:89031b2f5316 3753 buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps & 0x000000FF);
Charles MacNeill 5:89031b2f5316 3754
Charles MacNeill 5:89031b2f5316 3755 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3756 status =
Charles MacNeill 5:89031b2f5316 3757 VL53LX_WriteMulti(
Charles MacNeill 5:89031b2f5316 3758 Dev,
Charles MacNeill 5:89031b2f5316 3759 VL53LX_RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
Charles MacNeill 5:89031b2f5316 3760 buffer,
Charles MacNeill 5:89031b2f5316 3761 5);
Charles MacNeill 5:89031b2f5316 3762
Charles MacNeill 5:89031b2f5316 3763
Charles MacNeill 5:89031b2f5316 3764
Charles MacNeill 5:89031b2f5316 3765 buffer[0] = pssc_cfg->VL53LX_p_005;
Charles MacNeill 5:89031b2f5316 3766 buffer[1] = pssc_cfg->VL53LX_p_005;
Charles MacNeill 5:89031b2f5316 3767
Charles MacNeill 5:89031b2f5316 3768 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3769 status =
Charles MacNeill 5:89031b2f5316 3770 VL53LX_WriteMulti(
Charles MacNeill 5:89031b2f5316 3771 Dev,
Charles MacNeill 5:89031b2f5316 3772 VL53LX_SD_CONFIG__WOI_SD0,
Charles MacNeill 5:89031b2f5316 3773 buffer,
Charles MacNeill 5:89031b2f5316 3774 2);
Charles MacNeill 5:89031b2f5316 3775
Charles MacNeill 5:89031b2f5316 3776
Charles MacNeill 5:89031b2f5316 3777 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3778 status =
Charles MacNeill 5:89031b2f5316 3779 VL53LX_WrByte(
Charles MacNeill 5:89031b2f5316 3780 Dev,
Charles MacNeill 5:89031b2f5316 3781 VL53LX_NVM_BIST__CTRL,
Charles MacNeill 5:89031b2f5316 3782 pssc_cfg->array_select);
Charles MacNeill 5:89031b2f5316 3783
Charles MacNeill 5:89031b2f5316 3784 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3785
Charles MacNeill 5:89031b2f5316 3786 return status;
Charles MacNeill 5:89031b2f5316 3787 }
Charles MacNeill 5:89031b2f5316 3788
Charles MacNeill 5:89031b2f5316 3789
Charles MacNeill 5:89031b2f5316 3790 VL53LX_Error VL53LX_get_spad_rate_data(
Charles MacNeill 5:89031b2f5316 3791 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3792 VL53LX_spad_rate_data_t *pspad_rates)
Charles MacNeill 5:89031b2f5316 3793 {
Charles MacNeill 5:89031b2f5316 3794
Charles MacNeill 5:89031b2f5316 3795
Charles MacNeill 5:89031b2f5316 3796
Charles MacNeill 5:89031b2f5316 3797 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3798 int i = 0;
Charles MacNeill 5:89031b2f5316 3799
Charles MacNeill 5:89031b2f5316 3800 uint8_t VL53LX_p_003[512];
Charles MacNeill 5:89031b2f5316 3801 uint8_t *pdata = &VL53LX_p_003[0];
Charles MacNeill 5:89031b2f5316 3802
Charles MacNeill 5:89031b2f5316 3803 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3804
Charles MacNeill 5:89031b2f5316 3805
Charles MacNeill 5:89031b2f5316 3806
Charles MacNeill 5:89031b2f5316 3807 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3808 status = VL53LX_disable_firmware(Dev);
Charles MacNeill 5:89031b2f5316 3809
Charles MacNeill 5:89031b2f5316 3810
Charles MacNeill 5:89031b2f5316 3811
Charles MacNeill 5:89031b2f5316 3812 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3813 status =
Charles MacNeill 5:89031b2f5316 3814 VL53LX_ReadMulti(
Charles MacNeill 5:89031b2f5316 3815 Dev,
Charles MacNeill 5:89031b2f5316 3816 VL53LX_PRIVATE__PATCH_BASE_ADDR_RSLV,
Charles MacNeill 5:89031b2f5316 3817 pdata,
Charles MacNeill 5:89031b2f5316 3818 512);
Charles MacNeill 5:89031b2f5316 3819
Charles MacNeill 5:89031b2f5316 3820
Charles MacNeill 5:89031b2f5316 3821 pdata = &VL53LX_p_003[0];
Charles MacNeill 5:89031b2f5316 3822 for (i = 0; i < VL53LX_NO_OF_SPAD_ENABLES; i++) {
Charles MacNeill 5:89031b2f5316 3823 pspad_rates->rate_data[i] =
Charles MacNeill 5:89031b2f5316 3824 (uint16_t)VL53LX_decode_unsigned_integer(pdata, 2);
Charles MacNeill 5:89031b2f5316 3825 pdata += 2;
Charles MacNeill 5:89031b2f5316 3826 }
Charles MacNeill 5:89031b2f5316 3827
Charles MacNeill 5:89031b2f5316 3828
Charles MacNeill 5:89031b2f5316 3829
Charles MacNeill 5:89031b2f5316 3830 pspad_rates->VL53LX_p_020 = VL53LX_NO_OF_SPAD_ENABLES;
Charles MacNeill 5:89031b2f5316 3831 pspad_rates->no_of_values = VL53LX_NO_OF_SPAD_ENABLES;
Charles MacNeill 5:89031b2f5316 3832 pspad_rates->fractional_bits = 15;
Charles MacNeill 5:89031b2f5316 3833
Charles MacNeill 5:89031b2f5316 3834
Charles MacNeill 5:89031b2f5316 3835
Charles MacNeill 5:89031b2f5316 3836 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 3837 status = VL53LX_enable_firmware(Dev);
Charles MacNeill 5:89031b2f5316 3838
Charles MacNeill 5:89031b2f5316 3839 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3840
Charles MacNeill 5:89031b2f5316 3841 return status;
Charles MacNeill 5:89031b2f5316 3842 }
Charles MacNeill 5:89031b2f5316 3843
Charles MacNeill 5:89031b2f5316 3844
Charles MacNeill 5:89031b2f5316 3845
Charles MacNeill 5:89031b2f5316 3846 VL53LX_Error VL53LX_dynamic_xtalk_correction_calc_required_samples(
Charles MacNeill 5:89031b2f5316 3847 VL53LX_DEV Dev
Charles MacNeill 5:89031b2f5316 3848 )
Charles MacNeill 5:89031b2f5316 3849 {
Charles MacNeill 5:89031b2f5316 3850
Charles MacNeill 5:89031b2f5316 3851
Charles MacNeill 5:89031b2f5316 3852
Charles MacNeill 5:89031b2f5316 3853 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3854
Charles MacNeill 5:89031b2f5316 3855 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3856 VL53LX_LLDriverResults_t *pres = VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 3857 VL53LX_smudge_corrector_config_t *pconfig =
Charles MacNeill 5:89031b2f5316 3858 &(pdev->smudge_correct_config);
Charles MacNeill 5:89031b2f5316 3859 VL53LX_smudge_corrector_internals_t *pint =
Charles MacNeill 5:89031b2f5316 3860 &(pdev->smudge_corrector_internals);
Charles MacNeill 5:89031b2f5316 3861
Charles MacNeill 5:89031b2f5316 3862 VL53LX_range_results_t *presults = &(pres->range_results);
Charles MacNeill 5:89031b2f5316 3863 VL53LX_range_data_t *pxmonitor = &(presults->xmonitor);
Charles MacNeill 5:89031b2f5316 3864
Charles MacNeill 5:89031b2f5316 3865 uint32_t peak_duration_us = pxmonitor->peak_duration_us;
Charles MacNeill 5:89031b2f5316 3866
Charles MacNeill 5:89031b2f5316 3867 uint64_t temp64a;
Charles MacNeill 5:89031b2f5316 3868 uint64_t temp64z;
Charles MacNeill 5:89031b2f5316 3869
Charles MacNeill 5:89031b2f5316 3870 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3871
Charles MacNeill 5:89031b2f5316 3872 temp64a = pxmonitor->VL53LX_p_017 +
Charles MacNeill 5:89031b2f5316 3873 pxmonitor->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 3874 if (peak_duration_us == 0)
Charles MacNeill 5:89031b2f5316 3875 peak_duration_us = 1000;
Charles MacNeill 5:89031b2f5316 3876 temp64a = do_division_u((temp64a * 1000), peak_duration_us);
Charles MacNeill 5:89031b2f5316 3877 temp64a = do_division_u((temp64a * 1000), peak_duration_us);
Charles MacNeill 5:89031b2f5316 3878
Charles MacNeill 5:89031b2f5316 3879 temp64z = pconfig->noise_margin * pxmonitor->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 3880 if (temp64z == 0)
Charles MacNeill 5:89031b2f5316 3881 temp64z = 1;
Charles MacNeill 5:89031b2f5316 3882 temp64a = temp64a * 1000 * 256;
Charles MacNeill 5:89031b2f5316 3883 temp64a = do_division_u(temp64a, temp64z);
Charles MacNeill 5:89031b2f5316 3884 temp64a = temp64a * 1000 * 256;
Charles MacNeill 5:89031b2f5316 3885 temp64a = do_division_u(temp64a, temp64z);
Charles MacNeill 5:89031b2f5316 3886
Charles MacNeill 5:89031b2f5316 3887 pint->required_samples = (uint32_t)temp64a;
Charles MacNeill 5:89031b2f5316 3888
Charles MacNeill 5:89031b2f5316 3889
Charles MacNeill 5:89031b2f5316 3890 if (pint->required_samples < 2)
Charles MacNeill 5:89031b2f5316 3891 pint->required_samples = 2;
Charles MacNeill 5:89031b2f5316 3892
Charles MacNeill 5:89031b2f5316 3893 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 3894
Charles MacNeill 5:89031b2f5316 3895 return status;
Charles MacNeill 5:89031b2f5316 3896 }
Charles MacNeill 5:89031b2f5316 3897
Charles MacNeill 5:89031b2f5316 3898 VL53LX_Error VL53LX_dynamic_xtalk_correction_calc_new_xtalk(
Charles MacNeill 5:89031b2f5316 3899 VL53LX_DEV Dev,
Charles MacNeill 5:89031b2f5316 3900 uint32_t xtalk_offset_out,
Charles MacNeill 5:89031b2f5316 3901 VL53LX_smudge_corrector_config_t *pconfig,
Charles MacNeill 5:89031b2f5316 3902 VL53LX_smudge_corrector_data_t *pout,
Charles MacNeill 5:89031b2f5316 3903 uint8_t add_smudge,
Charles MacNeill 5:89031b2f5316 3904 uint8_t soft_update
Charles MacNeill 5:89031b2f5316 3905 )
Charles MacNeill 5:89031b2f5316 3906 {
Charles MacNeill 5:89031b2f5316 3907
Charles MacNeill 5:89031b2f5316 3908
Charles MacNeill 5:89031b2f5316 3909
Charles MacNeill 5:89031b2f5316 3910 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 3911 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 3912
Charles MacNeill 5:89031b2f5316 3913 int16_t x_gradient_scaler;
Charles MacNeill 5:89031b2f5316 3914 int16_t y_gradient_scaler;
Charles MacNeill 5:89031b2f5316 3915 uint32_t orig_xtalk_offset;
Charles MacNeill 5:89031b2f5316 3916 int16_t orig_x_gradient;
Charles MacNeill 5:89031b2f5316 3917 int16_t orig_y_gradient;
Charles MacNeill 5:89031b2f5316 3918 uint8_t histo_merge_nb;
Charles MacNeill 5:89031b2f5316 3919 uint8_t i;
Charles MacNeill 5:89031b2f5316 3920 int32_t itemp32;
Charles MacNeill 5:89031b2f5316 3921 uint32_t SmudgeFactor;
Charles MacNeill 5:89031b2f5316 3922 VL53LX_xtalk_config_t *pX = &(pdev->xtalk_cfg);
Charles MacNeill 5:89031b2f5316 3923 VL53LX_xtalk_calibration_results_t *pC = &(pdev->xtalk_cal);
Charles MacNeill 5:89031b2f5316 3924 uint32_t *pcpo;
Charles MacNeill 5:89031b2f5316 3925 uint32_t max, nXtalk, cXtalk;
Charles MacNeill 5:89031b2f5316 3926
Charles MacNeill 5:89031b2f5316 3927 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 3928
Charles MacNeill 5:89031b2f5316 3929
Charles MacNeill 5:89031b2f5316 3930 if (add_smudge == 1) {
Charles MacNeill 5:89031b2f5316 3931 pout->algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 5:89031b2f5316 3932 (uint32_t)xtalk_offset_out +
Charles MacNeill 5:89031b2f5316 3933 (uint32_t)pconfig->smudge_margin;
Charles MacNeill 5:89031b2f5316 3934 } else {
Charles MacNeill 5:89031b2f5316 3935 pout->algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 5:89031b2f5316 3936 (uint32_t)xtalk_offset_out;
Charles MacNeill 5:89031b2f5316 3937 }
Charles MacNeill 5:89031b2f5316 3938
Charles MacNeill 5:89031b2f5316 3939
Charles MacNeill 5:89031b2f5316 3940 orig_xtalk_offset =
Charles MacNeill 5:89031b2f5316 3941 pX->nvm_default__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 3942
Charles MacNeill 5:89031b2f5316 3943 orig_x_gradient =
Charles MacNeill 5:89031b2f5316 3944 pX->nvm_default__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 3945
Charles MacNeill 5:89031b2f5316 3946 orig_y_gradient =
Charles MacNeill 5:89031b2f5316 3947 pX->nvm_default__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 3948
Charles MacNeill 5:89031b2f5316 3949 if (((pconfig->user_scaler_set == 0) ||
Charles MacNeill 5:89031b2f5316 3950 (pconfig->scaler_calc_method == 1)) &&
Charles MacNeill 5:89031b2f5316 3951 (pC->algo__crosstalk_compensation_plane_offset_kcps != 0)) {
Charles MacNeill 5:89031b2f5316 3952
Charles MacNeill 5:89031b2f5316 3953 VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb);
Charles MacNeill 5:89031b2f5316 3954
Charles MacNeill 5:89031b2f5316 3955 if (histo_merge_nb == 0)
Charles MacNeill 5:89031b2f5316 3956 histo_merge_nb = 1;
Charles MacNeill 5:89031b2f5316 3957 if (pdev->tuning_parms.tp_hist_merge != 1)
Charles MacNeill 5:89031b2f5316 3958 orig_xtalk_offset =
Charles MacNeill 5:89031b2f5316 3959 pC->algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 3960 else
Charles MacNeill 5:89031b2f5316 3961 orig_xtalk_offset =
Charles MacNeill 5:89031b2f5316 3962 pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1];
Charles MacNeill 5:89031b2f5316 3963
Charles MacNeill 5:89031b2f5316 3964 orig_x_gradient =
Charles MacNeill 5:89031b2f5316 3965 pC->algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 3966
Charles MacNeill 5:89031b2f5316 3967 orig_y_gradient =
Charles MacNeill 5:89031b2f5316 3968 pC->algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 3969 }
Charles MacNeill 5:89031b2f5316 3970
Charles MacNeill 5:89031b2f5316 3971
Charles MacNeill 5:89031b2f5316 3972 if ((pconfig->user_scaler_set == 0) && (orig_x_gradient == 0))
Charles MacNeill 5:89031b2f5316 3973 pout->gradient_zero_flag |= 0x01;
Charles MacNeill 5:89031b2f5316 3974
Charles MacNeill 5:89031b2f5316 3975 if ((pconfig->user_scaler_set == 0) && (orig_y_gradient == 0))
Charles MacNeill 5:89031b2f5316 3976 pout->gradient_zero_flag |= 0x02;
Charles MacNeill 5:89031b2f5316 3977
Charles MacNeill 5:89031b2f5316 3978
Charles MacNeill 5:89031b2f5316 3979
Charles MacNeill 5:89031b2f5316 3980 if (orig_xtalk_offset == 0)
Charles MacNeill 5:89031b2f5316 3981 orig_xtalk_offset = 1;
Charles MacNeill 5:89031b2f5316 3982
Charles MacNeill 5:89031b2f5316 3983
Charles MacNeill 5:89031b2f5316 3984
Charles MacNeill 5:89031b2f5316 3985 if (pconfig->user_scaler_set == 1) {
Charles MacNeill 5:89031b2f5316 3986 x_gradient_scaler = pconfig->x_gradient_scaler;
Charles MacNeill 5:89031b2f5316 3987 y_gradient_scaler = pconfig->y_gradient_scaler;
Charles MacNeill 5:89031b2f5316 3988 } else {
Charles MacNeill 5:89031b2f5316 3989
Charles MacNeill 5:89031b2f5316 3990 x_gradient_scaler = (int16_t)do_division_s(
Charles MacNeill 5:89031b2f5316 3991 (((int32_t)orig_x_gradient) << 6),
Charles MacNeill 5:89031b2f5316 3992 orig_xtalk_offset);
Charles MacNeill 5:89031b2f5316 3993 pconfig->x_gradient_scaler = x_gradient_scaler;
Charles MacNeill 5:89031b2f5316 3994 y_gradient_scaler = (int16_t)do_division_s(
Charles MacNeill 5:89031b2f5316 3995 (((int32_t)orig_y_gradient) << 6),
Charles MacNeill 5:89031b2f5316 3996 orig_xtalk_offset);
Charles MacNeill 5:89031b2f5316 3997 pconfig->y_gradient_scaler = y_gradient_scaler;
Charles MacNeill 5:89031b2f5316 3998 }
Charles MacNeill 5:89031b2f5316 3999
Charles MacNeill 5:89031b2f5316 4000
Charles MacNeill 5:89031b2f5316 4001
Charles MacNeill 5:89031b2f5316 4002 if (pconfig->scaler_calc_method == 0) {
Charles MacNeill 5:89031b2f5316 4003
Charles MacNeill 5:89031b2f5316 4004
Charles MacNeill 5:89031b2f5316 4005 itemp32 = (int32_t)(
Charles MacNeill 5:89031b2f5316 4006 pout->algo__crosstalk_compensation_plane_offset_kcps *
Charles MacNeill 5:89031b2f5316 4007 x_gradient_scaler);
Charles MacNeill 5:89031b2f5316 4008 itemp32 = itemp32 >> 6;
Charles MacNeill 5:89031b2f5316 4009 if (itemp32 > 0xFFFF)
Charles MacNeill 5:89031b2f5316 4010 itemp32 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 4011
Charles MacNeill 5:89031b2f5316 4012 pout->algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 4013 (int16_t)itemp32;
Charles MacNeill 5:89031b2f5316 4014
Charles MacNeill 5:89031b2f5316 4015 itemp32 = (int32_t)(
Charles MacNeill 5:89031b2f5316 4016 pout->algo__crosstalk_compensation_plane_offset_kcps *
Charles MacNeill 5:89031b2f5316 4017 y_gradient_scaler);
Charles MacNeill 5:89031b2f5316 4018 itemp32 = itemp32 >> 6;
Charles MacNeill 5:89031b2f5316 4019 if (itemp32 > 0xFFFF)
Charles MacNeill 5:89031b2f5316 4020 itemp32 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 4021
Charles MacNeill 5:89031b2f5316 4022 pout->algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 4023 (int16_t)itemp32;
Charles MacNeill 5:89031b2f5316 4024 } else if (pconfig->scaler_calc_method == 1) {
Charles MacNeill 5:89031b2f5316 4025
Charles MacNeill 5:89031b2f5316 4026
Charles MacNeill 5:89031b2f5316 4027 itemp32 = (int32_t)(orig_xtalk_offset -
Charles MacNeill 5:89031b2f5316 4028 pout->algo__crosstalk_compensation_plane_offset_kcps);
Charles MacNeill 5:89031b2f5316 4029 itemp32 = (int32_t)(do_division_s(itemp32, 16));
Charles MacNeill 5:89031b2f5316 4030 itemp32 = itemp32 << 2;
Charles MacNeill 5:89031b2f5316 4031 itemp32 = itemp32 + (int32_t)(orig_x_gradient);
Charles MacNeill 5:89031b2f5316 4032 if (itemp32 > 0xFFFF)
Charles MacNeill 5:89031b2f5316 4033 itemp32 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 4034
Charles MacNeill 5:89031b2f5316 4035 pout->algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 4036 (int16_t)itemp32;
Charles MacNeill 5:89031b2f5316 4037
Charles MacNeill 5:89031b2f5316 4038 itemp32 = (int32_t)(orig_xtalk_offset -
Charles MacNeill 5:89031b2f5316 4039 pout->algo__crosstalk_compensation_plane_offset_kcps);
Charles MacNeill 5:89031b2f5316 4040 itemp32 = (int32_t)(do_division_s(itemp32, 80));
Charles MacNeill 5:89031b2f5316 4041 itemp32 = itemp32 << 2;
Charles MacNeill 5:89031b2f5316 4042 itemp32 = itemp32 + (int32_t)(orig_y_gradient);
Charles MacNeill 5:89031b2f5316 4043 if (itemp32 > 0xFFFF)
Charles MacNeill 5:89031b2f5316 4044 itemp32 = 0xFFFF;
Charles MacNeill 5:89031b2f5316 4045
Charles MacNeill 5:89031b2f5316 4046 pout->algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 4047 (int16_t)itemp32;
Charles MacNeill 5:89031b2f5316 4048 }
Charles MacNeill 5:89031b2f5316 4049
Charles MacNeill 5:89031b2f5316 4050
Charles MacNeill 5:89031b2f5316 4051 if ((pconfig->smudge_corr_apply_enabled == 1) &&
Charles MacNeill 5:89031b2f5316 4052 (soft_update != 1)) {
Charles MacNeill 5:89031b2f5316 4053
Charles MacNeill 5:89031b2f5316 4054 pout->new_xtalk_applied_flag = 1;
Charles MacNeill 5:89031b2f5316 4055 nXtalk = pout->algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 4056
Charles MacNeill 5:89031b2f5316 4057 VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb);
Charles MacNeill 5:89031b2f5316 4058 max = pdev->tuning_parms.tp_hist_merge_max_size;
Charles MacNeill 5:89031b2f5316 4059 pcpo = &(pC->algo__xtalk_cpo_HistoMerge_kcps[0]);
Charles MacNeill 5:89031b2f5316 4060 if ((histo_merge_nb > 0) &&
Charles MacNeill 5:89031b2f5316 4061 (pdev->tuning_parms.tp_hist_merge == 1) &&
Charles MacNeill 5:89031b2f5316 4062 (nXtalk != 0)) {
Charles MacNeill 5:89031b2f5316 4063 cXtalk =
Charles MacNeill 5:89031b2f5316 4064 pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1];
Charles MacNeill 5:89031b2f5316 4065 SmudgeFactor = cXtalk * 1000 / nXtalk;
Charles MacNeill 5:89031b2f5316 4066 if (SmudgeFactor >= pconfig->max_smudge_factor)
Charles MacNeill 5:89031b2f5316 4067 pout->new_xtalk_applied_flag = 0;
Charles MacNeill 5:89031b2f5316 4068 else if (SmudgeFactor > 0)
Charles MacNeill 5:89031b2f5316 4069 for (i = 0; i < max; i++) {
Charles MacNeill 5:89031b2f5316 4070 *pcpo *= 1000;
Charles MacNeill 5:89031b2f5316 4071 *pcpo /= SmudgeFactor;
Charles MacNeill 5:89031b2f5316 4072 pcpo++;
Charles MacNeill 5:89031b2f5316 4073 }
Charles MacNeill 5:89031b2f5316 4074 }
Charles MacNeill 5:89031b2f5316 4075 if (pout->new_xtalk_applied_flag) {
Charles MacNeill 5:89031b2f5316 4076
Charles MacNeill 5:89031b2f5316 4077 pX->algo__crosstalk_compensation_plane_offset_kcps =
Charles MacNeill 5:89031b2f5316 4078 pout->algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 4079 pX->algo__crosstalk_compensation_x_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 4080 pout->algo__crosstalk_compensation_x_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 4081 pX->algo__crosstalk_compensation_y_plane_gradient_kcps =
Charles MacNeill 5:89031b2f5316 4082 pout->algo__crosstalk_compensation_y_plane_gradient_kcps;
Charles MacNeill 5:89031b2f5316 4083
Charles MacNeill 5:89031b2f5316 4084 if (pconfig->smudge_corr_single_apply == 1) {
Charles MacNeill 5:89031b2f5316 4085
Charles MacNeill 5:89031b2f5316 4086 pconfig->smudge_corr_apply_enabled = 0;
Charles MacNeill 5:89031b2f5316 4087 pconfig->smudge_corr_single_apply = 0;
Charles MacNeill 5:89031b2f5316 4088 }
Charles MacNeill 5:89031b2f5316 4089 }
Charles MacNeill 5:89031b2f5316 4090 }
Charles MacNeill 5:89031b2f5316 4091
Charles MacNeill 5:89031b2f5316 4092
Charles MacNeill 5:89031b2f5316 4093 if (soft_update != 1)
Charles MacNeill 5:89031b2f5316 4094 pout->smudge_corr_valid = 1;
Charles MacNeill 5:89031b2f5316 4095
Charles MacNeill 5:89031b2f5316 4096 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4097
Charles MacNeill 5:89031b2f5316 4098 return status;
Charles MacNeill 5:89031b2f5316 4099 }
Charles MacNeill 5:89031b2f5316 4100
Charles MacNeill 5:89031b2f5316 4101 #define CONT_CONTINUE 0
Charles MacNeill 5:89031b2f5316 4102 #define CONT_NEXT_LOOP 1
Charles MacNeill 5:89031b2f5316 4103 #define CONT_RESET 2
Charles MacNeill 5:89031b2f5316 4104 VL53LX_Error VL53LX_dynamic_xtalk_correction_corrector(
Charles MacNeill 5:89031b2f5316 4105 VL53LX_DEV Dev
Charles MacNeill 5:89031b2f5316 4106 )
Charles MacNeill 5:89031b2f5316 4107 {
Charles MacNeill 5:89031b2f5316 4108
Charles MacNeill 5:89031b2f5316 4109
Charles MacNeill 5:89031b2f5316 4110
Charles MacNeill 5:89031b2f5316 4111 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4112
Charles MacNeill 5:89031b2f5316 4113 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4114 VL53LX_LLDriverResults_t *pres = VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 4115 VL53LX_smudge_corrector_config_t *pconfig =
Charles MacNeill 5:89031b2f5316 4116 &(pdev->smudge_correct_config);
Charles MacNeill 5:89031b2f5316 4117 VL53LX_smudge_corrector_internals_t *pint =
Charles MacNeill 5:89031b2f5316 4118 &(pdev->smudge_corrector_internals);
Charles MacNeill 5:89031b2f5316 4119 VL53LX_smudge_corrector_data_t *pout =
Charles MacNeill 5:89031b2f5316 4120 &(pres->range_results.smudge_corrector_data);
Charles MacNeill 5:89031b2f5316 4121 VL53LX_range_results_t *pR = &(pres->range_results);
Charles MacNeill 5:89031b2f5316 4122 VL53LX_xtalk_config_t *pX = &(pdev->xtalk_cfg);
Charles MacNeill 5:89031b2f5316 4123
Charles MacNeill 5:89031b2f5316 4124 uint8_t run_smudge_detection = 0;
Charles MacNeill 5:89031b2f5316 4125 uint8_t merging_complete = 0;
Charles MacNeill 5:89031b2f5316 4126 uint8_t run_nodetect = 0;
Charles MacNeill 5:89031b2f5316 4127 uint8_t ambient_check = 0;
Charles MacNeill 5:89031b2f5316 4128 int32_t itemp32 = 0;
Charles MacNeill 5:89031b2f5316 4129 uint64_t utemp64 = 0;
Charles MacNeill 5:89031b2f5316 4130 uint8_t continue_processing = CONT_CONTINUE;
Charles MacNeill 5:89031b2f5316 4131 uint32_t xtalk_offset_out = 0;
Charles MacNeill 5:89031b2f5316 4132 uint32_t xtalk_offset_in = 0;
Charles MacNeill 5:89031b2f5316 4133 uint32_t current_xtalk = 0;
Charles MacNeill 5:89031b2f5316 4134 uint32_t smudge_margin_adjusted = 0;
Charles MacNeill 5:89031b2f5316 4135 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 4136 uint8_t nodetect_index = 0;
Charles MacNeill 5:89031b2f5316 4137 uint16_t amr;
Charles MacNeill 5:89031b2f5316 4138 uint32_t cco;
Charles MacNeill 5:89031b2f5316 4139 uint8_t histo_merge_nb;
Charles MacNeill 5:89031b2f5316 4140
Charles MacNeill 5:89031b2f5316 4141
Charles MacNeill 5:89031b2f5316 4142 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4143
Charles MacNeill 5:89031b2f5316 4144 VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb);
Charles MacNeill 5:89031b2f5316 4145 if ((histo_merge_nb == 0) ||
Charles MacNeill 5:89031b2f5316 4146 (pdev->tuning_parms.tp_hist_merge != 1))
Charles MacNeill 5:89031b2f5316 4147 histo_merge_nb = 1;
Charles MacNeill 5:89031b2f5316 4148
Charles MacNeill 5:89031b2f5316 4149
Charles MacNeill 5:89031b2f5316 4150 VL53LX_dynamic_xtalk_correction_output_init(pres);
Charles MacNeill 5:89031b2f5316 4151
Charles MacNeill 5:89031b2f5316 4152
Charles MacNeill 5:89031b2f5316 4153 ambient_check = (pconfig->smudge_corr_ambient_threshold == 0) ||
Charles MacNeill 5:89031b2f5316 4154 ((pconfig->smudge_corr_ambient_threshold * histo_merge_nb) >
Charles MacNeill 5:89031b2f5316 4155 ((uint32_t)pR->xmonitor.ambient_count_rate_mcps));
Charles MacNeill 5:89031b2f5316 4156
Charles MacNeill 5:89031b2f5316 4157
Charles MacNeill 5:89031b2f5316 4158 merging_complete =
Charles MacNeill 5:89031b2f5316 4159 ((pdev->tuning_parms.tp_hist_merge != 1) ||
Charles MacNeill 5:89031b2f5316 4160 (histo_merge_nb == pdev->tuning_parms.tp_hist_merge_max_size));
Charles MacNeill 5:89031b2f5316 4161 run_smudge_detection =
Charles MacNeill 5:89031b2f5316 4162 (pconfig->smudge_corr_enabled == 1) &&
Charles MacNeill 5:89031b2f5316 4163 ambient_check &&
Charles MacNeill 5:89031b2f5316 4164 (pR->xmonitor.range_status
Charles MacNeill 5:89031b2f5316 4165 == VL53LX_DEVICEERROR_RANGECOMPLETE) &&
Charles MacNeill 5:89031b2f5316 4166 merging_complete;
Charles MacNeill 5:89031b2f5316 4167
Charles MacNeill 5:89031b2f5316 4168
Charles MacNeill 5:89031b2f5316 4169 if ((pR->xmonitor.range_status
Charles MacNeill 5:89031b2f5316 4170 != VL53LX_DEVICEERROR_RANGECOMPLETE) &&
Charles MacNeill 5:89031b2f5316 4171 (pconfig->smudge_corr_enabled == 1)) {
Charles MacNeill 5:89031b2f5316 4172
Charles MacNeill 5:89031b2f5316 4173 run_nodetect = 2;
Charles MacNeill 5:89031b2f5316 4174 for (i = 0; i < pR->active_results; i++) {
Charles MacNeill 5:89031b2f5316 4175 if (pR->VL53LX_p_003[i].range_status ==
Charles MacNeill 5:89031b2f5316 4176 VL53LX_DEVICEERROR_RANGECOMPLETE) {
Charles MacNeill 5:89031b2f5316 4177 if (pR->VL53LX_p_003[i].median_range_mm
Charles MacNeill 5:89031b2f5316 4178 <=
Charles MacNeill 5:89031b2f5316 4179 pconfig->nodetect_min_range_mm) {
Charles MacNeill 5:89031b2f5316 4180 run_nodetect = 0;
Charles MacNeill 5:89031b2f5316 4181 } else {
Charles MacNeill 5:89031b2f5316 4182 if (run_nodetect == 2) {
Charles MacNeill 5:89031b2f5316 4183 run_nodetect = 1;
Charles MacNeill 5:89031b2f5316 4184 nodetect_index = i;
Charles MacNeill 5:89031b2f5316 4185 }
Charles MacNeill 5:89031b2f5316 4186 }
Charles MacNeill 5:89031b2f5316 4187 }
Charles MacNeill 5:89031b2f5316 4188 }
Charles MacNeill 5:89031b2f5316 4189
Charles MacNeill 5:89031b2f5316 4190 if (run_nodetect == 2)
Charles MacNeill 5:89031b2f5316 4191
Charles MacNeill 5:89031b2f5316 4192 run_nodetect = 0;
Charles MacNeill 5:89031b2f5316 4193
Charles MacNeill 5:89031b2f5316 4194 amr =
Charles MacNeill 5:89031b2f5316 4195 pR->VL53LX_p_003[nodetect_index].ambient_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 4196
Charles MacNeill 5:89031b2f5316 4197 if (run_nodetect == 1) {
Charles MacNeill 5:89031b2f5316 4198
Charles MacNeill 5:89031b2f5316 4199
Charles MacNeill 5:89031b2f5316 4200
Charles MacNeill 5:89031b2f5316 4201
Charles MacNeill 5:89031b2f5316 4202 utemp64 = 1000 * ((uint64_t)amr);
Charles MacNeill 5:89031b2f5316 4203
Charles MacNeill 5:89031b2f5316 4204
Charles MacNeill 5:89031b2f5316 4205 utemp64 = utemp64 << 9;
Charles MacNeill 5:89031b2f5316 4206
Charles MacNeill 5:89031b2f5316 4207
Charles MacNeill 5:89031b2f5316 4208 if (utemp64 < pconfig->nodetect_ambient_threshold)
Charles MacNeill 5:89031b2f5316 4209 run_nodetect = 1;
Charles MacNeill 5:89031b2f5316 4210 else
Charles MacNeill 5:89031b2f5316 4211 run_nodetect = 0;
Charles MacNeill 5:89031b2f5316 4212
Charles MacNeill 5:89031b2f5316 4213 }
Charles MacNeill 5:89031b2f5316 4214 }
Charles MacNeill 5:89031b2f5316 4215
Charles MacNeill 5:89031b2f5316 4216
Charles MacNeill 5:89031b2f5316 4217 if (run_smudge_detection) {
Charles MacNeill 5:89031b2f5316 4218
Charles MacNeill 5:89031b2f5316 4219 pint->nodetect_counter = 0;
Charles MacNeill 5:89031b2f5316 4220
Charles MacNeill 5:89031b2f5316 4221
Charles MacNeill 5:89031b2f5316 4222 VL53LX_dynamic_xtalk_correction_calc_required_samples(Dev);
Charles MacNeill 5:89031b2f5316 4223
Charles MacNeill 5:89031b2f5316 4224
Charles MacNeill 5:89031b2f5316 4225 xtalk_offset_in =
Charles MacNeill 5:89031b2f5316 4226 pR->xmonitor.VL53LX_p_009;
Charles MacNeill 5:89031b2f5316 4227
Charles MacNeill 5:89031b2f5316 4228
Charles MacNeill 5:89031b2f5316 4229 cco = pX->algo__crosstalk_compensation_plane_offset_kcps;
Charles MacNeill 5:89031b2f5316 4230 current_xtalk = ((uint32_t)cco) << 2;
Charles MacNeill 5:89031b2f5316 4231
Charles MacNeill 5:89031b2f5316 4232
Charles MacNeill 5:89031b2f5316 4233 smudge_margin_adjusted =
Charles MacNeill 5:89031b2f5316 4234 ((uint32_t)(pconfig->smudge_margin)) << 2;
Charles MacNeill 5:89031b2f5316 4235
Charles MacNeill 5:89031b2f5316 4236
Charles MacNeill 5:89031b2f5316 4237 itemp32 = xtalk_offset_in - current_xtalk +
Charles MacNeill 5:89031b2f5316 4238 smudge_margin_adjusted;
Charles MacNeill 5:89031b2f5316 4239
Charles MacNeill 5:89031b2f5316 4240 if (itemp32 < 0)
Charles MacNeill 5:89031b2f5316 4241 itemp32 = itemp32 * (-1);
Charles MacNeill 5:89031b2f5316 4242
Charles MacNeill 5:89031b2f5316 4243
Charles MacNeill 5:89031b2f5316 4244 if (itemp32 > ((int32_t)pconfig->single_xtalk_delta)) {
Charles MacNeill 5:89031b2f5316 4245 if ((int32_t)xtalk_offset_in >
Charles MacNeill 5:89031b2f5316 4246 ((int32_t)current_xtalk -
Charles MacNeill 5:89031b2f5316 4247 (int32_t)smudge_margin_adjusted)) {
Charles MacNeill 5:89031b2f5316 4248 pout->single_xtalk_delta_flag = 1;
Charles MacNeill 5:89031b2f5316 4249 } else {
Charles MacNeill 5:89031b2f5316 4250 pout->single_xtalk_delta_flag = 2;
Charles MacNeill 5:89031b2f5316 4251 }
Charles MacNeill 5:89031b2f5316 4252 }
Charles MacNeill 5:89031b2f5316 4253
Charles MacNeill 5:89031b2f5316 4254
Charles MacNeill 5:89031b2f5316 4255 pint->current_samples = pint->current_samples + 1;
Charles MacNeill 5:89031b2f5316 4256
Charles MacNeill 5:89031b2f5316 4257
Charles MacNeill 5:89031b2f5316 4258 if (pint->current_samples > pconfig->sample_limit) {
Charles MacNeill 5:89031b2f5316 4259 pout->sample_limit_exceeded_flag = 1;
Charles MacNeill 5:89031b2f5316 4260 continue_processing = CONT_RESET;
Charles MacNeill 5:89031b2f5316 4261 } else {
Charles MacNeill 5:89031b2f5316 4262 pint->accumulator = pint->accumulator +
Charles MacNeill 5:89031b2f5316 4263 xtalk_offset_in;
Charles MacNeill 5:89031b2f5316 4264 }
Charles MacNeill 5:89031b2f5316 4265
Charles MacNeill 5:89031b2f5316 4266 if (pint->current_samples < pint->required_samples)
Charles MacNeill 5:89031b2f5316 4267 continue_processing = CONT_NEXT_LOOP;
Charles MacNeill 5:89031b2f5316 4268
Charles MacNeill 5:89031b2f5316 4269
Charles MacNeill 5:89031b2f5316 4270 xtalk_offset_out =
Charles MacNeill 5:89031b2f5316 4271 (uint32_t)(do_division_u(pint->accumulator,
Charles MacNeill 5:89031b2f5316 4272 pint->current_samples));
Charles MacNeill 5:89031b2f5316 4273
Charles MacNeill 5:89031b2f5316 4274
Charles MacNeill 5:89031b2f5316 4275 itemp32 = xtalk_offset_out - current_xtalk +
Charles MacNeill 5:89031b2f5316 4276 smudge_margin_adjusted;
Charles MacNeill 5:89031b2f5316 4277
Charles MacNeill 5:89031b2f5316 4278 if (itemp32 < 0)
Charles MacNeill 5:89031b2f5316 4279 itemp32 = itemp32 * (-1);
Charles MacNeill 5:89031b2f5316 4280
Charles MacNeill 5:89031b2f5316 4281 if (continue_processing == CONT_CONTINUE &&
Charles MacNeill 5:89031b2f5316 4282 (itemp32 >= ((int32_t)(pconfig->averaged_xtalk_delta)))
Charles MacNeill 5:89031b2f5316 4283 ) {
Charles MacNeill 5:89031b2f5316 4284 if ((int32_t)xtalk_offset_out >
Charles MacNeill 5:89031b2f5316 4285 ((int32_t)current_xtalk -
Charles MacNeill 5:89031b2f5316 4286 (int32_t)smudge_margin_adjusted))
Charles MacNeill 5:89031b2f5316 4287 pout->averaged_xtalk_delta_flag = 1;
Charles MacNeill 5:89031b2f5316 4288 else
Charles MacNeill 5:89031b2f5316 4289 pout->averaged_xtalk_delta_flag = 2;
Charles MacNeill 5:89031b2f5316 4290 }
Charles MacNeill 5:89031b2f5316 4291
Charles MacNeill 5:89031b2f5316 4292 if (continue_processing == CONT_CONTINUE &&
Charles MacNeill 5:89031b2f5316 4293 (itemp32 < ((int32_t)(pconfig->averaged_xtalk_delta)))
Charles MacNeill 5:89031b2f5316 4294 )
Charles MacNeill 5:89031b2f5316 4295
Charles MacNeill 5:89031b2f5316 4296 continue_processing = CONT_RESET;
Charles MacNeill 5:89031b2f5316 4297
Charles MacNeill 5:89031b2f5316 4298
Charles MacNeill 5:89031b2f5316 4299
Charles MacNeill 5:89031b2f5316 4300 pout->smudge_corr_clipped = 0;
Charles MacNeill 5:89031b2f5316 4301 if ((continue_processing == CONT_CONTINUE) &&
Charles MacNeill 5:89031b2f5316 4302 (pconfig->smudge_corr_clip_limit != 0)) {
Charles MacNeill 5:89031b2f5316 4303 if (xtalk_offset_out >
Charles MacNeill 5:89031b2f5316 4304 (pconfig->smudge_corr_clip_limit * histo_merge_nb)) {
Charles MacNeill 5:89031b2f5316 4305 pout->smudge_corr_clipped = 1;
Charles MacNeill 5:89031b2f5316 4306 continue_processing = CONT_RESET;
Charles MacNeill 5:89031b2f5316 4307 }
Charles MacNeill 5:89031b2f5316 4308 }
Charles MacNeill 5:89031b2f5316 4309
Charles MacNeill 5:89031b2f5316 4310
Charles MacNeill 5:89031b2f5316 4311
Charles MacNeill 5:89031b2f5316 4312 if (pconfig->user_xtalk_offset_limit_hi &&
Charles MacNeill 5:89031b2f5316 4313 (xtalk_offset_out >
Charles MacNeill 5:89031b2f5316 4314 pconfig->user_xtalk_offset_limit))
Charles MacNeill 5:89031b2f5316 4315 xtalk_offset_out =
Charles MacNeill 5:89031b2f5316 4316 pconfig->user_xtalk_offset_limit;
Charles MacNeill 5:89031b2f5316 4317
Charles MacNeill 5:89031b2f5316 4318
Charles MacNeill 5:89031b2f5316 4319
Charles MacNeill 5:89031b2f5316 4320 if ((pconfig->user_xtalk_offset_limit_hi == 0) &&
Charles MacNeill 5:89031b2f5316 4321 (xtalk_offset_out <
Charles MacNeill 5:89031b2f5316 4322 pconfig->user_xtalk_offset_limit))
Charles MacNeill 5:89031b2f5316 4323 xtalk_offset_out =
Charles MacNeill 5:89031b2f5316 4324 pconfig->user_xtalk_offset_limit;
Charles MacNeill 5:89031b2f5316 4325
Charles MacNeill 5:89031b2f5316 4326
Charles MacNeill 5:89031b2f5316 4327
Charles MacNeill 5:89031b2f5316 4328 xtalk_offset_out = xtalk_offset_out >> 2;
Charles MacNeill 5:89031b2f5316 4329 if (xtalk_offset_out > 0x3FFFF)
Charles MacNeill 5:89031b2f5316 4330 xtalk_offset_out = 0x3FFFF;
Charles MacNeill 5:89031b2f5316 4331
Charles MacNeill 5:89031b2f5316 4332
Charles MacNeill 5:89031b2f5316 4333 if (continue_processing == CONT_CONTINUE) {
Charles MacNeill 5:89031b2f5316 4334
Charles MacNeill 5:89031b2f5316 4335 VL53LX_dynamic_xtalk_correction_calc_new_xtalk(
Charles MacNeill 5:89031b2f5316 4336 Dev,
Charles MacNeill 5:89031b2f5316 4337 xtalk_offset_out,
Charles MacNeill 5:89031b2f5316 4338 pconfig,
Charles MacNeill 5:89031b2f5316 4339 pout,
Charles MacNeill 5:89031b2f5316 4340 1,
Charles MacNeill 5:89031b2f5316 4341 0
Charles MacNeill 5:89031b2f5316 4342 );
Charles MacNeill 5:89031b2f5316 4343
Charles MacNeill 5:89031b2f5316 4344
Charles MacNeill 5:89031b2f5316 4345 continue_processing = CONT_RESET;
Charles MacNeill 5:89031b2f5316 4346 } else {
Charles MacNeill 5:89031b2f5316 4347
Charles MacNeill 5:89031b2f5316 4348 VL53LX_dynamic_xtalk_correction_calc_new_xtalk(
Charles MacNeill 5:89031b2f5316 4349 Dev,
Charles MacNeill 5:89031b2f5316 4350 xtalk_offset_out,
Charles MacNeill 5:89031b2f5316 4351 pconfig,
Charles MacNeill 5:89031b2f5316 4352 pout,
Charles MacNeill 5:89031b2f5316 4353 1,
Charles MacNeill 5:89031b2f5316 4354 1
Charles MacNeill 5:89031b2f5316 4355 );
Charles MacNeill 5:89031b2f5316 4356 }
Charles MacNeill 5:89031b2f5316 4357
Charles MacNeill 5:89031b2f5316 4358
Charles MacNeill 5:89031b2f5316 4359 if (continue_processing == CONT_RESET) {
Charles MacNeill 5:89031b2f5316 4360 pint->accumulator = 0;
Charles MacNeill 5:89031b2f5316 4361 pint->current_samples = 0;
Charles MacNeill 5:89031b2f5316 4362 pint->nodetect_counter = 0;
Charles MacNeill 5:89031b2f5316 4363 }
Charles MacNeill 5:89031b2f5316 4364
Charles MacNeill 5:89031b2f5316 4365 }
Charles MacNeill 5:89031b2f5316 4366
Charles MacNeill 5:89031b2f5316 4367 continue_processing = CONT_CONTINUE;
Charles MacNeill 5:89031b2f5316 4368 if (run_nodetect == 1) {
Charles MacNeill 5:89031b2f5316 4369
Charles MacNeill 5:89031b2f5316 4370 pint->nodetect_counter += 1;
Charles MacNeill 5:89031b2f5316 4371
Charles MacNeill 5:89031b2f5316 4372
Charles MacNeill 5:89031b2f5316 4373 if (pint->nodetect_counter < pconfig->nodetect_sample_limit)
Charles MacNeill 5:89031b2f5316 4374 continue_processing = CONT_NEXT_LOOP;
Charles MacNeill 5:89031b2f5316 4375
Charles MacNeill 5:89031b2f5316 4376
Charles MacNeill 5:89031b2f5316 4377 xtalk_offset_out = (uint32_t)(pconfig->nodetect_xtalk_offset);
Charles MacNeill 5:89031b2f5316 4378
Charles MacNeill 5:89031b2f5316 4379 if (continue_processing == CONT_CONTINUE) {
Charles MacNeill 5:89031b2f5316 4380
Charles MacNeill 5:89031b2f5316 4381 VL53LX_dynamic_xtalk_correction_calc_new_xtalk(
Charles MacNeill 5:89031b2f5316 4382 Dev,
Charles MacNeill 5:89031b2f5316 4383 xtalk_offset_out,
Charles MacNeill 5:89031b2f5316 4384 pconfig,
Charles MacNeill 5:89031b2f5316 4385 pout,
Charles MacNeill 5:89031b2f5316 4386 0,
Charles MacNeill 5:89031b2f5316 4387 0
Charles MacNeill 5:89031b2f5316 4388 );
Charles MacNeill 5:89031b2f5316 4389
Charles MacNeill 5:89031b2f5316 4390
Charles MacNeill 5:89031b2f5316 4391 pout->smudge_corr_valid = 2;
Charles MacNeill 5:89031b2f5316 4392
Charles MacNeill 5:89031b2f5316 4393
Charles MacNeill 5:89031b2f5316 4394 continue_processing = CONT_RESET;
Charles MacNeill 5:89031b2f5316 4395 } else {
Charles MacNeill 5:89031b2f5316 4396
Charles MacNeill 5:89031b2f5316 4397 VL53LX_dynamic_xtalk_correction_calc_new_xtalk(
Charles MacNeill 5:89031b2f5316 4398 Dev,
Charles MacNeill 5:89031b2f5316 4399 xtalk_offset_out,
Charles MacNeill 5:89031b2f5316 4400 pconfig,
Charles MacNeill 5:89031b2f5316 4401 pout,
Charles MacNeill 5:89031b2f5316 4402 0,
Charles MacNeill 5:89031b2f5316 4403 1
Charles MacNeill 5:89031b2f5316 4404 );
Charles MacNeill 5:89031b2f5316 4405 }
Charles MacNeill 5:89031b2f5316 4406
Charles MacNeill 5:89031b2f5316 4407
Charles MacNeill 5:89031b2f5316 4408 if (continue_processing == CONT_RESET) {
Charles MacNeill 5:89031b2f5316 4409 pint->accumulator = 0;
Charles MacNeill 5:89031b2f5316 4410 pint->current_samples = 0;
Charles MacNeill 5:89031b2f5316 4411 pint->nodetect_counter = 0;
Charles MacNeill 5:89031b2f5316 4412 }
Charles MacNeill 5:89031b2f5316 4413 }
Charles MacNeill 5:89031b2f5316 4414
Charles MacNeill 5:89031b2f5316 4415 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4416
Charles MacNeill 5:89031b2f5316 4417 return status;
Charles MacNeill 5:89031b2f5316 4418 }
Charles MacNeill 5:89031b2f5316 4419
Charles MacNeill 5:89031b2f5316 4420 VL53LX_Error VL53LX_dynamic_xtalk_correction_data_init(
Charles MacNeill 5:89031b2f5316 4421 VL53LX_DEV Dev
Charles MacNeill 5:89031b2f5316 4422 )
Charles MacNeill 5:89031b2f5316 4423 {
Charles MacNeill 5:89031b2f5316 4424
Charles MacNeill 5:89031b2f5316 4425
Charles MacNeill 5:89031b2f5316 4426
Charles MacNeill 5:89031b2f5316 4427
Charles MacNeill 5:89031b2f5316 4428 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4429
Charles MacNeill 5:89031b2f5316 4430 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4431 VL53LX_LLDriverResults_t *pres = VL53LXDevStructGetLLResultsHandle(Dev);
Charles MacNeill 5:89031b2f5316 4432
Charles MacNeill 5:89031b2f5316 4433 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4434
Charles MacNeill 5:89031b2f5316 4435
Charles MacNeill 5:89031b2f5316 4436
Charles MacNeill 5:89031b2f5316 4437 pdev->smudge_correct_config.smudge_corr_enabled = 1;
Charles MacNeill 5:89031b2f5316 4438 pdev->smudge_correct_config.smudge_corr_apply_enabled = 1;
Charles MacNeill 5:89031b2f5316 4439 pdev->smudge_correct_config.smudge_corr_single_apply =
Charles MacNeill 5:89031b2f5316 4440 VL53LX_TUNINGPARM_DYNXTALK_SMUDGE_COR_SINGLE_APPLY_DEFAULT;
Charles MacNeill 5:89031b2f5316 4441
Charles MacNeill 5:89031b2f5316 4442 pdev->smudge_correct_config.smudge_margin =
Charles MacNeill 5:89031b2f5316 4443 VL53LX_TUNINGPARM_DYNXTALK_SMUDGE_MARGIN_DEFAULT;
Charles MacNeill 5:89031b2f5316 4444 pdev->smudge_correct_config.noise_margin =
Charles MacNeill 5:89031b2f5316 4445 VL53LX_TUNINGPARM_DYNXTALK_NOISE_MARGIN_DEFAULT;
Charles MacNeill 5:89031b2f5316 4446 pdev->smudge_correct_config.user_xtalk_offset_limit =
Charles MacNeill 5:89031b2f5316 4447 VL53LX_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_DEFAULT;
Charles MacNeill 5:89031b2f5316 4448 pdev->smudge_correct_config.user_xtalk_offset_limit_hi =
Charles MacNeill 5:89031b2f5316 4449 VL53LX_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_HI_DEFAULT;
Charles MacNeill 5:89031b2f5316 4450 pdev->smudge_correct_config.sample_limit =
Charles MacNeill 5:89031b2f5316 4451 VL53LX_TUNINGPARM_DYNXTALK_SAMPLE_LIMIT_DEFAULT;
Charles MacNeill 5:89031b2f5316 4452 pdev->smudge_correct_config.single_xtalk_delta =
Charles MacNeill 5:89031b2f5316 4453 VL53LX_TUNINGPARM_DYNXTALK_SINGLE_XTALK_DELTA_DEFAULT;
Charles MacNeill 5:89031b2f5316 4454 pdev->smudge_correct_config.averaged_xtalk_delta =
Charles MacNeill 5:89031b2f5316 4455 VL53LX_TUNINGPARM_DYNXTALK_AVERAGED_XTALK_DELTA_DEFAULT;
Charles MacNeill 5:89031b2f5316 4456 pdev->smudge_correct_config.smudge_corr_clip_limit =
Charles MacNeill 5:89031b2f5316 4457 VL53LX_TUNINGPARM_DYNXTALK_CLIP_LIMIT_DEFAULT;
Charles MacNeill 5:89031b2f5316 4458 pdev->smudge_correct_config.smudge_corr_ambient_threshold =
Charles MacNeill 5:89031b2f5316 4459 VL53LX_TUNINGPARM_DYNXTALK_XTALK_AMB_THRESHOLD_DEFAULT;
Charles MacNeill 5:89031b2f5316 4460 pdev->smudge_correct_config.scaler_calc_method =
Charles MacNeill 5:89031b2f5316 4461 0;
Charles MacNeill 5:89031b2f5316 4462 pdev->smudge_correct_config.x_gradient_scaler =
Charles MacNeill 5:89031b2f5316 4463 VL53LX_TUNINGPARM_DYNXTALK_XGRADIENT_SCALER_DEFAULT;
Charles MacNeill 5:89031b2f5316 4464 pdev->smudge_correct_config.y_gradient_scaler =
Charles MacNeill 5:89031b2f5316 4465 VL53LX_TUNINGPARM_DYNXTALK_YGRADIENT_SCALER_DEFAULT;
Charles MacNeill 5:89031b2f5316 4466 pdev->smudge_correct_config.user_scaler_set =
Charles MacNeill 5:89031b2f5316 4467 VL53LX_TUNINGPARM_DYNXTALK_USER_SCALER_SET_DEFAULT;
Charles MacNeill 5:89031b2f5316 4468 pdev->smudge_correct_config.nodetect_ambient_threshold =
Charles MacNeill 5:89031b2f5316 4469 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_AMB_THRESHOLD_KCPS_DEFAULT;
Charles MacNeill 5:89031b2f5316 4470 pdev->smudge_correct_config.nodetect_sample_limit =
Charles MacNeill 5:89031b2f5316 4471 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_SAMPLE_LIMIT_DEFAULT;
Charles MacNeill 5:89031b2f5316 4472 pdev->smudge_correct_config.nodetect_xtalk_offset =
Charles MacNeill 5:89031b2f5316 4473 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS_DEFAULT;
Charles MacNeill 5:89031b2f5316 4474 pdev->smudge_correct_config.nodetect_min_range_mm =
Charles MacNeill 5:89031b2f5316 4475 VL53LX_TUNINGPARM_DYNXTALK_NODETECT_MIN_RANGE_MM_DEFAULT;
Charles MacNeill 5:89031b2f5316 4476 pdev->smudge_correct_config.max_smudge_factor =
Charles MacNeill 5:89031b2f5316 4477 VL53LX_TUNINGPARM_DYNXTALK_MAX_SMUDGE_FACTOR_DEFAULT;
Charles MacNeill 5:89031b2f5316 4478
Charles MacNeill 5:89031b2f5316 4479
Charles MacNeill 5:89031b2f5316 4480 pdev->smudge_corrector_internals.current_samples = 0;
Charles MacNeill 5:89031b2f5316 4481 pdev->smudge_corrector_internals.required_samples = 0;
Charles MacNeill 5:89031b2f5316 4482 pdev->smudge_corrector_internals.accumulator = 0;
Charles MacNeill 5:89031b2f5316 4483 pdev->smudge_corrector_internals.nodetect_counter = 0;
Charles MacNeill 5:89031b2f5316 4484
Charles MacNeill 5:89031b2f5316 4485
Charles MacNeill 5:89031b2f5316 4486 VL53LX_dynamic_xtalk_correction_output_init(pres);
Charles MacNeill 5:89031b2f5316 4487
Charles MacNeill 5:89031b2f5316 4488 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4489
Charles MacNeill 5:89031b2f5316 4490 return status;
Charles MacNeill 5:89031b2f5316 4491 }
Charles MacNeill 5:89031b2f5316 4492
Charles MacNeill 5:89031b2f5316 4493 VL53LX_Error VL53LX_dynamic_xtalk_correction_output_init(
Charles MacNeill 5:89031b2f5316 4494 VL53LX_LLDriverResults_t *pres
Charles MacNeill 5:89031b2f5316 4495 )
Charles MacNeill 5:89031b2f5316 4496 {
Charles MacNeill 5:89031b2f5316 4497
Charles MacNeill 5:89031b2f5316 4498
Charles MacNeill 5:89031b2f5316 4499
Charles MacNeill 5:89031b2f5316 4500
Charles MacNeill 5:89031b2f5316 4501 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4502
Charles MacNeill 5:89031b2f5316 4503 VL53LX_smudge_corrector_data_t *pdata;
Charles MacNeill 5:89031b2f5316 4504
Charles MacNeill 5:89031b2f5316 4505 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4506
Charles MacNeill 5:89031b2f5316 4507
Charles MacNeill 5:89031b2f5316 4508 pdata = &(pres->range_results.smudge_corrector_data);
Charles MacNeill 5:89031b2f5316 4509
Charles MacNeill 5:89031b2f5316 4510 pdata->smudge_corr_valid = 0;
Charles MacNeill 5:89031b2f5316 4511 pdata->smudge_corr_clipped = 0;
Charles MacNeill 5:89031b2f5316 4512 pdata->single_xtalk_delta_flag = 0;
Charles MacNeill 5:89031b2f5316 4513 pdata->averaged_xtalk_delta_flag = 0;
Charles MacNeill 5:89031b2f5316 4514 pdata->sample_limit_exceeded_flag = 0;
Charles MacNeill 5:89031b2f5316 4515 pdata->gradient_zero_flag = 0;
Charles MacNeill 5:89031b2f5316 4516 pdata->new_xtalk_applied_flag = 0;
Charles MacNeill 5:89031b2f5316 4517
Charles MacNeill 5:89031b2f5316 4518 pdata->algo__crosstalk_compensation_plane_offset_kcps = 0;
Charles MacNeill 5:89031b2f5316 4519 pdata->algo__crosstalk_compensation_x_plane_gradient_kcps = 0;
Charles MacNeill 5:89031b2f5316 4520 pdata->algo__crosstalk_compensation_y_plane_gradient_kcps = 0;
Charles MacNeill 5:89031b2f5316 4521
Charles MacNeill 5:89031b2f5316 4522 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4523
Charles MacNeill 5:89031b2f5316 4524 return status;
Charles MacNeill 5:89031b2f5316 4525 }
Charles MacNeill 5:89031b2f5316 4526
Charles MacNeill 5:89031b2f5316 4527
Charles MacNeill 5:89031b2f5316 4528 VL53LX_Error VL53LX_xtalk_cal_data_init(
Charles MacNeill 5:89031b2f5316 4529 VL53LX_DEV Dev
Charles MacNeill 5:89031b2f5316 4530 )
Charles MacNeill 5:89031b2f5316 4531 {
Charles MacNeill 5:89031b2f5316 4532
Charles MacNeill 5:89031b2f5316 4533
Charles MacNeill 5:89031b2f5316 4534
Charles MacNeill 5:89031b2f5316 4535
Charles MacNeill 5:89031b2f5316 4536 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4537
Charles MacNeill 5:89031b2f5316 4538 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4539
Charles MacNeill 5:89031b2f5316 4540 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4541
Charles MacNeill 5:89031b2f5316 4542
Charles MacNeill 5:89031b2f5316 4543
Charles MacNeill 5:89031b2f5316 4544 pdev->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps = 0;
Charles MacNeill 5:89031b2f5316 4545 pdev->xtalk_cal.algo__crosstalk_compensation_x_plane_gradient_kcps = 0;
Charles MacNeill 5:89031b2f5316 4546 pdev->xtalk_cal.algo__crosstalk_compensation_y_plane_gradient_kcps = 0;
Charles MacNeill 5:89031b2f5316 4547 memset(&pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[0], 0,
Charles MacNeill 5:89031b2f5316 4548 sizeof(pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps));
Charles MacNeill 5:89031b2f5316 4549
Charles MacNeill 5:89031b2f5316 4550 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4551
Charles MacNeill 5:89031b2f5316 4552 return status;
Charles MacNeill 5:89031b2f5316 4553 }
Charles MacNeill 5:89031b2f5316 4554
Charles MacNeill 5:89031b2f5316 4555
Charles MacNeill 5:89031b2f5316 4556
Charles MacNeill 5:89031b2f5316 4557
Charles MacNeill 5:89031b2f5316 4558
Charles MacNeill 5:89031b2f5316 4559
Charles MacNeill 5:89031b2f5316 4560 VL53LX_Error VL53LX_low_power_auto_data_init(
Charles MacNeill 5:89031b2f5316 4561 VL53LX_DEV Dev
Charles MacNeill 5:89031b2f5316 4562 )
Charles MacNeill 5:89031b2f5316 4563 {
Charles MacNeill 5:89031b2f5316 4564
Charles MacNeill 5:89031b2f5316 4565
Charles MacNeill 5:89031b2f5316 4566
Charles MacNeill 5:89031b2f5316 4567
Charles MacNeill 5:89031b2f5316 4568 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4569
Charles MacNeill 5:89031b2f5316 4570 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4571
Charles MacNeill 5:89031b2f5316 4572 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4573
Charles MacNeill 5:89031b2f5316 4574 pdev->low_power_auto_data.vhv_loop_bound =
Charles MacNeill 5:89031b2f5316 4575 VL53LX_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT;
Charles MacNeill 5:89031b2f5316 4576 pdev->low_power_auto_data.is_low_power_auto_mode = 0;
Charles MacNeill 5:89031b2f5316 4577 pdev->low_power_auto_data.low_power_auto_range_count = 0;
Charles MacNeill 5:89031b2f5316 4578 pdev->low_power_auto_data.saved_interrupt_config = 0;
Charles MacNeill 5:89031b2f5316 4579 pdev->low_power_auto_data.saved_vhv_init = 0;
Charles MacNeill 5:89031b2f5316 4580 pdev->low_power_auto_data.saved_vhv_timeout = 0;
Charles MacNeill 5:89031b2f5316 4581 pdev->low_power_auto_data.first_run_phasecal_result = 0;
Charles MacNeill 5:89031b2f5316 4582 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
Charles MacNeill 5:89031b2f5316 4583 pdev->low_power_auto_data.dss__required_spads = 0;
Charles MacNeill 5:89031b2f5316 4584
Charles MacNeill 5:89031b2f5316 4585 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4586
Charles MacNeill 5:89031b2f5316 4587 return status;
Charles MacNeill 5:89031b2f5316 4588 }
Charles MacNeill 5:89031b2f5316 4589
Charles MacNeill 5:89031b2f5316 4590 VL53LX_Error VL53LX_low_power_auto_data_stop_range(
Charles MacNeill 5:89031b2f5316 4591 VL53LX_DEV Dev
Charles MacNeill 5:89031b2f5316 4592 )
Charles MacNeill 5:89031b2f5316 4593 {
Charles MacNeill 5:89031b2f5316 4594
Charles MacNeill 5:89031b2f5316 4595
Charles MacNeill 5:89031b2f5316 4596
Charles MacNeill 5:89031b2f5316 4597
Charles MacNeill 5:89031b2f5316 4598 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4599
Charles MacNeill 5:89031b2f5316 4600 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4601
Charles MacNeill 5:89031b2f5316 4602 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4603
Charles MacNeill 5:89031b2f5316 4604
Charles MacNeill 5:89031b2f5316 4605
Charles MacNeill 5:89031b2f5316 4606 pdev->low_power_auto_data.low_power_auto_range_count = 0xFF;
Charles MacNeill 5:89031b2f5316 4607
Charles MacNeill 5:89031b2f5316 4608 pdev->low_power_auto_data.first_run_phasecal_result = 0;
Charles MacNeill 5:89031b2f5316 4609 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
Charles MacNeill 5:89031b2f5316 4610 pdev->low_power_auto_data.dss__required_spads = 0;
Charles MacNeill 5:89031b2f5316 4611
Charles MacNeill 5:89031b2f5316 4612
Charles MacNeill 5:89031b2f5316 4613 if (pdev->low_power_auto_data.saved_vhv_init != 0)
Charles MacNeill 5:89031b2f5316 4614 pdev->stat_nvm.vhv_config__init =
Charles MacNeill 5:89031b2f5316 4615 pdev->low_power_auto_data.saved_vhv_init;
Charles MacNeill 5:89031b2f5316 4616 if (pdev->low_power_auto_data.saved_vhv_timeout != 0)
Charles MacNeill 5:89031b2f5316 4617 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
Charles MacNeill 5:89031b2f5316 4618 pdev->low_power_auto_data.saved_vhv_timeout;
Charles MacNeill 5:89031b2f5316 4619
Charles MacNeill 5:89031b2f5316 4620
Charles MacNeill 5:89031b2f5316 4621 pdev->gen_cfg.phasecal_config__override = 0x00;
Charles MacNeill 5:89031b2f5316 4622
Charles MacNeill 5:89031b2f5316 4623 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4624
Charles MacNeill 5:89031b2f5316 4625 return status;
Charles MacNeill 5:89031b2f5316 4626 }
Charles MacNeill 5:89031b2f5316 4627
Charles MacNeill 5:89031b2f5316 4628 VL53LX_Error VL53LX_config_low_power_auto_mode(
Charles MacNeill 5:89031b2f5316 4629 VL53LX_general_config_t *pgeneral,
Charles MacNeill 5:89031b2f5316 4630 VL53LX_dynamic_config_t *pdynamic,
Charles MacNeill 5:89031b2f5316 4631 VL53LX_low_power_auto_data_t *plpadata
Charles MacNeill 5:89031b2f5316 4632 )
Charles MacNeill 5:89031b2f5316 4633 {
Charles MacNeill 5:89031b2f5316 4634
Charles MacNeill 5:89031b2f5316 4635
Charles MacNeill 5:89031b2f5316 4636
Charles MacNeill 5:89031b2f5316 4637
Charles MacNeill 5:89031b2f5316 4638 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4639
Charles MacNeill 5:89031b2f5316 4640 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4641
Charles MacNeill 5:89031b2f5316 4642
Charles MacNeill 5:89031b2f5316 4643 plpadata->is_low_power_auto_mode = 1;
Charles MacNeill 5:89031b2f5316 4644
Charles MacNeill 5:89031b2f5316 4645
Charles MacNeill 5:89031b2f5316 4646 plpadata->low_power_auto_range_count = 0;
Charles MacNeill 5:89031b2f5316 4647
Charles MacNeill 5:89031b2f5316 4648
Charles MacNeill 5:89031b2f5316 4649 pdynamic->system__sequence_config =
Charles MacNeill 5:89031b2f5316 4650 VL53LX_SEQUENCE_VHV_EN |
Charles MacNeill 5:89031b2f5316 4651 VL53LX_SEQUENCE_PHASECAL_EN |
Charles MacNeill 5:89031b2f5316 4652 VL53LX_SEQUENCE_DSS1_EN |
Charles MacNeill 5:89031b2f5316 4653
Charles MacNeill 5:89031b2f5316 4654
Charles MacNeill 5:89031b2f5316 4655
Charles MacNeill 5:89031b2f5316 4656 VL53LX_SEQUENCE_RANGE_EN;
Charles MacNeill 5:89031b2f5316 4657
Charles MacNeill 5:89031b2f5316 4658
Charles MacNeill 5:89031b2f5316 4659 pgeneral->dss_config__manual_effective_spads_select = 200 << 8;
Charles MacNeill 5:89031b2f5316 4660 pgeneral->dss_config__roi_mode_control =
Charles MacNeill 5:89031b2f5316 4661 VL53LX_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
Charles MacNeill 5:89031b2f5316 4662
Charles MacNeill 5:89031b2f5316 4663 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4664
Charles MacNeill 5:89031b2f5316 4665 return status;
Charles MacNeill 5:89031b2f5316 4666 }
Charles MacNeill 5:89031b2f5316 4667
Charles MacNeill 5:89031b2f5316 4668 VL53LX_Error VL53LX_low_power_auto_setup_manual_calibration(
Charles MacNeill 5:89031b2f5316 4669 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 4670 {
Charles MacNeill 5:89031b2f5316 4671
Charles MacNeill 5:89031b2f5316 4672
Charles MacNeill 5:89031b2f5316 4673
Charles MacNeill 5:89031b2f5316 4674 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4675
Charles MacNeill 5:89031b2f5316 4676
Charles MacNeill 5:89031b2f5316 4677 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4678
Charles MacNeill 5:89031b2f5316 4679 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4680
Charles MacNeill 5:89031b2f5316 4681
Charles MacNeill 5:89031b2f5316 4682 pdev->low_power_auto_data.saved_vhv_init =
Charles MacNeill 5:89031b2f5316 4683 pdev->stat_nvm.vhv_config__init;
Charles MacNeill 5:89031b2f5316 4684 pdev->low_power_auto_data.saved_vhv_timeout =
Charles MacNeill 5:89031b2f5316 4685 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound;
Charles MacNeill 5:89031b2f5316 4686
Charles MacNeill 5:89031b2f5316 4687
Charles MacNeill 5:89031b2f5316 4688 pdev->stat_nvm.vhv_config__init &= 0x7F;
Charles MacNeill 5:89031b2f5316 4689
Charles MacNeill 5:89031b2f5316 4690 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
Charles MacNeill 5:89031b2f5316 4691 (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) +
Charles MacNeill 5:89031b2f5316 4692 (pdev->low_power_auto_data.vhv_loop_bound << 2);
Charles MacNeill 5:89031b2f5316 4693
Charles MacNeill 5:89031b2f5316 4694 pdev->gen_cfg.phasecal_config__override = 0x01;
Charles MacNeill 5:89031b2f5316 4695 pdev->low_power_auto_data.first_run_phasecal_result =
Charles MacNeill 5:89031b2f5316 4696 pdev->dbg_results.phasecal_result__vcsel_start;
Charles MacNeill 5:89031b2f5316 4697 pdev->gen_cfg.cal_config__vcsel_start =
Charles MacNeill 5:89031b2f5316 4698 pdev->low_power_auto_data.first_run_phasecal_result;
Charles MacNeill 5:89031b2f5316 4699
Charles MacNeill 5:89031b2f5316 4700 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4701
Charles MacNeill 5:89031b2f5316 4702 return status;
Charles MacNeill 5:89031b2f5316 4703 }
Charles MacNeill 5:89031b2f5316 4704
Charles MacNeill 5:89031b2f5316 4705 VL53LX_Error VL53LX_low_power_auto_update_DSS(
Charles MacNeill 5:89031b2f5316 4706 VL53LX_DEV Dev)
Charles MacNeill 5:89031b2f5316 4707 {
Charles MacNeill 5:89031b2f5316 4708
Charles MacNeill 5:89031b2f5316 4709
Charles MacNeill 5:89031b2f5316 4710
Charles MacNeill 5:89031b2f5316 4711 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4712
Charles MacNeill 5:89031b2f5316 4713 VL53LX_system_results_t *pS = &(pdev->sys_results);
Charles MacNeill 5:89031b2f5316 4714
Charles MacNeill 5:89031b2f5316 4715
Charles MacNeill 5:89031b2f5316 4716 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4717
Charles MacNeill 5:89031b2f5316 4718 uint32_t utemp32a;
Charles MacNeill 5:89031b2f5316 4719
Charles MacNeill 5:89031b2f5316 4720 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 4721
Charles MacNeill 5:89031b2f5316 4722
Charles MacNeill 5:89031b2f5316 4723
Charles MacNeill 5:89031b2f5316 4724
Charles MacNeill 5:89031b2f5316 4725 utemp32a =
Charles MacNeill 5:89031b2f5316 4726 pS->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0
Charles MacNeill 5:89031b2f5316 4727 + pS->result__ambient_count_rate_mcps_sd0;
Charles MacNeill 5:89031b2f5316 4728
Charles MacNeill 5:89031b2f5316 4729
Charles MacNeill 5:89031b2f5316 4730 if (utemp32a > 0xFFFF)
Charles MacNeill 5:89031b2f5316 4731 utemp32a = 0xFFFF;
Charles MacNeill 5:89031b2f5316 4732
Charles MacNeill 5:89031b2f5316 4733
Charles MacNeill 5:89031b2f5316 4734
Charles MacNeill 5:89031b2f5316 4735 utemp32a = utemp32a << 16;
Charles MacNeill 5:89031b2f5316 4736
Charles MacNeill 5:89031b2f5316 4737
Charles MacNeill 5:89031b2f5316 4738 if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0)
Charles MacNeill 5:89031b2f5316 4739 status = VL53LX_ERROR_DIVISION_BY_ZERO;
Charles MacNeill 5:89031b2f5316 4740 else {
Charles MacNeill 5:89031b2f5316 4741
Charles MacNeill 5:89031b2f5316 4742 utemp32a = utemp32a /
Charles MacNeill 5:89031b2f5316 4743 pdev->sys_results.result__dss_actual_effective_spads_sd0;
Charles MacNeill 5:89031b2f5316 4744
Charles MacNeill 5:89031b2f5316 4745 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps =
Charles MacNeill 5:89031b2f5316 4746 utemp32a;
Charles MacNeill 5:89031b2f5316 4747
Charles MacNeill 5:89031b2f5316 4748
Charles MacNeill 5:89031b2f5316 4749 utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps <<
Charles MacNeill 5:89031b2f5316 4750 16;
Charles MacNeill 5:89031b2f5316 4751
Charles MacNeill 5:89031b2f5316 4752
Charles MacNeill 5:89031b2f5316 4753 if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps
Charles MacNeill 5:89031b2f5316 4754 == 0)
Charles MacNeill 5:89031b2f5316 4755 status = VL53LX_ERROR_DIVISION_BY_ZERO;
Charles MacNeill 5:89031b2f5316 4756 else {
Charles MacNeill 5:89031b2f5316 4757
Charles MacNeill 5:89031b2f5316 4758 utemp32a = utemp32a /
Charles MacNeill 5:89031b2f5316 4759 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps;
Charles MacNeill 5:89031b2f5316 4760
Charles MacNeill 5:89031b2f5316 4761
Charles MacNeill 5:89031b2f5316 4762 if (utemp32a > 0xFFFF)
Charles MacNeill 5:89031b2f5316 4763 utemp32a = 0xFFFF;
Charles MacNeill 5:89031b2f5316 4764
Charles MacNeill 5:89031b2f5316 4765
Charles MacNeill 5:89031b2f5316 4766 pdev->low_power_auto_data.dss__required_spads =
Charles MacNeill 5:89031b2f5316 4767 (uint16_t)utemp32a;
Charles MacNeill 5:89031b2f5316 4768
Charles MacNeill 5:89031b2f5316 4769
Charles MacNeill 5:89031b2f5316 4770 pdev->gen_cfg.dss_config__manual_effective_spads_select
Charles MacNeill 5:89031b2f5316 4771 = pdev->low_power_auto_data.dss__required_spads;
Charles MacNeill 5:89031b2f5316 4772 pdev->gen_cfg.dss_config__roi_mode_control =
Charles MacNeill 5:89031b2f5316 4773 VL53LX_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
Charles MacNeill 5:89031b2f5316 4774 }
Charles MacNeill 5:89031b2f5316 4775
Charles MacNeill 5:89031b2f5316 4776 }
Charles MacNeill 5:89031b2f5316 4777
Charles MacNeill 5:89031b2f5316 4778 if (status == VL53LX_ERROR_DIVISION_BY_ZERO) {
Charles MacNeill 5:89031b2f5316 4779
Charles MacNeill 5:89031b2f5316 4780
Charles MacNeill 5:89031b2f5316 4781
Charles MacNeill 5:89031b2f5316 4782 pdev->low_power_auto_data.dss__required_spads = 0x8000;
Charles MacNeill 5:89031b2f5316 4783
Charles MacNeill 5:89031b2f5316 4784
Charles MacNeill 5:89031b2f5316 4785 pdev->gen_cfg.dss_config__manual_effective_spads_select =
Charles MacNeill 5:89031b2f5316 4786 pdev->low_power_auto_data.dss__required_spads;
Charles MacNeill 5:89031b2f5316 4787 pdev->gen_cfg.dss_config__roi_mode_control =
Charles MacNeill 5:89031b2f5316 4788 VL53LX_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
Charles MacNeill 5:89031b2f5316 4789
Charles MacNeill 5:89031b2f5316 4790
Charles MacNeill 5:89031b2f5316 4791 status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4792 }
Charles MacNeill 5:89031b2f5316 4793
Charles MacNeill 5:89031b2f5316 4794 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 4795
Charles MacNeill 5:89031b2f5316 4796 return status;
Charles MacNeill 5:89031b2f5316 4797 }
Charles MacNeill 5:89031b2f5316 4798
Charles MacNeill 5:89031b2f5316 4799
Charles MacNeill 5:89031b2f5316 4800
Charles MacNeill 5:89031b2f5316 4801
Charles MacNeill 5:89031b2f5316 4802 VL53LX_Error VL53LX_compute_histo_merge_nb(
Charles MacNeill 5:89031b2f5316 4803 VL53LX_DEV Dev, uint8_t *histo_merge_nb)
Charles MacNeill 5:89031b2f5316 4804 {
Charles MacNeill 5:89031b2f5316 4805 VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
Charles MacNeill 5:89031b2f5316 4806 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 4807 uint8_t i, timing;
Charles MacNeill 5:89031b2f5316 4808 uint8_t sum = 0;
Charles MacNeill 5:89031b2f5316 4809
Charles MacNeill 5:89031b2f5316 4810 timing = (pdev->hist_data.bin_seq[0] == 7 ? 1 : 0);
Charles MacNeill 5:89031b2f5316 4811 for (i = 0; i < VL53LX_BIN_REC_SIZE; i++)
Charles MacNeill 5:89031b2f5316 4812 if (pdev->multi_bins_rec[i][timing][7] > 0)
Charles MacNeill 5:89031b2f5316 4813 sum++;
Charles MacNeill 5:89031b2f5316 4814 *histo_merge_nb = sum;
Charles MacNeill 5:89031b2f5316 4815
Charles MacNeill 5:89031b2f5316 4816 return status;
Charles MacNeill 5:89031b2f5316 4817 }
Charles MacNeill 5:89031b2f5316 4818