wangfq/DLD154V4B
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
cd0fd907708d11dd5726362f66e4de8d31778d77
DLD154V4B/utilities/at32f421_freertos_demo/src/TaskLoop.c
T
wangfq cd0fd90770 feat(V4B): V4.23 — 红灯呼吸对齐vd960Loop(≈2.6s) + 上电自检基准判稳 
现场测试 DLD154V4 反馈两问题修复:

1. 红灯呼吸太快 → 借鉴 vd960Loop 效果 (main.c poll_red_pwm)
   - 参数按 665/6800 缩放取整: 大步 100→39(≈5.9%), 近顶 20→10(≈1.5%),
     分界 585(88%), 峰值保持 660×3 步
   - 周期 ≈1.6s → ≈2.6s (仿真 42 步×60ms=2520ms)

2. 绿灯上电自检闪到"稳定基准值"才停 (TaskLoop.c 稳定期判稳)
   - 根因: 固定 128 样本即判稳, 与基准是否稳定无关; static 计数不清零,
     二次稳定期(安全复位)1 样本瞬间判稳
   - 修复: 全局 g_stable_cnt/g_settle_cnt, 每窗(100样本≈1s)Origin 均值
     漂移 ≤0.1% → settle++, 连续 2 窗 + 最少 128 样本判稳; 硬兜底 500
     样本(5s); 计数判稳退出/INIT_VD 清零

验证: tests/test_power_on_stable.c (200/300/500/200 样本 PASS),
      tests/test_red_breath.c (周期 2520ms/峰值保持/写值≤ARR PASS),
      gcc -fsyntax-only 两文件 0 error (借 vd960Loop AT32F421 库头)
文档: devlog 置顶 + 修订表回填 V2.12~V2.20 + V2.21; product-manual §5.1/
      核心特性; technical-spec V2.2 (§5.2 参数表/§7 判稳语义/§15 修订记录)

待现场验证: 呼吸观感、绿灯自检时长 (无车 2~3s / 首窗尖峰 3s / 5s 兜底)
2026-09-02 16:26:03 +08:00

1098 lines
42 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#include "TaskLoop.h"
#include "at32f421_board.h"
#include "cmcng.h"
#include "FreeRTOS.h"
#include "task.h"
#include <stdlib.h>
/*===========================================================================
* 灵敏度表 — 对齐 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 + 进入确认 + 斜率限幅
}
}
Reference in New Issue View Git Blame Copy Permalink