diff --git a/utilities/at32f421_freertos_demo/src/TaskLoop.c b/utilities/at32f421_freertos_demo/src/TaskLoop.c index eb05e51..aea83cb 100644 --- a/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -654,25 +654,31 @@ void vd1_task(void) *================================================================*/ /* 1a. 慢速 IIR — 斜率限幅 */ - if (loop1_CAPVD == 0) { - loop1_CAPVD = loop1_Value; - loop1_CAPVD_fast = loop1_Value; - } else { - /* 斜率限幅: 物理车辆不可能让频率瞬间跳变 > MAX_SLOPE_RATE% */ - 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 (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); - } + { + uint32_t fast_input; // 快速 IIR 的输入:限幅后的原始值,不经慢速 IIR 滞后 - /* 1b. 快速 IIR — α=0.5: (old + new) / 2 */ - if (loop1_CAPVD_fast == 0) { - loop1_CAPVD_fast = loop1_CAPVD; // 首次直接锁定 - } else { - loop1_CAPVD_fast = (loop1_CAPVD_fast + loop1_CAPVD) / 2; + if (loop1_CAPVD == 0) { + loop1_CAPVD = loop1_Value; + fast_input = loop1_Value; + } else { + /* 斜率限幅: 物理车辆不可能让频率瞬间跳变 > MAX_SLOPE_RATE% */ + 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 (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; + } + + /* 1b. 快速 IIR — α=0.5: (old + new) / 2 + * 输入直接用限幅后的原始值,不经过慢速 IIR,保持 τ≈28ms 的响应速度 */ + if (loop1_CAPVD_fast == 0) { + loop1_CAPVD_fast = fast_input; + } else { + loop1_CAPVD_fast = (loop1_CAPVD_fast + fast_input) / 2; + } } /*--- 2. 稳定期:只跟踪基线,不检测车辆 ---*/