diff --git a/utilities/at32f421_freertos_demo/src/TaskLoop.c b/utilities/at32f421_freertos_demo/src/TaskLoop.c index aea83cb..2fc62a8 100644 --- a/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -653,27 +653,38 @@ void vd1_task(void) * - 用途: 进入/离开检测判定 *================================================================*/ - /* 1a. 慢速 IIR — 斜率限幅 */ + /* 1a. 慢速 IIR — 斜率限幅(参考 CAPVD)*/ { - uint32_t fast_input; // 快速 IIR 的输入:限幅后的原始值,不经慢速 IIR 滞后 + uint32_t fast_input; if (loop1_CAPVD == 0) { loop1_CAPVD = loop1_Value; fast_input = loop1_Value; } else { - /* 斜率限幅: 物理车辆不可能让频率瞬间跳变 > MAX_SLOPE_RATE% */ + /*--- 慢速路径:斜率限幅参考 CAPVD (τ=135ms),保护基线跟踪 ---*/ int32_t raw_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD; int32_t max_step = (int32_t)(loop1_CAPVD * MAX_SLOPE_RATE / 100); - if (max_step < 100) max_step = 100; // 最小限幅,防止 origin 很小时锁死 + 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)loop1_CAPVD + raw_delta); loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD); - fast_input = clamped_value; + + /*--- 快速路径:斜率限幅参考 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 - * 输入直接用限幅后的原始值,不经过慢速 IIR,保持 τ≈28ms 的响应速度 */ + /* 1b. 快速 IIR — α=0.5: (old + new) / 2 */ if (loop1_CAPVD_fast == 0) { loop1_CAPVD_fast = fast_input; } else {