diff --git a/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c b/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c index cbd9198..3c89164 100644 --- a/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/vd960Loop/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -449,15 +449,15 @@ void vd1_task_per_channel(Loop154_Unit *unit) * - 用途: 进入/离开检测判定 *================================================================*/ - /* 1a. 慢速 IIR — 斜率限幅 */ + /* 1a. 慢速 IIR — 斜率限幅(参考 CAPVD)*/ { - uint32_t fast_input; // 快速 IIR 的输入:限幅后的原始值,不经慢速 IIR 滞后 + uint32_t fast_input; if (unit->loop_CAPVD == 0) { unit->loop_CAPVD = unit->loop_Value; fast_input = unit->loop_Value; } else { - /* 斜率限幅: 物理车辆不可能让频率瞬间跳变 > MAX_SLOPE_RATE% */ + /*--- 慢速路径:斜率限幅参考 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; @@ -465,11 +465,21 @@ void vd1_task_per_channel(Loop154_Unit *unit) 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); - fast_input = clamped_value; + + /*--- 快速路径:斜率限幅参考 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 - * 输入直接用限幅后的原始值,不经过慢速 IIR,保持 τ≈28ms 的响应速度 */ + /* 1b. 快速 IIR — α=0.5: (old + new) / 2 */ if (unit->loop_CAPVD_fast == 0) { unit->loop_CAPVD_fast = fast_input; } else {