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;