#include "TaskLoop.h" #include "at32f421_board.h" #include "cmcng.h" #include "FreeRTOS.h" #include "task.h" #include /*=========================================================================== * 灵敏度表 — 对齐 M1H 参考实现 * 进入阈值 = Origin × SensTable[SENS] / 65536 * 离开阈值（滞回）= Origin × SensTable_1[SENS] / 65536 * SENS: 0=低灵敏, 3=高灵敏 *===========================================================================*/ #if USE_PLANB_TABLE /* 方案B (V4.07): 现场实测调校 — SENS=3 进入 21→25、释放 14→12 接近 PD132T 最高灵敏手感 * 其他三档按等比 (进入 ×25/21=1.190, 释放 ×12/14=0.857) 推算: * {300,60,32,21}→{357,71,38,25} {60,32,21,14}→{51,27,18,12} * V4.07: SENS=0 进入 357→337 现场回调 (等比值偏低档触发过深, 折中) * 物理: 进入加深(触发更严) + 释放调浅(释放更晚), 滞回 66%→47% * ⚠ 与 PD132T 表值不再一致(行为等效) */ const uint16_t SensTable[4] = {337, 71, 38, 25}; // 方案B: 进入调深 (V4.07: SENS=0 回调 357→337) const uint16_t SensTable_1[4] = { 51, 27, 18, 12}; // 方案B: 释放调浅 (V4.06 现场调校) #else const uint16_t SensTable[4] = {300, 60, 32, 21}; // V2.11: 全档与 PD132T SensTable 完全一致 (0.458%/0.092%/0.049%/0.032% df/f) const uint16_t SensTable_1[4] = { 60, 32, 21, 14}; // V2.11: 离开分档与 PD132T 完全一致 (SENUP 标贴OFF=SET_ASB=1=出厂默认) #endif /*=========================================================================== * 全局状态变量 — 捕获 & 测量 *===========================================================================*/ uint32_t g_sys_freq = 0; uint8_t g_input_div = 1; uint16_t loop1_Xn; uint16_t loop1_CapThis; uint16_t loop1_CapLast; uint16_t loop1_LPCNT; uint16_t loop1_CapCnt; uint32_t loop1_CapSum; uint32_t loop1_Value; uint32_t loop1_CAPVD; uint32_t loop1_CAPVD_slow = 0; // 释放判定慢滤波值 (方案A) uint32_t loop1_Origin; uint32_t loop1_ORG_SUM; uint16_t loop1_ORG_CNT; uint16_t loop1_dlt_ORG; uint8_t Flt_Reg; volatile uint16_t g_wdg_counter = 0; // 看门狗活性计数 (TMR15 ISR 5ms 递增) /* M4 优化: 快速 IIR + 进入确认 */ uint8_t loop1_entry_cnt; uint16_t loop1_freeze_cnt; uint32_t loop1_freeze_ref; /*=========================================================================== * 全局状态变量 — 标志位 *===========================================================================*/ uint8_t loop1_INI_LOOP; uint8_t loop1_CAP_OK; uint8_t loop1_VD_FLAG; uint8_t loop1_VD_HOLD; uint8_t loop1_RF_FLAG; uint8_t loop1_LOOP_OK; uint8_t loop1_LOOP_OK0; uint8_t loop1_FLAG_IN; uint8_t loop1_FLAG_OUT; uint8_t loop1_FLAG_PLUSE; uint8_t loop1_SensLevel; uint8_t loop1_cnt_release; uint8_t g_loop_stable; /* V4.23: 稳定期(上电自检)计数器 — 基准值稳定后才结束自检 */ uint16_t g_stable_cnt = 0; // 稳定期累计样本 (每次 CAP_OK +1) uint16_t g_settle_cnt = 0; // 基准稳定连续窗口计数 (窗口=100样本≈1s) #if USE_FLATNESS_EXIT uint8_t g_exit_state; uint16_t g_max_slope; uint16_t g_max_slope_rate; uint16_t g_delta2; uint16_t g_delta3; int32_t g_prev_capvd; int32_t g_prev_first_deriv; uint8_t g_slope_flat_cnt; uint8_t g_flat_ok_cnt; #endif /*=========================================================================== * 全局状态变量 — 计数器 *===========================================================================*/ uint16_t Hold_CNT = 0; uint8_t loop1_INCNT = 0; uint8_t loop1_OUTCNT = 0; uint8_t TM1cnt = 0; /*=========================================================================== * 拨码开关状态 *===========================================================================*/ uint8_t SET_PLUS = 0; uint8_t SET_DLY = 0; // 离开延时: 0=无, 1=500ms uint8_t SET_SAFE = 0; /*=========================================================================== * 拨码开关去抖变量 *===========================================================================*/ uint8_t sw0, swok, sw1, swcnt, SENS = 0, SENS_LAST = 0; /*=========================================================================== * 安全复位相关 *===========================================================================*/ uint8_t loop1_LC_HOLD = 0; uint8_t loop1_LC_Reset = 0; uint32_t LC_Hold_CNT = 0; uint16_t g_safe_max_cnt = LC_HOLD_TIME; /*=========================================================================== * 故障指示相关 — 黄灯 *===========================================================================*/ uint8_t g_loop_power_up_state = 0; uint8_t g_disconnect_count = 0; uint8_t g_disconnect_active = 0; uint8_t g_fault_phase = 0; uint16_t g_fault_tick = 0; /*=========================================================================== * 调试计数器 *===========================================================================*/ uint32_t g_xn_counter = 0; /*=========================================================================== * 拨码开关轮询（5 次去抖，对齐 M1H） *===========================================================================*/ void poll_sw_state(void) { sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN) << 4; sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN) << 3; sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN) << 2; sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN) << 1; sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN) << 0; if (sw0 == sw1) { if (swok != sw0) { swcnt++; if (swcnt > 5) { // 对齐 M1H: 5 次确认 swok = sw0; SET_PLUS = swok & 0x04; SET_DLY = (swok & 0x08) >> 3; SET_SAFE = (swok & 0x10) >> 4; } } } else { sw1 = sw0; swcnt = 0; } SENS = swok & 0x03; if (SENS != SENS_LAST) { PRINT("SENS changed: %02X -> %02X, resetting...\n", SENS_LAST, SENS); nvic_system_reset(); } } /*=========================================================================== * 一阶 IIR 低通滤波器（对齐 M1H 公式） * CAPVD_new = α × new_value + (1-α) × CAPVD_old * α = Flt_Reg / 256 = 79/256 ≈ 0.3086 *===========================================================================*/ uint32_t get_flt_value(uint32_t new_value, uint32_t last_Value) { uint32_t value_Flt; uint32_t delta = (new_value > last_Value) ? (new_value - last_Value) : (last_Value - new_value); uint32_t scaled_delta = (delta * (uint32_t)Flt_Reg) >> 8; if (new_value > last_Value) { value_Flt = last_Value + scaled_delta; } else { value_Flt = last_Value - scaled_delta; } return value_Flt; } /* 释放慢滤波(方案 A): 独立系数 RELEASE_ALFA, 不复用 Flt_Reg 全局 * 复刻 PD132T 滤波路径(Value -> IIR), tau~142ms @10ms */ uint32_t get_flt_value_slow(uint32_t new_value, uint32_t last_value) { uint32_t value_Flt; uint32_t delta = (new_value > last_value) ? (new_value - last_value) : (last_value - new_value); uint32_t scaled_delta = (delta * (uint32_t)RELEASE_ALFA) >> 8; if (new_value > last_value) { value_Flt = last_value + scaled_delta; } else { value_Flt = last_value - scaled_delta; } return value_Flt; } /*=========================================================================== * 滑动平均基线更新 * 累加 new_value，当计数达到 window 时计算平均值并重置 * 返回 1 表示本轮更新了平均值 *===========================================================================*/ uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt, uint32_t* p_origin, uint32_t new_value, uint16_t window) { if (!p_sum || !p_cnt || !p_origin || window == 0) { return 0; } *p_sum += new_value; (*p_cnt)++; if (*p_cnt >= window) { *p_origin = *p_sum / window; *p_cnt = 0; *p_sum = 0; return 1; } return 0; } /*=========================================================================== * 红灯呼吸 (LEDA, PB1) — 每 5ms 由 TMR15 ISR 调用 *===========================================================================*/ void LEDA_ON_OFF(void) { LED_RED_GPIO->odt ^= LED_RED_PIN; } /*=========================================================================== * 绿灯指示 — LEDA (PA9) * * 由 TMR15 ISR 每 5ms 驱动一次，是绿灯的唯一控制入口。 * * 模式： * 自检/稳定中 (loop1_INI_LOOP || 线圈未连 || !g_loop_stable): * → 慢闪 (200ms 亮 / 200ms 灭) * 正常工作: * → 有车亮 (VD_FLAG==1), 无车灭 *===========================================================================*/ void poll_green_led(void) { #define GREEN_SLOW_HALF 40 // 200ms (40 × 5ms) static uint16_t _slow_tick = 0; /*--- 线圈断开中 → 灭（黄灯快闪负责故障指示） ---*/ if (g_disconnect_active) { LEDA_OFF; _slow_tick = 0; return; } /*--- 自检阶段：慢闪 ---*/ if (loop1_INI_LOOP || !g_loop_power_up_state || !g_loop_stable) { _slow_tick++; if (_slow_tick >= GREEN_SLOW_HALF) { _slow_tick = 0; LED_GREEN_GPIO->odt ^= LED_GREEN_PIN; // toggle } return; } /*--- 正常工作：车辆存在指示 ---*/ _slow_tick = 0; if (loop1_VD_FLAG) LEDA_ON; else LEDA_OFF; } /*=========================================================================== * 黄灯故障指示 — LEDC (PA10) * * 由 TMR15 ISR 每 5ms 驱动一次。 * * 模式判定： * 当前线圈断开 或 上电后从未接线圈: * → 快闪 (200ms 亮 / 200ms 灭) * 线圈已恢复连接, 且上电后有断开记录 (g_disconnect_count >= 1): * → N 短闪 (80ms亮/200ms灭 × N, 1.2s间隔, N=断开次数≤3) * 线圈正常连接, 无断开记录: * → 灭 *===========================================================================*/ void poll_yellow_led(void) { #define FLT_ON_SHORT 16 // 80ms (16 × 5ms) #define FLT_GAP_SHORT 40 // 200ms #define FLT_GAP_LONG 240 // 1.2s (240 × 5ms) #define FLT_FAST_HALF 40 // 200ms 快闪半周期 g_fault_tick++; /*--- 模式1: 快闪 — 当前断开中 或 从无线圈上电 ---*/ if (g_disconnect_active || !g_loop_power_up_state) { if (g_fault_tick >= FLT_FAST_HALF) { g_fault_tick = 0; LED_YELLOW_GPIO->odt ^= LED_YELLOW_PIN; // toggle } return; } uint8_t N = g_disconnect_count; if (N == 0) { /*--- 模式2: 无断开记录 → 灭 ---*/ LED_YELLOW_OFF; g_fault_tick = 0; g_fault_phase = 0; return; } /*--- 模式3: N 短闪 (连接中, 但有断开历史) ---*/ if (N > 3) N = 3; uint8_t phases_total = N * 2; // N 个 ON_short + N 个 GAP_short uint16_t duration; if (g_fault_phase < phases_total) { duration = (g_fault_phase & 1) ? FLT_GAP_SHORT : FLT_ON_SHORT; if (g_fault_phase & 1) LED_YELLOW_OFF; else LED_YELLOW_ON; } else { /* LONG 间隔 */ duration = FLT_GAP_LONG; LED_YELLOW_OFF; } if (g_fault_tick >= duration) { g_fault_tick = 0; g_fault_phase++; if (g_fault_phase > phases_total) g_fault_phase = 0; } } /*=========================================================================== * 看门狗 (IWDT) - V2.10 新增 * 双保险: TMR15 ISR 5ms 递增 g_wdg_counter, 主循环 poll_wdg() 每 500ms 喂狗 * ISR 死 -> counter 不涨 -> 永不喂狗; 主循环死 -> poll_wdg 不执行 -> 超时复位 *===========================================================================*/ void wdt_init(void) { wdt_register_write_enable(TRUE); wdt_divider_set((wdt_division_type)WDT_DIV); // WDT_DIV=2 -> WDT_CLK_DIV_16 wdt_reload_value_set(WDT_RLD); wdt_enable(); wdt_counter_reload(); // 使能后立即喂一次, 给 FreeRTOS 启动留足时间 g_wdg_counter = 0; } void poll_wdg(void) { if (g_wdg_counter >= WDT_COUNTER_LIMIT) { // 100 x 5ms = 500ms g_wdg_counter = 0; wdt_counter_reload(); } } /*=========================================================================== * 初始化所有状态变量（对齐 M1H INIT_VD） *===========================================================================*/ void INIT_VD(void) { loop1_INCNT = 0; loop1_OUTCNT = 0; loop1_CapCnt = 0; loop1_CapLast = 0; loop1_LPCNT = 0; loop1_INI_LOOP = 1; loop1_VD_FLAG = 0; loop1_RF_FLAG = 0; loop1_LOOP_OK = 1; loop1_LOOP_OK0 = 0; loop1_CAP_OK = 0; loop1_CAPVD = 0; loop1_CAPVD_slow = 0; // 方案A: 释放慢滤波复位 loop1_SensLevel = 2; // 默认中灵敏度 Flt_Reg = ALFA_CAP1; // 79 loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; loop1_FLAG_IN = 0; loop1_FLAG_OUT = 0; loop1_FLAG_PLUSE = 0; loop1_cnt_release = 0; g_loop_stable = 0; g_stable_cnt = 0; // V4.23: 自检计数从零开始 g_settle_cnt = 0; /* M4 优化: 快速 IIR + 进入确认 */ loop1_entry_cnt = 0; loop1_freeze_cnt = 0; loop1_freeze_ref = 0; #if USE_FLATNESS_EXIT g_exit_state = 0; g_max_slope = 0; g_max_slope_rate = 0; g_delta2 = 0; g_delta3 = 0; g_prev_capvd = 0; g_prev_first_deriv = 0; g_slope_flat_cnt = 0; g_flat_ok_cnt = 0; #endif loop1_LC_HOLD = 0; loop1_LC_Reset = 0; LC_Hold_CNT = 0; Hold_CNT = 0; g_loop_power_up_state = 0; g_disconnect_count = 0; g_disconnect_active = 0; g_fault_phase = 0; g_fault_tick = 0; /* 读取拨码初始状态 */ sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN) << 4; sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN) << 3; sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN) << 2; sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN) << 1; sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN) << 0; swok = sw0; sw1 = sw0; SENS = swok & 0x03; SENS_LAST = SENS; swcnt = 0; switch (SENS) { case 0x00: loop1_SensLevel = 0; break; case 0x02: loop1_SensLevel = 1; break; case 0x01: loop1_SensLevel = 2; break; case 0x03: loop1_SensLevel = 3; break; default: loop1_SensLevel = 0; break; } SET_PLUS = swok & 0x04; SET_DLY = (swok & 0x08) >> 3; SET_SAFE = (swok & 0x10) >> 4; } /*=========================================================================== * TMR3 输入捕获中断 — 线圈频率测量（对齐 M1H CAP0 ISR） * * PA7 → TIM3_CH2, 上升沿触发。 * 每次捕获中断读取 CCR2，计算相邻边沿差 Xn。 * 累加 LPCNT 次后产生一个测量窗口的 Value。 *===========================================================================*/ void TMR3_GLOBAL_IRQHandler(void) { if (tmr_interrupt_flag_get(TMR3, TMR_C2_FLAG) != RESET) { tmr_flag_clear(TMR3, TMR_C2_FLAG); loop1_CapThis = tmr_channel_value_get(TMR3, TMR_SELECT_CHANNEL_2); if (!loop1_RF_FLAG) { /* 首次捕获，记录基准 */ loop1_CapLast = loop1_CapThis; loop1_RF_FLAG = 1; } else { /* 计算相邻边沿周期差（16bit 溢出回绕处理） */ if (loop1_CapThis > loop1_CapLast) { loop1_Xn = loop1_CapThis - loop1_CapLast; } else { loop1_Xn = (0x10000 - loop1_CapLast) + loop1_CapThis; } g_xn_counter++; loop1_CapLast = loop1_CapThis; loop1_CapCnt++; if (loop1_INI_LOOP) { /*--- 初始化阶段：计算自适应测量窗口大小 ---*/ if (loop1_LPCNT == 0) { if (loop1_CapCnt > 10) { /* * 自适应测量窗口: MEASUREMENT_BASE / Xn * 使归一化 Value ≈ 131072，无需后续 >>6 */ loop1_LPCNT = MEASUREMENT_BASE / loop1_Xn; PRINT("First_calc_loop1_LPCNT:%d, loop1_Xn:%d\n", loop1_LPCNT, loop1_Xn); if (loop1_LPCNT == 0) { loop1_LPCNT = 100; } loop1_CAPVD = 0; loop1_CapSum = 0; loop1_CapCnt = 0; } } else { /* 第一个窗口：直接作为基线 Origin */ loop1_CapSum += loop1_Xn; if (loop1_CapCnt >= loop1_LPCNT) { loop1_CAPVD = loop1_CapSum; loop1_Origin = loop1_CAPVD; // 不再 >>6 PRINT("First_capSum:%d, Origin:%d\n", loop1_CapSum, loop1_Origin); loop1_INI_LOOP = 0; loop1_CapSum = 0; loop1_Value = 0; loop1_CapCnt = 0; } } } else { /*--- 正常运行：累加到测量窗口 ---*/ loop1_CapSum += loop1_Xn; if (loop1_CapCnt >= loop1_LPCNT) { loop1_Value = loop1_CapSum; loop1_CAP_OK = 1; loop1_CapSum = 0; loop1_CapCnt = 0; loop1_INI_LOOP = 0; } } } } } /*=========================================================================== * TMR15 定时器中断 — 5ms tick，主状态机（对齐 M1H Timer1 ISR） * * 职责： * 1. 50ms tick 计数器（TM1cnt ÷ 10） * 2. 进入/离开/脉冲时序状态机 * 3. 有限存在超时 / 安全复位超时 * 4. 线圈载波检测 (RF_FLAG) 及继电器输出刷新 * 5. 呼吸灯、故障灯驱动 *===========================================================================*/ void TMR15_GLOBAL_IRQHandler(void) { static uint16_t _counter1_init = 0; if (tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) { /*--- 看门狗活性计数 (5ms) ---*/ g_wdg_counter++; /*--- 初始化阶段红灯快闪 (200ms 周期) ---*/ if (!g_loop_power_up_state) { _counter1_init++; if (_counter1_init >= 40) { // 40 × 5ms = 200ms _counter1_init = 0; LEDA_ON_OFF(); } } /*--- 50ms tick 分频 ---*/ TM1cnt++; if (TM1cnt >= 10) { // 10 × 5ms = 50ms TM1cnt = 0; /*================================================================ * 时序状态机（对齐 M1H Timer1 ISR） * * IN_DELAY OUT_DELAY * [空闲] ──有车──→ [进入延时] ──→ [有车确认] ──离开──→ * ↑ │ * └──脉冲结束──── [脉冲输出] ←──── [离开延时] ←──────┘ * PULSE_DELAY *================================================================*/ /* FLAG_IN: 进入延时 500ms */ if (loop1_FLAG_IN) { loop1_INCNT++; if (loop1_INCNT > IN_DELAY) { loop1_FLAG_IN = 0; loop1_INCNT = 0; } } /* FLAG_OUT: 离开延时（SW_4=ON 时 500ms，OFF 时 0） */ if (loop1_FLAG_OUT) { if (!SET_DLY) { loop1_OUTCNT++; if (loop1_OUTCNT > OUT_DELAY) { loop1_FLAG_OUT = 0; loop1_FLAG_PLUSE = 1; loop1_OUTCNT = 0; } } else { /* 无离开延时：立即触发脉冲 */ loop1_FLAG_OUT = 0; loop1_FLAG_PLUSE = 1; loop1_OUTCNT = 0; } } /* FLAG_PLUSE: 脉冲宽度 500ms */ if (loop1_FLAG_PLUSE) { loop1_OUTCNT++; if (loop1_OUTCNT > PULSE_DELAY) { loop1_FLAG_PLUSE = 0; loop1_OUTCNT = 0; } } /* 有限存在超时 */ if (loop1_VD_HOLD) { Hold_CNT++; if (Hold_CNT > HOLD_TIME) { loop1_VD_HOLD = 0; Hold_CNT = 0; if (loop1_VD_FLAG) { /* 超时：强制释放 */ RLY1_OFF; RLY2_OFF; } } } /* 安全复位超时 */ if (loop1_LC_HOLD) { LC_Hold_CNT++; if (LC_Hold_CNT > g_safe_max_cnt) { loop1_LC_Reset = 1; loop1_INI_LOOP = 1; loop1_LOOP_OK0 = 0; g_loop_stable = 0; #if USE_FLATNESS_EXIT g_exit_state = 0; g_max_slope = 0; g_max_slope_rate = 0; #endif LC_Hold_CNT = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; } } } /*================================================================ * 线圈载波检测 & 继电器输出刷新（每 5ms） * RF_FLAG 由 TMR3 ISR 置位，此处消费后清除。 *================================================================*/ if (loop1_RF_FLAG) { loop1_LOOP_OK = 1; loop1_RF_FLAG = 0; if (loop1_LC_Reset == 0) { /* 继电器 2（辅助/方向） */ if (loop1_VD_FLAG) RLY2_ON; else RLY2_OFF; /* 继电器 1（主输出） */ if (SET_PLUS) { /* 存在输出模式：有车或离开延时中 */ if (loop1_VD_FLAG || loop1_FLAG_OUT) RLY1_ON; else RLY1_OFF; } else { /* 脉冲输出模式：脉冲期间吸合 */ if (loop1_FLAG_PLUSE) RLY1_ON; else RLY1_OFF; } } } else { loop1_LOOP_OK0 = loop1_LOOP_OK; loop1_LOOP_OK = 0; } /*--- 硬件驱动：呼吸灯 + 指示灯 ---*/ poll_red_pwm(); poll_green_led(); poll_yellow_led(); tmr_flag_clear(TMR15, TMR_OVF_FLAG); } } /*=========================================================================== * 调试打印辅助 — V4.20 debug 打印重组 (2026-09-01) * * calc_freq_khz: 由窗口量反算线圈振荡频率 * f = 捕获时钟(60MHz) / Xn_avg, Xn_avg = CAPVD / LPCNT * → f_kHz = 60000 × LPCNT / CAPVD * 无车: CAPVD≈131072, LPCNT≈218 → ≈99.8kHz; 有车: Xn↓ → f↑ *===========================================================================*/ uint32_t g_slope_limit_cnt = 0; // 斜率限幅截断计数 (EMI/干扰线索, 周期打印带出) static uint32_t calc_freq_khz(uint32_t capvd, uint16_t lp_cnt) { if (capvd == 0) return 0; return (60000UL * lp_cnt) / capvd; } /*=========================================================================== * vd1_task — 核心检测算法（对齐 M1H VD1_TASK） * * 每次 loop1_CAP_OK 时调用一次。 * 流程： * 1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD * 2. 无车时：基线跟踪 + 进入检测 * 3. 有车时：离开检测（带滞回） *===========================================================================*/ void vd1_task(void) { #define STABLE_SAMPLES 128 // 稳定期最少样本数 (128×10ms≈1.3s) #define STABLE_ORIGIN_PPT 1 // 基准稳定判据: 窗口均值漂移 ≤ Origin×1/1000 (0.1%) #define STABLE_SETTLE_WINDOWS 2 // 连续 2 个窗口(每窗100样本≈1s)漂移达标 → 判稳 #define STABLE_MAX_SAMPLES 500 // 硬兜底: 5s 未判稳强制结束自检(防绿灯无限闪) if (loop1_Origin == 0) return; /*================================================================ * 1. IIR 滤波 + 斜率限幅 (M4 优化) * * ALFA_CAP1=79 @10ms → τ≈32ms * 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰) *================================================================*/ if (loop1_CAPVD == 0) { loop1_CAPVD = loop1_Value; } else { 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; if (raw_delta > max_step) { raw_delta = max_step; g_slope_limit_cnt++; } if (raw_delta < -max_step) { raw_delta = -max_step; g_slope_limit_cnt++; } uint32_t clamped_value = (uint32_t)((int32_t)loop1_CAPVD + raw_delta); loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD); } #if USE_SLOW_RELEASE /* 方案A: 释放慢滤波独立跟踪(吃原始 Value, 复刻 PD132T 滤波路径) */ if (loop1_CAPVD_slow == 0) { loop1_CAPVD_slow = loop1_Value; } else { loop1_CAPVD_slow = get_flt_value_slow(loop1_Value, loop1_CAPVD_slow); } #endif /*--- 2. 稳定期：绕过 IIR 和斜率限幅，小窗口快速收敛; * 基准值稳定(连续窗口 Origin 漂移达标)后才结束自检 ---*/ if (!g_loop_stable) { uint32_t _prev_origin = loop1_Origin; uint8_t _win_done; loop1_CAPVD = loop1_Value; #if USE_SLOW_RELEASE loop1_CAPVD_slow = loop1_Value; // 方案A: 稳定期同步直锁 #endif _win_done = update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, 100); g_stable_cnt++; /* V4.23: 每窗(100样本≈1s)完成时比较 Origin 均值漂移; * 漂移 ≤ Origin×STABLE_ORIGIN_PPT/1000 → settle 计数+1, 否则清零 * (首个窗口常与 TMR3 首窗 Origin 偏差大, 自然多等一窗再判稳) */ if (_win_done) { uint32_t _drift = (loop1_Origin > _prev_origin) ? (loop1_Origin - _prev_origin) : (_prev_origin - loop1_Origin); uint32_t _band = (uint32_t)(loop1_Origin * STABLE_ORIGIN_PPT / 1000); if (_drift <= _band) { if (g_settle_cnt < 0xFFFF) g_settle_cnt++; } else { g_settle_cnt = 0; } } /* 判稳条件: 最少样本 + 连续 2 窗基准稳定; 硬兜底 STABLE_MAX_SAMPLES */ if ((g_stable_cnt >= STABLE_SAMPLES && g_settle_cnt >= STABLE_SETTLE_WINDOWS) || g_stable_cnt >= STABLE_MAX_SAMPLES) { g_loop_stable = 1; g_stable_cnt = 0; // 计数退出时清零: 二次稳定期(安全复位)从零开始 g_settle_cnt = 0; PRINT("Loop stable, Origin:%d\n", loop1_Origin); } return; } if (!loop1_VD_FLAG) { /*================================================================ * 无车状态 *================================================================*/ /*--- 基线跟踪（仿 TLD-110：有车时冻结） * V4.20 修复: 双向对称保护 — 负偏差(负variation)也冻结基线 * 原单边保护只挡 dev>+4×dlt, 负variation铁块(磁导率主导, CAPVD>Origin) * 会把 Origin 污染抬高 → 铁块离开时假进入 → 释放线抬高 → 锁死不释放 * 修复: ① 偏差窗口对称 [-4×dlt, +4×dlt] * ② 负偏差(dev>0, CAPVD高于Origin) 只冻结, 永不超时更新 Origin *---*/ loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel]) >> 16; { int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin; int32_t freeze_band = (int32_t)(loop1_dlt_ORG * 4); if (dev < freeze_band && dev > -freeze_band) { /* 对称窗口内 → 正常基线跟踪 */ loop1_freeze_cnt = 0; loop1_freeze_ref = 0; // V4.20: 清残留参考, 打印 FRZ 干净 update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN); } else if (dev < 0) { /* 正variation (CAPVD低于Origin, 疑似车辆) → 冻结, 保留原超时兜底更新 */ if (loop1_freeze_cnt == 0) { loop1_freeze_ref = loop1_CAPVD; } else { int32_t drift = (int32_t)loop1_CAPVD - (int32_t)loop1_freeze_ref; if (drift < 0) drift = -drift; if (drift > (int32_t)(loop1_freeze_ref * FREEZE_STABILITY_RATE / 100)) { loop1_freeze_cnt = 0; loop1_freeze_ref = loop1_CAPVD; } } loop1_freeze_cnt++; if (loop1_freeze_cnt >= FREEZE_TIMEOUT) { loop1_Origin = loop1_CAPVD; loop1_freeze_cnt = 0; loop1_freeze_ref = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; PRINT("Baseline timeout update, new Origin:%d\\n", loop1_Origin); } else { loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; } } else { /* 负variation (CAPVD高于Origin, 特殊铁块磁导率主导/异常高) → 只冻结, 永不更新 Origin * 防 Origin 污染锁死; 权衡: 长时间负温漂基线不跟随(安全方向, 检测仍正常) */ if (loop1_freeze_cnt == 0) { loop1_freeze_ref = loop1_CAPVD; PRINT("EVT|neg_var_freeze CAPVD:%d Origin:%d dev:%d dlt:%d f:%dkHz\n", loop1_CAPVD, loop1_Origin, (int)dev, loop1_dlt_ORG, calc_freq_khz(loop1_CAPVD, loop1_LPCNT)); } loop1_freeze_cnt++; if (loop1_freeze_cnt > FREEZE_TIMEOUT) loop1_freeze_cnt = FREEZE_TIMEOUT; // 饱和, 不更新 loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; } } /*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车 ---*/ if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) { if (loop1_entry_cnt == 0) { /* 过渡事件: 第一次命中进入线 (定位"为什么开始进入") */ PRINT("EVT|enter_try Value:%d CAPVD:%d Origin:%d dlt:%d f:%dkHz LPCNT:%d Xn:%d\n", loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, calc_freq_khz(loop1_CAPVD, loop1_LPCNT), loop1_LPCNT, loop1_Xn); } loop1_entry_cnt++; if (loop1_entry_cnt >= ENTRY_CONFIRM) { PRINT("Car_In, Value:%d, CAPVD:%d, Origin:%d, dlt:%d\\n", loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG); loop1_VD_FLAG = 1; loop1_FLAG_IN = 1; loop1_entry_cnt = 0; loop1_freeze_cnt = 0; loop1_freeze_ref = 0; #if USE_SLOW_RELEASE /* V4.04: 进入瞬态假释放修正 — snap 慢滤波到当前快滤波值 * 进入线(档3: 0.032%)恒深于离开线(0.021%), snap 后释放条件结构性为假, * 消除"定在HR0/2保持即释放"窗口(2026-09-01 仿真+现场复核) */ loop1_CAPVD_slow = loop1_CAPVD; #endif if (!SET_SAFE) { loop1_LC_HOLD = 1; } else { loop1_LC_HOLD = 0; } if (loop1_LC_Reset) loop1_LC_Reset = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; /* 重置平坦性状态（专利 CN200910309382） */ #if USE_FLATNESS_EXIT g_exit_state = 0; g_max_slope = 0; g_max_slope_rate = 0; g_delta2 = 0; g_delta3 = 0; g_slope_flat_cnt = 0; g_flat_ok_cnt = 0; #endif } } else { if (loop1_entry_cnt > 0) loop1_entry_cnt = 0; // 离开阈值区域则重置确认计数 } } else { #if USE_FLATNESS_EXIT /*================================================================ * 有车状态 — 平坦性判定离开（专利 CN200910309382） *================================================================*/ #define K1 8 #define K2 8 #define SLOPE_FLAT_THRESH 100 #define MIN_DELTA2 5 #define MIN_DELTA3 2 #define FLAT_CONFIRM_CNT 3 int32_t first_deriv, abs_fd, abs_sd, second_deriv; first_deriv = (int32_t)loop1_CAPVD - g_prev_capvd; second_deriv = first_deriv - g_prev_first_deriv; abs_fd = (first_deriv >= 0) ? first_deriv : -first_deriv; abs_sd = (second_deriv >= 0) ? second_deriv : -second_deriv; if (g_exit_state == 0) { if (abs_fd > g_max_slope) g_max_slope = abs_fd; if (abs_sd > g_max_slope_rate) g_max_slope_rate = abs_sd; if (abs_fd < SLOPE_FLAT_THRESH) { g_slope_flat_cnt++; if (g_slope_flat_cnt >= 3) { g_delta2 = g_max_slope / K1; g_delta3 = g_max_slope_rate / K2; if (g_delta2 < MIN_DELTA2) g_delta2 = MIN_DELTA2; if (g_delta3 < MIN_DELTA3) g_delta3 = MIN_DELTA3; g_exit_state = 1; g_flat_ok_cnt = 0; } } else { g_slope_flat_cnt = 0; } } else { int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin; int32_t cond1 = (dev >= 0) ? dev : -dev; loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; if (cond1 < (int32_t)loop1_dlt_ORG && abs_fd < (int32_t)g_delta2 && abs_sd < (int32_t)g_delta3) { g_flat_ok_cnt++; if (g_flat_ok_cnt >= FLAT_CONFIRM_CNT) { PRINT("Car_OFF_FLAT, CAPVD:%d Origin:%d d1:%d d2:%d d3:%d f':%d f'':%d\n", loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, g_delta2, g_delta3, first_deriv, second_deriv); loop1_VD_FLAG = 0; loop1_FLAG_OUT = 1; loop1_VD_HOLD = 0; loop1_LC_HOLD = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; Hold_CNT = 0; g_flat_ok_cnt = 0; g_exit_state = 0; } } else { g_flat_ok_cnt = 0; } } g_prev_capvd = loop1_CAPVD; g_prev_first_deriv = first_deriv; #else /*================================================================ * 有车状态 — 简单 cnt_release 防抖离开 *================================================================*/ loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; // 方案B(V4.06): 调浅释放表已并入 SensTable_1 #if USE_SLOW_RELEASE if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD_slow) { // 方案A: 用慢滤波值对齐 PD132T 释放手感 #else if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD) { // 方案B/原逻辑: 快滤波直接判释放 #endif if (loop1_cnt_release == 0) { /* 过渡事件: 第一次命中离开线 (定位"为什么开始释放") */ PRINT("EVT|leave_try Value:%d CAPVD:%d slow:%d Origin:%d dlt:%d f:%dkHz LPCNT:%d Xn:%d\n", loop1_Value, loop1_CAPVD, #if USE_SLOW_RELEASE loop1_CAPVD_slow, #else loop1_CAPVD, #endif loop1_Origin, loop1_dlt_ORG, calc_freq_khz(loop1_CAPVD, loop1_LPCNT), loop1_LPCNT, loop1_Xn); } loop1_cnt_release++; if (loop1_cnt_release >= 3) { PRINT("Car_OFF, Value:%d CAPVD:%d Origin:%d dlt:%d\n", loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG); loop1_VD_FLAG = 0; loop1_FLAG_OUT = 1; loop1_VD_HOLD = 0; loop1_LC_HOLD = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; Hold_CNT = 0; loop1_cnt_release = 0; } } else { if (loop1_cnt_release > 0) loop1_cnt_release = 0; } #endif } } /*=========================================================================== * loop_task_function — FreeRTOS 主任务（对齐 M1H main 循环） *===========================================================================*/ void loop_task_function(void *pvParameters) { g_sys_freq = g_crm_clocks_freq_struct.sclk_freq; INIT_VD(); while (1) { if (loop1_LOOP_OK) { /*--- 线圈重连 ---*/ if (!loop1_LOOP_OK0) { loop1_LOOP_OK0 = 1; g_disconnect_active = 0; // 重连，清除断开标记 loop1_CAPVD = 0; // 强制 IIR 从首个有效值重新收敛 loop1_CAPVD_slow = 0; // 方案A: 释放慢滤波同步收敛 PRINT("EVT|loop_reconnect\n"); } if (loop1_CAP_OK) { loop1_CAP_OK = 0; /* 首次成功测量 → 标记线圈已连接 */ if (!g_loop_power_up_state) { g_loop_power_up_state = 1; } /* 核心检测算法 */ vd1_task(); /* 拨码开关轮询 */ poll_sw_state(); } } else { /*--- 线圈断开 ---*/ /* 注意: 不清除 loop1_VD_FLAG，保留断开前的检测状态 */ RLY1_OFF; RLY2_OFF; /* * 断开计数规则: * - 从未连接过上电 (g_loop_power_up_state == 0): 不计入 * - g_disconnect_active == 0: 新的断开 → 计数+1 * - g_disconnect_active == 1: 同一次断开，不重复计数 */ if (g_loop_power_up_state && !g_disconnect_active) { g_disconnect_active = 1; if (g_disconnect_count < 3) { g_disconnect_count++; } g_fault_phase = 0; g_fault_tick = 0; PRINT("EVT|loop_disconnect cnt=%d Xn:%d CAPVD:%d Origin:%d f:%dkHz\n", g_disconnect_count, loop1_Xn, loop1_CAPVD, loop1_Origin, calc_freq_khz(loop1_CAPVD, loop1_LPCNT)); } vTaskDelay(150); } /*--- 调试输出（每 2 秒）: V4.20 重组 — 频率/采样/基准全量状态 ---*/ #ifdef DEBUG { static uint32_t _dbg_cnt = 0; _dbg_cnt++; if (_dbg_cnt >= 200) { // 200 × 10ms = 2s uint32_t _t_ms = _dbg_cnt * 10; // V4.20: 先取时间再归零 _dbg_cnt = 0; PRINT("DBG|t=%ums f=%dkHz Xn=%d LPCNT=%d Value=%d CAPVD=%d slow=%d " "Origin=%d dlt=%d SENS=%d VD=%d FRZ=%d/%d LIM=%d LOOP=%d STBL=%d\n", _t_ms, calc_freq_khz(loop1_CAPVD, loop1_LPCNT), loop1_Xn, loop1_LPCNT, loop1_Value, loop1_CAPVD, loop1_CAPVD_slow, loop1_Origin, loop1_dlt_ORG, loop1_SensLevel, loop1_VD_FLAG, loop1_freeze_cnt, loop1_freeze_ref, g_slope_limit_cnt, loop1_LOOP_OK, g_loop_stable); g_slope_limit_cnt = 0; // 限幅计数按周期清 (统计最近 2s) } } #endif poll_wdg(); // IWDT 喂狗 (500ms, TMR15 活性双保险) vTaskDelay(10); // 10ms tick, M4 优化: 双路 IIR + 进入确认 + 斜率限幅 } }