From ff467d26bc3b3d156457739e2c827b1eafe5b19e Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Wed, 8 Apr 2026 12:49:16 +0800 Subject: [PATCH] 1.0 --- App/VL53L0X_API/platform/vl53_board.c | 7 +- .../platform/inc/vl53l1_platform.h | 38 + .../platform/inc/vl53l1_platform_user_data.h | 15 +- .../platform/src/vl53l1_platform.c | 1045 ++++++----------- .../platform/src/vl53l1_platform_init.c | 53 +- .../platform/src/vl53l1_platform_log.c | 2 +- App/VL53L1X_API/platform/vl53_board.c | 229 ++++ App/VL53L1X_API/platform/vl53_board.h | 56 + .../platform/vl53_calibration_config.h | 29 + App/app_tasks.c | 5 +- App/robot_params.h | 12 + App/vl53_board.h | 14 + CMakeLists.txt | 3 + 13 files changed, 781 insertions(+), 727 deletions(-) create mode 100644 App/VL53L1X_API/platform/vl53_board.c create mode 100644 App/VL53L1X_API/platform/vl53_board.h create mode 100644 App/VL53L1X_API/platform/vl53_calibration_config.h create mode 100644 App/vl53_board.h diff --git a/App/VL53L0X_API/platform/vl53_board.c b/App/VL53L0X_API/platform/vl53_board.c index e2f525e..0020c0f 100644 --- a/App/VL53L0X_API/platform/vl53_board.c +++ b/App/VL53L0X_API/platform/vl53_board.c @@ -1,7 +1,10 @@ +#include "robot_params.h" + +#if !PARAM_VL53_USE_L1X + #include "vl53_board.h" #include "FreeRTOS.h" #include "task.h" -#include "robot_params.h" #include "vl53_calibration_config.h" /* ================= EMA滤波底层实现 ================= */ @@ -210,3 +213,5 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho } return VL53L0X_ERROR_NONE; } + +#endif diff --git a/App/VL53L1X_API/platform/inc/vl53l1_platform.h b/App/VL53L1X_API/platform/inc/vl53l1_platform.h index 3c738cd..5cb60fd 100644 --- a/App/VL53L1X_API/platform/inc/vl53l1_platform.h +++ b/App/VL53L1X_API/platform/inc/vl53l1_platform.h @@ -42,6 +42,14 @@ extern "C" { #endif +#ifndef VL53_USE_FREERTOS_DELAY +#define VL53_USE_FREERTOS_DELAY 1 +#endif + +#ifndef VL53L1_DEFAULT_ADDR_8BIT +#define VL53L1_DEFAULT_ADDR_8BIT 0x52u +#endif + /** * @file vl53l1_platform.h * @@ -396,8 +404,38 @@ VL53L1_Error VL53L1_GpioInterruptDisable(void); */ VL53L1_Error VL53L1_GetTickCount( + VL53L1_Dev_t *pdev, uint32_t *ptime_ms); +void VL53L1_PlatformAttachBus( + VL53L1_Dev_t *pdev, + I2C_HandleTypeDef *hi2c, + uint8_t i2c_addr_8bit, + uint16_t io_timeout_ms, + void *bus_lock); + +void VL53L1_PlatformAttachPins( + VL53L1_Dev_t *pdev, + GPIO_TypeDef *xshut_port, + uint16_t xshut_pin, + GPIO_TypeDef *gpio1_port, + uint16_t gpio1_pin); + +VL53L1_Error VL53L1_PlatformSetXShut( + VL53L1_Dev_t *pdev, + GPIO_PinState state); + +VL53L1_Error VL53L1_PlatformBootDevice( + VL53L1_Dev_t *pdev, + uint32_t reset_low_ms, + uint32_t boot_wait_ms); + +VL53L1_Error VL53L1_PlatformChangeAddress( + VL53L1_Dev_t *pdev, + uint8_t new_addr_8bit); + +uint32_t VL53L1_PlatformGetTick(void); + /** * @brief Register "wait for value" polling routine diff --git a/App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h b/App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h index 2810890..cf9b972 100644 --- a/App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h +++ b/App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h @@ -34,6 +34,7 @@ #include #endif +#include "main.h" #include "vl53l1_def.h" #ifdef __cplusplus @@ -56,12 +57,24 @@ typedef struct { /*!< Low Level Driver data structure */ uint8_t i2c_slave_address; - /*!< i2c device address user specific field */ + /*!< ST API expects the 8-bit I2C address, default 0x52 */ uint8_t comms_type; /*!< Type of comms : VL53L1_I2C or VL53L1_SPI */ uint16_t comms_speed_khz; /*!< Comms speed [kHz] : typically 400kHz for I2C */ + I2C_HandleTypeDef *hi2c; + GPIO_TypeDef *xshut_port; + uint16_t xshut_pin; + GPIO_TypeDef *gpio1_port; + uint16_t gpio1_pin; + + void *bus_lock; + const char *name; + uint8_t id; + uint8_t is_present; + uint16_t io_timeout_ms; + uint32_t new_data_ready_poll_duration_ms; /*!< New data ready poll duration in ms - for debug */ } VL53L1_Dev_t; diff --git a/App/VL53L1X_API/platform/src/vl53l1_platform.c b/App/VL53L1X_API/platform/src/vl53l1_platform.c index d648e98..95d060c 100644 --- a/App/VL53L1X_API/platform/src/vl53l1_platform.c +++ b/App/VL53L1X_API/platform/src/vl53l1_platform.c @@ -1,767 +1,478 @@ -/******************************************************************************* - Copyright (C) 2016, STMicroelectronics International N.V. - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * 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. - * Neither the name of STMicroelectronics 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, FITNESS FOR A PARTICULAR PURPOSE, AND - NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED. - IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. 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_platform.c - * @brief Code function definitions for EwokPlus25 Platform Layer - * RANGING SENSOR VERSION - * - */ -#include - -#include // sprintf(), vsnprintf(), printf() -#include -#include // strncpy(), strnlen() - -#ifdef _MSC_VER -#define snprintf _snprintf -#endif - #include "vl53l1_platform.h" -#include "vl53l1_platform_log.h" +#include "vl53l1_api.h" -#ifdef PAL_EXTENDED - #include "vl53l1_register_strings.h" -#else - #define VL53L1_get_register_name(a,b) +#if VL53_USE_FREERTOS_DELAY +#include "FreeRTOS.h" +#include "task.h" #endif -#include "ranging_sensor_comms.h" -#include "power_board_defs.h" +#if defined(configSUPPORT_STATIC_ALLOCATION) || defined(configUSE_MUTEXES) +#include "semphr.h" +#define VL53L1_USE_FREERTOS_MUTEX 1 +#else +#define VL53L1_USE_FREERTOS_MUTEX 0 +#endif -// flag to indicate if power board has been accessed -const uint32_t _power_board_in_use = 0; +static VL53L1_Dev_t *s_platform_default_dev; -// flag to indicate if we can use the extended voltage ranges (not laser safe!) -uint32_t _power_board_extended = 0; +static VL53L1_Error vl53l1_hal_to_pal(HAL_StatusTypeDef hal_status) +{ + switch (hal_status) { + case HAL_OK: + return VL53L1_ERROR_NONE; + case HAL_TIMEOUT: + return VL53L1_ERROR_TIME_OUT; + default: + return VL53L1_ERROR_CONTROL_INTERFACE; + } +} -// cache of the comms type flag -uint8_t global_comms_type = 0; +static VL53L1_Error vl53l1_validate_dev(VL53L1_Dev_t *pdev) +{ + if ((pdev == NULL) || (pdev->hi2c == NULL)) { + return VL53L1_ERROR_CONTROL_INTERFACE; + } + if (pdev->i2c_slave_address == 0u) { + return VL53L1_ERROR_INVALID_PARAMS; + } + return VL53L1_ERROR_NONE; +} -#define VL53L1_COMMS_CHUNK_SIZE 56 -#define VL53L1_COMMS_BUFFER_SIZE 64 +static VL53L1_Error vl53l1_bus_lock(VL53L1_Dev_t *pdev) +{ +#if VL53L1_USE_FREERTOS_MUTEX + if ((pdev != NULL) && (pdev->bus_lock != NULL)) { + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { + const TickType_t wait_ticks = pdMS_TO_TICKS((pdev->io_timeout_ms == 0u) ? 10u : pdev->io_timeout_ms); + if (xSemaphoreTake((SemaphoreHandle_t)pdev->bus_lock, wait_ticks) != pdTRUE) { + return VL53L1_ERROR_TIME_OUT; + } + } + } +#else + SUPPRESS_UNUSED_WARNING(pdev); +#endif + return VL53L1_ERROR_NONE; +} -#define GPIO_INTERRUPT RS_GPIO62 -#define GPIO_POWER_ENABLE RS_GPIO60 -#define GPIO_XSHUTDOWN RS_GPIO61 -#define GPIO_SPI_CHIP_SELECT RS_GPIO51 +static void vl53l1_bus_unlock(VL53L1_Dev_t *pdev) +{ +#if VL53L1_USE_FREERTOS_MUTEX + if ((pdev != NULL) && (pdev->bus_lock != NULL)) { + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { + (void)xSemaphoreGive((SemaphoreHandle_t)pdev->bus_lock); + } + } +#else + SUPPRESS_UNUSED_WARNING(pdev); +#endif +} -/*! - * The intent of this Abstraction layer is to provide the same API - * to the underlying SystemVerilog tasks as the C driver will have - * to ST Comms DLL's for the talking to Ewok via the USB + STM32 - * or if the C-driver is implemented directly on the STM32 - */ +static void vl53l1_delay_ms(uint32_t wait_ms) +{ + if (wait_ms == 0u) { + return; + } +#if VL53_USE_FREERTOS_DELAY + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { + vTaskDelay(pdMS_TO_TICKS(wait_ms)); + return; + } +#endif + HAL_Delay(wait_ms); +} -#define trace_print(level, ...) \ - _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_PLATFORM, \ - level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) +static void vl53l1_enable_cycle_counter(void) +{ + if ((CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) == 0u) { + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + } + if ((DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk) == 0u) { + DWT->CYCCNT = 0u; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + } +} -#define trace_i2c(...) \ - _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_NONE, \ - VL53L1_TRACE_LEVEL_NONE, VL53L1_TRACE_FUNCTION_I2C, ##__VA_ARGS__) +static void vl53l1_delay_us_blocking(uint32_t wait_us) +{ + uint32_t cycles_per_us; + uint32_t start_cycles; + uint32_t wait_cycles; + if (wait_us == 0u) { + return; + } + + cycles_per_us = HAL_RCC_GetHCLKFreq() / 1000000u; + if (cycles_per_us == 0u) { + HAL_Delay(1u); + return; + } + + vl53l1_enable_cycle_counter(); + start_cycles = DWT->CYCCNT; + wait_cycles = cycles_per_us * wait_us; + while ((DWT->CYCCNT - start_cycles) < wait_cycles) { + } +} + +void VL53L1_PlatformAttachBus( + VL53L1_Dev_t *pdev, + I2C_HandleTypeDef *hi2c, + uint8_t i2c_addr_8bit, + uint16_t io_timeout_ms, + void *bus_lock) +{ + if (pdev == NULL) { + return; + } + pdev->hi2c = hi2c; + pdev->i2c_slave_address = i2c_addr_8bit; + pdev->io_timeout_ms = (io_timeout_ms == 0u) ? 100u : io_timeout_ms; + pdev->bus_lock = bus_lock; + pdev->comms_type = VL53L1_I2C; + if (pdev->comms_speed_khz == 0u) { + pdev->comms_speed_khz = 400u; + } + if (s_platform_default_dev == NULL) { + s_platform_default_dev = pdev; + } +} + +void VL53L1_PlatformAttachPins( + VL53L1_Dev_t *pdev, + GPIO_TypeDef *xshut_port, + uint16_t xshut_pin, + GPIO_TypeDef *gpio1_port, + uint16_t gpio1_pin) +{ + if (pdev == NULL) { + return; + } + pdev->xshut_port = xshut_port; + pdev->xshut_pin = xshut_pin; + pdev->gpio1_port = gpio1_port; + pdev->gpio1_pin = gpio1_pin; +} + +VL53L1_Error VL53L1_PlatformSetXShut( + VL53L1_Dev_t *pdev, + GPIO_PinState state) +{ + if ((pdev == NULL) || (pdev->xshut_port == NULL)) { + return VL53L1_ERROR_INVALID_PARAMS; + } + HAL_GPIO_WritePin(pdev->xshut_port, pdev->xshut_pin, state); + return VL53L1_ERROR_NONE; +} + +VL53L1_Error VL53L1_PlatformBootDevice( + VL53L1_Dev_t *pdev, + uint32_t reset_low_ms, + uint32_t boot_wait_ms) +{ + VL53L1_Error status = VL53L1_PlatformSetXShut(pdev, GPIO_PIN_RESET); + if (status != VL53L1_ERROR_NONE) { + return status; + } + vl53l1_delay_ms(reset_low_ms); + status = VL53L1_PlatformSetXShut(pdev, GPIO_PIN_SET); + if (status != VL53L1_ERROR_NONE) { + return status; + } + vl53l1_delay_ms(boot_wait_ms); + return VL53L1_ERROR_NONE; +} + +uint32_t VL53L1_PlatformGetTick(void) +{ + return HAL_GetTick(); +} VL53L1_Error VL53L1_CommsInitialise( VL53L1_Dev_t *pdev, uint8_t comms_type, uint16_t comms_speed_khz) { - VL53L1_Error status = VL53L1_ERROR_NONE; - char comms_error_string[ERROR_TEXT_LENGTH]; - - SUPPRESS_UNUSED_WARNING(pdev); - SUPPRESS_UNUSED_WARNING(comms_speed_khz); - - global_comms_type = comms_type; - - if(global_comms_type == VL53L1_I2C) - { - if ((CP_STATUS)RANGING_SENSOR_COMMS_Init_CCI(0, 0, 0) != CP_STATUS_OK) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_CommsInitialise: RANGING_SENSOR_COMMS_Init_CCI() failed\n"); - trace_i2c(comms_error_string); - status = VL53L1_ERROR_CONTROL_INTERFACE; - } + if (pdev == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; } - else if(global_comms_type == VL53L1_SPI) - { - if ((CP_STATUS)RANGING_SENSOR_COMMS_Init_SPI_V2W8(0, 0, 0) != CP_STATUS_OK) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_CommsInitialise: RANGING_SENSOR_COMMS_Init_SPI_V2W8() failed\n"); - trace_i2c(comms_error_string); - status = VL53L1_ERROR_CONTROL_INTERFACE; - } + pdev->comms_type = comms_type; + pdev->comms_speed_khz = comms_speed_khz; + if (comms_type != VL53L1_I2C) { + return VL53L1_ERROR_CONTROL_INTERFACE; } - else - { - trace_i2c("VL53L1_CommsInitialise: Comms must be one of VL53L1_I2C or VL53L1_SPI\n"); - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - - return status; + pdev->comms_type = VL53L1_I2C; + return vl53l1_validate_dev(pdev); } - -VL53L1_Error VL53L1_CommsClose( - VL53L1_Dev_t *pdev) +VL53L1_Error VL53L1_CommsClose(VL53L1_Dev_t *pdev) { - VL53L1_Error status = VL53L1_ERROR_NONE; - char comms_error_string[ERROR_TEXT_LENGTH]; - SUPPRESS_UNUSED_WARNING(pdev); - - if(global_comms_type == VL53L1_I2C) - { - if((CP_STATUS)RANGING_SENSOR_COMMS_Fini_CCI() != CP_STATUS_OK) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_CommsClose: RANGING_SENSOR_COMMS_Fini_CCI() failed\n"); - trace_i2c(comms_error_string); - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - else if(global_comms_type == VL53L1_SPI) - { - if((CP_STATUS)RANGING_SENSOR_COMMS_Fini_SPI_V2W8() != CP_STATUS_OK) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_CommsClose: RANGING_SENSOR_COMMS_Fini_SPI_V2W8() failed\n"); - trace_i2c(comms_error_string); - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - else - { - trace_i2c("VL53L1_CommsClose: Comms must be one of VL53L1_I2C or VL53L1_SPI\n"); - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - - return status; + return VL53L1_ERROR_NONE; } -/* - * ----------------- COMMS FUNCTIONS ----------------- - */ - VL53L1_Error VL53L1_WriteMulti( VL53L1_Dev_t *pdev, uint16_t index, uint8_t *pdata, uint32_t count) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint32_t position = 0; - uint32_t data_size = 0; + HAL_StatusTypeDef hal_status; + VL53L1_Error status; - char comms_error_string[ERROR_TEXT_LENGTH]; - - _LOG_STRING_BUFFER(register_name); - _LOG_STRING_BUFFER(value_as_str); - - if(global_comms_type == VL53L1_I2C) - { - for(position=0; position VL53L1_COMMS_CHUNK_SIZE) - { - if((position + VL53L1_COMMS_CHUNK_SIZE) > count) - { - data_size = count - position; - } - else - { - data_size = VL53L1_COMMS_CHUNK_SIZE; - } - } - else - { - data_size = count; - } - - if (status == VL53L1_ERROR_NONE) - { - if( RANGING_SENSOR_COMMS_Write_CCI( - pdev->i2c_slave_address, - 0, - index+position, - pdata+position, - data_size) != 0 ) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - -#ifdef VL53L1_LOG_ENABLE - if (status == VL53L1_ERROR_NONE) { - char* pvalue_as_str; - uint32_t i; - - /*lint --e{661} Suppress out of bounds walkthrough warning */ - /*lint --e{662} Suppress out of bounds walkthrough warning */ - - // Build value as string; - pvalue_as_str = value_as_str; - for(i = 0 ; i < data_size ; i++) - { - sprintf(pvalue_as_str, ", 0x%02X", *(pdata+position+i)); - pvalue_as_str = pvalue_as_str + 6; - } - - register_name[0] = 0; - VL53L1_get_register_name( - index+(uint16_t)position, - register_name); - - trace_i2c( - /* "WriteAutoIncrement(0x%04X%s); // %3u bytes\n", - index+(uint16_t)position, */ - "WriteAutoIncrement(%s%s); // %3u bytes\n", - register_name, - value_as_str, - data_size); - } -#endif // VL53L1_LOG_ENABLE - } - - if(status != VL53L1_ERROR_NONE) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_WriteMulti RANGING_SENSOR_COMMS_Write_CCI() failed\n"); - trace_i2c(comms_error_string); - } + if ((pdata == NULL) || (count == 0u) || (count > 0xFFFFu)) { + return VL53L1_ERROR_INVALID_PARAMS; } - else if(global_comms_type == VL53L1_SPI) - { - if((CP_STATUS)RANGING_SENSOR_COMMS_Write_SPI_16I(0, 0, index, pdata, count) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - - if(status != VL53L1_ERROR_NONE) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_WriteMulti RANGING_SENSOR_COMMS_Write_SPI_16I() failed\n"); - trace_i2c(comms_error_string); - } + status = vl53l1_validate_dev(pdev); + if (status != VL53L1_ERROR_NONE) { + return status; } - else - { - trace_i2c("VL53L1_WriteMulti: Comms must be one of VL53L1_I2C or VL53L1_SPI\n"); - status = VL53L1_ERROR_CONTROL_INTERFACE; + status = vl53l1_bus_lock(pdev); + if (status != VL53L1_ERROR_NONE) { + return status; } - - return status; + hal_status = HAL_I2C_Mem_Write( + pdev->hi2c, + pdev->i2c_slave_address, + index, + I2C_MEMADD_SIZE_16BIT, + pdata, + (uint16_t)count, + pdev->io_timeout_ms); + vl53l1_bus_unlock(pdev); + return vl53l1_hal_to_pal(hal_status); } - VL53L1_Error VL53L1_ReadMulti( VL53L1_Dev_t *pdev, uint16_t index, uint8_t *pdata, uint32_t count) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint32_t position = 0; - uint32_t data_size = 0; + HAL_StatusTypeDef hal_status; + VL53L1_Error status; - char comms_error_string[ERROR_TEXT_LENGTH]; - - _LOG_STRING_BUFFER(register_name); - _LOG_STRING_BUFFER(value_as_str); - - if(global_comms_type == VL53L1_I2C) - { - for(position=0; position VL53L1_COMMS_CHUNK_SIZE) - { - if((position + VL53L1_COMMS_CHUNK_SIZE) > count) - { - data_size = count - position; - } - else - { - data_size = VL53L1_COMMS_CHUNK_SIZE; - } - } - else - data_size = count; - - if(status == VL53L1_ERROR_NONE) - { - if( RANGING_SENSOR_COMMS_Read_CCI( - pdev->i2c_slave_address, - 0, - index+position, - pdata+position, - data_size) != 0 ) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } -#ifdef VL53L1_LOG_ENABLE - if(status == VL53L1_ERROR_NONE) - { - char* pvalue_as_str; - uint32_t i; - - /*lint --e{661} Suppress out of bounds walkthrough warning */ - /*lint --e{662} Suppress out of bounds walkthrough warning */ - pvalue_as_str = value_as_str; - for(i = 0 ; i < data_size ; i++) { - sprintf(pvalue_as_str, "0x%02X", *(pdata+position+i)); - if (i == 0) { - pvalue_as_str = pvalue_as_str + 4; - } - else { - pvalue_as_str = pvalue_as_str + 6; - } - } - - register_name[0] = 0; - VL53L1_get_register_name( - index+(uint16_t)position, - register_name); - - trace_i2c( - /* "ReadAutoIncrement(0x%04X,%3u); // = (%s)\n", - index+(uint16_t)position, */ - "ReadAutoIncrement(%s,%3u); // = (%s)\n", - register_name, - data_size, - value_as_str); - } -#endif // VL53L1_LOG_ENABLE - } - - if(status != VL53L1_ERROR_NONE) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_ReadMulti: RANGING_SENSOR_COMMS_Read_CCI() failed\n"); - trace_i2c(comms_error_string); - } + if ((pdata == NULL) || (count == 0u) || (count > 0xFFFFu)) { + return VL53L1_ERROR_INVALID_PARAMS; } - else if(global_comms_type == VL53L1_SPI) - { - if((CP_STATUS)RANGING_SENSOR_COMMS_Read_SPI_16I(0, 0, index, pdata, count) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - - if(status != VL53L1_ERROR_NONE) - { - RANGING_SENSOR_COMMS_Get_Error_Text(comms_error_string); /*lint !e534 ignoring return value */ - trace_i2c("VL53L1_ReadMulti: RANGING_SENSOR_COMMS_Read_SPI_16I() failed\n"); - trace_i2c(comms_error_string); - } + status = vl53l1_validate_dev(pdev); + if (status != VL53L1_ERROR_NONE) { + return status; } - else - { - trace_i2c("VL53L1_ReadMulti: Comms must be one of VL53L1_I2C or VL53L1_SPI\n"); - status = VL53L1_ERROR_CONTROL_INTERFACE; + status = vl53l1_bus_lock(pdev); + if (status != VL53L1_ERROR_NONE) { + return status; } - - return status; + hal_status = HAL_I2C_Mem_Read( + pdev->hi2c, + pdev->i2c_slave_address, + index, + I2C_MEMADD_SIZE_16BIT, + pdata, + (uint16_t)count, + pdev->io_timeout_ms); + vl53l1_bus_unlock(pdev); + return vl53l1_hal_to_pal(hal_status); } - VL53L1_Error VL53L1_WrByte( VL53L1_Dev_t *pdev, uint16_t index, uint8_t data) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint8_t buffer[1]; - - // Split 16-bit word into MS and LS uint8_t - buffer[0] = (uint8_t)(data); - - status = VL53L1_WriteMulti(pdev, index, buffer, 1); - - return status; + return VL53L1_WriteMulti(pdev, index, &data, 1u); } - VL53L1_Error VL53L1_WrWord( VL53L1_Dev_t *pdev, uint16_t index, uint16_t data) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint8_t buffer[2]; + uint8_t buffer[2]; - // Split 16-bit word into MS and LS uint8_t buffer[0] = (uint8_t)(data >> 8); - buffer[1] = (uint8_t)(data & 0x00FF); - - status = VL53L1_WriteMulti(pdev, index, buffer, VL53L1_BYTES_PER_WORD); - - return status; + buffer[1] = (uint8_t)(data & 0xFFu); + return VL53L1_WriteMulti(pdev, index, buffer, VL53L1_BYTES_PER_WORD); } - VL53L1_Error VL53L1_WrDWord( VL53L1_Dev_t *pdev, uint16_t index, uint32_t data) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint8_t buffer[4]; + uint8_t buffer[4]; - // Split 32-bit word into MS ... LS bytes - buffer[0] = (uint8_t) (data >> 24); - buffer[1] = (uint8_t)((data & 0x00FF0000) >> 16); - buffer[2] = (uint8_t)((data & 0x0000FF00) >> 8); - buffer[3] = (uint8_t) (data & 0x000000FF); - - status = VL53L1_WriteMulti(pdev, index, buffer, VL53L1_BYTES_PER_DWORD); - - return status; + buffer[0] = (uint8_t)(data >> 24); + buffer[1] = (uint8_t)(data >> 16); + buffer[2] = (uint8_t)(data >> 8); + buffer[3] = (uint8_t)(data & 0xFFu); + return VL53L1_WriteMulti(pdev, index, buffer, VL53L1_BYTES_PER_DWORD); } - VL53L1_Error VL53L1_RdByte( VL53L1_Dev_t *pdev, uint16_t index, uint8_t *pdata) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint8_t buffer[1]; - - status = VL53L1_ReadMulti(pdev, index, buffer, 1); - - *pdata = buffer[0]; - - return status; + if (pdata == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + return VL53L1_ReadMulti(pdev, index, pdata, 1u); } - VL53L1_Error VL53L1_RdWord( VL53L1_Dev_t *pdev, uint16_t index, uint16_t *pdata) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint8_t buffer[2]; - - status = VL53L1_ReadMulti( - pdev, - index, - buffer, - VL53L1_BYTES_PER_WORD); - - *pdata = (uint16_t)(((uint16_t)(buffer[0])<<8) + (uint16_t)buffer[1]); + uint8_t buffer[2] = {0u, 0u}; + VL53L1_Error status; + if (pdata == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + status = VL53L1_ReadMulti(pdev, index, buffer, VL53L1_BYTES_PER_WORD); + if (status == VL53L1_ERROR_NONE) { + *pdata = (uint16_t)(((uint16_t)buffer[0] << 8) | buffer[1]); + } return status; } - VL53L1_Error VL53L1_RdDWord( VL53L1_Dev_t *pdev, uint16_t index, uint32_t *pdata) { - VL53L1_Error status = VL53L1_ERROR_NONE; - uint8_t buffer[4]; - - status = VL53L1_ReadMulti( - pdev, - index, - buffer, - VL53L1_BYTES_PER_DWORD); - - *pdata = ((uint32_t)buffer[0]<<24) + ((uint32_t)buffer[1]<<16) + ((uint32_t)buffer[2]<<8) + (uint32_t)buffer[3]; + uint8_t buffer[4] = {0u, 0u, 0u, 0u}; + VL53L1_Error status; + if (pdata == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + status = VL53L1_ReadMulti(pdev, index, buffer, VL53L1_BYTES_PER_DWORD); + if (status == VL53L1_ERROR_NONE) { + *pdata = ((uint32_t)buffer[0] << 24) + | ((uint32_t)buffer[1] << 16) + | ((uint32_t)buffer[2] << 8) + | (uint32_t)buffer[3]; + } return status; } -/* - * ----------------- HOST TIMING FUNCTIONS ----------------- - */ - VL53L1_Error VL53L1_WaitUs( VL53L1_Dev_t *pdev, int32_t wait_us) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - float wait_ms = (float)wait_us/1000.0f; /*lint !e586 float in windows platform*/ - HANDLE hEvent = CreateEvent(0, TRUE, FALSE, 0); - SUPPRESS_UNUSED_WARNING(pdev); - - /* - * Use windows event handling to perform non-blocking wait. - */ - WaitForSingleObject(hEvent, (DWORD)(wait_ms + 0.5f)); /*lint !e534 ignoring return value */ - - trace_i2c("WaitUs(%6d);\n", wait_us); - - return status; + if (wait_us <= 0) { + return VL53L1_ERROR_NONE; + } + if (wait_us >= 1000) { + vl53l1_delay_ms((uint32_t)wait_us / 1000u); + wait_us %= 1000; + } + vl53l1_delay_us_blocking((uint32_t)wait_us); + return VL53L1_ERROR_NONE; } - VL53L1_Error VL53L1_WaitMs( VL53L1_Dev_t *pdev, int32_t wait_ms) { - return VL53L1_WaitUs(pdev, wait_ms * 1000); + SUPPRESS_UNUSED_WARNING(pdev); + if (wait_ms > 0) { + vl53l1_delay_ms((uint32_t)wait_ms); + } + return VL53L1_ERROR_NONE; } -/* - * ----------------- DEVICE TIMING FUNCTIONS ----------------- - */ - VL53L1_Error VL53L1_GetTimerFrequency(int32_t *ptimer_freq_hz) { - *ptimer_freq_hz = 0; - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GetTimerFrequency: Freq : %dHz\n", *ptimer_freq_hz); + if (ptimer_freq_hz == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + *ptimer_freq_hz = 1000; return VL53L1_ERROR_NONE; } - VL53L1_Error VL53L1_GetTimerValue(int32_t *ptimer_count) { - *ptimer_count = 0; - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GetTimerValue: Freq : %dHz\n", *ptimer_count); + if (ptimer_count == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + *ptimer_count = (int32_t)HAL_GetTick(); return VL53L1_ERROR_NONE; } - -/* - * ----------------- GPIO FUNCTIONS ----------------- - */ - VL53L1_Error VL53L1_GpioSetMode(uint8_t pin, uint8_t mode) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Mode((RS_GPIO_Pin)pin, (RS_GPIO_Mode)mode) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GpioSetMode: Status %d. Pin %d, Mode %d\n", status, pin, mode); - return status; + SUPPRESS_UNUSED_WARNING(pin); + SUPPRESS_UNUSED_WARNING(mode); + return VL53L1_ERROR_NONE; } - -VL53L1_Error VL53L1_GpioSetValue(uint8_t pin, uint8_t value) +VL53L1_Error VL53L1_GpioSetValue(uint8_t pin, uint8_t value) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value((RS_GPIO_Pin)pin, value) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GpioSetValue: Status %d. Pin %d, Mode %d\n", status, pin, value); - return status; - + SUPPRESS_UNUSED_WARNING(pin); + SUPPRESS_UNUSED_WARNING(value); + return VL53L1_ERROR_NONE; } - -VL53L1_Error VL53L1_GpioGetValue(uint8_t pin, uint8_t *pvalue) +VL53L1_Error VL53L1_GpioGetValue(uint8_t pin, uint8_t *pvalue) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - DWORD value = 0; - - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Get_Value((RS_GPIO_Pin)pin, &value) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; + SUPPRESS_UNUSED_WARNING(pin); + if (pvalue == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; } - else - { - *pvalue = (uint8_t)value; - } - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GpioGetValue: Status %d. Pin %d, Mode %d\n", status, pin, *pvalue); - return status; + *pvalue = 0u; + return VL53L1_ERROR_NONE; } -/* - * ----------------- HARDWARE STATE FUNCTIONS ----------------- - */ - -VL53L1_Error VL53L1_GpioXshutdown(uint8_t value) +VL53L1_Error VL53L1_GpioXshutdown(uint8_t value) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - if(status == VL53L1_ERROR_NONE) /*lint !e774 always true */ - { - status = VL53L1_GpioSetMode((uint8_t)GPIO_XSHUTDOWN, (uint8_t)GPIO_OutputPP); + if (s_platform_default_dev == NULL) { + return VL53L1_ERROR_CONTROL_INTERFACE; } - - if(status == VL53L1_ERROR_NONE) - { - if(value) - { - if ((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value(GPIO_XSHUTDOWN, (DWORD)Pin_State_High) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - else - { - if ((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value(GPIO_XSHUTDOWN, (DWORD)Pin_State_Low) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - } - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GpioXShutdown: Status %d. Value %d\n", status, value); - return status; + return VL53L1_PlatformSetXShut(s_platform_default_dev, value ? GPIO_PIN_SET : GPIO_PIN_RESET); } - -VL53L1_Error VL53L1_GpioCommsSelect(uint8_t value) +VL53L1_Error VL53L1_GpioCommsSelect(uint8_t value) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - if(status == VL53L1_ERROR_NONE) /*lint !e774 always true */ - { - status = VL53L1_GpioSetMode((uint8_t)GPIO_SPI_CHIP_SELECT, (uint8_t)GPIO_OutputPP); - } - - if(status == VL53L1_ERROR_NONE) - { - if(value) - { - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value(GPIO_SPI_CHIP_SELECT, (DWORD)Pin_State_High) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - else - { - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value(GPIO_SPI_CHIP_SELECT, (DWORD)Pin_State_Low) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - } - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GpioCommsSelect: Status %d. Value %d\n", status, value); - return status; + SUPPRESS_UNUSED_WARNING(value); + return VL53L1_ERROR_NONE; } - -VL53L1_Error VL53L1_GpioPowerEnable(uint8_t value) +VL53L1_Error VL53L1_GpioPowerEnable(uint8_t value) { - VL53L1_Error status = VL53L1_ERROR_NONE; - POWER_BOARD_CMD power_cmd; - - if(status == VL53L1_ERROR_NONE) /*lint !e774 always true */ - { - status = VL53L1_GpioSetMode((uint8_t)GPIO_POWER_ENABLE, (uint8_t)GPIO_OutputPP); - } - - if(status == VL53L1_ERROR_NONE) - { - if(value) - { - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value(GPIO_POWER_ENABLE, (DWORD)Pin_State_High) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - else - { - if((CP_STATUS)RANGING_SENSOR_COMMS_GPIO_Set_Value(GPIO_POWER_ENABLE, (DWORD)Pin_State_Low) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - } - - if(status == VL53L1_ERROR_NONE && _power_board_in_use == 1 && value) /*lint !e506 !e845 !e774*/ - { - memset(&power_cmd, 0, sizeof(POWER_BOARD_CMD)); - power_cmd.command = ENABLE_DUT_POWER; - - if((CP_STATUS)RANGING_SENSOR_COMMS_Write_System_I2C( - POWER_BOARD_I2C_ADDRESS, sizeof(POWER_BOARD_CMD), (uint8_t*)&power_cmd) != CP_STATUS_OK) - { - status = VL53L1_ERROR_CONTROL_INTERFACE; - } - } - - trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GpioPowerEnable: Status %d. Value %d\n", status, value); - return status; + SUPPRESS_UNUSED_WARNING(value); + return VL53L1_ERROR_NONE; } - -VL53L1_Error VL53L1_GpioInterruptEnable(void (*function)(void), uint8_t edge_type) +VL53L1_Error VL53L1_GpioInterruptEnable(void (*function)(void), uint8_t edge_type) { - VL53L1_Error status = VL53L1_ERROR_NONE; - SUPPRESS_UNUSED_WARNING(function); SUPPRESS_UNUSED_WARNING(edge_type); - - return status; + return VL53L1_ERROR_NONE; } - -VL53L1_Error VL53L1_GpioInterruptDisable(void) +VL53L1_Error VL53L1_GpioInterruptDisable(void) { - VL53L1_Error status = VL53L1_ERROR_NONE; - - return status; + return VL53L1_ERROR_NONE; } - VL53L1_Error VL53L1_GetTickCount( + VL53L1_Dev_t *pdev, uint32_t *ptick_count_ms) { - - /* Returns current tick count in [ms] */ - - VL53L1_Error status = VL53L1_ERROR_NONE; - - *ptick_count_ms = timeGetTime(); - - trace_print( - VL53L1_TRACE_LEVEL_DEBUG, - "VL53L1_GetTickCount() = %5u ms;\n", - *ptick_count_ms); - - return status; - + SUPPRESS_UNUSED_WARNING(pdev); + if (ptick_count_ms == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + *ptick_count_ms = HAL_GetTick(); + return VL53L1_ERROR_NONE; } - VL53L1_Error VL53L1_WaitValueMaskEx( VL53L1_Dev_t *pdev, uint32_t timeout_ms, @@ -770,91 +481,59 @@ VL53L1_Error VL53L1_WaitValueMaskEx( uint8_t mask, uint32_t poll_delay_ms) { - /* - * Platform implementation of WaitValueMaskEx V2WReg script command - * - * WaitValueMaskEx( - * duration_ms, - * index, - * value, - * mask, - * poll_delay_ms); - */ + VL53L1_Error status = VL53L1_ERROR_NONE; + uint32_t start_time_ms = 0u; + uint32_t current_time_ms = 0u; + uint8_t byte_value = 0u; + uint8_t found = 0u; - VL53L1_Error status = VL53L1_ERROR_NONE; - uint32_t start_time_ms = 0; - uint32_t current_time_ms = 0; - uint8_t byte_value = 0; - uint8_t found = 0; -#ifdef VL53L1_LOG_ENABLE - uint32_t trace_functions = 0; -#endif - - _LOG_STRING_BUFFER(register_name); - - SUPPRESS_UNUSED_WARNING(poll_delay_ms); - -#ifdef VL53L1_LOG_ENABLE - /* look up register name */ - VL53L1_get_register_name( - index, - register_name); - - /* Output to I2C logger for FMT/DFT */ - trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n", - timeout_ms, register_name, value, mask, poll_delay_ms); -#endif // VL53L1_LOG_ENABLE - - /* calculate time limit in absolute time */ - - VL53L1_GetTickCount(&start_time_ms); - pdev->new_data_ready_poll_duration_ms = 0; - - /* remember current trace functions and temporarily disable - * function logging - */ - -#ifdef VL53L1_LOG_ENABLE - trace_functions = _LOG_GET_TRACE_FUNCTIONS(); -#endif - _LOG_SET_TRACE_FUNCTIONS(VL53L1_TRACE_FUNCTION_NONE); - - /* wait until value is found, timeout reached on error occurred */ + if (pdev == NULL) { + return VL53L1_ERROR_INVALID_PARAMS; + } + status = VL53L1_GetTickCount(pdev, &start_time_ms); + if (status != VL53L1_ERROR_NONE) { + return status; + } + pdev->new_data_ready_poll_duration_ms = 0u; while ((status == VL53L1_ERROR_NONE) && (pdev->new_data_ready_poll_duration_ms < timeout_ms) && - (found == 0)) - { - status = VL53L1_RdByte( - pdev, - index, - &byte_value); - - if ((byte_value & mask) == value) - { - found = 1; + (found == 0u)) { + status = VL53L1_RdByte(pdev, index, &byte_value); + if (status != VL53L1_ERROR_NONE) { + break; } - - /*if (status == VL53L1_ERROR_NONE && - found == 0 && - poll_delay_ms > 0) - status = VL53L1_WaitMs( - pdev, - poll_delay_ms); - */ - - /* Update polling time (Compare difference rather than absolute to - negate 32bit wrap around issue) */ - VL53L1_GetTickCount(¤t_time_ms); + if ((byte_value & mask) == value) { + found = 1u; + break; + } + if (poll_delay_ms > 0u) { + status = VL53L1_WaitMs(pdev, (int32_t)poll_delay_ms); + } + VL53L1_GetTickCount(pdev, ¤t_time_ms); pdev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms; } - /* Restore function logging */ - _LOG_SET_TRACE_FUNCTIONS(trace_functions); - - if (found == 0 && status == VL53L1_ERROR_NONE) + if ((found == 0u) && (status == VL53L1_ERROR_NONE)) { status = VL53L1_ERROR_TIME_OUT; - + } + return status; +} + +VL53L1_Error VL53L1_PlatformChangeAddress( + VL53L1_Dev_t *pdev, + uint8_t new_addr_8bit) +{ + VL53L1_Error status; + + if ((pdev == NULL) || (new_addr_8bit == 0u)) { + return VL53L1_ERROR_INVALID_PARAMS; + } + status = VL53L1_SetDeviceAddress(pdev, new_addr_8bit); + if (status == VL53L1_ERROR_NONE) { + pdev->i2c_slave_address = new_addr_8bit; + vl53l1_delay_ms(2u); + } return status; } diff --git a/App/VL53L1X_API/platform/src/vl53l1_platform_init.c b/App/VL53L1X_API/platform/src/vl53l1_platform_init.c index ded7370..f1cfe1b 100644 --- a/App/VL53L1X_API/platform/src/vl53l1_platform_init.c +++ b/App/VL53L1X_API/platform/src/vl53l1_platform_init.c @@ -51,11 +51,14 @@ VL53L1_Error VL53L1_platform_init( VL53L1_Error status = VL53L1_ERROR_NONE; + if (pdev == NULL) + return VL53L1_ERROR_INVALID_PARAMS; + /* remember comms settings */ - pdev->i2c_slave_address = i2c_slave_address; + pdev->i2c_slave_address = (i2c_slave_address == 0u) ? VL53L1_DEFAULT_ADDR_8BIT : i2c_slave_address; pdev->comms_type = comms_type; - pdev->comms_speed_khz = comms_speed_khz; + pdev->comms_speed_khz = (comms_speed_khz == 0u) ? 400u : comms_speed_khz; if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/ status = @@ -64,37 +67,13 @@ VL53L1_Error VL53L1_platform_init( pdev->comms_type, pdev->comms_speed_khz); - /* Ensure device is in reset */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_GpioXshutdown(0); + pdev->io_timeout_ms = (pdev->io_timeout_ms == 0u) ? 100u : pdev->io_timeout_ms; - /* disable the platform regulators */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_GpioPowerEnable(0); - - /* set the NCS pin for I2C mode */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_GpioCommsSelect(0); - - /* 1ms Wait to ensure XSHUTD / NCS are in the right state */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_WaitUs(pdev, 1000); - - /* enable the platform regulators */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_GpioPowerEnable(1); - - /* 1ms Wait for power regs to settle */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_WaitUs(pdev, 1000); - - /* finally, bring the device out of reset */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_GpioXshutdown(1); - - /* Wait 100us for device to exit reset */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_WaitUs(pdev, 100); + if ((status == VL53L1_ERROR_NONE) && (pdev->xshut_port != NULL)) { + status = VL53L1_PlatformBootDevice(pdev, 2u, 2u); + } else if (status == VL53L1_ERROR_NONE) { + status = VL53L1_WaitMs(pdev, 2); + } return status; } @@ -110,13 +89,9 @@ VL53L1_Error VL53L1_platform_terminate( VL53L1_Error status = VL53L1_ERROR_NONE; - /* put device in reset */ - if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/ - status = VL53L1_GpioXshutdown(0); - - /* disable the platform regulators */ - if (status == VL53L1_ERROR_NONE) - status = VL53L1_GpioPowerEnable(0); + /* put device in reset when an XSHUT pin is attached */ + if ((status == VL53L1_ERROR_NONE) && (pdev != NULL) && (pdev->xshut_port != NULL)) + status = VL53L1_PlatformSetXShut(pdev, GPIO_PIN_RESET); /* close the comms interfaces */ if (status == VL53L1_ERROR_NONE) diff --git a/App/VL53L1X_API/platform/src/vl53l1_platform_log.c b/App/VL53L1X_API/platform/src/vl53l1_platform_log.c index f627d3d..385cc71 100644 --- a/App/VL53L1X_API/platform/src/vl53l1_platform_log.c +++ b/App/VL53L1X_API/platform/src/vl53l1_platform_log.c @@ -20,7 +20,7 @@ #include // sprintf(), vsnprintf(), printf() #include #include -#include +#include #include "vl53l1_platform_log.h" #include "vl53l1_platform_user_config.h" diff --git a/App/VL53L1X_API/platform/vl53_board.c b/App/VL53L1X_API/platform/vl53_board.c new file mode 100644 index 0000000..7d2a71f --- /dev/null +++ b/App/VL53L1X_API/platform/vl53_board.c @@ -0,0 +1,229 @@ +#include "robot_params.h" + +#if PARAM_VL53_USE_L1X + +#include "vl53_board.h" +#include "FreeRTOS.h" +#include "task.h" +#include "vl53_calibration_config.h" + +static void vl53_ema_init(Vl53EMA_t *ema, float alpha) +{ + ema->x = 0.0f; + ema->alpha = alpha; + ema->initialized = 0u; +} + +static float vl53_ema_update(Vl53EMA_t *ema, float measurement) +{ + if (ema->initialized == 0u) { + ema->x = measurement; + ema->initialized = 1u; + return ema->x; + } + + ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x; + return ema->x; +} + +static const Vl53L1RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id) +{ + switch (id) { + case 0: return &k_vl53l1_left_calibration[0]; + case 1: return &k_vl53l1_left_calibration[1]; + case 2: return &k_vl53l1_right_calibration[0]; + case 3: return &k_vl53l1_right_calibration[1]; + default: return NULL; + } +} + +static VL53L1_Error vl53_apply_runtime_calibration(VL53L1_DEV dev, uint8_t id) +{ + const Vl53L1RuntimeCalibration_t *cal = vl53_get_runtime_calibration(id); + + if ((cal == NULL) || (cal->calibrated == 0u)) { + return VL53L1_ERROR_NONE; + } + + return VL53L1_SetCalibrationData(dev, (VL53L1_CalibrationData_t *)&cal->data); +} + +static uint32_t vl53_sanitize_timing_budget_us(uint32_t timing_budget_us) +{ + if (timing_budget_us < 20000u) { + return 20000u; + } + if (timing_budget_us > 1000000u) { + return 1000000u; + } + return timing_budget_us; +} + +static uint32_t vl53_compute_inter_measurement_ms(uint32_t timing_budget_us) +{ + uint32_t timing_budget_ms = (timing_budget_us + 999u) / 1000u; + + /* UM2356: inter-measurement period shorter than timing budget时会立即开始下一帧。 */ + if (timing_budget_ms < 25u) { + timing_budget_ms = 25u; + } + return timing_budget_ms; +} + +static VL53L1_Error vl53_configure_ranging_profile(VL53L1_DEV dev, uint32_t timing_budget_us) +{ + VL53L1_Error status; + const uint32_t inter_measurement_ms = vl53_compute_inter_measurement_ms(timing_budget_us); + + status = VL53L1_SetPresetMode(dev, VL53L1_PRESETMODE_LITE_RANGING); + if (status != VL53L1_ERROR_NONE) return status; + + status = VL53L1_SetDistanceMode(dev, VL53L1_DISTANCEMODE_LONG); + if (status != VL53L1_ERROR_NONE) return status; + + status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us); + if (status != VL53L1_ERROR_NONE) return status; + + status = VL53L1_SetInterMeasurementPeriodMilliSeconds(dev, inter_measurement_ms); + if (status != VL53L1_ERROR_NONE) return status; + + status = VL53L1_SetLimitCheckEnable(dev, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, 1u); + if (status != VL53L1_ERROR_NONE) return status; + + status = VL53L1_SetLimitCheckEnable(dev, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1u); + if (status != VL53L1_ERROR_NONE) return status; + + return VL53L1_ERROR_NONE; +} + +static VL53L1_Error vl53_do_static_init(VL53L1_DEV dev, uint32_t timing_budget_us, uint8_t id) +{ + VL53L1_Error status; + + status = VL53L1_StaticInit(dev); + if (status != VL53L1_ERROR_NONE) return status; + + status = vl53_apply_runtime_calibration(dev, id); + if (status != VL53L1_ERROR_NONE) return status; + + return vl53_configure_ranging_profile(dev, timing_budget_us); +} + +VL53L1_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us) +{ + if ((board == NULL) || (hw_cfgs == NULL) || (count == 0u) || (count > VL53_MAX_DEVS_PER_BOARD)) { + return VL53L1_ERROR_INVALID_PARAMS; + } + + memset(board, 0, sizeof(Vl53Board_t)); + board->dev_count = count; + board->timing_budget_us = vl53_sanitize_timing_budget_us((timing_budget_us == 0u) ? 33000u : timing_budget_us); + + for (uint8_t i = 0; i < count; i++) { + board->dev[i].name = hw_cfgs[i].name; + board->dev[i].id = hw_cfgs[i].id; + board->dev[i].comms_type = VL53L1_I2C; + board->dev[i].comms_speed_khz = 400u; + board->dev[i].is_present = 0u; + + VL53L1_PlatformAttachBus(&board->dev[i], hw_cfgs[i].hi2c, VL53L1_DEFAULT_ADDR_8BIT, 100u, NULL); + VL53L1_PlatformAttachPins(&board->dev[i], hw_cfgs[i].xshut_port, hw_cfgs[i].xshut_pin, NULL, 0u); + (void)VL53L1_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET); + + vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA); + } + + vTaskDelay(pdMS_TO_TICKS(10u)); + + for (uint8_t i = 0; i < count; i++) { + if (VL53L1_PlatformSetXShut(&board->dev[i], GPIO_PIN_SET) != VL53L1_ERROR_NONE) continue; + vTaskDelay(pdMS_TO_TICKS(20u)); + + board->dev[i].i2c_slave_address = VL53L1_DEFAULT_ADDR_8BIT; + if (VL53L1_CommsInitialise(&board->dev[i], VL53L1_I2C, board->dev[i].comms_speed_khz) != VL53L1_ERROR_NONE) continue; + if (VL53L1_WaitDeviceBooted(&board->dev[i]) != VL53L1_ERROR_NONE) continue; + if (VL53L1_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L1_ERROR_NONE) continue; + if (VL53L1_DataInit(&board->dev[i]) != VL53L1_ERROR_NONE) continue; + if (vl53_do_static_init(&board->dev[i], board->timing_budget_us, hw_cfgs[i].id) != VL53L1_ERROR_NONE) continue; + + board->init_mask |= (uint8_t)(1u << i); + board->dev[i].is_present = 1u; + } + + return VL53L1_ERROR_NONE; +} + +VL53L1_Error Vl53Board_StartContinuous(Vl53Board_t *board) +{ + if (board == NULL) return VL53L1_ERROR_INVALID_PARAMS; + + for (uint8_t i = 0; i < board->dev_count; i++) { + if (board->init_mask & (1u << i)) { + (void)VL53L1_StartMeasurement(&board->dev[i]); + } + } + + return VL53L1_ERROR_NONE; +} + +VL53L1_Error Vl53Board_StopContinuous(Vl53Board_t *board) +{ + if (board == NULL) return VL53L1_ERROR_INVALID_PARAMS; + + for (uint8_t i = 0; i < board->dev_count; i++) { + if (board->init_mask & (1u << i)) { + (void)VL53L1_StopMeasurement(&board->dev[i]); + } + } + + return VL53L1_ERROR_NONE; +} + +VL53L1_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot) +{ + if ((board == NULL) || (snapshot == NULL)) return VL53L1_ERROR_INVALID_PARAMS; + + memset(snapshot, 0, sizeof(Vl53BoardSnapshot_t)); + snapshot->tick_ms = xTaskGetTickCount() * portTICK_PERIOD_MS; + + for (uint8_t i = 0; i < board->dev_count; i++) { + if ((board->init_mask & (1u << i)) == 0u) { + snapshot->range_status[i] = 255u; + continue; + } + + uint8_t ready = 0u; + if (VL53L1_GetMeasurementDataReady(&board->dev[i], &ready) != VL53L1_ERROR_NONE) continue; + + if (ready != 0u) { + VL53L1_RangingMeasurementData_t data; + memset(&data, 0, sizeof(data)); + + if (VL53L1_GetRangingMeasurementData(&board->dev[i], &data) == VL53L1_ERROR_NONE) { + snapshot->range_mm[i] = (data.RangeMilliMeter < 0) ? 0u : (uint16_t)data.RangeMilliMeter; + snapshot->range_status[i] = data.RangeStatus; + + if (data.RangeStatus == 0u) { + snapshot->valid_mask |= (uint8_t)(1u << i); +#if PARAM_VL53_USE_EMA_FILTER + snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)snapshot->range_mm[i]); +#else + snapshot->range_mm_filtered[i] = (float)snapshot->range_mm[i]; + board->ema[i].x = (float)snapshot->range_mm[i]; + board->ema[i].initialized = 1u; +#endif + } else { + snapshot->range_mm_filtered[i] = board->ema[i].x; + } + + (void)VL53L1_ClearInterruptAndStartMeasurement(&board->dev[i]); + } + } else { + snapshot->range_mm_filtered[i] = board->ema[i].x; + } + } + + return VL53L1_ERROR_NONE; +} + +#endif diff --git a/App/VL53L1X_API/platform/vl53_board.h b/App/VL53L1X_API/platform/vl53_board.h new file mode 100644 index 0000000..a8e9977 --- /dev/null +++ b/App/VL53L1X_API/platform/vl53_board.h @@ -0,0 +1,56 @@ +#ifndef VL53_BOARD_H +#define VL53_BOARD_H + +#include +#include + +#include "vl53l1_api.h" +#include "vl53l1_platform.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define VL53_MAX_DEVS_PER_BOARD 4 + +typedef struct { + I2C_HandleTypeDef *hi2c; + GPIO_TypeDef *xshut_port; + uint16_t xshut_pin; + uint8_t runtime_addr_8bit; + const char *name; + uint8_t id; +} Vl53BoardHwCfg_t; + +typedef struct { + float x; + float alpha; + uint8_t initialized; +} Vl53EMA_t; + +typedef struct { + uint32_t tick_ms; + uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD]; + float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; + uint8_t range_status[VL53_MAX_DEVS_PER_BOARD]; + uint8_t valid_mask; +} Vl53BoardSnapshot_t; + +typedef struct { + VL53L1_Dev_t dev[VL53_MAX_DEVS_PER_BOARD]; + Vl53EMA_t ema[VL53_MAX_DEVS_PER_BOARD]; + uint8_t init_mask; + uint8_t dev_count; + uint32_t timing_budget_us; +} Vl53Board_t; + +VL53L1_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us); +VL53L1_Error Vl53Board_StartContinuous(Vl53Board_t *board); +VL53L1_Error Vl53Board_StopContinuous(Vl53Board_t *board); +VL53L1_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot); + +#ifdef __cplusplus +} +#endif + +#endif /* VL53_BOARD_H */ diff --git a/App/VL53L1X_API/platform/vl53_calibration_config.h b/App/VL53L1X_API/platform/vl53_calibration_config.h new file mode 100644 index 0000000..7ba4947 --- /dev/null +++ b/App/VL53L1X_API/platform/vl53_calibration_config.h @@ -0,0 +1,29 @@ +#ifndef VL53L1_CALIBRATION_CONFIG_H +#define VL53L1_CALIBRATION_CONFIG_H + +#include + +#include "vl53l1_def.h" + +typedef struct { + uint8_t calibrated; + VL53L1_CalibrationData_t data; +} Vl53L1RuntimeCalibration_t; + +/* + * VL53L1X 按 UM2356 建议应在产线完成 RefSPAD / Offset / Xtalk 校准， + * 上电后在 DataInit() + StaticInit() 之后用 SetCalibrationData() 回灌。 + * + * 当前先提供空白占位，未标定时保持 calibrated = 0，驱动将跳过加载。 + */ +static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = { + { .calibrated = 0u, .data = {0} }, + { .calibrated = 0u, .data = {0} }, +}; + +static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = { + { .calibrated = 0u, .data = {0} }, + { .calibrated = 0u, .data = {0} }, +}; + +#endif /* VL53L1_CALIBRATION_CONFIG_H */ diff --git a/App/app_tasks.c b/App/app_tasks.c index ebf2bec..d139d2f 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -76,7 +76,8 @@ static void App_PrintStatus(void) } // 5. 打印侧向阵列雷达 (强制转为 int，去掉多余的小数点) - printf("[VL53L0X] Left-F : %4d mm | Left-R : %4d mm\r\n", + printf("[%s] Left-F : %4d mm | Left-R : %4d mm\r\n", + VL53_BOARD_DRIVER_NAME, (int)snap.dist_left_f.value, (int)snap.dist_left_r.value); printf(" Right-F: %4d mm | Right-R: %4d mm\r\n", (int)snap.dist_right_f.value, (int)snap.dist_right_r.value); @@ -204,7 +205,7 @@ void AppTasks_RunImuTask_Impl(void *argument) } /* ========================================================= - * 3. 侧向阵列雷达任务 (VL53L0X 双总线) + * 3. 侧向阵列雷达任务 (VL53 双总线) * ========================================================= */ void AppTasks_RunVl53Task_Impl(void *argument) { diff --git a/App/robot_params.h b/App/robot_params.h index e7f7948..bc6b7f9 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -119,6 +119,18 @@ extern "C" { */ #define PARAM_ENCODER_CPR 3680 +/* ========================================================= + * 【P1-EXT】VL53 驱动切换 + * ========================================================= */ + +/** @brief 侧向 ToF 驱动选择 + * 0 = VL53L0X + * 1 = VL53L1X + */ +#ifndef PARAM_VL53_USE_L1X +#define PARAM_VL53_USE_L1X 0 +#endif + /* ========================================================= * 【P1】里程计参数 (实车标定) * ========================================================= */ diff --git a/App/vl53_board.h b/App/vl53_board.h new file mode 100644 index 0000000..5516e5d --- /dev/null +++ b/App/vl53_board.h @@ -0,0 +1,14 @@ +#ifndef APP_VL53_BOARD_H +#define APP_VL53_BOARD_H + +#include "robot_params.h" + +#if PARAM_VL53_USE_L1X +#include "VL53L1X_API/platform/vl53_board.h" +#define VL53_BOARD_DRIVER_NAME "VL53L1X" +#else +#include "VL53L0X_API/platform/vl53_board.h" +#define VL53_BOARD_DRIVER_NAME "VL53L0X" +#endif + +#endif /* APP_VL53_BOARD_H */ diff --git a/CMakeLists.txt b/CMakeLists.txt index 62e4f33..c9a78df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,9 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE App/VL53L0X_API/core/inc App/VL53L0X_API/core/src App/VL53L0X_API/platform + App/VL53L1X_API/core/inc + App/VL53L1X_API/core/src + App/VL53L1X_API/platform/inc ) # Add project symbols (macros)