ST Expansion SW Team / VL53L1CB

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

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

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

src/vl53l1_core.c@7:1add29d51e72, 2021-06-08 (annotated)

Committer:
Charles MacNeill
Date:
Tue Jun 08 10:34:47 2021 +0100
Revision:
7:1add29d51e72
Parent:
0:3ac96e360672
Child:
18:0696efe39d08
Update to v6.6.5 of bare_driver

Who changed what in which revision?

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