Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-08 12:49:16 +08:00
parent fda081c1f0
commit ff467d26bc
13 changed files with 781 additions and 727 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
App/VL53L0X_API/platform/vl53_board.c
View File

@@ -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

38
App/VL53L1X_API/platform/inc/vl53l1_platform.h
View File

@@ -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

15
App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h
View File

@@ -34,6 +34,7 @@
#include <stdlib.h>
#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;

1045
App/VL53L1X_API/platform/src/vl53l1_platform.c
View File

File diff suppressed because it is too large Load Diff

53
App/VL53L1X_API/platform/src/vl53l1_platform_init.c
View File

@@ -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)

2
App/VL53L1X_API/platform/src/vl53l1_platform_log.c
View File

@@ -20,7 +20,7 @@
#include <stdio.h> // sprintf(), vsnprintf(), printf()
#include <string.h>
#include <stdarg.h>
#include <malloc.h>
#include <stdlib.h>
#include "vl53l1_platform_log.h"
#include "vl53l1_platform_user_config.h"

229
App/VL53L1X_API/platform/vl53_board.c Normal file
View File

@@ -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

56
App/VL53L1X_API/platform/vl53_board.h Normal file
View File

@@ -0,0 +1,56 @@
#ifndef VL53_BOARD_H
#define VL53_BOARD_H
#include <stdint.h>
#include <string.h>
#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 */

29
App/VL53L1X_API/platform/vl53_calibration_config.h Normal file
View File

@@ -0,0 +1,29 @@
#ifndef VL53L1_CALIBRATION_CONFIG_H
#define VL53L1_CALIBRATION_CONFIG_H
#include <stdint.h>
#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 */

5
App/app_tasks.c
View File

@@ -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)
{

12
App/robot_params.h
View File

@@ -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】里程计参数 (实车标定)
* ========================================================= */

14
App/vl53_board.h Normal file
View File

@@ -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 */

3
CMakeLists.txt
View File

@@ -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)