wangfq/vd_960
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: 去掉快速 IIR，ALFA_CAP1=79 @10ms (同步 DLD154V4B)

Browse Source
This commit is contained in:
wangfq
2026-06-29 18:35:15 +08:00
parent 0a5a640195
commit 4d77ef8c71
2 changed files with 22 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
vd960Loop/utilities/at32f421_freertos_demo/inc/TaskLoop.h
View File

@@ -30,8 +30,7 @@
* 滤波参数 — M4 优化版 (V2.0+) * 滤波参数 — M4 优化版 (V2.0+)
* tick 提升到 10ms，滤波系数同步调整，保持等效时间常数 * tick 提升到 10ms，滤波系数同步调整，保持等效时间常数
*===========================================================================*/ *===========================================================================*/
#define ALFA_CAP1 18 // IIR α = 18/256 ≈ 0.07 (@10ms → τ≈135ms, 等效 50ms 的 79/256) #define ALFA_CAP1 79 // IIR α = 79/256 ≈ 0.31 (@10ms → τ≈32ms)
/* 快速 IIR (CAPVD_fast): α=128/256=0.5, τ≈28ms, 用 (old+new)/2 实现，无需宏 */
#define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5% #define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5%
#define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值 #define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值
#define STABLE_SAMPLES 128 // 稳定期样本数 #define STABLE_SAMPLES 128 // 稳定期样本数
@@ -102,11 +101,8 @@ typedef struct {
uint16_t loop_dlt_ORG; // 当前灵敏度阈值 uint16_t loop_dlt_ORG; // 当前灵敏度阈值
uint8_t Flt_Reg; // IIR 滤波系数 uint8_t Flt_Reg; // IIR 滤波系数
/*--- M4 优化 V2.0: 快速 IIR + 进入确认 ---*/ /*--- M4 优化: 进入确认 + 冻结超时 ---*/
uint32_t loop_CAPVD_fast; // 快速 IIR 值 (α=0.5, τ≈28ms)
uint8_t loop_entry_cnt; // 进入确认计数 uint8_t loop_entry_cnt; // 进入确认计数
/*--- M4 优化 V2.3~2.5: 基线冻结超时 ---*/
uint16_t loop_freeze_cnt; // 冻结持续计数 uint16_t loop_freeze_cnt; // 冻结持续计数
uint32_t loop_freeze_ref; // 冻结参考值 uint32_t loop_freeze_ref; // 冻结参考值

83
vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c
View File

@@ -110,7 +110,6 @@ void init_vd_single(Loop154_Unit *unit)
unit->loop_SensLevel = 2; // 默认中灵敏度 unit->loop_SensLevel = 2; // 默认中灵敏度
unit->Flt_Reg = ALFA_CAP1; unit->Flt_Reg = ALFA_CAP1;
unit->loop_CAPVD_fast = 0;
unit->loop_entry_cnt = 0; unit->loop_entry_cnt = 0;
unit->loop_freeze_cnt = 0; unit->loop_freeze_cnt = 0;
unit->loop_freeze_ref = 0; unit->loop_freeze_ref = 0;
@@ -438,57 +437,24 @@ void vd1_task_per_channel(Loop154_Unit *unit)
if (unit->loop_Origin == 0) return; if (unit->loop_Origin == 0) return;
/*================================================================ /*================================================================
* 1. 双路 IIR 滤波 (M4 优化 V2.0) * 1. IIR 滤波 + 斜率限幅 (M4 优化)
* * ALFA_CAP1=79 @10ms → τ≈32ms
* 慢速 IIR (CAPVD): α=18/256 ≈ 0.07, τ≈135ms
* - 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰)
* - 用途: 基线跟踪
*
* 快速 IIR (CAPVD_fast): α=0.5, τ≈28ms
* - 从斜率限幅后的 CAPVD 派生
* - 用途: 进入/离开检测判定
*================================================================*/ *================================================================*/
if (unit->loop_CAPVD == 0) {
/* 1a. 慢速 IIR — 斜率限幅（参考 CAPVD）*/ unit->loop_CAPVD = unit->loop_Value;
{ } else {
uint32_t fast_input; 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 (unit->loop_CAPVD == 0) { if (max_step < 100) max_step = 100;
unit->loop_CAPVD = unit->loop_Value; if (raw_delta > max_step) raw_delta = max_step;
fast_input = unit->loop_Value; if (raw_delta < -max_step) raw_delta = -max_step;
} else { uint32_t clamped_value = (uint32_t)((int32_t)unit->loop_CAPVD + raw_delta);
/*--- 慢速路径：斜率限幅参考 CAPVD (τ=135ms)，保护基线跟踪 ---*/ unit->loop_CAPVD = get_flt_value(clamped_value, unit->loop_CAPVD);
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;
}
} }
/*--- 2. 稳定期：绕过 IIR 和斜率限幅 ---*/
if (!unit->loop_stable) { if (!unit->loop_stable) {
unit->loop_CAPVD = unit->loop_Value; unit->loop_CAPVD = unit->loop_Value;
unit->loop_CAPVD_fast = unit->loop_Value;
update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT, update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT,
&unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN); &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; 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; int32_t dev = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_Origin;
if (dev < (int32_t)(unit->loop_dlt_ORG * 4)) { if (dev < (int32_t)(unit->loop_dlt_ORG * 4)) {
/* CAPVD 未显著高于基线 → 安全跟踪，重置冻结计数 */
unit->loop_freeze_cnt = 0; unit->loop_freeze_cnt = 0;
update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT, update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT,
&unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN); &unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN);
} else { } else {
/* CAPVD 异常偏高 → 冻结跟踪 */
if (unit->loop_freeze_cnt == 0) { if (unit->loop_freeze_cnt == 0) {
unit->loop_freeze_ref = unit->loop_CAPVD; // 记录冻结起始值 unit->loop_freeze_ref = unit->loop_CAPVD;
} else { } else {
/* 稳定性检查: CAPVD 偏离参考值超过 FREEZE_STABILITY_RATE% 则重置 */
int32_t drift = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_freeze_ref; int32_t drift = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_freeze_ref;
if (drift < 0) drift = -drift; if (drift < 0) drift = -drift;
if (drift > (int32_t)(unit->loop_freeze_ref * FREEZE_STABILITY_RATE / 100)) { if (drift > (int32_t)(unit->loop_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
/* 波动过大 → 不是稳定值，重置计数并以当前值重新计时 */
unit->loop_freeze_cnt = 0; unit->loop_freeze_cnt = 0;
unit->loop_freeze_ref = unit->loop_CAPVD; unit->loop_freeze_ref = unit->loop_CAPVD;
} }
} }
unit->loop_freeze_cnt++; unit->loop_freeze_cnt++;
if (unit->loop_freeze_cnt >= FREEZE_TIMEOUT) { if (unit->loop_freeze_cnt >= FREEZE_TIMEOUT) {
/* 超时: CAPVD 持续偏高且稳定 → 环境变化，接受新基线 */
unit->loop_Origin = unit->loop_CAPVD; unit->loop_Origin = unit->loop_CAPVD;
unit->loop_freeze_cnt = 0; unit->loop_freeze_cnt = 0;
unit->loop_freeze_ref = 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", PRINT("Loop%d Baseline timeout, new Origin:%d\n",
unit->loop_num + 1, unit->loop_Origin); unit->loop_num + 1, unit->loop_Origin);
} else { } else {
/* 未超时: 保持冻结，重置累计（防止突然解冻时旧数据污染） */
unit->loop_ORG_CNT = 0; unit->loop_ORG_CNT = 0;
unit->loop_ORG_SUM = 0; unit->loop_ORG_SUM = 0;
} }
} }
} }
/*--- M4 优化: 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车 /*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值 ---*/
* 使用快速 IIR (CAPVD_fast) 提高响应速度 ---*/ if (unit->loop_CAPVD < (unit->loop_Origin - unit->loop_dlt_ORG)) {
if (unit->loop_CAPVD_fast < (unit->loop_Origin - unit->loop_dlt_ORG)) {
unit->loop_entry_cnt++; unit->loop_entry_cnt++;
if (unit->loop_entry_cnt >= ENTRY_CONFIRM) { 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_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_VD_FLAG = 1;
unit->loop_FLAG_IN = 1; unit->loop_FLAG_IN = 1;
unit->loop_entry_cnt = 0; unit->loop_entry_cnt = 0;
unit->loop_freeze_cnt = 0; // 入场时重置冻结状态 unit->loop_freeze_cnt = 0;
unit->loop_freeze_ref = 0; unit->loop_freeze_ref = 0;
if (unit->hold_time > 0) { if (unit->hold_time > 0) {
unit->loop_VD_HOLD = 1; unit->loop_VD_HOLD = 1;