wangfq
ef5959d6fe
问题: 43nF/高灵敏/低Q(≤50µH) 金属板边界试探 → 触发稳定但释放点外移 发散至锁死(绿灯长亮); 开 DEBUG 打印复现不了(阻塞降采样平均抖动) = 关键旁证 根因: 释放带回 dlt_rel=23 ≈ 低Q σ(±15~25), 快值判释放(5样本)被抖断 修复(Q 分档, 高/中Q 零影响): - rel_release_hit(): 低Q 用 g_rel_smooth(τ50ms REL_ALFA=46) 判释放 - 低Q 释放带回 ×1.5 (REL_BAND_LO_MULT=150: 23→~35) - g_rel_smooth: IIR 后跟随/CAPVD=0 与重连归零/Car_In snap 语法0 error, 4测试全绿; spec V2.35 / devlog V2.54
1609 lines
71 KiB
C
1609 lines
71 KiB
C
|
||
|
||
#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 g_rel_smooth = 0; // V4.36.19: 低Q 释放平滑值 (τ≈50ms, 复刻打印降采样平均效应)
|
||
uint8_t g_q_lo = 0; // V4.36.19: 低Q 档标志 (空场 f>100kHz, update_q_confirm_need 同步)
|
||
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;
|
||
|
||
/*===========================================================================
|
||
* 断开保持/重连参数 (ISR 5ms tick 使用, 须定义于 TMR15 ISR 之前)
|
||
*===========================================================================*/
|
||
#define RELAY_HOLD_MS 1000 /* V4.36.3/7: 有车断开继电器保持超时(可调宏, ms)
|
||
* 断开前感应有车且继电器输出中 → 不断开而保持,
|
||
* 超时仍断开才释放(防瞬时断开误释放/防砸);
|
||
* 保持期内重连且有车 → 输出无缝续持 */
|
||
|
||
/* V4.36.5/6: 重连起振抑制窗 — 按线圈 Q 分档 (Q∝1/f, 用空场频率代理)
|
||
* 低Q(高频小L): 起振慢/幅度弱 → 前几窗测量偏差大 → 假"有车"跳输出
|
||
* 高Q(低频大L): 起振快样本干净 → 仅需短兜底窗 (V4.36.6: 100ms)
|
||
* 分界(实测): 46~50µH→109~113k 必跳 → >100k 归低Q; 80~100k 中Q; <=80k 高Q */
|
||
#define RECONNECT_Q_HI_F_KHZ 80 /* f <= 80kHz (L≳92µH@43nF) → 高Q */
|
||
#define RECONNECT_Q_MID_F_KHZ 100 /* 80< f <=100kHz → 中Q */
|
||
#define RECONNECT_BLANK_Q_HI_MS 200 /* V4.36.7: 高Q 短兜底窗(起振/插头瞬态) */
|
||
#define RECONNECT_BLANK_Q_MID_MS 300 /* V4.36.7: 中Q 短抑制 */
|
||
#define RECONNECT_BLANK_Q_LO_MS 1500 /* V4.36.11: 低Q(f>100k, L<~59µH): 长抑制 */
|
||
|
||
/*===========================================================================
|
||
* 全局状态变量 — 标志位
|
||
*===========================================================================*/
|
||
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)
|
||
|
||
/* V4.24: 环境重校准(受控基线更新)状态 — 现场永久环境变化(如金属板A拿走)自动恢复
|
||
* 触发链: dev>+4dlt 高位连续 60s → 学习基线 B (连续空闲窗均值+判稳) → 空闲原子替换 Origin
|
||
* 黄灯常亮指示学习过程; 与双向保护并存: 60s 内瞬态污染仍只冻结不污染 Origin */
|
||
uint8_t g_env_resync = 0; // 1=学习中 (黄灯常亮指示)
|
||
uint16_t g_env_wait_cnt = 0; // dev>+4dlt 高位连续累计 (≥ENV_RESYNC_WAIT 触发学习)
|
||
uint32_t g_env_sum = 0; // 学习窗口 CAPVD 累加
|
||
uint16_t g_env_cnt = 0; // 学习窗口样本数
|
||
uint32_t g_env_prev = 0; // 上一窗口均值 (B 候选)
|
||
uint8_t g_env_settle = 0; // 连续稳定窗口计数 (窗内均值漂移 ≤0.1% 则+1)
|
||
uint16_t g_env_tick = 0; // 学习预算 (超时放弃防死锁)
|
||
|
||
/* V4.25+: 环境异常预判锁存 — 一旦触发黄灯常亮, 回摆/目标靠近不灭;
|
||
* 仅当环境回正常(空闲, dev 未进入线以下)连续 ENV_RECOVER_WAIT 才解除 */
|
||
uint8_t g_env_warn = 0; // 1=环境异常预判锁存 (黄灯常亮)
|
||
uint16_t g_env_recover_cnt = 0; // 恢复确认计数 (空闲持续 5s → 解除预判)
|
||
|
||
#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_reconn_candidate = 0; // V4.36.2: 重连确认窗候选(载波上升沿进入)
|
||
uint16_t g_reconn_cnt = 0; // V4.36.2: 载波连续恢复计数 (确认窗 ~100ms)
|
||
uint16_t g_relay_hold_cnt = 0; // V4.36.3: 有车断开继电器保持计时(ms, ISR 5ms 累加)
|
||
uint8_t g_relay_hold_done = 0; // V4.36.3: 本次有车断开已兜底释放(防重复OFF)
|
||
uint16_t g_reconn_blank_ms = 0; // V4.36.5: 重连起振抑制倒计时(ms, 按Q分档, vd1_task 每样本-10)
|
||
uint8_t g_entry_need = 3; // V4.36.8: 进入确认样本数 (按Q档, 高3/中4/低5)
|
||
uint8_t g_prev_ok = 0; // V4.36.14: loop_task 主循环拍级上一拍 LOOP_OK (自身上升沿检测)
|
||
uint8_t g_reconn_stab = 0; // V4.36.15: 重连判稳门控(无车重连: 值稳定才放行)
|
||
uint16_t g_stab_ms = 0; // V4.36.15: 判稳已耗时(兜底 5s)
|
||
uint8_t g_stab_step_tick = 0; // V4.36.15: 200ms 步进计数
|
||
uint8_t g_stab_ok_cnt = 0; // V4.36.15: 连续无漂移步数
|
||
uint32_t g_stab_base = 0; // V4.36.15: 步进比较基准 CAPVD
|
||
uint8_t g_release_need = 3; // V4.36.8: 释放确认样本数 (按Q档, 高3/中4/低5)
|
||
uint8_t g_fault_phase = 0;
|
||
uint16_t g_fault_tick = 0;
|
||
uint8_t g_coil_fault = 0; // V4.26: 线圈电感量异常 0=正常 1=L偏大(f<20kHz) 2=L偏小(f>120kHz)
|
||
uint16_t g_coil_bad_cnt = 0; // V4.26: 电感异常防抖计数 (100tick=1s 置位/清除)
|
||
|
||
/*===========================================================================
|
||
* 调试计数器
|
||
*===========================================================================*/
|
||
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;
|
||
}
|
||
|
||
/* V4.36.19: 低Q 释放平滑 (α=REL_ALFA/256≈0.18, τ≈50ms @10ms)
|
||
* 复刻 DEBUG 打印阻塞降采样的平均效应 — 低Q(高频)释放边界抖动大, 快值直接判
|
||
* 会被抖动打断计数(回滞仅 23 counts ≈ σ), 平滑后判定稳定 */
|
||
uint32_t get_flt_value_rel(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)REL_ALFA) >> 8;
|
||
|
||
if (new_value > last_value) {
|
||
value_Flt = last_value + scaled_delta;
|
||
} else {
|
||
value_Flt = last_value - scaled_delta;
|
||
}
|
||
|
||
return value_Flt;
|
||
}
|
||
|
||
/* V4.36.19: 释放命中判定 — 低Q(21~50µH)高灵敏释放带回仅 ~23 counts ≈ 抖动 σ,
|
||
* 快值直接判在边界试探时计数被抖断 → 释放点外移/卡死;
|
||
* 低Q 用 τ50ms 平滑值(复刻打印阻塞降采样平均效应) + 带回 ×1.5(留 σ 余量).
|
||
* 高Q/中Q 走原快值逻辑, 零影响 */
|
||
static uint8_t rel_release_hit(void)
|
||
{
|
||
uint32_t _cmp = loop1_CAPVD;
|
||
uint32_t _line = loop1_Origin - loop1_dlt_ORG;
|
||
|
||
if (g_q_lo) {
|
||
_cmp = g_rel_smooth;
|
||
_line = loop1_Origin - ((loop1_dlt_ORG * REL_BAND_LO_MULT) / 100);
|
||
}
|
||
return (_line < _cmp) ? 1 : 0;
|
||
}
|
||
|
||
/*===========================================================================
|
||
* 滑动平均基线更新
|
||
* 累加 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 灭) — 就绪过程指示, 此阶段继电器未驱动
|
||
* 稳定后 (V4.36.4/10):
|
||
* → 绿灯 = 默认继电器 RLY2 (PA5, 恒随车辆存在) 实际输出状态:
|
||
* 吸合(PA5 高) → 绿灯亮; 释放(PA5 低) → 绿灯灭
|
||
* - 有车 → 亮; 无车 → 灭
|
||
* - 有车断开保持(RELAY_HOLD_MS)期间 → 保持亮 (与继电器一致)
|
||
* - 断开超时释放/无车 → 灭
|
||
* (V4.36.10 勘误: 原镜像 RLY1(PA6 脉冲/存在继电器) 指反 →
|
||
* 用户: 绿灯指示【默认继电器】, RLY1 为拨码3 可选脉冲继电器)
|
||
*===========================================================================*/
|
||
void poll_green_led(void)
|
||
{
|
||
#define GREEN_SLOW_HALF 40 // 200ms (40 × 5ms)
|
||
|
||
static uint16_t _slow_tick = 0;
|
||
|
||
/*--- 自检阶段：慢闪（就绪过程指示；此阶段继电器未驱动） ---*/
|
||
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;
|
||
}
|
||
|
||
/*--- 稳定后: 绿灯 = 默认继电器 RLY2 (PA5) 实际输出状态 (V4.36.4/10) ---
|
||
* 读 RLY2_GPIO->odt (与 RLY2_ON/OFF 宏同源, 即真实驱动状态):
|
||
* 吸合(高) → 绿灯亮; 释放(低) → 绿灯灭
|
||
* RLY1(PA6) 为拨码3 可配置的脉冲/存在继电器 — 不指示
|
||
* 断开时无强制灭分支 → 与继电器保持逻辑一致 (防误判"继电器释放") */
|
||
_slow_tick = 0;
|
||
if (RLY2_GPIO->odt & RLY2_PIN)
|
||
LEDA_ON; // 默认继电器吸合 → 绿灯亮 (PA9 低电平点亮)
|
||
else
|
||
LEDA_OFF; // 默认继电器释放 → 绿灯灭
|
||
}
|
||
|
||
/*===========================================================================
|
||
* 黄灯故障指示 — LEDC (PA10)
|
||
*
|
||
* 由 TMR15 ISR 每 5ms 驱动一次。
|
||
*
|
||
* 模式判定 (V4.26 优先级: 1>4>5>3>2):
|
||
* 1 当前线圈断开 或 上电后从未接线圈:
|
||
* → 快闪 (200ms 亮 / 200ms 灭)
|
||
* 4 环境异常预判/重校准学习 (g_env_warn/g_env_resync):
|
||
* → 常亮 (V4.24/25, 故障补偿 10s 内完成)
|
||
* 5 线圈电感量不合理 (g_coil_fault, 频率 20~120kHz 窗之外):
|
||
* → 一长一短 (长亮900ms/灭300ms/短亮100ms/灭700ms 循环)
|
||
* 3 线圈已恢复连接, 且上电后有断开记录 (g_disconnect_count >= 1):
|
||
* → N 短闪 (80ms亮/200ms灭 × N, 1.2s间隔, N=断开次数≤3)
|
||
* 2 线圈正常连接, 无断开记录:
|
||
* → 灭
|
||
*===========================================================================*/
|
||
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: 快闪 — 载波缺失/重连判稳接管 (V4.36.16/17):
|
||
* !loop1_LOOP_OK: 上电无线圈 + 运行中断开 (5ms 实时, 不等 loop_task 150ms 慢拍
|
||
* 置 disconnect_active; 上电 phase power_up=0 时不置位, 由本条件兜底)
|
||
* g_reconn_stab: 运行中无车重连判稳门控
|
||
* 上电有线圈/判稳中: LOOP_OK=1 → 黄灯不闪, 自检指示归绿灯慢闪 ---*/
|
||
if (!loop1_LOOP_OK || g_reconn_stab) {
|
||
if (g_fault_tick >= FLT_FAST_HALF) {
|
||
g_fault_tick = 0;
|
||
LED_YELLOW_GPIO->odt ^= LED_YELLOW_PIN; // toggle
|
||
}
|
||
return;
|
||
}
|
||
|
||
/*--- V4.25 模式4: 环境异常预判/重校准学习 → 黄灯常亮
|
||
* g_env_warn == 1: 环境重大变化预判锁存 (高位连续 ~5s 触发; 回摆/目标靠近保持,
|
||
* 环境回正常空闲连续 ~5s 才解除)
|
||
* g_env_resync == 1: 已进入重校准学习 (高位累计 60s 后自动开始, 学完原子替换 Origin)
|
||
* 优先级: 断开快闪 > 环境预判/学习常亮 > N短闪 > 灭 ---*/
|
||
if (g_env_resync || g_env_warn) {
|
||
LED_YELLOW_ON;
|
||
g_fault_tick = 0;
|
||
g_fault_phase = 0;
|
||
return;
|
||
}
|
||
|
||
/*--- V4.26 模式5: 线圈电感量不合理 → 一长一短
|
||
* 长亮900ms(180tick) → 灭300ms(60) → 短亮100ms(20) → 灭700ms(140), 循环
|
||
* g_coil_fault: 1=L偏大(f<20kHz) 2=L偏小(f>120kHz)
|
||
* 优先级: 断开(1) > 环境(4) > 电感量(5) > N短闪(3) > 灭 ---*/
|
||
if (g_coil_fault) {
|
||
static const uint16_t dur[4] = { 180, 60, 20, 140 }; // 900/300/100/700ms (V4.36.13: 灭 100→300ms)
|
||
if (g_fault_phase > 3) g_fault_phase = 0;
|
||
if (g_fault_phase & 1)
|
||
LED_YELLOW_OFF;
|
||
else
|
||
LED_YELLOW_ON;
|
||
if (g_fault_tick >= dur[g_fault_phase]) {
|
||
g_fault_tick = 0;
|
||
g_fault_phase++;
|
||
}
|
||
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;
|
||
g_reconn_stab = 0; /* V4.36.15: 判稳门控清零 */
|
||
g_stab_ms = 0;
|
||
g_stab_step_tick = 0;
|
||
g_stab_ok_cnt = 0;
|
||
g_stab_base = 0;
|
||
g_rel_smooth = 0; /* V4.36.19: 低Q 释放平滑清零 */
|
||
g_q_lo = 0; /* V4.36.19: 低Q 档标志清零 */
|
||
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;
|
||
|
||
/* V4.24: 环境重校准复位 */
|
||
g_env_resync = 0;
|
||
g_env_wait_cnt = 0;
|
||
g_env_sum = 0;
|
||
g_env_cnt = 0;
|
||
g_env_prev = 0;
|
||
g_env_settle = 0;
|
||
g_env_tick = 0;
|
||
g_env_warn = 0; // V4.25+: 环境预判锁存复位
|
||
g_env_recover_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;
|
||
}
|
||
|
||
/*--- V4.36.3: 有车断开 → 继电器保持 (5ms tick, 500ms 兜底释放) ---
|
||
* 断开前感应有车(VD_FLAG)且已上电 → RLY 不断开, 保持输出:
|
||
* - 500ms 内重连(经 100ms 确认窗) + vd1_task 判有车 → 输出无缝续持
|
||
* - 500ms 内重连 + 判无车 → vd1_task 正常离开释放
|
||
* - 超时仍断开 → 强制释放(防真故障无限保持)
|
||
* 无车断开(VD_FLAG=0)不受影响, 仍立即释放 */
|
||
if (!loop1_LOOP_OK && loop1_VD_FLAG && g_loop_power_up_state && !g_relay_hold_done) {
|
||
RLY1_ON; /* 保持: 防止瞬时断开继电器抖落 */
|
||
RLY2_ON;
|
||
g_relay_hold_cnt += 5;
|
||
if (g_relay_hold_cnt >= RELAY_HOLD_MS) {
|
||
RLY1_OFF; /* 兜底: 超时仍断开 → 释放 */
|
||
RLY2_OFF;
|
||
g_relay_hold_done = 1;
|
||
}
|
||
} else {
|
||
g_relay_hold_cnt = 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: 由窗口量反算线圈振荡频率
|
||
* 捕获链路: 线圈振荡信号 → PA7(TMR3_CH2) 输入捕获, 捕获分频 ÷2 (DIV_2),
|
||
* TMR3 计数时钟 = sclk = 120MHz (HEXT×10, AHB/APB1 ÷1, at32f421_clock.c)
|
||
* Xn = 捕获间隔计数 = 2 × 120MHz / f_real → f_kHz = 240000 / Xn
|
||
* CAPVD ≈ LPCNT × Xn → 240000 × LPCNT / CAPVD
|
||
* → f_kHz = 240000 × LPCNT / CAPVD
|
||
* ⚠ V4.27 修正: 原 60000 按 60MHz 假设 + 未计 DIV_2, 少算 4 倍
|
||
* (示波器 30.23kHz 打印 7kHz 实测偏差; 旧日志 16.3kHz 实为 ~65.2kHz)
|
||
* L 粗估: f=120kHz 边界 @66nF → L≈27µH (仅供参考, 窗口判定以拍板频率窗为准)
|
||
*===========================================================================*/
|
||
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 (240000UL * lp_cnt) / capvd;
|
||
}
|
||
|
||
/*===========================================================================
|
||
* V4.24: 环境重校准 — 学习被打断/放弃时复位 (黄灯恢复) / 完成时原子替换 Origin
|
||
*
|
||
* 替换安全设计:
|
||
* - 仅空闲(无车, dev 高位)且学习 B 连续 2 窗稳定后才调用
|
||
* - B ≈ 当前空闲 CAPVD → 替换瞬间 dev≈0, 无跳变不会触发继电器
|
||
* - 同步清 freeze/ORG/entry 残留, slow 滤波 snap (同 V4.04 进入时)
|
||
*===========================================================================*/
|
||
static void env_resync_abort(void)
|
||
{
|
||
g_env_resync = 0;
|
||
g_env_sum = 0;
|
||
g_env_cnt = 0;
|
||
g_env_prev = 0;
|
||
g_env_settle = 0;
|
||
g_env_tick = 0;
|
||
}
|
||
|
||
static void env_resync_apply(uint32_t new_origin)
|
||
{
|
||
PRINT("EVT|env_resync Origin:%d->%d CAPVD:%d\n",
|
||
loop1_Origin, new_origin, loop1_CAPVD);
|
||
loop1_Origin = new_origin;
|
||
loop1_freeze_cnt = 0;
|
||
loop1_freeze_ref = 0;
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_entry_cnt = 0;
|
||
#if USE_SLOW_RELEASE
|
||
loop1_CAPVD_slow = loop1_CAPVD; // 防替换后瞬态假释放 (同 V4.04 snap)
|
||
#endif
|
||
env_resync_abort();
|
||
}
|
||
/*===========================================================================
|
||
* V4.26~28: 线圈电感量粗估
|
||
* L_uH = 2.533e7 / (f_kHz² × C_nF) [f=1/(2π√(LC)), f_kHz 为修正后真实频率]
|
||
* C 用 COIL_CAL_C_NF (标定等效谐振电容, V4.34):
|
||
* 三极管库存板低频档(名义66nF, R19在/R27空): 实测 C_eff≈43nF (41.3~45.5, 2台×5颗) → 宏43
|
||
* (0Ω 板 R19拆/R27焊0Ω 实验值 68.4 仅作机理验证, 产品沿用三极管板 → 标43)
|
||
* (V4.28 的 43 是 5.1K 旧板带病特性 — 三极管开关未饱和, 第二颗33nF未全量并入,
|
||
* 换 0Ω 后并联全导通, C_eff 回到≈名义66; 43 作废)
|
||
* 高频档(名义33nF): 旧板实测 C_eff≈34.4 — 0Ω 板待复测, 拨33nF档请把宏改34
|
||
* 标定法: LCR 测 L → C=2.533e7/(f²×L), 多颗平均
|
||
* 例(0Ω板): f=52kHz→L≈138µH(实136); f=88kHz→L≈48µH(实48); f=127kHz→L≈23µH(实23)
|
||
*===========================================================================*/
|
||
static uint32_t coil_l_uH(uint32_t f_khz)
|
||
{
|
||
if (f_khz == 0) return 0;
|
||
return 25330000UL / (f_khz * f_khz * COIL_CAL_C_NF);
|
||
}
|
||
|
||
/*===========================================================================
|
||
* V4.26: 线圈电感量合理范围监视
|
||
* 拍板规格: 默认低频档口径, 空场频率合理窗 20~120kHz, 出界即判定电感量不合理
|
||
* (黄灯"一长一短"告警, 见 poll_yellow_led 模式5)
|
||
* 调用点保证: 仅稳定 + 无车空闲 + 未断开时执行 (车压/自检/断开不判)
|
||
* g_coil_fault: 0=正常 1=L偏大(f<20kHz) 2=L偏小(f>120kHz)
|
||
* 防抖: 越界/恢复均连续 100tick(=1s) 才置位/清除
|
||
*===========================================================================*/
|
||
static void coil_monitor(void)
|
||
{
|
||
uint32_t f_khz = calc_freq_khz(loop1_Origin, loop1_LPCNT);
|
||
uint8_t want = 0;
|
||
|
||
if (loop1_Origin == 0 || f_khz == 0)
|
||
return; /* 无有效基准 → 维持原状 */
|
||
|
||
if (f_khz < COIL_F_OK_MIN_KHZ)
|
||
want = 1; /* L 偏大 (>合理上限) */
|
||
else if (f_khz > COIL_F_OK_MAX_KHZ)
|
||
want = 2; /* L 偏小 (<合理下限) */
|
||
|
||
if (want == g_coil_fault) {
|
||
g_coil_bad_cnt = 0; /* 状态一致 → 计时复位 */
|
||
} else {
|
||
g_coil_bad_cnt++;
|
||
if (g_coil_bad_cnt >= 100) {
|
||
if (g_coil_fault == 0)
|
||
PRINT("EVT|coil_fault f:%d kHz L~%lu uH Xn:%d %s\n",
|
||
f_khz, coil_l_uH(f_khz), loop1_Xn,
|
||
want == 1 ? "too_large" : "too_small");
|
||
else
|
||
PRINT("EVT|coil_ok f:%d kHz\n", f_khz);
|
||
g_coil_fault = want;
|
||
g_coil_bad_cnt = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*===========================================================================
|
||
* vd1_task — 核心检测算法（对齐 M1H VD1_TASK）
|
||
*
|
||
* 每次 loop1_CAP_OK 时调用一次。
|
||
* 流程：
|
||
* 1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD
|
||
* 2. 无车时：基线跟踪 + 进入检测
|
||
* 3. 有车时：离开检测（带滞回）
|
||
*===========================================================================*/
|
||
static void update_q_confirm_need(void); // V4.36.8 前置声明 (定义在 loop_task 上方)
|
||
|
||
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 未判稳强制结束自检(防绿灯无限闪)
|
||
#define ENV_RESYNC_WAIT 700 // V4.31: dev>+4dlt 高位连续 7s → 判定永久环境变化 (拍板: 预判2s + 补偿7s + 恢复2s)
|
||
#define ENV_LEARN_WINDOW 200 // V4.24: 环境学习窗口 2s (200×10ms)
|
||
#define ENV_LEARN_MAX_TICKS 3000 // V4.24: 学习预算 30s, 超时放弃 (防黄灯常亮死锁)
|
||
|
||
|
||
if (loop1_Origin == 0) return;
|
||
|
||
/*================================================================
|
||
* 1. IIR 滤波 + 斜率限幅 (M4 优化)
|
||
*
|
||
* ALFA_CAP1=79 @10ms → τ≈32ms
|
||
* 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰)
|
||
*================================================================*/
|
||
if (loop1_CAPVD == 0) {
|
||
loop1_CAPVD = loop1_Value;
|
||
g_rel_smooth = 0; /* V4.36.19: 直锁时释放平滑同步归零 */
|
||
|
||
/* V4.36.5: 首样本直锁(重连重置后) → 按线圈 Q(空场频率)分档设定抑制窗
|
||
* Origin 为断开前空场基线(同线圈同环境) → f 即 Q 档;
|
||
* 上电(Origin=0)时 f=0 → 归高Q档 0ms, 不影响正常判稳流程 */
|
||
g_reconn_blank_ms = 0;
|
||
{
|
||
uint32_t _f = calc_freq_khz(loop1_Origin, loop1_LPCNT);
|
||
if (_f > RECONNECT_Q_MID_F_KHZ)
|
||
g_reconn_blank_ms = RECONNECT_BLANK_Q_LO_MS; /* 低Q(高频) 长抑制 */
|
||
else if (_f > RECONNECT_Q_HI_F_KHZ)
|
||
g_reconn_blank_ms = RECONNECT_BLANK_Q_MID_MS; /* 中Q 短抑制 */
|
||
else
|
||
g_reconn_blank_ms = RECONNECT_BLANK_Q_HI_MS; /* 高Q(低频) 短兜底窗 */
|
||
if (g_reconn_blank_ms)
|
||
PRINT("EVT|reconn_blank %dms f:%d kHz\n",
|
||
g_reconn_blank_ms, _f);
|
||
}
|
||
} 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
|
||
|
||
/* V4.36.19: 低Q 释放平滑 (τ≈50ms; 无条件跟随, 只用于低Q 释放判定) */
|
||
if (loop1_CAPVD != 0) {
|
||
if (g_rel_smooth == 0)
|
||
g_rel_smooth = loop1_CAPVD;
|
||
else
|
||
g_rel_smooth = get_flt_value_rel(loop1_CAPVD, g_rel_smooth);
|
||
}
|
||
|
||
/* V4.36.5: 重连起振抑制倒计时 (每样本 ~10ms) */
|
||
if (g_reconn_blank_ms) {
|
||
if (g_reconn_blank_ms > 10) g_reconn_blank_ms -= 10;
|
||
else g_reconn_blank_ms = 0;
|
||
}
|
||
|
||
/* V4.36.8: 进入/释放确认样本数按 Q 档更新 (每样本; Origin=0 时归高Q 3) */
|
||
update_q_confirm_need();
|
||
|
||
/*--- V4.36.15: 重连判稳门控 (无车重连: 值稳定且回带才放行) ---
|
||
* blank 固定窗只挡"最脏起振期"; 低Q 起振蠕变(21µH 实测 ~2.2s)由本门控自适应接管:
|
||
* 200ms 步进检查 CAPVD 漂移 ≤ dlt/2 → 连续 3 步(600ms)无漂移,
|
||
* 且 |CAPVD-Origin| ≤ dlt(回带内=空场确认) → 判稳完成 → Origin 对齐当前空场
|
||
* (消除"新空场≠旧 Origin"基线差, 根治窗末假车; 浅车/蠕变尾 dev 超带时不完成)
|
||
* 强车例外: CAPVD < Origin-8×dlt(真车深压) → 不等判稳立即放行
|
||
* 兜底 5s: 超时强制放行(避免永久无输出; dev 深则进入判定正常接管) */
|
||
if (g_reconn_stab) {
|
||
uint32_t _dlt = loop1_dlt_ORG;
|
||
if (_dlt < 8) _dlt = 8;
|
||
|
||
if (g_stab_base == 0)
|
||
g_stab_base = loop1_CAPVD;
|
||
|
||
if (loop1_CAPVD < (loop1_Origin - 8 * _dlt)) {
|
||
/* 强车例外: 真车深压不等判稳 */
|
||
g_reconn_stab = 0;
|
||
PRINT("EVT|stab bypass strong_car dev:%d\n",
|
||
(int32_t)loop1_CAPVD - (int32_t)loop1_Origin);
|
||
} else {
|
||
g_stab_ms += 10;
|
||
g_stab_step_tick++;
|
||
if (g_stab_step_tick >= STABILIZE_STEP_TICKS) {
|
||
g_stab_step_tick = 0;
|
||
int32_t _drift = (int32_t)loop1_CAPVD - (int32_t)g_stab_base;
|
||
if (_drift < 0) _drift = -_drift;
|
||
if (_drift <= (int32_t)(_dlt / 2))
|
||
g_stab_ok_cnt++;
|
||
else
|
||
g_stab_ok_cnt = 0;
|
||
g_stab_base = loop1_CAPVD;
|
||
|
||
if (g_stab_ok_cnt >= STABILIZE_STEPS_OK) {
|
||
int32_t _d = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin;
|
||
if (_d < 0) _d = -_d;
|
||
if (_d <= (int32_t)_dlt) {
|
||
/* 回带内=空场确认 → Origin 对齐当前空场(消基线差) */
|
||
loop1_Origin = loop1_CAPVD;
|
||
loop1_CAPVD_slow = loop1_CAPVD;
|
||
loop1_dlt_ORG = ((uint32_t)loop1_Origin *
|
||
SensTable[loop1_SensLevel]) >> 16;
|
||
PRINT("EVT|stab done Origin->%d dev:%d\n",
|
||
loop1_Origin, _d);
|
||
} else {
|
||
/* dev 仍超带(浅车/蠕变尾): 继续抑制等回带 */
|
||
PRINT("EVT|stab wait dev:%d drift:%d\n", _d, _drift);
|
||
g_stab_ok_cnt = 0;
|
||
}
|
||
}
|
||
}
|
||
/* 兜底 5s: 强制放行 (dev 深则进入判定正常接管) */
|
||
if (g_stab_ms >= STABILIZE_MAX_MS) {
|
||
g_reconn_stab = 0;
|
||
PRINT("EVT|stab timeout %dms\n", g_stab_ms);
|
||
}
|
||
}
|
||
}
|
||
|
||
/*--- 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 干净
|
||
g_env_wait_cnt = 0; // V4.24: 高位回摆/目标靠近 → 等待重来
|
||
env_resync_abort(); // V4.24: 学习中被打断 → 重算
|
||
/* V4.25: 恢复确认 — 环境回正常(dev 未进进入线以下)连续 5s → 解除预判锁存
|
||
* (预判一旦触发, 回摆/目标靠近都保持黄灯常亮; 只有稳定空闲才灭) */
|
||
if (dev > -(int32_t)loop1_dlt_ORG) {
|
||
if (g_env_warn) {
|
||
g_env_recover_cnt++;
|
||
if (g_env_recover_cnt >= ENV_RECOVER_WAIT) {
|
||
g_env_warn = 0;
|
||
g_env_recover_cnt = 0;
|
||
}
|
||
}
|
||
} else {
|
||
g_env_recover_cnt = 0; // 目标进入确认区 → 恢复计时重来
|
||
}
|
||
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
|
||
&loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN);
|
||
} else if (dev < 0) {
|
||
g_env_wait_cnt = 0; // V4.24: 目标靠近(车方向) → 等待重来
|
||
env_resync_abort(); // V4.24: 学习中被打断 → 重算
|
||
g_env_recover_cnt = 0; // V4.25: 目标深压 → 恢复确认计时清零
|
||
/* 正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, 特殊铁块磁导率主导/异常高)
|
||
* V4.24 两段式:
|
||
* ① 前 7s(ENV_RESYNC_WAIT): 只冻结, 不更新 Origin
|
||
* (36321a1 双向保护原语义: 防铁块污染 Origin → 离开假进入锁死)
|
||
* ② 高位连续 ≥60s 未回摆 → 判定"永久环境变化"(如金属板A拿走、基准被学低)
|
||
* → 进入受控重校准: 连续空闲窗口均值 B + settle 判稳 → 空闲原子替换 Origin
|
||
* 权衡: 铁块静止 60s+ 也会被学入 → 靠等待时长 + 黄灯常亮提示(操作接口) */
|
||
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;
|
||
|
||
if (!g_env_resync) {
|
||
/*--- ① 等待确认: dev>+4dlt 高位连续累计 (回摆清零见窗口/dev<0分支)
|
||
* V4.25: 累计满 5s(ENV_WARN_WAIT) → 预判锁存 g_env_warn=1 (黄灯常亮),
|
||
* 锁存后回摆/目标靠近不解除; 高位重现则恢复计时清零 ---*/
|
||
g_env_wait_cnt++;
|
||
if (g_env_wait_cnt >= ENV_WARN_WAIT)
|
||
g_env_warn = 1; // 预判锁存 (只增不清)
|
||
g_env_recover_cnt = 0; // 高位异常中 → 恢复确认失效
|
||
if (g_env_wait_cnt >= ENV_RESYNC_WAIT) {
|
||
g_env_resync = 1;
|
||
g_env_wait_cnt = 0;
|
||
g_env_sum = 0;
|
||
g_env_cnt = 0;
|
||
g_env_prev = 0;
|
||
g_env_settle = 0;
|
||
g_env_tick = 0;
|
||
PRINT("EVT|env_learn_start Origin:%d CAPVD:%d dev:%d\n",
|
||
loop1_Origin, loop1_CAPVD, (int)dev);
|
||
}
|
||
} else {
|
||
/*--- ② 学习基线 B: 连续空闲窗口均值 + settle 判稳 (同 V4.23 上电自检语义) ---*/
|
||
g_env_tick++;
|
||
g_env_sum += loop1_CAPVD;
|
||
g_env_cnt++;
|
||
if (g_env_cnt >= ENV_LEARN_WINDOW) {
|
||
uint32_t _m = g_env_sum / g_env_cnt;
|
||
uint32_t _drift, _band;
|
||
g_env_sum = 0;
|
||
g_env_cnt = 0;
|
||
if (g_env_prev != 0) {
|
||
_drift = (_m > g_env_prev) ? (_m - g_env_prev) : (g_env_prev - _m);
|
||
_band = _m * STABLE_ORIGIN_PPT / 1000;
|
||
if (_drift <= _band) {
|
||
g_env_settle++;
|
||
} else {
|
||
g_env_settle = 0; // 窗口不稳 → 重算
|
||
}
|
||
if (g_env_settle >= STABLE_SETTLE_WINDOWS) {
|
||
/* 连续 2 窗稳定 → B 可信; 空闲由上下文保证, 原子替换 */
|
||
env_resync_apply(_m);
|
||
return; // 本 tick 不再走进入判定
|
||
}
|
||
}
|
||
g_env_prev = _m;
|
||
}
|
||
if (g_env_tick >= ENV_LEARN_MAX_TICKS) {
|
||
/* 学习预算超时 → 放弃, 保留旧 Origin (宁可灵敏度低不冒险污染) */
|
||
PRINT("EVT|env_learn_abort timeout Origin:%d CAPVD:%d\n",
|
||
loop1_Origin, loop1_CAPVD);
|
||
env_resync_abort();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*--- V4.26: 线圈电感量监视 (调用点=稳定无车空闲, 见 coil_monitor) ---*/
|
||
coil_monitor();
|
||
|
||
/*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车 ---*/
|
||
if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG) && g_reconn_blank_ms == 0 && !g_reconn_stab) {
|
||
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 >= g_entry_need) { // V4.36.8: 按Q档 (高3/中4/低5)
|
||
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
|
||
g_rel_smooth = loop1_CAPVD; /* V4.36.19: 低Q 释放平滑 snap 到进入点 */
|
||
|
||
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 (rel_release_hit()) { // V4.36.19: 低Q 平滑值+带回加宽; 高/中Q 退回原快值逻辑
|
||
#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 >= g_release_need) { // V4.36.8: 按Q档 (高3/中4/低5)
|
||
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 循环）
|
||
*===========================================================================*/
|
||
/* V4.36.8: 按 Q 档(空场频率代理)更新进入/释放确认样本数 — 每样本调用一次
|
||
* 低Q(高频小L): 测量抖动大 → 多样本防假进入/输出抖落
|
||
* 高Q(低频大L): 信号稳 → 维持快响应 (3 样本) */
|
||
static void update_q_confirm_need(void)
|
||
{
|
||
uint32_t _f = calc_freq_khz(loop1_Origin, loop1_LPCNT);
|
||
if (_f > RECONNECT_Q_MID_F_KHZ) { /* 低Q f>100k */
|
||
g_entry_need = ENTRY_CONFIRM_LO;
|
||
g_release_need = REL_CONFIRM_LO;
|
||
g_q_lo = 1; /* V4.36.19: 低Q 释放抗抖档 */
|
||
} else if (_f > RECONNECT_Q_HI_F_KHZ) { /* 中Q 80~100k */
|
||
g_entry_need = ENTRY_CONFIRM_MID;
|
||
g_release_need = REL_CONFIRM_MID;
|
||
g_q_lo = 0;
|
||
} else { /* 高Q f<=80k (含 Origin=0 f=0) */
|
||
g_entry_need = ENTRY_CONFIRM_HI;
|
||
g_release_need = REL_CONFIRM_HI;
|
||
g_q_lo = 0;
|
||
}
|
||
}
|
||
|
||
#define RECONNECT_CONFIRM_TICKS 10 /* V4.36.2: 重连确认窗 10×10ms=100ms
|
||
* 载波连续恢复满窗才确认重连重置;
|
||
* 抖动断开↔恢复每掉线取消重新计 */
|
||
void loop_task_function(void *pvParameters)
|
||
{
|
||
g_sys_freq = g_crm_clocks_freq_struct.sclk_freq;
|
||
|
||
INIT_VD();
|
||
|
||
while (1) {
|
||
/*--- V4.36.14: 主循环拍级 RF 上升沿检测 (不依赖 ISR LOOP_OK0) ---
|
||
* 原 !LOOP_OK0 依赖 ISR 5ms 拍与主循环 150ms 慢 tick 的竞态:
|
||
* 插头抖动(断开↔恢复几拍)可使 LOOP_OK0 卡 1 → 恢复时漏触发确认窗
|
||
* → CAPVD 不重置直接跑判定 → 插回瞬态假"有车"跳输出 (现场 21µH 复现)
|
||
* 现每 10ms 主循环拍自采样 LOOP_OK: 任何 0→1 恢复都强制进确认窗 */
|
||
if (loop1_LOOP_OK && !g_prev_ok) {
|
||
g_prev_ok = 1;
|
||
g_reconn_candidate = 1;
|
||
g_reconn_cnt = 0;
|
||
} else if (!loop1_LOOP_OK) {
|
||
g_prev_ok = 0;
|
||
}
|
||
|
||
if (loop1_LOOP_OK) {
|
||
/*--- 线圈重连: V4.36.2 确认窗 (载波连续恢复 ~100ms 才确认重置) ---
|
||
* RF 上升沿(顶部检测)仅进入候选; 期间保持 g_disconnect_active=1 → 黄灯仍快闪,
|
||
* 断开计数不重复; 抖动(断开↔恢复快速交替)每次掉线取消候选 → 免疫.
|
||
* 确认后才: 清断开标记 / CAPVD 强制重收敛(首样本此时已干净, 无斜率限幅绕过风险) */
|
||
if (g_reconn_candidate) {
|
||
g_reconn_cnt++;
|
||
if (g_reconn_cnt >= RECONNECT_CONFIRM_TICKS) {
|
||
g_reconn_candidate = 0;
|
||
g_reconn_cnt = 0;
|
||
g_disconnect_active = 0; // 确认重连: 清除断开标记
|
||
loop1_CAPVD = 0; // 强制 IIR 从首个有效值重新收敛
|
||
loop1_CAPVD_slow = 0; // 方案A: 释放慢滤波同步收敛
|
||
g_rel_smooth = 0; // V4.36.19: 释放平滑同步收敛
|
||
g_env_wait_cnt = 0; // V4.24: 断开重连后环境学习状态复位
|
||
g_relay_hold_cnt = 0; // V4.36.3: 保持计时复位
|
||
g_relay_hold_done = 0;
|
||
g_reconn_blank_ms = 0; // V4.36.5: 抑制倒计时(首样本直锁时按 Q 定档)
|
||
/* V4.36.15/16: 无车断开重连(运行中 stable=1) → 判稳门控(值稳定才放行);
|
||
* 有车断开重连(VD_FLAG=1, 保持路径输出中)不判稳: 重连后走离开判定;
|
||
* 上电/安全复位判稳(stable=0)不触发: 上电自检归绿灯慢闪, 黄灯不闪 */
|
||
if (loop1_VD_FLAG == 0 && g_loop_stable) {
|
||
g_reconn_stab = 1;
|
||
g_stab_ms = 0;
|
||
g_stab_step_tick = 0;
|
||
g_stab_ok_cnt = 0;
|
||
g_stab_base = 0; // 首样本直锁后置当前 CAPVD
|
||
PRINT("EVT|stab gate on (no-car reconnect)\n");
|
||
} else {
|
||
g_reconn_stab = 0;
|
||
}
|
||
env_resync_abort();
|
||
PRINT("EVT|loop_reconnect confirmed %dms\n",
|
||
RECONNECT_CONFIRM_TICKS * 10);
|
||
}
|
||
/* 确认窗内: 等载波稳定, 不跑检测 (黄灯由 disconnect_active=1 维持快闪) */
|
||
} else {
|
||
if (loop1_CAP_OK) {
|
||
loop1_CAP_OK = 0;
|
||
|
||
/* 首次成功测量 → 标记线圈已连接 */
|
||
if (!g_loop_power_up_state) {
|
||
g_loop_power_up_state = 1;
|
||
}
|
||
|
||
/* 核心检测算法 (确认窗结束后 CAPVD=0 → 首样本干净重建) */
|
||
vd1_task();
|
||
|
||
/* 拨码开关轮询 */
|
||
poll_sw_state();
|
||
}
|
||
}
|
||
} else {
|
||
/*--- 线圈断开 ---*/
|
||
/* V4.36.2: 掉线即取消重连候选 (抖动免疫: 每次掉线重新计确认窗) */
|
||
g_reconn_candidate = 0;
|
||
g_reconn_cnt = 0;
|
||
/* V4.36.3: 有车断开 → 继电器保持(ISR 计时, RELAY_HOLD_MS 兜底释放);
|
||
* 无车断开 → 立即释放 (原行为) */
|
||
if (!(loop1_VD_FLAG && g_loop_power_up_state)) {
|
||
RLY1_OFF;
|
||
RLY2_OFF;
|
||
}
|
||
/* 注意: 不清除 loop1_VD_FLAG，保留断开前的检测状态 */
|
||
|
||
/*
|
||
* 断开计数规则:
|
||
* - 从未连接过上电 (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;
|
||
uint32_t _fk = calc_freq_khz(loop1_CAPVD, loop1_LPCNT);
|
||
PRINT("DBG|t=%ums f=%dkHz L=%luuH 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,
|
||
_fk,
|
||
coil_l_uH(_fk),
|
||
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 + 进入确认 + 斜率限幅
|
||
}
|
||
}
|