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

feat(vd960Loop): 移植 DLD154V4B V2.0-V2.5 M4 优化 — 双路 IIR + 斜率限幅 + 进入确认 + 冻结超时

Browse Source 
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
每通道独立状态，四路并行运行
This commit is contained in:
wangfq
2026-06-29 11:25:44 +08:00
parent 5a12044c1a
commit 0b951768ec
2 changed files with 128 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,6 +18,7 @@
/*=========================================================================== /*===========================================================================
* 时序参数（每 tick = 50ms，由 TMR15 5ms×10 产生） * 时序参数（每 tick = 50ms，由 TMR15 5ms×10 产生）
* 主循环: vTaskDelay(10ms), M4 优化
*===========================================================================*/ *===========================================================================*/
#define HOLD_TIME (5 * 1200) // 有限存在保持（约 5 分钟） #define HOLD_TIME (5 * 1200) // 有限存在保持（约 5 分钟）
#define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约 4 分钟） #define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约 4 分钟）
@@ -26,11 +27,21 @@
#define PULSE_DELAY 10 // 脉冲宽度 500ms #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 // 稳定期样本数 #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 防抖 * 离开检测: 1 = 平坦性三条件 (CN200910309382), 0 = cnt_release 防抖
*===========================================================================*/ *===========================================================================*/
@@ -91,6 +102,14 @@ 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 + 进入确认 ---*/
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_INI_LOOP;
uint8_t loop_CAP_OK; // 新测量数据就绪 uint8_t loop_CAP_OK; // 新测量数据就绪

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

@@ -110,6 +110,11 @@ 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_freeze_cnt = 0;
unit->loop_freeze_ref = 0;
unit->loop_ORG_CNT = 0; unit->loop_ORG_CNT = 0;
unit->loop_ORG_SUM = 0; unit->loop_ORG_SUM = 0;
@@ -432,15 +437,45 @@ void vd1_task_per_channel(Loop154_Unit *unit)
{ {
if (unit->loop_Origin == 0) return; 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) { if (unit->loop_CAPVD == 0) {
unit->loop_CAPVD = unit->loop_Value; unit->loop_CAPVD = unit->loop_Value;
unit->loop_CAPVD_fast = unit->loop_Value;
} else { } 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) { 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, 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);
unit->stable_cnt++; unit->stable_cnt++;
@@ -456,53 +491,90 @@ void vd1_task_per_channel(Loop154_Unit *unit)
* 无车状态 * 无车状态
*================================================================*/ *================================================================*/
/* 基线跟踪（有车时冻结） /*--- 基线跟踪（仿 TLD-110：有车时冻结）
* 保护: CAPVD 异常上升时暂停跟踪，防止基线被污染 */ * 额外保护: 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;
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 {
unit->loop_ORG_CNT = 0; /* CAPVD 异常偏高 → 冻结跟踪 */
unit->loop_ORG_SUM = 0; 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;
}
} }
} }
/* 进入检测 */ /*--- M4 优化: 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车
if (unit->loop_CAPVD < (unit->loop_Origin - unit->loop_dlt_ORG)) { * 使用快速 IIR (CAPVD_fast) 提高响应速度 ---*/
PRINT("Loop%d Car_In, Value:%d CAPVD:%d Origin:%d dlt:%d\n", if (unit->loop_CAPVD_fast < (unit->loop_Origin - unit->loop_dlt_ORG)) {
unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD, unit->loop_entry_cnt++;
unit->loop_Origin, unit->loop_dlt_ORG); 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_VD_FLAG = 1;
unit->loop_FLAG_IN = 1; unit->loop_FLAG_IN = 1;
if (unit->hold_time > 0) { unit->loop_entry_cnt = 0;
unit->loop_VD_HOLD = 1; unit->loop_freeze_cnt = 0; // 入场时重置冻结状态
} unit->loop_freeze_ref = 0;
at32_led_on(unit->loop_num); if (unit->hold_time > 0) {
unit->loop_VD_HOLD = 1;
}
at32_led_on(unit->loop_num);
if (!unit->SET_SAFE) { if (!unit->SET_SAFE) {
unit->LC_HOLD = 1; unit->LC_HOLD = 1;
} else { } else {
unit->LC_HOLD = 0; unit->LC_HOLD = 0;
} }
if (unit->LC_Reset) if (unit->LC_Reset)
unit->LC_Reset = 0; unit->LC_Reset = 0;
unit->loop_ORG_CNT = 0; unit->loop_ORG_CNT = 0;
unit->loop_ORG_SUM = 0; unit->loop_ORG_SUM = 0;
#if USE_FLATNESS_EXIT #if USE_FLATNESS_EXIT
unit->exit_state = 0; unit->exit_state = 0;
unit->max_slope = 0; unit->max_slope = 0;
unit->max_slope_rate = 0; unit->max_slope_rate = 0;
unit->delta2 = 0; unit->delta2 = 0;
unit->delta3 = 0; unit->delta3 = 0;
unit->slope_flat_cnt = 0; unit->slope_flat_cnt = 0;
unit->flat_ok_cnt = 0; unit->flat_ok_cnt = 0;
#endif #endif
}
} else {
if (unit->loop_entry_cnt > 0) unit->loop_entry_cnt = 0;
} }
} else { } else {
#if USE_FLATNESS_EXIT #if USE_FLATNESS_EXIT
@@ -640,7 +712,7 @@ void loop_task_function(void *pvParameters)
} }
wdt_feed(); wdt_feed();
vTaskDelay(50); // 50ms tick，对齐 DLD154V4B 原始设计 (TMR15 5ms×10) vTaskDelay(10); // 10ms tick, M4 优化: 双路 IIR + 进入确认 + 斜率限幅
} }
} }