/* * Copyright (c) 2017, STMicroelectronics - All Rights Reserved * * This file is part of VL53L1 Core and is dual licensed, * either 'STMicroelectronics * Proprietary license' * or 'BSD 3-clause "New" or "Revised" License' , at your option. * ******************************************************************************** * * 'STMicroelectronics Proprietary license' * ******************************************************************************** * * License terms: STMicroelectronics Proprietary in accordance with licensing * terms at www.st.com/sla0081 * * STMicroelectronics confidential * Reproduction and Communication of this document is strictly prohibited unless * specifically authorized in writing by STMicroelectronics. * * ******************************************************************************** * * Alternatively, VL53L1 Core may be distributed under the terms of * 'BSD 3-clause "New" or "Revised" License', in which case the following * provisions apply instead of the ones mentioned above : * ******************************************************************************** * * License terms: BSD 3-clause "New" or "Revised" License. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * 3. Neither the name of the copyright holder nor the names of its contributors * may be used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * ******************************************************************************** * */ /** * @file vl53l1_core.c * * @brief EwokPlus25 core function definition */ #include "vl53l1_ll_def.h" #include "vl53l1_ll_device.h" #include "vl53l1_platform.h" #include "vl53l1_register_map.h" #include "vl53l1_register_funcs.h" #include "vl53l1_register_settings.h" #include "vl53l1_api_preset_modes.h" #include "vl53l1_core.h" #include "vl53l1_tuning_parm_defaults.h" #ifdef VL53L1_LOGGING #include "vl53l1_api_debug.h" #include "vl53l1_debug.h" #include "vl53l1_register_debug.h" #endif #define LOG_FUNCTION_START(fmt, ...) \ _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__) #define LOG_FUNCTION_END(status, ...) \ _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__) #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \ status, fmt, ##__VA_ARGS__) #define trace_print(level, ...) \ _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \ level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) void VL53L1_init_version( VL53L1_DEV Dev) { /** * Initialise version structure */ VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); pdev->version.ll_major = VL53L1_LL_API_IMPLEMENTATION_VER_MAJOR; pdev->version.ll_minor = VL53L1_LL_API_IMPLEMENTATION_VER_MINOR; pdev->version.ll_build = VL53L1_LL_API_IMPLEMENTATION_VER_SUB; pdev->version.ll_revision = VL53L1_LL_API_IMPLEMENTATION_VER_REVISION; } void VL53L1_init_ll_driver_state( VL53L1_DEV Dev, VL53L1_DeviceState device_state) { /** * Initialise LL Driver state variables */ VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); pstate->cfg_device_state = device_state; pstate->cfg_stream_count = 0; pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; pstate->cfg_timing_status = 0; pstate->rd_device_state = device_state; pstate->rd_stream_count = 0; pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; pstate->rd_timing_status = 0; } VL53L1_Error VL53L1_update_ll_driver_rd_state( VL53L1_DEV Dev) { /** * State machine for read device state * * VL53L1_DEVICESTATE_SW_STANDBY * VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC * VL53L1_DEVICESTATE_RANGING_GATHER_DATA * VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); /* if top bits of mode start reset are zero then in standby state */ LOG_FUNCTION_START(""); #ifdef VL53L1_LOGGING VL53L1_print_ll_driver_state(pstate); #endif if ((pdev->sys_ctrl.system__mode_start & VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) { pstate->rd_device_state = VL53L1_DEVICESTATE_SW_STANDBY; pstate->rd_stream_count = 0; pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; pstate->rd_timing_status = 0; } else { /* * implement read stream count */ if (pstate->rd_stream_count == 0xFF) { pstate->rd_stream_count = 0x80; } else { pstate->rd_stream_count++; } /* * Toggle grouped parameter hold ID */ pstate->rd_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; /* Ok now ranging */ switch (pstate->rd_device_state) { case VL53L1_DEVICESTATE_SW_STANDBY: if ((pdev->dyn_cfg.system__grouped_parameter_hold & VL53L1_GROUPEDPARAMETERHOLD_ID_MASK) > 0) { pstate->rd_device_state = VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC; } else { pstate->rd_device_state = VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; } pstate->rd_stream_count = 0; pstate->rd_timing_status = 0; break; case VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC: pstate->rd_stream_count = 0; pstate->rd_device_state = VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; break; case VL53L1_DEVICESTATE_RANGING_GATHER_DATA: pstate->rd_device_state = VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; break; case VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA: pstate->rd_timing_status ^= 0x01; pstate->rd_device_state = VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; break; default: pstate->rd_device_state = VL53L1_DEVICESTATE_SW_STANDBY; pstate->rd_stream_count = 0; pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; pstate->rd_timing_status = 0; break; } } #ifdef VL53L1_LOGGING VL53L1_print_ll_driver_state(pstate); #endif LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_check_ll_driver_rd_state( VL53L1_DEV Dev) { /* * Checks if the LL Driver Read state and expected stream count * matches the state and stream count received from the device * * Check is only use in back to back mode */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); VL53L1_system_results_t *psys_results = &(pdev->sys_results); uint8_t device_range_status = 0; uint8_t device_stream_count = 0; uint8_t device_gph_id = 0; LOG_FUNCTION_START(""); #ifdef VL53L1_LOGGING VL53L1_print_ll_driver_state(pstate); #endif device_range_status = psys_results->result__range_status & VL53L1_RANGE_STATUS__RANGE_STATUS_MASK; device_stream_count = psys_results->result__stream_count; /* load the correct GPH ID */ device_gph_id = (psys_results->result__interrupt_status & VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4; /* only apply checks in back to back mode */ if ((pdev->sys_ctrl.system__mode_start & VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) == VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) { /* if read state is wait for GPH sync interrupt then check the * device returns a GPH range status value otherwise check that * the stream count matches * * In theory the stream count should zero for the GPH interrupt * but that is not the case after at abort .... */ if (pstate->rd_device_state == VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC) { if (device_range_status != VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY) { status = VL53L1_ERROR_GPH_SYNC_CHECK_FAIL; } } else { if (pstate->rd_stream_count != device_stream_count) { status = VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL; } /* * Check Read state GPH ID */ if (pstate->rd_gph_id != device_gph_id) { status = VL53L1_ERROR_GPH_ID_CHECK_FAIL; #ifdef VL53L1_LOGGING trace_print(VL53L1_TRACE_LEVEL_ALL, " RDSTATECHECK: Check failed: rd_gph_id: %d, device_gph_id: %d\n", pstate->rd_gph_id, device_gph_id); #endif } else { #ifdef VL53L1_LOGGING trace_print(VL53L1_TRACE_LEVEL_ALL, " RDSTATECHECK: Check passed: rd_gph_id: %d, device_gph_id: %d\n", pstate->rd_gph_id, device_gph_id); #endif } } /* else (not in WAIT_GPH_SYNC) */ } /* if back to back */ LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_update_ll_driver_cfg_state( VL53L1_DEV Dev) { /** * State machine for configuration device state */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); LOG_FUNCTION_START(""); #ifdef VL53L1_LOGGING VL53L1_print_ll_driver_state(pstate); #endif /* if top bits of mode start reset are zero then in standby state */ if ((pdev->sys_ctrl.system__mode_start & VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) { pstate->cfg_device_state = VL53L1_DEVICESTATE_SW_STANDBY; pstate->cfg_stream_count = 0; pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; pstate->cfg_timing_status = 0; } else { /* * implement configuration stream count */ if (pstate->cfg_stream_count == 0xFF) { pstate->cfg_stream_count = 0x80; } else { pstate->cfg_stream_count++; } /* * Toggle grouped parameter hold ID */ pstate->cfg_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; /* * Implement configuration state machine */ switch (pstate->cfg_device_state) { case VL53L1_DEVICESTATE_SW_STANDBY: pstate->cfg_timing_status ^= 0x01; pstate->cfg_stream_count = 1; pstate->cfg_device_state = VL53L1_DEVICESTATE_RANGING_DSS_AUTO; break; case VL53L1_DEVICESTATE_RANGING_DSS_AUTO: pstate->cfg_timing_status ^= 0x01; break; default: pstate->cfg_device_state = VL53L1_DEVICESTATE_SW_STANDBY; pstate->cfg_stream_count = 0; pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; pstate->cfg_timing_status = 0; break; } } #ifdef VL53L1_LOGGING VL53L1_print_ll_driver_state(pstate); #endif LOG_FUNCTION_END(status); return status; } void VL53L1_copy_rtn_good_spads_to_buffer( VL53L1_nvm_copy_data_t *pdata, uint8_t *pbuffer) { /* * Convenience function to copy return SPAD enables to buffer */ *(pbuffer + 0) = pdata->global_config__spad_enables_rtn_0; *(pbuffer + 1) = pdata->global_config__spad_enables_rtn_1; *(pbuffer + 2) = pdata->global_config__spad_enables_rtn_2; *(pbuffer + 3) = pdata->global_config__spad_enables_rtn_3; *(pbuffer + 4) = pdata->global_config__spad_enables_rtn_4; *(pbuffer + 5) = pdata->global_config__spad_enables_rtn_5; *(pbuffer + 6) = pdata->global_config__spad_enables_rtn_6; *(pbuffer + 7) = pdata->global_config__spad_enables_rtn_7; *(pbuffer + 8) = pdata->global_config__spad_enables_rtn_8; *(pbuffer + 9) = pdata->global_config__spad_enables_rtn_9; *(pbuffer + 10) = pdata->global_config__spad_enables_rtn_10; *(pbuffer + 11) = pdata->global_config__spad_enables_rtn_11; *(pbuffer + 12) = pdata->global_config__spad_enables_rtn_12; *(pbuffer + 13) = pdata->global_config__spad_enables_rtn_13; *(pbuffer + 14) = pdata->global_config__spad_enables_rtn_14; *(pbuffer + 15) = pdata->global_config__spad_enables_rtn_15; *(pbuffer + 16) = pdata->global_config__spad_enables_rtn_16; *(pbuffer + 17) = pdata->global_config__spad_enables_rtn_17; *(pbuffer + 18) = pdata->global_config__spad_enables_rtn_18; *(pbuffer + 19) = pdata->global_config__spad_enables_rtn_19; *(pbuffer + 20) = pdata->global_config__spad_enables_rtn_20; *(pbuffer + 21) = pdata->global_config__spad_enables_rtn_21; *(pbuffer + 22) = pdata->global_config__spad_enables_rtn_22; *(pbuffer + 23) = pdata->global_config__spad_enables_rtn_23; *(pbuffer + 24) = pdata->global_config__spad_enables_rtn_24; *(pbuffer + 25) = pdata->global_config__spad_enables_rtn_25; *(pbuffer + 26) = pdata->global_config__spad_enables_rtn_26; *(pbuffer + 27) = pdata->global_config__spad_enables_rtn_27; *(pbuffer + 28) = pdata->global_config__spad_enables_rtn_28; *(pbuffer + 29) = pdata->global_config__spad_enables_rtn_29; *(pbuffer + 30) = pdata->global_config__spad_enables_rtn_30; *(pbuffer + 31) = pdata->global_config__spad_enables_rtn_31; } void VL53L1_init_system_results( VL53L1_system_results_t *pdata) { /* * Initialises the system results to all 0xFF just like the * device firmware does a the start of a range */ pdata->result__interrupt_status = 0xFF; pdata->result__range_status = 0xFF; pdata->result__report_status = 0xFF; pdata->result__stream_count = 0xFF; pdata->result__dss_actual_effective_spads_sd0 = 0xFFFF; pdata->result__peak_signal_count_rate_mcps_sd0 = 0xFFFF; pdata->result__ambient_count_rate_mcps_sd0 = 0xFFFF; pdata->result__sigma_sd0 = 0xFFFF; pdata->result__phase_sd0 = 0xFFFF; pdata->result__final_crosstalk_corrected_range_mm_sd0 = 0xFFFF; pdata->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = 0xFFFF; pdata->result__mm_inner_actual_effective_spads_sd0 = 0xFFFF; pdata->result__mm_outer_actual_effective_spads_sd0 = 0xFFFF; pdata->result__avg_signal_count_rate_mcps_sd0 = 0xFFFF; pdata->result__dss_actual_effective_spads_sd1 = 0xFFFF; pdata->result__peak_signal_count_rate_mcps_sd1 = 0xFFFF; pdata->result__ambient_count_rate_mcps_sd1 = 0xFFFF; pdata->result__sigma_sd1 = 0xFFFF; pdata->result__phase_sd1 = 0xFFFF; pdata->result__final_crosstalk_corrected_range_mm_sd1 = 0xFFFF; pdata->result__spare_0_sd1 = 0xFFFF; pdata->result__spare_1_sd1 = 0xFFFF; pdata->result__spare_2_sd1 = 0xFFFF; pdata->result__spare_3_sd1 = 0xFF; } void VL53L1_i2c_encode_uint16_t( uint16_t ip_value, uint16_t count, uint8_t *pbuffer) { /* * Encodes a uint16_t register value into an I2C write buffer * MS byte first order (as per I2C register map. */ uint16_t i = 0; uint16_t data = 0; data = ip_value; for (i = 0; i < count ; i++) { pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); data = data >> 8; } } uint16_t VL53L1_i2c_decode_uint16_t( uint16_t count, uint8_t *pbuffer) { /* * Decodes a uint16_t from the input I2C read buffer * (MS byte first order) */ uint16_t value = 0x00; while (count-- > 0) { value = (value << 8) | (uint16_t)*pbuffer++; } return value; } void VL53L1_i2c_encode_int16_t( int16_t ip_value, uint16_t count, uint8_t *pbuffer) { /* * Encodes a int16_t register value into an I2C write buffer * MS byte first order (as per I2C register map. */ uint16_t i = 0; int16_t data = 0; data = ip_value; for (i = 0; i < count ; i++) { pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); data = data >> 8; } } int16_t VL53L1_i2c_decode_int16_t( uint16_t count, uint8_t *pbuffer) { /* * Decodes a int16_t from the input I2C read buffer * (MS byte first order) */ int16_t value = 0x00; /* implement sign extension */ if (*pbuffer >= 0x80) { value = 0xFFFF; } while (count-- > 0) { value = (value << 8) | (int16_t)*pbuffer++; } return value; } void VL53L1_i2c_encode_uint32_t( uint32_t ip_value, uint16_t count, uint8_t *pbuffer) { /* * Encodes a uint32_t register value into an I2C write buffer * MS byte first order (as per I2C register map. */ uint16_t i = 0; uint32_t data = 0; data = ip_value; for (i = 0; i < count ; i++) { pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); data = data >> 8; } } uint32_t VL53L1_i2c_decode_uint32_t( uint16_t count, uint8_t *pbuffer) { /* * Decodes a uint32_t from the input I2C read buffer * (MS byte first order) */ uint32_t value = 0x00; while (count-- > 0) { value = (value << 8) | (uint32_t)*pbuffer++; } return value; } uint32_t VL53L1_i2c_decode_with_mask( uint16_t count, uint8_t *pbuffer, uint32_t bit_mask, uint32_t down_shift, uint32_t offset) { /* * Decodes an integer from the input I2C read buffer * (MS byte first order) */ uint32_t value = 0x00; /* extract from buffer */ while (count-- > 0) { value = (value << 8) | (uint32_t)*pbuffer++; } /* Apply bit mask and down shift */ value = value & bit_mask; if (down_shift > 0) { value = value >> down_shift; } /* add offset */ value = value + offset; return value; } void VL53L1_i2c_encode_int32_t( int32_t ip_value, uint16_t count, uint8_t *pbuffer) { /* * Encodes a int32_t register value into an I2C write buffer * MS byte first order (as per I2C register map. */ uint16_t i = 0; int32_t data = 0; data = ip_value; for (i = 0; i < count ; i++) { pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); data = data >> 8; } } int32_t VL53L1_i2c_decode_int32_t( uint16_t count, uint8_t *pbuffer) { /* * Decodes a int32_t from the input I2C read buffer * (MS byte first order) */ int32_t value = 0x00; /* implement sign extension */ if (*pbuffer >= 0x80) { value = 0xFFFFFFFF; } while (count-- > 0) { value = (value << 8) | (int32_t)*pbuffer++; } return value; } #ifndef VL53L1_NOCALIB VL53L1_Error VL53L1_start_test( VL53L1_DEV Dev, uint8_t test_mode__ctrl) { /* * Triggers the start of a test mode */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); if (status == VL53L1_ERROR_NONE) { /*lint !e774 always true*/ status = VL53L1_WrByte( Dev, VL53L1_TEST_MODE__CTRL, test_mode__ctrl); } LOG_FUNCTION_END(status); return status; } #endif VL53L1_Error VL53L1_set_firmware_enable_register( VL53L1_DEV Dev, uint8_t value) { /* * Set FIRMWARE__ENABLE register */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); pdev->sys_ctrl.firmware__enable = value; status = VL53L1_WrByte( Dev, VL53L1_FIRMWARE__ENABLE, pdev->sys_ctrl.firmware__enable); return status; } VL53L1_Error VL53L1_enable_firmware( VL53L1_DEV Dev) { /* * Enable firmware */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); status = VL53L1_set_firmware_enable_register(Dev, 0x01); LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_disable_firmware( VL53L1_DEV Dev) { /* * Disable firmware */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); status = VL53L1_set_firmware_enable_register(Dev, 0x00); LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_set_powerforce_register( VL53L1_DEV Dev, uint8_t value) { /* * Set power force register */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); pdev->sys_ctrl.power_management__go1_power_force = value; status = VL53L1_WrByte( Dev, VL53L1_POWER_MANAGEMENT__GO1_POWER_FORCE, pdev->sys_ctrl.power_management__go1_power_force); return status; } VL53L1_Error VL53L1_enable_powerforce( VL53L1_DEV Dev) { /* * Enable power force */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); status = VL53L1_set_powerforce_register(Dev, 0x01); LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_disable_powerforce( VL53L1_DEV Dev) { /* * Disable power force */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); status = VL53L1_set_powerforce_register(Dev, 0x00); LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_clear_interrupt( VL53L1_DEV Dev) { /* * Clear Ranging interrupt by writing to */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); LOG_FUNCTION_START(""); pdev->sys_ctrl.system__interrupt_clear = VL53L1_CLEAR_RANGE_INT; status = VL53L1_WrByte( Dev, VL53L1_SYSTEM__INTERRUPT_CLEAR, pdev->sys_ctrl.system__interrupt_clear); LOG_FUNCTION_END(status); return status; } #ifdef VL53L1_DEBUG VL53L1_Error VL53L1_force_shadow_stream_count_to_zero( VL53L1_DEV Dev) { /* * Forces shadow stream count to zero */ VL53L1_Error status = VL53L1_ERROR_NONE; if (status == VL53L1_ERROR_NONE) { /*lint !e774 always true*/ status = VL53L1_disable_firmware(Dev); } if (status == VL53L1_ERROR_NONE) { status = VL53L1_WrByte( Dev, VL53L1_SHADOW_RESULT__STREAM_COUNT, 0x00); } if (status == VL53L1_ERROR_NONE) { status = VL53L1_enable_firmware(Dev); } return status; } #endif uint32_t VL53L1_calc_macro_period_us( uint16_t fast_osc_frequency, uint8_t vcsel_period) { /* Calculates macro period in [us] from the input fast oscillator * frequency and VCSEL period * * Macro period fixed point format = unsigned 12.12 * Maximum supported macro period = 4095.9999 us */ uint32_t pll_period_us = 0; uint8_t vcsel_period_pclks = 0; uint32_t macro_period_us = 0; LOG_FUNCTION_START(""); /* Calculate PLL period in [us] from the fast_osc_frequency * Fast osc frequency fixed point format = unsigned 4.12 */ pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency); /* VCSEL period * - the real VCSEL period in PLL clocks = 2*(VCSEL_PERIOD+1) */ vcsel_period_pclks = VL53L1_decode_vcsel_period(vcsel_period); /* Macro period * - PLL period [us] = 0.24 format * - for 1.0 MHz fast oscillator freq * - max PLL period = 1/64 (6-bits) * - i.e only the lower 18-bits of PLL Period value are used * - Macro period [vclks] = 2304 (12-bits) * * Max bits (24 - 6) + 12 = 30-bits usage * * Downshift by 6 before multiplying by the VCSEL Period */ macro_period_us = (uint32_t)VL53L1_MACRO_PERIOD_VCSEL_PERIODS * pll_period_us; macro_period_us = macro_period_us >> 6; macro_period_us = macro_period_us * (uint32_t)vcsel_period_pclks; macro_period_us = macro_period_us >> 6; #ifdef VL53L1_LOGGING trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u\n", "pll_period_us", pll_period_us); trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u\n", "vcsel_period_pclks", vcsel_period_pclks); trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u\n", "macro_period_us", macro_period_us); #endif LOG_FUNCTION_END(0); return macro_period_us; } uint16_t VL53L1_calc_range_ignore_threshold( uint32_t central_rate, int16_t x_gradient, int16_t y_gradient, uint8_t rate_mult) { /* Calculates Range Ignore Threshold rate per spad * in Mcps - 3.13 format * * Calculates worst case xtalk rate per spad in array corner * based on input central xtalk and x and y gradients * * Worst case rate = central rate + (8*(magnitude(xgrad)) + * (8*(magnitude(ygrad))) * * Range ignore threshold rate is then multiplied by user input * rate_mult (in 3.5 fractional format) * */ int32_t range_ignore_thresh_int = 0; uint16_t range_ignore_thresh_kcps = 0; int32_t central_rate_int = 0; int16_t x_gradient_int = 0; int16_t y_gradient_int = 0; LOG_FUNCTION_START(""); /* Shift central_rate to .13 fractional for simple addition */ central_rate_int = ((int32_t)central_rate * (1 << 4)) / (1000); if (x_gradient < 0) { x_gradient_int = x_gradient * -1; } if (y_gradient < 0) { y_gradient_int = y_gradient * -1; } /* Calculate full rate per spad - worst case from measured xtalk */ /* Generated here from .11 fractional kcps */ /* Additional factor of 4 applied to bring fractional precision to .13 */ range_ignore_thresh_int = (8 * x_gradient_int * 4) + (8 * y_gradient_int * 4); /* Convert Kcps to Mcps */ range_ignore_thresh_int = range_ignore_thresh_int / 1000; /* Combine with Central Rate - Mcps .13 format*/ range_ignore_thresh_int = range_ignore_thresh_int + central_rate_int; /* Mult by user input */ range_ignore_thresh_int = (int32_t)rate_mult * range_ignore_thresh_int; range_ignore_thresh_int = (range_ignore_thresh_int + (1<<4)) / (1<<5); /* Finally clip and output in correct format */ if (range_ignore_thresh_int > 0xFFFF) { range_ignore_thresh_kcps = 0xFFFF; } else { range_ignore_thresh_kcps = (uint16_t)range_ignore_thresh_int; } #ifdef VL53L1_LOGGING trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u\n", "range_ignore_thresh_kcps", range_ignore_thresh_kcps); #endif LOG_FUNCTION_END(0); return range_ignore_thresh_kcps; } uint32_t VL53L1_calc_timeout_mclks( uint32_t timeout_us, uint32_t macro_period_us) { /* Calculates the timeout value in macro periods based on the input * timeout period in milliseconds and the macro period in [us] * * Max timeout supported is 1000000 us (1 sec) -> 20-bits * Max timeout in 20.12 format = 32-bits * * Macro period [us] = 12.12 format */ uint32_t timeout_mclks = 0; LOG_FUNCTION_START(""); timeout_mclks = ((timeout_us << 12) + (macro_period_us>>1)) / macro_period_us; LOG_FUNCTION_END(0); return timeout_mclks; } uint16_t VL53L1_calc_encoded_timeout( uint32_t timeout_us, uint32_t macro_period_us) { /* Calculates the encoded timeout register value based on the input * timeout period in milliseconds and the macro period in [us] * * Max timeout supported is 1000000 us (1 sec) -> 20-bits * Max timeout in 20.12 format = 32-bits * * Macro period [us] = 12.12 format */ uint32_t timeout_mclks = 0; uint16_t timeout_encoded = 0; LOG_FUNCTION_START(""); timeout_mclks = VL53L1_calc_timeout_mclks(timeout_us, macro_period_us); timeout_encoded = VL53L1_encode_timeout(timeout_mclks); #ifdef VL53L1_LOGGING trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u (0x%04X)\n", "timeout_mclks", timeout_mclks, timeout_mclks); trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u (0x%04X)\n", "timeout_encoded", timeout_encoded, timeout_encoded); #endif LOG_FUNCTION_END(0); return timeout_encoded; } uint32_t VL53L1_calc_timeout_us( uint32_t timeout_mclks, uint32_t macro_period_us) { /* Calculates the timeout in [us] based on the input * encoded timeout and the macro period in [us] * * Max timeout supported is 1000000 us (1 sec) -> 20-bits * Max timeout in 20.12 format = 32-bits * * Macro period [us] = 12.12 format */ uint32_t timeout_us = 0; uint64_t tmp = 0; LOG_FUNCTION_START(""); tmp = (uint64_t)timeout_mclks * (uint64_t)macro_period_us; tmp += 0x00800; tmp = tmp >> 12; timeout_us = (uint32_t)tmp; #ifdef VL53L1_LOGGING trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u (0x%04X)\n", "timeout_mclks", timeout_mclks, timeout_mclks); trace_print(VL53L1_TRACE_LEVEL_DEBUG, " %-48s : %10u us\n", "timeout_us", timeout_us, timeout_us); #endif LOG_FUNCTION_END(0); return timeout_us; } uint32_t VL53L1_calc_crosstalk_plane_offset_with_margin( uint32_t plane_offset_kcps, int16_t margin_offset_kcps) { uint32_t plane_offset_with_margin = 0; int32_t plane_offset_kcps_temp = 0; LOG_FUNCTION_START(""); plane_offset_kcps_temp = (int32_t)plane_offset_kcps + (int32_t)margin_offset_kcps; if (plane_offset_kcps_temp < 0) { plane_offset_kcps_temp = 0; } else { if (plane_offset_kcps_temp > 0x3FFFF) { plane_offset_kcps_temp = 0x3FFFF; } } plane_offset_with_margin = (uint32_t) plane_offset_kcps_temp; LOG_FUNCTION_END(0); return plane_offset_with_margin; } uint32_t VL53L1_calc_decoded_timeout_us( uint16_t timeout_encoded, uint32_t macro_period_us) { /* Calculates the timeout in [us] based on the input * encoded timeout and the macro period in [us] * * Max timeout supported is 1000000 us (1 sec) -> 20-bits * Max timeout in 20.12 format = 32-bits * * Macro period [us] = 12.12 format */ uint32_t timeout_mclks = 0; uint32_t timeout_us = 0; LOG_FUNCTION_START(""); timeout_mclks = VL53L1_decode_timeout(timeout_encoded); timeout_us = VL53L1_calc_timeout_us(timeout_mclks, macro_period_us); LOG_FUNCTION_END(0); return timeout_us; } uint16_t VL53L1_encode_timeout(uint32_t timeout_mclks) { /* * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */ uint16_t encoded_timeout = 0; uint32_t ls_byte = 0; uint16_t ms_byte = 0; if (timeout_mclks > 0) { ls_byte = timeout_mclks - 1; while ((ls_byte & 0xFFFFFF00) > 0) { ls_byte = ls_byte >> 1; ms_byte++; } encoded_timeout = (ms_byte << 8) + (uint16_t) (ls_byte & 0x000000FF); } return encoded_timeout; } uint32_t VL53L1_decode_timeout(uint16_t encoded_timeout) { /* * Decode 16-bit timeout register value * format (LSByte * 2^MSByte) + 1 */ uint32_t timeout_macro_clks = 0; timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF) << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1; return timeout_macro_clks; } VL53L1_Error VL53L1_calc_timeout_register_values( uint32_t phasecal_config_timeout_us, uint32_t mm_config_timeout_us, uint32_t range_config_timeout_us, uint16_t fast_osc_frequency, VL53L1_general_config_t *pgeneral, VL53L1_timing_config_t *ptiming) { /* * Converts the input MM and range timeouts in [us] * into the appropriate register values * * Must also be run after the VCSEL period settings are changed */ VL53L1_Error status = VL53L1_ERROR_NONE; uint32_t macro_period_us = 0; uint32_t timeout_mclks = 0; uint16_t timeout_encoded = 0; LOG_FUNCTION_START(""); if (fast_osc_frequency == 0) { status = VL53L1_ERROR_DIVISION_BY_ZERO; } else { /* Update Macro Period for Range A VCSEL Period */ macro_period_us = VL53L1_calc_macro_period_us( fast_osc_frequency, ptiming->range_config__vcsel_period_a); /* Update Phase timeout - uses Timing A */ timeout_mclks = VL53L1_calc_timeout_mclks( phasecal_config_timeout_us, macro_period_us); /* clip as the phase cal timeout register is only 8-bits */ if (timeout_mclks > 0xFF) timeout_mclks = 0xFF; pgeneral->phasecal_config__timeout_macrop = (uint8_t)timeout_mclks; /* Update MM Timing A timeout */ timeout_encoded = VL53L1_calc_encoded_timeout( mm_config_timeout_us, macro_period_us); ptiming->mm_config__timeout_macrop_a_hi = (uint8_t)((timeout_encoded & 0xFF00) >> 8); ptiming->mm_config__timeout_macrop_a_lo = (uint8_t) (timeout_encoded & 0x00FF); /* Update Range Timing A timeout */ timeout_encoded = VL53L1_calc_encoded_timeout( range_config_timeout_us, macro_period_us); ptiming->range_config__timeout_macrop_a_hi = (uint8_t)((timeout_encoded & 0xFF00) >> 8); ptiming->range_config__timeout_macrop_a_lo = (uint8_t) (timeout_encoded & 0x00FF); /* Update Macro Period for Range B VCSEL Period */ macro_period_us = VL53L1_calc_macro_period_us( fast_osc_frequency, ptiming->range_config__vcsel_period_b); /* Update MM Timing B timeout */ timeout_encoded = VL53L1_calc_encoded_timeout( mm_config_timeout_us, macro_period_us); ptiming->mm_config__timeout_macrop_b_hi = (uint8_t)((timeout_encoded & 0xFF00) >> 8); ptiming->mm_config__timeout_macrop_b_lo = (uint8_t) (timeout_encoded & 0x00FF); /* Update Range Timing B timeout */ timeout_encoded = VL53L1_calc_encoded_timeout( range_config_timeout_us, macro_period_us); ptiming->range_config__timeout_macrop_b_hi = (uint8_t)((timeout_encoded & 0xFF00) >> 8); ptiming->range_config__timeout_macrop_b_lo = (uint8_t) (timeout_encoded & 0x00FF); } LOG_FUNCTION_END(0); return status; } uint8_t VL53L1_encode_vcsel_period(uint8_t vcsel_period_pclks) { /* * Converts the encoded VCSEL period register value into * the real period in PLL clocks */ uint8_t vcsel_period_reg = 0; vcsel_period_reg = (vcsel_period_pclks >> 1) - 1; return vcsel_period_reg; } uint32_t VL53L1_decode_unsigned_integer( uint8_t *pbuffer, uint8_t no_of_bytes) { /* * Decodes a integer number from the buffer */ uint8_t i = 0; uint32_t decoded_value = 0; for (i = 0 ; i < no_of_bytes ; i++) { decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i]; } return decoded_value; } void VL53L1_encode_unsigned_integer( uint32_t ip_value, uint8_t no_of_bytes, uint8_t *pbuffer) { /* * Encodes an integer number into the buffer */ uint8_t i = 0; uint32_t data = 0; data = ip_value; for (i = 0; i < no_of_bytes ; i++) { pbuffer[no_of_bytes-i-1] = data & 0x00FF; data = data >> 8; } } void VL53L1_spad_number_to_byte_bit_index( uint8_t spad_number, uint8_t *pbyte_index, uint8_t *pbit_index, uint8_t *pbit_mask) { /** * Converts the input SPAD number into the SPAD Enable byte index, bit index and bit mask * * byte_index = (spad_no >> 3) * bit_index = spad_no & 0x07 * bit_mask = 0x01 << bit_index */ *pbyte_index = spad_number >> 3; *pbit_index = spad_number & 0x07; *pbit_mask = 0x01 << *pbit_index; } void VL53L1_encode_row_col( uint8_t row, uint8_t col, uint8_t *pspad_number) { /** * Encodes the input array(row,col) location as SPAD number. */ if (row > 7) { *pspad_number = 128 + (col << 3) + (15-row); } else { *pspad_number = ((15-col) << 3) + row; } } void VL53L1_decode_zone_size( uint8_t encoded_xy_size, uint8_t *pwidth, uint8_t *pheight) { /* extract x and y sizes * * Important: the sense of the device width and height is swapped * versus the API sense * * MS Nibble = height * LS Nibble = width */ *pheight = encoded_xy_size >> 4; *pwidth = encoded_xy_size & 0x0F; } void VL53L1_encode_zone_size( uint8_t width, uint8_t height, uint8_t *pencoded_xy_size) { /* merge x and y sizes * * Important: the sense of the device width and height is swapped * versus the API sense * * MS Nibble = height * LS Nibble = width */ *pencoded_xy_size = (height << 4) + width; } void VL53L1_decode_zone_limits( uint8_t encoded_xy_centre, uint8_t encoded_xy_size, int16_t *px_ll, int16_t *py_ll, int16_t *px_ur, int16_t *py_ur) { /* * compute zone lower left and upper right limits * * centre (8,8) width = 16, height = 16 -> (0,0) -> (15,15) * centre (8,8) width = 14, height = 16 -> (1,0) -> (14,15) */ uint8_t x_centre = 0; uint8_t y_centre = 0; uint8_t width = 0; uint8_t height = 0; /* decode zone centre and size information */ VL53L1_decode_row_col( encoded_xy_centre, &y_centre, &x_centre); VL53L1_decode_zone_size( encoded_xy_size, &width, &height); /* compute bounds and clip */ *px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2; if (*px_ll < 0) *px_ll = 0; *px_ur = *px_ll + (int16_t)width; if (*px_ur > (VL53L1_SPAD_ARRAY_WIDTH-1)) *px_ur = VL53L1_SPAD_ARRAY_WIDTH-1; *py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2; if (*py_ll < 0) *py_ll = 0; *py_ur = *py_ll + (int16_t)height; if (*py_ur > (VL53L1_SPAD_ARRAY_HEIGHT-1)) *py_ur = VL53L1_SPAD_ARRAY_HEIGHT-1; } uint8_t VL53L1_is_aperture_location( uint8_t row, uint8_t col) { /* * Returns > 0 if input (row,col) location is an aperture */ uint8_t is_aperture = 0; uint8_t mod_row = row % 4; uint8_t mod_col = col % 4; if (mod_row == 0 && mod_col == 2) is_aperture = 1; if (mod_row == 2 && mod_col == 0) is_aperture = 1; return is_aperture; } void VL53L1_calc_mm_effective_spads( uint8_t encoded_mm_roi_centre, uint8_t encoded_mm_roi_size, uint8_t encoded_zone_centre, uint8_t encoded_zone_size, uint8_t *pgood_spads, uint16_t aperture_attenuation, uint16_t *pmm_inner_effective_spads, uint16_t *pmm_outer_effective_spads) { /* Calculates the effective SPAD counts for the MM inner and outer * regions based on the input MM ROI, Zone info and return good * SPAD map */ int16_t x = 0; int16_t y = 0; int16_t mm_x_ll = 0; int16_t mm_y_ll = 0; int16_t mm_x_ur = 0; int16_t mm_y_ur = 0; int16_t zone_x_ll = 0; int16_t zone_y_ll = 0; int16_t zone_x_ur = 0; int16_t zone_y_ur = 0; uint8_t spad_number = 0; uint8_t byte_index = 0; uint8_t bit_index = 0; uint8_t bit_mask = 0; uint8_t is_aperture = 0; uint16_t spad_attenuation = 0; /* decode the MM ROI and Zone limits */ VL53L1_decode_zone_limits( encoded_mm_roi_centre, encoded_mm_roi_size, &mm_x_ll, &mm_y_ll, &mm_x_ur, &mm_y_ur); VL53L1_decode_zone_limits( encoded_zone_centre, encoded_zone_size, &zone_x_ll, &zone_y_ll, &zone_x_ur, &zone_y_ur); /* * Loop though all SPAD within the zone. Check if it is * a good SPAD then add the transmission value to either * the inner or outer effective SPAD count dependent if * the SPAD lies within the MM ROI. */ *pmm_inner_effective_spads = 0; *pmm_outer_effective_spads = 0; for (y = zone_y_ll ; y <= zone_y_ur ; y++) { for (x = zone_x_ll ; x <= zone_x_ur ; x++) { /* Convert location into SPAD number */ VL53L1_encode_row_col( (uint8_t)y, (uint8_t)x, &spad_number); /* Convert spad number into byte and bit index * this is required to look up the appropriate * SPAD enable bit with the 32-byte good SPAD * enable buffer */ VL53L1_spad_number_to_byte_bit_index( spad_number, &byte_index, &bit_index, &bit_mask); /* If spad is good then add it */ if ((pgood_spads[byte_index] & bit_mask) > 0) { /* determine if apertured SPAD or not */ is_aperture = VL53L1_is_aperture_location( (uint8_t)y, (uint8_t)x); if (is_aperture > 0) spad_attenuation = aperture_attenuation; else spad_attenuation = 0x0100; /* * if inside MM roi add to inner effective SPAD count * otherwise add to outer effective SPAD Count */ if (x >= mm_x_ll && x <= mm_x_ur && y >= mm_y_ll && y <= mm_y_ur) *pmm_inner_effective_spads += spad_attenuation; else *pmm_outer_effective_spads += spad_attenuation; } } } } /* * Encodes VL53L1_GPIO_interrupt_config_t structure to FW register format */ uint8_t VL53L1_encode_GPIO_interrupt_config( VL53L1_GPIO_interrupt_config_t *pintconf) { uint8_t system__interrupt_config; system__interrupt_config = pintconf->intr_mode_distance; system__interrupt_config |= ((pintconf->intr_mode_rate) << 2); system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5); system__interrupt_config |= ((pintconf->intr_no_target) << 6); system__interrupt_config |= ((pintconf->intr_combined_mode) << 7); return system__interrupt_config; } /* * Decodes FW register to VL53L1_GPIO_interrupt_config_t structure */ VL53L1_GPIO_interrupt_config_t VL53L1_decode_GPIO_interrupt_config( uint8_t system__interrupt_config) { VL53L1_GPIO_interrupt_config_t intconf; intconf.intr_mode_distance = system__interrupt_config & 0x03; intconf.intr_mode_rate = (system__interrupt_config >> 2) & 0x03; intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01; intconf.intr_no_target = (system__interrupt_config >> 6) & 0x01; intconf.intr_combined_mode = (system__interrupt_config >> 7) & 0x01; /* set some default values */ intconf.threshold_rate_low = 0; intconf.threshold_rate_high = 0; intconf.threshold_distance_low = 0; intconf.threshold_distance_high = 0; return intconf; } /* * Set GPIO distance threshold */ VL53L1_Error VL53L1_set_GPIO_distance_threshold( VL53L1_DEV Dev, uint16_t threshold_high, uint16_t threshold_low) { VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); LOG_FUNCTION_START(""); pdev->dyn_cfg.system__thresh_high = threshold_high; pdev->dyn_cfg.system__thresh_low = threshold_low; LOG_FUNCTION_END(status); return status; } /* * Set GPIO rate threshold */ VL53L1_Error VL53L1_set_GPIO_rate_threshold( VL53L1_DEV Dev, uint16_t threshold_high, uint16_t threshold_low) { VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); LOG_FUNCTION_START(""); pdev->gen_cfg.system__thresh_rate_high = threshold_high; pdev->gen_cfg.system__thresh_rate_low = threshold_low; LOG_FUNCTION_END(status); return status; } /* * Set GPIO thresholds from structure */ VL53L1_Error VL53L1_set_GPIO_thresholds_from_struct( VL53L1_DEV Dev, VL53L1_GPIO_interrupt_config_t *pintconf) { VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); status = VL53L1_set_GPIO_distance_threshold( Dev, pintconf->threshold_distance_high, pintconf->threshold_distance_low); if (status == VL53L1_ERROR_NONE) { status = VL53L1_set_GPIO_rate_threshold( Dev, pintconf->threshold_rate_high, pintconf->threshold_rate_low); } LOG_FUNCTION_END(status); return status; } #ifndef VL53L1_NOCALIB VL53L1_Error VL53L1_set_ref_spad_char_config( VL53L1_DEV Dev, uint8_t vcsel_period_a, uint32_t phasecal_timeout_us, uint16_t total_rate_target_mcps, uint16_t max_count_rate_rtn_limit_mcps, uint16_t min_count_rate_rtn_limit_mcps, uint16_t fast_osc_frequency) { /* * Initialises the VCSEL period A and phasecal timeout registers * for the Reference SPAD Characterisation test */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); uint8_t buffer[2]; uint32_t macro_period_us = 0; uint32_t timeout_mclks = 0; LOG_FUNCTION_START(""); /* * Update Macro Period for Range A VCSEL Period */ macro_period_us = VL53L1_calc_macro_period_us( fast_osc_frequency, vcsel_period_a); /* * Calculate PhaseCal timeout and clip to max of 255 macro periods */ timeout_mclks = phasecal_timeout_us << 12; timeout_mclks = timeout_mclks + (macro_period_us>>1); timeout_mclks = timeout_mclks / macro_period_us; if (timeout_mclks > 0xFF) pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF; else pdev->gen_cfg.phasecal_config__timeout_macrop = (uint8_t)timeout_mclks; pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a; /* * Update device settings */ if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/ status = VL53L1_WrByte( Dev, VL53L1_PHASECAL_CONFIG__TIMEOUT_MACROP, pdev->gen_cfg.phasecal_config__timeout_macrop); if (status == VL53L1_ERROR_NONE) status = VL53L1_WrByte( Dev, VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A, pdev->tim_cfg.range_config__vcsel_period_a); /* * Copy vcsel register value to the WOI registers to ensure that * it is correctly set for the specified VCSEL period */ buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a; buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a; if (status == VL53L1_ERROR_NONE) status = VL53L1_WriteMulti( Dev, VL53L1_SD_CONFIG__WOI_SD0, buffer, 2); /* It should be be replaced with a define */ /* * Set min, target and max rate limits */ pdev->customer.ref_spad_char__total_rate_target_mcps = total_rate_target_mcps; if (status == VL53L1_ERROR_NONE) status = VL53L1_WrWord( Dev, VL53L1_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS, total_rate_target_mcps); /* 9.7 format */ if (status == VL53L1_ERROR_NONE) status = VL53L1_WrWord( Dev, VL53L1_RANGE_CONFIG__SIGMA_THRESH, max_count_rate_rtn_limit_mcps); if (status == VL53L1_ERROR_NONE) status = VL53L1_WrWord( Dev, VL53L1_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, min_count_rate_rtn_limit_mcps); LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_set_ssc_config( VL53L1_DEV Dev, VL53L1_ssc_config_t *pssc_cfg, uint16_t fast_osc_frequency) { /** * Builds and sends a single I2C multiple byte transaction to * initialize the device for SSC. * * The function also sets the WOI registers based on the input * vcsel period register value. */ VL53L1_Error status = VL53L1_ERROR_NONE; uint8_t buffer[5]; uint32_t macro_period_us = 0; uint16_t timeout_encoded = 0; LOG_FUNCTION_START(""); /* * Update Macro Period for Range A VCSEL Period */ macro_period_us = VL53L1_calc_macro_period_us( fast_osc_frequency, pssc_cfg->vcsel_period); /* * Update MM Timing A timeout */ timeout_encoded = VL53L1_calc_encoded_timeout( pssc_cfg->timeout_us, macro_period_us); /* update VCSEL timings */ if (status == VL53L1_ERROR_NONE) status = VL53L1_WrByte( Dev, VL53L1_CAL_CONFIG__VCSEL_START, pssc_cfg->vcsel_start); if (status == VL53L1_ERROR_NONE) status = VL53L1_WrByte( Dev, VL53L1_GLOBAL_CONFIG__VCSEL_WIDTH, pssc_cfg->vcsel_width); /* build buffer for timeouts, period and rate limit */ buffer[0] = (uint8_t)((timeout_encoded & 0x0000FF00) >> 8); buffer[1] = (uint8_t) (timeout_encoded & 0x000000FF); buffer[2] = pssc_cfg->vcsel_period; buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8); buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps & 0x000000FF); if (status == VL53L1_ERROR_NONE) status = VL53L1_WriteMulti( Dev, VL53L1_RANGE_CONFIG__TIMEOUT_MACROP_B_HI, buffer, 5); /* * Copy vcsel register value to the WOI registers to ensure that * it is correctly set for the specified VCSEL period */ buffer[0] = pssc_cfg->vcsel_period; buffer[1] = pssc_cfg->vcsel_period; if (status == VL53L1_ERROR_NONE) status = VL53L1_WriteMulti( Dev, VL53L1_SD_CONFIG__WOI_SD0, buffer, 2); /* * Write zero to NVM_BIST_CTRL to send RTN CountRate to Patch RAM * or 1 to write REF CountRate to Patch RAM */ if (status == VL53L1_ERROR_NONE) status = VL53L1_WrByte( Dev, VL53L1_NVM_BIST__CTRL, pssc_cfg->array_select); LOG_FUNCTION_END(status); return status; } #endif #ifndef VL53L1_NOCALIB VL53L1_Error VL53L1_get_spad_rate_data( VL53L1_DEV Dev, VL53L1_spad_rate_data_t *pspad_rates) { /** * Gets the SSC rate map output */ VL53L1_Error status = VL53L1_ERROR_NONE; int i = 0; uint8_t data[512]; uint8_t *pdata = &data[0]; LOG_FUNCTION_START(""); /* Disable Firmware to Read Patch Ram */ if (status == VL53L1_ERROR_NONE) status = VL53L1_disable_firmware(Dev); /* * Read Return SPADs Rates from patch RAM. * Note : platform layer splits the I2C comms into smaller chunks */ if (status == VL53L1_ERROR_NONE) status = VL53L1_ReadMulti( Dev, VL53L1_PRIVATE__PATCH_BASE_ADDR_RSLV, pdata, 512); /* now convert into 16-bit number */ pdata = &data[0]; for (i = 0 ; i < VL53L1_NO_OF_SPAD_ENABLES ; i++) { pspad_rates->rate_data[i] = (uint16_t)VL53L1_decode_unsigned_integer(pdata, 2); pdata += 2; } /* Initialise structure info */ pspad_rates->buffer_size = VL53L1_NO_OF_SPAD_ENABLES; pspad_rates->no_of_values = VL53L1_NO_OF_SPAD_ENABLES; pspad_rates->fractional_bits = 15; /* Re-enable Firmware */ if (status == VL53L1_ERROR_NONE) status = VL53L1_enable_firmware(Dev); LOG_FUNCTION_END(status); return status; } #endif /* Start Patch_LowPowerAutoMode */ VL53L1_Error VL53L1_low_power_auto_data_init( VL53L1_DEV Dev ) { /* * Initializes internal data structures for low power auto mode */ /* don't really use this here */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); LOG_FUNCTION_START(""); pdev->low_power_auto_data.vhv_loop_bound = VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT; pdev->low_power_auto_data.is_low_power_auto_mode = 0; pdev->low_power_auto_data.low_power_auto_range_count = 0; pdev->low_power_auto_data.saved_interrupt_config = 0; pdev->low_power_auto_data.saved_vhv_init = 0; pdev->low_power_auto_data.saved_vhv_timeout = 0; pdev->low_power_auto_data.first_run_phasecal_result = 0; pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0; pdev->low_power_auto_data.dss__required_spads = 0; LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_low_power_auto_data_stop_range( VL53L1_DEV Dev ) { /* * Range has been paused but may continue later */ /* don't really use this here */ VL53L1_Error status = VL53L1_ERROR_NONE; VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); LOG_FUNCTION_START(""); /* doing this ensures stop_range followed by a get_device_results does * not mess up the counters */ pdev->low_power_auto_data.low_power_auto_range_count = 0xFF; pdev->low_power_auto_data.first_run_phasecal_result = 0; pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0; pdev->low_power_auto_data.dss__required_spads = 0; /* restore vhv configs */ if (pdev->low_power_auto_data.saved_vhv_init != 0) pdev->stat_nvm.vhv_config__init = pdev->low_power_auto_data.saved_vhv_init; if (pdev->low_power_auto_data.saved_vhv_timeout != 0) pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = pdev->low_power_auto_data.saved_vhv_timeout; /* remove phasecal override */ pdev->gen_cfg.phasecal_config__override = 0x00; LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_config_low_power_auto_mode( VL53L1_general_config_t *pgeneral, VL53L1_dynamic_config_t *pdynamic, VL53L1_low_power_auto_data_t *plpadata ) { /* * Initializes configs for when low power auto presets are selected */ /* don't really use this here */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); /* set low power auto mode */ plpadata->is_low_power_auto_mode = 1; /* set low power range count to 0 */ plpadata->low_power_auto_range_count = 0; /* Turn off MM1/MM2 and DSS2 */ pdynamic->system__sequence_config = \ VL53L1_SEQUENCE_VHV_EN | \ VL53L1_SEQUENCE_PHASECAL_EN | \ VL53L1_SEQUENCE_DSS1_EN | \ /* VL53L1_SEQUENCE_DSS2_EN | \*/ /* VL53L1_SEQUENCE_MM1_EN | \*/ /* VL53L1_SEQUENCE_MM2_EN | \*/ VL53L1_SEQUENCE_RANGE_EN; /* Set DSS to manual/expected SPADs */ pgeneral->dss_config__manual_effective_spads_select = 200 << 8; pgeneral->dss_config__roi_mode_control = VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration( VL53L1_DEV Dev) { /* * Setup ranges after the first one in low power auto mode by turning * off FW calibration steps and programming static values */ VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); /* don't really use this here */ VL53L1_Error status = VL53L1_ERROR_NONE; LOG_FUNCTION_START(""); /* save original vhv configs */ pdev->low_power_auto_data.saved_vhv_init = pdev->stat_nvm.vhv_config__init; pdev->low_power_auto_data.saved_vhv_timeout = pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound; /* disable VHV init */ pdev->stat_nvm.vhv_config__init &= 0x7F; /* set loop bound to tuning param */ pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) + (pdev->low_power_auto_data.vhv_loop_bound << 2); /* override phasecal */ pdev->gen_cfg.phasecal_config__override = 0x01; pdev->low_power_auto_data.first_run_phasecal_result = pdev->dbg_results.phasecal_result__vcsel_start; pdev->gen_cfg.cal_config__vcsel_start = pdev->low_power_auto_data.first_run_phasecal_result; LOG_FUNCTION_END(status); return status; } VL53L1_Error VL53L1_low_power_auto_update_DSS( VL53L1_DEV Dev) { /* * Do a DSS calculation and update manual config */ VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); /* don't really use this here */ VL53L1_Error status = VL53L1_ERROR_NONE; uint32_t utemp32a; LOG_FUNCTION_START(""); /* Calc total rate per spad */ /* 9.7 format */ utemp32a = pdev->sys_results.result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 + pdev->sys_results.result__ambient_count_rate_mcps_sd0; /* clip to 16 bits */ if (utemp32a > 0xFFFF) utemp32a = 0xFFFF; /* shift up to take advantage of 32 bits */ /* 9.23 format */ utemp32a = utemp32a << 16; /* check SPAD count */ if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0) status = VL53L1_ERROR_DIVISION_BY_ZERO; else { /* format 17.15 */ utemp32a = utemp32a / pdev->sys_results.result__dss_actual_effective_spads_sd0; /* save intermediate result */ pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = utemp32a; /* get the target rate and shift up by 16 * format 9.23 */ utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps << 16; /* check for divide by zero */ if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps == 0) status = VL53L1_ERROR_DIVISION_BY_ZERO; else { /* divide by rate per spad * format 24.8 */ utemp32a = utemp32a / pdev->low_power_auto_data.dss__total_rate_per_spad_mcps; /* clip to 16 bit */ if (utemp32a > 0xFFFF) utemp32a = 0xFFFF; /* save result in low power auto data */ pdev->low_power_auto_data.dss__required_spads = (uint16_t)utemp32a; /* override DSS config */ pdev->gen_cfg.dss_config__manual_effective_spads_select = pdev->low_power_auto_data.dss__required_spads; pdev->gen_cfg.dss_config__roi_mode_control = VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; } } if (status == VL53L1_ERROR_DIVISION_BY_ZERO) { /* We want to gracefully set a spad target, not just exit with * an error */ /* set target to mid point */ pdev->low_power_auto_data.dss__required_spads = 0x8000; /* override DSS config */ pdev->gen_cfg.dss_config__manual_effective_spads_select = pdev->low_power_auto_data.dss__required_spads; pdev->gen_cfg.dss_config__roi_mode_control = VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; /* reset error */ status = VL53L1_ERROR_NONE; } LOG_FUNCTION_END(status); return status; } /* End Patch_LowPowerAutoMode */