diff --git a/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h b/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h index 0db34d2..ded986b 100644 --- a/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h +++ b/vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h @@ -30,8 +30,7 @@ * 滤波参数 — M4 优化版 (V2.0+) * tick 提升到 10ms,滤波系数同步调整,保持等效时间常数 *===========================================================================*/ -#define ALFA_CAP1 18 // IIR α = 18/256 ≈ 0.07 (@10ms → τ≈135ms, 等效 50ms 的 79/256) -/* 快速 IIR (CAPVD_fast): α=128/256=0.5, τ≈28ms, 用 (old+new)/2 实现,无需宏 */ +#define ALFA_CAP1 79 // IIR α = 79/256 ≈ 0.31 (@10ms → τ≈32ms) #define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5% #define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值 #define STABLE_SAMPLES 128 // 稳定期样本数 @@ -102,11 +101,8 @@ 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) + /*--- M4 优化: 进入确认 + 冻结超时 ---*/ uint8_t loop_entry_cnt; // 进入确认计数 - - /*--- M4 优化 V2.3~2.5: 基线冻结超时 ---*/ uint16_t loop_freeze_cnt; // 冻结持续计数 uint32_t loop_freeze_ref; // 冻结参考值 diff --git a/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c b/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c index 3c89164..248acee 100644 --- a/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -110,7 +110,6 @@ 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; @@ -438,57 +437,24 @@ void vd1_task_per_channel(Loop154_Unit *unit) if (unit->loop_Origin == 0) return; /*================================================================ - * 1. 双路 IIR 滤波 (M4 优化 V2.0) - * - * 慢速 IIR (CAPVD): α=18/256 ≈ 0.07, τ≈135ms - * - 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰) - * - 用途: 基线跟踪 - * - * 快速 IIR (CAPVD_fast): α=0.5, τ≈28ms - * - 从斜率限幅后的 CAPVD 派生 - * - 用途: 进入/离开检测判定 + * 1. IIR 滤波 + 斜率限幅 (M4 优化) + * ALFA_CAP1=79 @10ms → τ≈32ms *================================================================*/ - - /* 1a. 慢速 IIR — 斜率限幅(参考 CAPVD)*/ - { - uint32_t fast_input; - - if (unit->loop_CAPVD == 0) { - unit->loop_CAPVD = unit->loop_Value; - fast_input = unit->loop_Value; - } else { - /*--- 慢速路径:斜率限幅参考 CAPVD (τ=135ms),保护基线跟踪 ---*/ - 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); - - /*--- 快速路径:斜率限幅参考 CAPVD_fast (τ=28ms),限幅窗口随快速值扩展 ---*/ - if (unit->loop_CAPVD_fast == 0) { - fast_input = clamped_value; - } else { - int32_t f_delta = (int32_t)unit->loop_Value - (int32_t)unit->loop_CAPVD_fast; - int32_t f_max = (int32_t)(unit->loop_CAPVD_fast * MAX_SLOPE_RATE / 100); - if (f_max < 100) f_max = 100; - if (f_delta > f_max) f_delta = f_max; - if (f_delta < -f_max) f_delta = -f_max; - fast_input = (uint32_t)((int32_t)unit->loop_CAPVD_fast + f_delta); - } - } - - /* 1b. 快速 IIR — α=0.5: (old + new) / 2 */ - if (unit->loop_CAPVD_fast == 0) { - unit->loop_CAPVD_fast = fast_input; - } else { - unit->loop_CAPVD_fast = (unit->loop_CAPVD_fast + fast_input) / 2; - } + if (unit->loop_CAPVD == 0) { + unit->loop_CAPVD = unit->loop_Value; + } else { + 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. 稳定期:绕过 IIR 和斜率限幅 ---*/ 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); @@ -505,33 +471,26 @@ void vd1_task_per_channel(Loop154_Unit *unit) * 无车状态 *================================================================*/ - /*--- 基线跟踪(仿 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 { - /* CAPVD 异常偏高 → 冻结跟踪 */ if (unit->loop_freeze_cnt == 0) { - unit->loop_freeze_ref = unit->loop_CAPVD; // 记录冻结起始值 + 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; @@ -540,26 +499,24 @@ void vd1_task_per_channel(Loop154_Unit *unit) 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; } } } - /*--- M4 优化: 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车 - * 使用快速 IIR (CAPVD_fast) 提高响应速度 ---*/ - if (unit->loop_CAPVD_fast < (unit->loop_Origin - unit->loop_dlt_ORG)) { + /*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值 ---*/ + if (unit->loop_CAPVD < (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", + 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_CAPVD_fast, unit->loop_Origin, unit->loop_dlt_ORG); + unit->loop_Origin, unit->loop_dlt_ORG); unit->loop_VD_FLAG = 1; unit->loop_FLAG_IN = 1; unit->loop_entry_cnt = 0; - unit->loop_freeze_cnt = 0; // 入场时重置冻结状态 + unit->loop_freeze_cnt = 0; unit->loop_freeze_ref = 0; if (unit->hold_time > 0) { unit->loop_VD_HOLD = 1;