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-04 21:51:55 +08:00
parent e18a7c949c
commit 5e0901a4d9
2 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -191,9 +191,15 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho
snapshot->range_status[i] = data.RangeStatus; snapshot->range_status[i] = data.RangeStatus;
if (data.RangeStatus == 0u) { if (data.RangeStatus == 0u) {
/* 2. 标记有效并更新卡尔曼滤波器 */ /* 2. 标记有效并按开关决定是否应用卡尔曼滤波 */
snapshot->valid_mask |= (1u << i); 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); snapshot->range_mm_filtered[i] = vl53_kalman_update(&board->kf[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;
#endif
} else { } else {
/* 测距失败时，滤波值维持上一次的历史最佳估计不变 */ /* 测距失败时，滤波值维持上一次的历史最佳估计不变 */
snapshot->range_mm_filtered[i] = board->kf[i].x; snapshot->range_mm_filtered[i] = board->kf[i].x;

6
App/robot_params.h
View File

@@ -351,6 +351,12 @@ extern "C" {
*/ */
#define PARAM_VL53_TIMING_BUDGET 33000U #define PARAM_VL53_TIMING_BUDGET 33000U
/** @brief [调试] 是否启用 VL53L0X 端卡尔曼滤波
* 1: 使用滤波后的 range_mm_filtered
* 0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比
*/
#define PARAM_VL53_USE_KALMAN_FILTER 1
/** @brief [调优] VL53L0X 卡尔曼滤波参数 Q /** @brief [调优] VL53L0X 卡尔曼滤波参数 Q
* 含义：测距过程噪声 * 含义：测距过程噪声
* 典型值：5.0~20.0 * 典型值：5.0~20.0