From 0b951768ecff1949259b8b0e2605d4a37f6ff195 Mon Sep 17 00:00:00 2001 From: wangfq Date: Mon, 29 Jun 2026 11:25:44 +0800 Subject: [PATCH] =?UTF-8?q?feat(vd960Loop):=20=E7=A7=BB=E6=A4=8D=20DLD154V?= =?UTF-8?q?4B=20V2.0-V2.5=20M4=20=E4=BC=98=E5=8C=96=20=E2=80=94=20?= =?UTF-8?q?=E5=8F=8C=E8=B7=AF=20IIR=20+=20=E6=96=9C=E7=8E=87=E9=99=90?= =?UTF-8?q?=E5=B9=85=20+=20=E8=BF=9B=E5=85=A5=E7=A1=AE=E8=AE=A4=20+=20?= =?UTF-8?q?=E5=86=BB=E7=BB=93=E8=B6=85=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit V2.0: 双路 IIR (慢速 α=18/256 τ=135ms + 快速 α=0.5 τ=28ms) 斜率限幅 MAX_SLOPE_RATE=5% 进入确认 ENTRY_CONFIRM=3 vTaskDelay 50→10ms (tick 加速 5×) V2.1: CAPVD_fast 初始化修复 (==0 时首次锁定) V2.2: 稳定期绕过 IIR 和斜率限幅，直接用 Value 收敛基线 V2.3: 基线冻结超时自动恢复 (FREEZE_TIMEOUT) V2.4: 冻结稳定性检查 (FREEZE_STABILITY_RATE=2% 窗口) V2.5: 冻结超时 10s (1000 tick @ 10ms) 结构体新增: loop_CAPVD_fast, loop_entry_cnt, loop_freeze_cnt, loop_freeze_ref 每通道独立状态，四路并行运行 --- .../at32f421_freertos_demo/inc/TaskLoop.h | 23 ++- .../at32f421_freertos_demo/src/TaskLoop.c | 142 +++++++++++++----- 2 files changed, 128 insertions(+), 37 deletions(-) diff --git a/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h b/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h index 85b53bd..7e9fc9b 100644 --- a/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h +++ b/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h @@ -18,6 +18,7 @@ /*=========================================================================== * 时序参数（每 tick = 50ms，由 TMR15 5ms×10 产生） + * 主循环: vTaskDelay(10ms), M4 优化 *===========================================================================*/ #define HOLD_TIME (5 * 1200) // 有限存在保持（约 5 分钟） #define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约 4 分钟） @@ -26,11 +27,21 @@ #define PULSE_DELAY 10 // 脉冲宽度 500ms /*=========================================================================== - * 滤波参数 + * 滤波参数 — M4 优化版 (V2.0+) + * tick 提升到 10ms，滤波系数同步调整，保持等效时间常数 *===========================================================================*/ -#define ALFA_CAP1 79 // IIR 指数平滑 α = 79/256 ≈ 0.31 +#define ALFA_CAP1 18 // IIR α = 18/256 ≈ 0.07 (@10ms → τ≈135ms, 等效 50ms 的 79/256) +#define ALFA_FAST 128 // 快速 IIR α = 128/256 = 0.5 (@10ms → τ≈28ms, 用于检测) +#define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5% +#define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值 #define STABLE_SAMPLES 128 // 稳定期样本数 +/*=========================================================================== + * 基线冻结超时 (V2.3~V2.5) + *===========================================================================*/ +#define FREEZE_TIMEOUT 1000 // 冻结超时: ~10s @ 10ms/tick +#define FREEZE_STABILITY_RATE 2 // 稳定性窗口: 参考值的 ±2% + /*=========================================================================== * 离开检测: 1 = 平坦性三条件 (CN200910309382), 0 = cnt_release 防抖 *===========================================================================*/ @@ -91,6 +102,14 @@ typedef struct { uint16_t loop_dlt_ORG; // 当前灵敏度阈值 uint8_t Flt_Reg; // IIR 滤波系数 + /*--- M4 优化 V2.0: 快速 IIR + 进入确认 ---*/ + uint32_t loop_CAPVD_fast; // 快速 IIR 值 (α=0.5, τ≈28ms) + uint8_t loop_entry_cnt; // 进入确认计数 + + /*--- M4 优化 V2.3~2.5: 基线冻结超时 ---*/ + uint16_t loop_freeze_cnt; // 冻结持续计数 + uint32_t loop_freeze_ref; // 冻结参考值 + /*--- 标志位 ---*/ uint8_t loop_INI_LOOP; uint8_t loop_CAP_OK; // 新测量数据就绪 diff --git a/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c b/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c index d4a32a4..4dde1c6 100644 --- a/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -110,6 +110,11 @@ void init_vd_single(Loop154_Unit *unit) unit->loop_SensLevel = 2; // 默认中灵敏度 unit->Flt_Reg = ALFA_CAP1; + unit->loop_CAPVD_fast = 0; + unit->loop_entry_cnt = 0; + unit->loop_freeze_cnt = 0; + unit->loop_freeze_ref = 0; + unit->loop_ORG_CNT = 0; unit->loop_ORG_SUM = 0; @@ -432,15 +437,45 @@ void vd1_task_per_channel(Loop154_Unit *unit) { if (unit->loop_Origin == 0) return; - /*--- 1. IIR 一阶低通滤波 ---*/ + /*================================================================ + * 1. 双路 IIR 滤波 (M4 优化 V2.0) + * + * 慢速 IIR (CAPVD): α=18/256 ≈ 0.07, τ≈135ms + * - 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰) + * - 用途: 基线跟踪 + * + * 快速 IIR (CAPVD_fast): α=0.5, τ≈28ms + * - 从斜率限幅后的 CAPVD 派生 + * - 用途: 进入/离开检测判定 + *================================================================*/ + + /* 1a. 慢速 IIR — 斜率限幅 */ if (unit->loop_CAPVD == 0) { unit->loop_CAPVD = unit->loop_Value; + unit->loop_CAPVD_fast = unit->loop_Value; } else { - unit->loop_CAPVD = get_flt_value(unit->loop_Value, unit->loop_CAPVD); + /* 斜率限幅: 物理车辆不可能让频率瞬间跳变 > MAX_SLOPE_RATE% */ + int32_t raw_delta = (int32_t)unit->loop_Value - (int32_t)unit->loop_CAPVD; + int32_t max_step = (int32_t)(unit->loop_CAPVD * MAX_SLOPE_RATE / 100); + if (max_step < 100) max_step = 100; + if (raw_delta > max_step) raw_delta = max_step; + if (raw_delta < -max_step) raw_delta = -max_step; + uint32_t clamped_value = (uint32_t)((int32_t)unit->loop_CAPVD + raw_delta); + unit->loop_CAPVD = get_flt_value(clamped_value, unit->loop_CAPVD); } - /*--- 2. 稳定期：只跟踪基线，不检测车辆 ---*/ + /* 1b. 快速 IIR — α=0.5: (old + new) / 2 */ + if (unit->loop_CAPVD_fast == 0) { + unit->loop_CAPVD_fast = unit->loop_CAPVD; // 首次直接锁定 + } else { + unit->loop_CAPVD_fast = (unit->loop_CAPVD_fast + unit->loop_CAPVD) / 2; + } + + /*--- 2. 稳定期：绕过 IIR 和斜率限幅，直接用 Value 快速收敛 (V2.2) ---*/ if (!unit->loop_stable) { + unit->loop_CAPVD = unit->loop_Value; + unit->loop_CAPVD_fast = unit->loop_Value; + update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT, &unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN); unit->stable_cnt++; @@ -456,53 +491,90 @@ void vd1_task_per_channel(Loop154_Unit *unit) * 无车状态 *================================================================*/ - /* 基线跟踪（有车时冻结） - * 保护: CAPVD 异常上升时暂停跟踪，防止基线被污染 */ + /*--- 基线跟踪（仿 TLD-110：有车时冻结） + * 额外保护: CAPVD 异常上升时暂停跟踪，防止基线被污染 ---*/ unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable[unit->loop_SensLevel]) >> 16; { int32_t dev = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_Origin; if (dev < (int32_t)(unit->loop_dlt_ORG * 4)) { + /* CAPVD 未显著高于基线 → 安全跟踪，重置冻结计数 */ + unit->loop_freeze_cnt = 0; update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT, &unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN); } else { - unit->loop_ORG_CNT = 0; - unit->loop_ORG_SUM = 0; + /* CAPVD 异常偏高 → 冻结跟踪 */ + if (unit->loop_freeze_cnt == 0) { + unit->loop_freeze_ref = unit->loop_CAPVD; // 记录冻结起始值 + } else { + /* 稳定性检查: CAPVD 偏离参考值超过 FREEZE_STABILITY_RATE% 则重置 */ + int32_t drift = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_freeze_ref; + if (drift < 0) drift = -drift; + if (drift > (int32_t)(unit->loop_freeze_ref * FREEZE_STABILITY_RATE / 100)) { + /* 波动过大 → 不是稳定值，重置计数并以当前值重新计时 */ + unit->loop_freeze_cnt = 0; + unit->loop_freeze_ref = unit->loop_CAPVD; + } + } + unit->loop_freeze_cnt++; + if (unit->loop_freeze_cnt >= FREEZE_TIMEOUT) { + /* 超时: CAPVD 持续偏高且稳定 → 环境变化，接受新基线 */ + unit->loop_Origin = unit->loop_CAPVD; + unit->loop_freeze_cnt = 0; + unit->loop_freeze_ref = 0; + unit->loop_ORG_CNT = 0; + unit->loop_ORG_SUM = 0; + PRINT("Loop%d Baseline timeout, new Origin:%d\n", + unit->loop_num + 1, unit->loop_Origin); + } else { + /* 未超时: 保持冻结，重置累计（防止突然解冻时旧数据污染） */ + unit->loop_ORG_CNT = 0; + unit->loop_ORG_SUM = 0; + } } } - /* 进入检测 */ - if (unit->loop_CAPVD < (unit->loop_Origin - unit->loop_dlt_ORG)) { - PRINT("Loop%d Car_In, Value:%d CAPVD:%d Origin:%d dlt:%d\n", - unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD, - unit->loop_Origin, unit->loop_dlt_ORG); + /*--- M4 优化: 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车 + * 使用快速 IIR (CAPVD_fast) 提高响应速度 ---*/ + if (unit->loop_CAPVD_fast < (unit->loop_Origin - unit->loop_dlt_ORG)) { + unit->loop_entry_cnt++; + if (unit->loop_entry_cnt >= ENTRY_CONFIRM) { + PRINT("Loop%d Car_In, Value:%d CAPVD:%d CAPVD_fast:%d Origin:%d dlt:%d\n", + unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD, + unit->loop_CAPVD_fast, unit->loop_Origin, unit->loop_dlt_ORG); - unit->loop_VD_FLAG = 1; - unit->loop_FLAG_IN = 1; - if (unit->hold_time > 0) { - unit->loop_VD_HOLD = 1; - } - at32_led_on(unit->loop_num); + unit->loop_VD_FLAG = 1; + unit->loop_FLAG_IN = 1; + unit->loop_entry_cnt = 0; + unit->loop_freeze_cnt = 0; // 入场时重置冻结状态 + unit->loop_freeze_ref = 0; + if (unit->hold_time > 0) { + unit->loop_VD_HOLD = 1; + } + at32_led_on(unit->loop_num); - if (!unit->SET_SAFE) { - unit->LC_HOLD = 1; - } else { - unit->LC_HOLD = 0; - } - if (unit->LC_Reset) - unit->LC_Reset = 0; + if (!unit->SET_SAFE) { + unit->LC_HOLD = 1; + } else { + unit->LC_HOLD = 0; + } + if (unit->LC_Reset) + unit->LC_Reset = 0; - unit->loop_ORG_CNT = 0; - unit->loop_ORG_SUM = 0; + unit->loop_ORG_CNT = 0; + unit->loop_ORG_SUM = 0; #if USE_FLATNESS_EXIT - unit->exit_state = 0; - unit->max_slope = 0; - unit->max_slope_rate = 0; - unit->delta2 = 0; - unit->delta3 = 0; - unit->slope_flat_cnt = 0; - unit->flat_ok_cnt = 0; + unit->exit_state = 0; + unit->max_slope = 0; + unit->max_slope_rate = 0; + unit->delta2 = 0; + unit->delta3 = 0; + unit->slope_flat_cnt = 0; + unit->flat_ok_cnt = 0; #endif + } + } else { + if (unit->loop_entry_cnt > 0) unit->loop_entry_cnt = 0; } } else { #if USE_FLATNESS_EXIT @@ -640,7 +712,7 @@ void loop_task_function(void *pvParameters) } wdt_feed(); - vTaskDelay(50); // 50ms tick，对齐 DLD154V4B 原始设计 (TMR15 5ms×10) + vTaskDelay(10); // 10ms tick, M4 优化: 双路 IIR + 进入确认 + 斜率限幅 } }