This commit is contained in:
2026-04-05 10:15:40 +08:00
parent 46e7fcdbb2
commit c2a70c9470
7 changed files with 148 additions and 89 deletions

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]; /* 接口2EMA平滑数据 */
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;