refactor: 去掉快速 IIR,单路 IIR ALFA_CAP1=79 @10ms (τ≈32ms)
ALFA_CAP1=79 @10ms, τ≈32ms — 已足够快,无需双路 IIR 的复杂度。 保留所有 V2 保护机制: 斜率限幅、进入确认、冻结超时+稳定性检查。 改动: - ALFA_CAP1: 18→79 - 删除 CAPVD_fast 变量及双路 IIR 逻辑 - 进入检测改用 CAPVD 直接判定(仍保留 ENTRY_CONFIRM=3) - 净删 50 行,架构更简洁
This commit is contained in:
@@ -30,12 +30,11 @@
|
|||||||
* 滤波参数 — M4 优化版
|
* 滤波参数 — M4 优化版
|
||||||
*
|
*
|
||||||
* 与 M1H (8051, 50ms tick) 的关键差异:
|
* 与 M1H (8051, 50ms tick) 的关键差异:
|
||||||
* - tick 提升到 10ms,滤波系数同步调整,保持等效时间常数
|
* - tick 提升到 10ms,ALFA_CAP1=79 @10ms (τ≈32ms, 等效原 79@50ms 的 5× 响应速度)
|
||||||
* - 新增斜率限幅 (MAX_SLOPE_RATE) 过滤瞬态尖峰
|
* - 斜率限幅 (MAX_SLOPE_RATE) 过滤瞬态尖峰
|
||||||
* - 进入确认 (ENTRY_CONFIRM) 替代单次阈值判定
|
* - 进入确认 (ENTRY_CONFIRM) 替代单次阈值判定
|
||||||
*===========================================================================*/
|
*===========================================================================*/
|
||||||
#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% (物理限制, 拒绝 EMI 尖峰)
|
#define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5% (物理限制, 拒绝 EMI 尖峰)
|
||||||
#define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值才判定有车
|
#define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值才判定有车
|
||||||
#define FREEZE_TIMEOUT 1000 // 基线冻结超时: ~10s @ 10ms/tick, 持续偏高且稳定后强制更新基线
|
#define FREEZE_TIMEOUT 1000 // 基线冻结超时: ~10s @ 10ms/tick, 持续偏高且稳定后强制更新基线
|
||||||
@@ -104,11 +103,10 @@ extern uint8_t g_loop_stable; // 线圈数值已稳定 (0=稳定中,
|
|||||||
/* 离开防抖计数器(连续 CAPVD 恢复到阈值以上才释放) */
|
/* 离开防抖计数器(连续 CAPVD 恢复到阈值以上才释放) */
|
||||||
extern uint8_t loop1_cnt_release; // 离开防抖计数
|
extern uint8_t loop1_cnt_release; // 离开防抖计数
|
||||||
|
|
||||||
/* M4 优化: 快速 IIR + 进入确认 */
|
/* 进入确认 (M4 优化) */
|
||||||
extern uint32_t loop1_CAPVD_fast; // 快速 IIR 值 (α=0.5, τ≈28ms, 用于检测)
|
|
||||||
extern uint8_t loop1_entry_cnt; // 进入确认计数
|
extern uint8_t loop1_entry_cnt; // 进入确认计数
|
||||||
extern uint16_t loop1_freeze_cnt; // 基线冻结持续计数(超时后强制更新 Origin)
|
extern uint16_t loop1_freeze_cnt; // 基线冻结持续计数
|
||||||
extern uint32_t loop1_freeze_ref; // 冻结参考值(CAPVD 偏离此值超限则重置计数)
|
extern uint32_t loop1_freeze_ref; // 冻结参考值
|
||||||
#if USE_FLATNESS_EXIT
|
#if USE_FLATNESS_EXIT
|
||||||
extern uint8_t g_exit_state; // 离开检测: 0=追踪斜率, 1=等待平坦
|
extern uint8_t g_exit_state; // 离开检测: 0=追踪斜率, 1=等待平坦
|
||||||
extern uint16_t g_max_slope; // 第一上升坡面最大 |f'|
|
extern uint16_t g_max_slope; // 第一上升坡面最大 |f'|
|
||||||
|
|||||||
@@ -37,7 +37,6 @@ uint16_t loop1_dlt_ORG;
|
|||||||
uint8_t Flt_Reg;
|
uint8_t Flt_Reg;
|
||||||
|
|
||||||
/* M4 优化: 快速 IIR + 进入确认 */
|
/* M4 优化: 快速 IIR + 进入确认 */
|
||||||
uint32_t loop1_CAPVD_fast;
|
|
||||||
uint8_t loop1_entry_cnt;
|
uint8_t loop1_entry_cnt;
|
||||||
uint16_t loop1_freeze_cnt;
|
uint16_t loop1_freeze_cnt;
|
||||||
uint32_t loop1_freeze_ref;
|
uint32_t loop1_freeze_ref;
|
||||||
@@ -342,7 +341,6 @@ void INIT_VD(void)
|
|||||||
g_loop_stable = 0;
|
g_loop_stable = 0;
|
||||||
|
|
||||||
/* M4 优化: 快速 IIR + 进入确认 */
|
/* M4 优化: 快速 IIR + 进入确认 */
|
||||||
loop1_CAPVD_fast = 0;
|
|
||||||
loop1_entry_cnt = 0;
|
loop1_entry_cnt = 0;
|
||||||
loop1_freeze_cnt = 0;
|
loop1_freeze_cnt = 0;
|
||||||
loop1_freeze_ref = 0;
|
loop1_freeze_ref = 0;
|
||||||
@@ -642,26 +640,14 @@ void vd1_task(void)
|
|||||||
if (loop1_Origin == 0) return;
|
if (loop1_Origin == 0) return;
|
||||||
|
|
||||||
/*================================================================
|
/*================================================================
|
||||||
* 1. 双路 IIR 滤波 (M4 优化)
|
* 1. IIR 滤波 + 斜率限幅 (M4 优化)
|
||||||
*
|
*
|
||||||
* 慢速 IIR (CAPVD): α=18/256 ≈ 0.07, τ≈135ms
|
* ALFA_CAP1=79 @10ms → τ≈32ms
|
||||||
* - 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰)
|
* 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰)
|
||||||
* - 用途: 基线跟踪
|
|
||||||
*
|
|
||||||
* 快速 IIR (CAPVD_fast): α=0.5, τ≈28ms
|
|
||||||
* - 从斜率限幅后的 CAPVD 派生
|
|
||||||
* - 用途: 进入/离开检测判定
|
|
||||||
*================================================================*/
|
*================================================================*/
|
||||||
|
|
||||||
/* 1a. 慢速 IIR — 斜率限幅(参考 CAPVD)*/
|
|
||||||
{
|
|
||||||
uint32_t fast_input;
|
|
||||||
|
|
||||||
if (loop1_CAPVD == 0) {
|
if (loop1_CAPVD == 0) {
|
||||||
loop1_CAPVD = loop1_Value;
|
loop1_CAPVD = loop1_Value;
|
||||||
fast_input = loop1_Value;
|
|
||||||
} else {
|
} else {
|
||||||
/*--- 慢速路径:斜率限幅参考 CAPVD (τ=135ms),保护基线跟踪 ---*/
|
|
||||||
int32_t raw_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD;
|
int32_t raw_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD;
|
||||||
int32_t max_step = (int32_t)(loop1_CAPVD * MAX_SLOPE_RATE / 100);
|
int32_t max_step = (int32_t)(loop1_CAPVD * MAX_SLOPE_RATE / 100);
|
||||||
if (max_step < 100) max_step = 100;
|
if (max_step < 100) max_step = 100;
|
||||||
@@ -669,34 +655,11 @@ void vd1_task(void)
|
|||||||
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)loop1_CAPVD + raw_delta);
|
uint32_t clamped_value = (uint32_t)((int32_t)loop1_CAPVD + raw_delta);
|
||||||
loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD);
|
loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD);
|
||||||
|
|
||||||
/*--- 快速路径:斜率限幅参考 CAPVD_fast (τ=28ms),限幅窗口随快速值扩展
|
|
||||||
* 解决 ALFA_CAP1=18 时 CAPVD 移动太慢限制限幅天花板的问题 ---*/
|
|
||||||
if (loop1_CAPVD_fast == 0) {
|
|
||||||
fast_input = clamped_value;
|
|
||||||
} else {
|
|
||||||
int32_t f_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD_fast;
|
|
||||||
int32_t f_max = (int32_t)(loop1_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)loop1_CAPVD_fast + f_delta);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 1b. 快速 IIR — α=0.5: (old + new) / 2 */
|
/*--- 2. 稳定期:绕过 IIR 和斜率限幅,直接用 Value 快速收敛 ---*/
|
||||||
if (loop1_CAPVD_fast == 0) {
|
|
||||||
loop1_CAPVD_fast = fast_input;
|
|
||||||
} else {
|
|
||||||
loop1_CAPVD_fast = (loop1_CAPVD_fast + fast_input) / 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*--- 2. 稳定期:只跟踪基线,不检测车辆 ---*/
|
|
||||||
if (!g_loop_stable) {
|
if (!g_loop_stable) {
|
||||||
/* 稳定期内不做斜率限幅和 IIR — 直接用 Value 快速收敛到真实基线 */
|
|
||||||
loop1_CAPVD = loop1_Value;
|
loop1_CAPVD = loop1_Value;
|
||||||
loop1_CAPVD_fast = loop1_Value;
|
|
||||||
|
|
||||||
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
|
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
|
||||||
&loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN);
|
&loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN);
|
||||||
@@ -714,33 +677,27 @@ void vd1_task(void)
|
|||||||
*================================================================*/
|
*================================================================*/
|
||||||
|
|
||||||
/*--- 基线跟踪(仿 TLD-110:有车时冻结)
|
/*--- 基线跟踪(仿 TLD-110:有车时冻结)
|
||||||
* 额外保护: CAPVD 异常上升时暂停跟踪,防止基线被污染
|
* 额外保护: CAPVD 异常上升时暂停跟踪 → 冻结超时 + 稳定性检查 ---*/
|
||||||
* 否则车辆驶入时若 Xn 先增大,Origin 被顶上去后无法释放 ---*/
|
|
||||||
loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel]) >> 16;
|
loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel]) >> 16;
|
||||||
{
|
{
|
||||||
int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin;
|
int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin;
|
||||||
if (dev < (int32_t)(loop1_dlt_ORG * 4)) {
|
if (dev < (int32_t)(loop1_dlt_ORG * 4)) {
|
||||||
/* CAPVD 未显著高于基线 → 安全跟踪,重置冻结计数 */
|
|
||||||
loop1_freeze_cnt = 0;
|
loop1_freeze_cnt = 0;
|
||||||
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
|
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
|
||||||
&loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN);
|
&loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN);
|
||||||
} else {
|
} else {
|
||||||
/* CAPVD 异常偏高 → 冻结跟踪 */
|
|
||||||
if (loop1_freeze_cnt == 0) {
|
if (loop1_freeze_cnt == 0) {
|
||||||
loop1_freeze_ref = loop1_CAPVD; // 记录冻结起始值
|
loop1_freeze_ref = loop1_CAPVD;
|
||||||
} else {
|
} else {
|
||||||
/* 稳定性检查: CAPVD 偏离参考值超过 FREEZE_STABILITY_RATE% 则重置 */
|
|
||||||
int32_t drift = (int32_t)loop1_CAPVD - (int32_t)loop1_freeze_ref;
|
int32_t drift = (int32_t)loop1_CAPVD - (int32_t)loop1_freeze_ref;
|
||||||
if (drift < 0) drift = -drift;
|
if (drift < 0) drift = -drift;
|
||||||
if (drift > (int32_t)(loop1_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
|
if (drift > (int32_t)(loop1_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
|
||||||
/* 波动过大 → 不是稳定值,重置计数并以当前值重新计时 */
|
|
||||||
loop1_freeze_cnt = 0;
|
loop1_freeze_cnt = 0;
|
||||||
loop1_freeze_ref = loop1_CAPVD;
|
loop1_freeze_ref = loop1_CAPVD;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
loop1_freeze_cnt++;
|
loop1_freeze_cnt++;
|
||||||
if (loop1_freeze_cnt >= FREEZE_TIMEOUT) {
|
if (loop1_freeze_cnt >= FREEZE_TIMEOUT) {
|
||||||
/* 超时: CAPVD 持续偏高且稳定 → 环境变化,接受新基线 */
|
|
||||||
loop1_Origin = loop1_CAPVD;
|
loop1_Origin = loop1_CAPVD;
|
||||||
loop1_freeze_cnt = 0;
|
loop1_freeze_cnt = 0;
|
||||||
loop1_freeze_ref = 0;
|
loop1_freeze_ref = 0;
|
||||||
@@ -748,30 +705,25 @@ void vd1_task(void)
|
|||||||
loop1_ORG_SUM = 0;
|
loop1_ORG_SUM = 0;
|
||||||
PRINT("Baseline timeout update, new Origin:%d\\n", loop1_Origin);
|
PRINT("Baseline timeout update, new Origin:%d\\n", loop1_Origin);
|
||||||
} else {
|
} else {
|
||||||
/* 未超时: 保持冻结,重置累计(防止突然解冻时旧数据污染) */
|
|
||||||
loop1_ORG_CNT = 0;
|
loop1_ORG_CNT = 0;
|
||||||
loop1_ORG_SUM = 0;
|
loop1_ORG_SUM = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*--- M4 优化: 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车
|
/*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车 ---*/
|
||||||
* 使用快速 IIR (CAPVD_fast) 提高响应速度
|
if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) {
|
||||||
* 配合确认机制防止瞬态干扰误触发 ---*/
|
|
||||||
if (loop1_CAPVD_fast < (loop1_Origin - loop1_dlt_ORG)) {
|
|
||||||
loop1_entry_cnt++;
|
loop1_entry_cnt++;
|
||||||
if (loop1_entry_cnt >= ENTRY_CONFIRM) {
|
if (loop1_entry_cnt >= ENTRY_CONFIRM) {
|
||||||
PRINT("Car_In, Value:%d, CAPVD:%d, CAPVD_fast:%d, Origin:%d, dlt:%d\\n",
|
PRINT("Car_In, Value:%d, CAPVD:%d, Origin:%d, dlt:%d\\n",
|
||||||
loop1_Value, loop1_CAPVD, loop1_CAPVD_fast,
|
loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG);
|
||||||
loop1_Origin, loop1_dlt_ORG);
|
|
||||||
|
|
||||||
loop1_VD_FLAG = 1;
|
loop1_VD_FLAG = 1;
|
||||||
loop1_FLAG_IN = 1;
|
loop1_FLAG_IN = 1;
|
||||||
loop1_entry_cnt = 0;
|
loop1_entry_cnt = 0;
|
||||||
loop1_freeze_cnt = 0; // 入场时重置冻结状态
|
loop1_freeze_cnt = 0;
|
||||||
loop1_freeze_ref = 0;
|
loop1_freeze_ref = 0;
|
||||||
|
|
||||||
/* 有限存在计时(非安全复位模式下) */
|
|
||||||
if (!SET_SAFE) {
|
if (!SET_SAFE) {
|
||||||
loop1_LC_HOLD = 1;
|
loop1_LC_HOLD = 1;
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
Reference in New Issue
Block a user