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-05 10:15:40 +08:00
parent 46e7fcdbb2
commit c2a70c9470
7 changed files with 148 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,26 +4,22 @@
#include "robot_params.h"
#include "vl53_calibration_config.h"
/* ================= 卡尔曼滤波底层实现 ================= */
static void vl53_kalman_init(Vl53Kalman_t *kf, float q, float r) {
kf->x = 0.0f;
kf->p = 1.0f;
kf->q = q;
kf->r = r;
kf->initialized = 0;
/* ================= EMA滤波底层实现 ================= */
static void vl53_ema_init(Vl53EMA_t *ema, float alpha) {
ema->x = 0.0f;
ema->alpha = alpha;
ema->initialized = 0;
}
static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) {
if (kf->initialized == 0) {
kf->x = measurement;
kf->initialized = 1;
return kf->x;
static float vl53_ema_update(Vl53EMA_t *ema, float measurement) {
if (ema->initialized == 0) {
ema->x = measurement;
ema->initialized = 1;
return ema->x;
}
kf->p = kf->p + kf->q;
float k = kf->p / (kf->p + kf->r);
kf->x = kf->x + k * (measurement - kf->x);
kf->p = (1.0f - k) * kf->p;
return kf->x;
// EMA公式: x_new = alpha * measurement + (1 - alpha) * x_old
ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x;
return ema->x;
}
static const Vl53RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id)
@@ -124,8 +120,8 @@ VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs
VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);
/* 初始化卡尔曼滤波器：Q/R 从 robot_params.h 读取 */
vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R);
/* 初始化EMA滤波器：alpha 从 robot_params.h 读取 */
vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA);
}
vTaskDelay(pdMS_TO_TICKS(10));
@@ -191,25 +187,25 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho
snapshot->range_status[i] = data.RangeStatus;
if (data.RangeStatus == 0u) {
/* 2. 标记有效并按开关决定是否应用卡尔曼滤波 */
/* 2. 标记有效并按开关决定是否应用EMA滤波 */
snapshot->valid_mask |= (1u << i);
#if PARAM_VL53_USE_KALMAN_FILTER
snapshot->range_mm_filtered[i] = vl53_kalman_update(&board->kf[i], (float)data.RangeMilliMeter);
#if PARAM_VL53_USE_EMA_FILTER
snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)data.RangeMilliMeter);
#else
snapshot->range_mm_filtered[i] = (float)data.RangeMilliMeter;
board->kf[i].x = (float)data.RangeMilliMeter;
board->kf[i].initialized = 1u;
board->ema[i].x = (float)data.RangeMilliMeter;
board->ema[i].initialized = 1u;
#endif
} else {
/* 测距失败时，滤波值维持上一次的历史最佳估计不变 */
snapshot->range_mm_filtered[i] = board->kf[i].x;
snapshot->range_mm_filtered[i] = board->ema[i].x;
}
VL53L0X_ClearInterruptMask(&board->dev[i], 0u);
}
} else {
/* 如果没准备好，把上一帧的历史值顺延下来，防止读到 0 */
snapshot->range_mm_filtered[i] = board->kf[i].x;
snapshot->range_mm_filtered[i] = board->ema[i].x;
}
}
return VL53L0X_ERROR_NONE;

14
App/VL53L0X_API/platform/vl53_board.h
View File

@@ -21,27 +21,25 @@ extern "C" {
uint8_t id;
} Vl53BoardHwCfg_t;
/* ================= 内部卡尔曼滤波器结构 ================= */
/* ================= 内部EMA滤波器结构 ================= */
typedef struct {
float x; // 最优估计值 (滤波后的距离)
float p; // 估计误差协方差
float q; // 过程噪声
float r; // 测量噪声
float x; // 当前滤波值 (滤波后的距离)
float alpha; // 平滑系数 (0.0~1.0, 越大响应越快)
uint8_t initialized; // 防止第一次开机从0缓慢爬升
} Vl53Kalman_t;
} Vl53EMA_t;
/* ================= 快照数据结构 (对外接口) ================= */
typedef struct {
uint32_t tick_ms;
uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD]; /* 接口1：原始硬件数据 */
float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：卡尔曼平滑数据 */
float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：EMA平滑数据 */
uint8_t range_status[VL53_MAX_DEVS_PER_BOARD];
uint8_t valid_mask;
} Vl53BoardSnapshot_t;
typedef struct {
VL53L0X_Dev_t dev[VL53_MAX_DEVS_PER_BOARD];
Vl53Kalman_t kf[VL53_MAX_DEVS_PER_BOARD]; /* 每个传感器配备一个专属卡尔曼滤波器 */
Vl53EMA_t ema[VL53_MAX_DEVS_PER_BOARD]; /* 每个传感器配备一个专属EMA滤波器 */
uint8_t init_mask;
uint8_t dev_count;
uint32_t timing_budget_us;

8
App/VL53L0X_API/platform/vl53_calibration_config.h
View File

@@ -22,13 +22,13 @@ typedef struct {
*/
static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = {
{
.offset_micro_meters = 10000,
.offset_micro_meters = 8000,
.xtalk_compensation_rate_mcps = 0,
.offset_calibrated = 1,
.xtalk_calibrated = 0,
},
{
.offset_micro_meters = 10000,
.offset_micro_meters = 8000,
.xtalk_compensation_rate_mcps = 0,
.offset_calibrated = 1,
.xtalk_calibrated = 0,
@@ -37,13 +37,13 @@ static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = {
static const Vl53RuntimeCalibration_t k_vl53_right_calibration[2] = {
{
.offset_micro_meters = 9000,
.offset_micro_meters = 2000,
.xtalk_compensation_rate_mcps = 0,
.offset_calibrated = 1,
.xtalk_calibrated = 0,
},
{
.offset_micro_meters = 13000,
.offset_micro_meters = 9000,
.xtalk_compensation_rate_mcps = 0,
.offset_calibrated = 1,
.xtalk_calibrated = 0,