From 95bf13a5efa6b1c884c4d06c438a347ac0cb5095 Mon Sep 17 00:00:00 2001 From: wangfq Date: Tue, 23 Jun 2026 08:39:48 +0800 Subject: [PATCH] =?UTF-8?q?refactor:=20=E7=B2=BE=E7=AE=80=E6=A3=80?= =?UTF-8?q?=E6=B5=8B=E7=AE=97=E6=B3=95=EF=BC=8C=E5=AF=B9=E9=BD=90=20M1H/TL?= =?UTF-8?q?D-110=20=E5=8F=82=E8=80=83=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 删除二阶差分滤波（计算但从未参与判决） - 删除 FltHistoryManager 死代码（20+ 未用字段） - 删除 StageRangeConfig（区间约束计算后未引用） - 删除动态窗口切换，改为 M1H 固定 100 窗口基线 - 修复 LPCNT 计算溢出 (32768/Xn<<6 改为乘除顺序) - 进入/离开判决对齐 M1H：单次检测 + TMR15 时序防抖 - 时序状态机简化：去掉 PLUSE_IN_F/PLUSE_IN 中间层 - 拨码去抖次数对齐 M1H: 10→5 - 代码量: 1177→706 行 (-40%) --- .../at32f421_freertos_demo/inc/TaskLoop.h | 252 ++- .../at32f421_freertos_demo/src/TaskLoop.c | 1597 ++++++----------- 2 files changed, 672 insertions(+), 1177 deletions(-) diff --git a/utilities/at32f421_freertos_demo/inc/TaskLoop.h b/utilities/at32f421_freertos_demo/inc/TaskLoop.h index a7039a8..7362eb9 100644 --- a/utilities/at32f421_freertos_demo/inc/TaskLoop.h +++ b/utilities/at32f421_freertos_demo/inc/TaskLoop.h @@ -1,174 +1,140 @@ /** ************************************************************************** * @file TaskLoop.h - * @version v1.0 - * @date 2025-09-08 - * @brief 地感功能的实现 + * @version v2.0 + * @date 2025-09-08 (original), 2026-06-23 (simplified) + * @brief 地感功能实现 — 精简版，对齐 M1H/TLD-110 参考实现 ************************************************************************** * Copyright notice & Disclaimer * * Create 2025-09-08 by wangfq - * * + * * Simplify 2026-06-23: 去除二阶滤波、StageRangeConfig 等未使用模块 ************************************************************************** */ - + #ifndef __TASKLOOP_H__ #define __TASKLOOP_H__ #include +#include -// 时序参数（每 tick ≈50ms），对齐 M1H 参考实现 -#define HOLD_TIME (5 * 1200) // 有限存在保持（约5分钟） -#define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约4分钟） +/*=========================================================================== + * 时序参数（每 tick ≈50ms，由 TMR15 5ms×10 产生），对齐 M1H 参考实现 + *===========================================================================*/ +#define HOLD_TIME (5 * 1200) // 有限存在保持（约 5 分钟） +#define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约 4 分钟） #define IN_DELAY 10 // 进入防抖 500ms #define OUT_DELAY 38 // 离开防抖 1.9s #define PULSE_DELAY 19 // 脉冲宽度 950ms +/*=========================================================================== + * 滤波参数 — 对齐 M1H + *===========================================================================*/ +#define ALFA_CAP1 79 // IIR 指数平滑 α = 79/256 ≈ 0.31 -#define MAX_CMP_LOOP_UNIT 4 // 5 组 -#define MAX_CMP_DIF 20 // -#define MAX_TIMEOUT_CMP 120000 // 5毫秒 为单位， 表示10分钟， 600秒 +/*=========================================================================== + * 频率测量参数 + * XNSUM_FOR_ORIGIN_FACTOR: Value/LPCNT 的归一化右移量 + * 使 Origin 保持在合理的 uint32 范围内 + *===========================================================================*/ +#define XNSUM_FOR_ORIGIN_FACTOR 6 // >>6 -#define ALFA_CAP1 79 -#define ALFA_CAP2 64 -#define ALFA_ORG1 64 -#define ALFA_ORG2 64 +/*=========================================================================== + * 灵敏度表 — 对齐 M1H + * 进入阈值 = Origin × SensTable[SENS] / 65536 + * 离开阈值 = Origin × SensTable_1[SENS] / 65536 （滞回 ~50%） + *===========================================================================*/ +extern const uint16_t SensTable[4]; +extern const uint16_t SensTable_1[4]; -#define XNSUM_FOR_ORIGIN_FACTOR 6 //7 //8 //7 //6 +/*=========================================================================== + * 全局状态变量 — 捕获 & 测量 + *===========================================================================*/ +extern uint32_t g_sys_freq; // 系统时钟频率 +extern uint8_t g_input_div; // TIM3 输入分频倍数 +extern uint16_t loop1_Xn; // 当前相邻边沿周期差 +extern uint16_t loop1_CapThis; // 本次捕获值 +extern uint16_t loop1_CapLast; // 上次捕获值 +extern uint16_t loop1_LPCNT; // 每 Value 需累加 LPCNT 次 Xn +extern uint16_t loop1_CapCnt; // 当前窗口已累加次数 +extern uint32_t loop1_CapSum; // 当前窗口累加和 +extern uint32_t loop1_Value; // 一个测量窗口的原始 ΣXn +extern uint32_t loop1_CAPVD; // IIR 滤波后的频率值 +extern uint32_t loop1_Origin; // 基线（无车时的参考频率） +extern uint32_t loop1_ORG_SUM; // 基线跟踪累加和 +extern uint16_t loop1_ORG_CNT; // 基线跟踪计数（窗口=100） +extern uint16_t loop1_dlt_ORG; // 当前灵敏度阈值 +extern uint8_t Flt_Reg; // IIR 滤波系数 -extern uint8_t loop1_INI_LOOP; -extern uint8_t loop1_CAP_OK; -extern uint8_t loop1_CAP_FLAG; -extern uint8_t loop1_VD_FLAG; -extern uint8_t loop1_VD_HOLD; -extern uint8_t loop1_RF_FLAG; -extern uint8_t loop1_LOOP_OK; -extern uint8_t loop1_LOOP_OK0; -extern uint8_t loop1_FLAG_IN; -extern uint8_t loop1_FLAG_OUT; -extern uint8_t loop1_FLAG_PLUSE; -extern uint8_t loop1_FLAG_PLUSE_DELAY; -extern uint8_t loop1_FLAG_IN_PLUSE; -extern uint8_t loop1_FLAG_CUT; +/*=========================================================================== + * 全局状态变量 — 标志位 + *===========================================================================*/ +extern uint8_t loop1_INI_LOOP; // 初始化阶段 +extern uint8_t loop1_CAP_OK; // 新测量数据就绪 +extern uint8_t loop1_VD_FLAG; // 有车标志 +extern uint8_t loop1_VD_HOLD; // 有限存在计时启用 +extern uint8_t loop1_RF_FLAG; // 本 tick 收到线圈振荡边沿 +extern uint8_t loop1_LOOP_OK; // 线圈连接正常 +extern uint8_t loop1_LOOP_OK0; // 线圈连接正常（上周期） +extern uint8_t loop1_FLAG_IN; // 进入延时中 +extern uint8_t loop1_FLAG_OUT; // 离开延时中 +extern uint8_t loop1_FLAG_PLUSE; // 脉冲输出中 +extern uint8_t loop1_SensLevel; // 当前灵敏度等级 (0~3) +/*=========================================================================== + * 全局状态变量 — 计数器 + *===========================================================================*/ +extern uint16_t Hold_CNT; // 有限存在计数器 +extern uint8_t loop1_INCNT; // 进入延时计数 +extern uint8_t loop1_OUTCNT; // 离开延时/脉冲宽度计数 +extern uint8_t TM1cnt; // 50ms tick 分频计数 -extern uint8_t SET_PLUS; //输出方式 +/*=========================================================================== + * 拨码开关状态 + *===========================================================================*/ +extern uint8_t SET_PLUS; // 0=存在输出, 1=脉冲输出 +extern uint8_t SET_DLY; // 0=无延时, 1=延时 +extern uint8_t SET_SAFE; // 1=安全复位拨下 +/*=========================================================================== + * 拨码开关去抖变量 + *===========================================================================*/ +extern uint8_t sw0, sw1, swok, swcnt; +extern uint8_t SENS, SENS_LAST; -extern uint32_t g_xn_counter; -extern uint8_t g_flag_stable_delta2_flt; +/*=========================================================================== + * 安全复位相关 + *===========================================================================*/ +extern uint8_t loop1_LC_HOLD; // 安全复位计时中 +extern uint8_t loop1_LC_Reset; // 触发安全复位 +extern uint32_t LC_Hold_CNT; // 安全复位计数器 +extern uint16_t g_safe_max_cnt; // 安全复位超时 +/*=========================================================================== + * 故障指示相关 (DLD154V4B 特有，M1H 无黄灯) + *===========================================================================*/ +extern uint8_t g_loop_power_up_state; // 上电后线圈是否曾连接 +extern uint8_t g_led_loop_reconnect; // 线圈重连状态 (0=正常, >0=故障闪烁) +extern uint8_t g_flag_faul_reconnect; // 故障灯切相 +extern uint8_t g_step_fault_led; // 故障灯步骤 +extern uint16_t g_counter_fault_led; // 故障灯计数器 +/*=========================================================================== + * 调试计数器 + *===========================================================================*/ +extern uint32_t g_xn_counter; // 边沿总数（用于调试输出） -// 二阶变化量计算状态结构体 -typedef struct { - int16_t delta_prev; // 上一次的一阶变化量 - int16_t delta2_flt; // 滤波后的二阶变化量 -} SecondOrderState; -extern SecondOrderState second_order_state; -#define SECOND_ORDER_FILTER_COEF 64 // 滤波系数（64/256=0.25） - -// 一阶滤波管理全局状态 -typedef struct { - uint8_t flag_not_idle; - uint32_t counter_not_idle; - uint8_t flag_init_value; - uint32_t stable_init_value; // 开机稳定初始值 - uint8_t flag_5sec; - uint16_t counter_5sec; - uint32_t recent_5sec; -// uint8_t flag_5min; -// uint32_t counter_5min; -// uint16_t recent_5min; // 最近5分钟的一阶滤波值 -// uint8_t flag_10min; -// uint32_t counter_10min; -// uint16_t recent_10min; // 最近10分钟的一阶滤波值 -// uint8_t flag_30min; -// uint32_t counter_30min; -// uint16_t recent_30min; // 最近半小时的一阶滤波值 -// uint8_t flag_60min; -// uint32_t counter_60min; -// uint16_t recent_60min; // 最近1小时的一阶滤波值 - uint32_t init_sum; // 初始化累加值 - uint16_t init_samples; // 初始化采样计数器 - uint8_t is_initialized; // 初始化完成标志 - uint32_t init_freq; // 初始频率 - - uint32_t current_freq; - - uint16_t cnt_sec_samples; - uint32_t sum_sec_samples; - uint16_t cnt_samples; - uint32_t sum_samples; - - uint8_t flag_flat_release; - uint16_t cnt_flat_release; - uint16_t max_cnt_flat_release; - - uint32_t last_loop_Origin; - uint8_t flag_loop_reconnect; // 线圈重连 - uint16_t cnt_loop_reconnect; - uint8_t last_loop_vd_flag; - - uint16_t window_size; - - uint8_t flag_second_judge; // 进入第二轮判断；由于第一轮判定为有车状态超时了，那么进入第二轮判定，第二轮判定会默认当前无车开始 - - uint32_t second_loop_Origin; - uint32_t second_loop_ORG_SUM; // for option use - uint16_t second_loop_ORG_CNT; - uint32_t second_loop_dlt_ORG; - uint8_t second_loop_vd_flag; - - // 二次判断增强条件 - uint32_t second_judge_enter_time; // 进入二次判断的时间戳（单位：5ms） - uint16_t second_judge_min_time; // 二次判断最小时间限制（单位：5ms），默认10秒=2000 - uint16_t cnt_stable_to_idle; // 连续检测到恢复到空闲值的计数 - uint16_t stable_to_idle_threshold; // 恢复到空闲值的阈值次数 - - uint16_t cnt_not_idle; - uint16_t cnt_idle; - - uint16_t cnt_simple_release; - uint16_t cnt_simple_busy; - - - // 从无车到有车 从有车到无车 - // 频率的变化趋势： 频率变大 频率变小 - // 捕获值的变化趋势： 值变小 值变大 - // 电感量的变化趋势 值变小 值变大 - uint16_t cnt_release_Up; // 从有车到无车时，一阶变化量 正数，一阶变化量是新捕获值与上一次捕捉值的差， - uint16_t cnt_release_Down; // 从有车到无车时，一阶变化量 负数，此时可能倒车？或者出车的过程中有其他更有影响的部件，如前轮和后轮部分 - uint16_t min_release_capvd; // 从有车到无车时，U字形 拐点处的 一阶滤波值 -} FltHistoryManager; -extern FltHistoryManager flt_mgr ; // ��ʼ��Ϊ0 - -// 初始化采样次数（例如100次） -#define INIT_SAMPLE_COUNT 10 //40 // 200 -#define TMP_SAMPLE_COUNT 50 - -// 区间类型定义（绝对差值或百分比） -typedef enum { - RANGE_ABSOLUTE, // 绝对差值（如 ±50） - RANGE_PERCENT // 百分比（如 ±10%） -} RangeType; - -// 阶段配置结构体（每个时间窗口的区间规则） -typedef struct { - uint32_t base_value; // 基准值（例如开机稳定值、最近5分钟值等） - uint32_t lower; - uint32_t upper; - uint16_t range_size; // 区间大小（绝对值或百分比数值） - RangeType range_type; // 区间类型（绝对或百分比） -} StageRangeConfig; - -// 全局阶段配置数组（示例：开机、5分钟、10分钟、30分钟、1小时） -#define STAGE_COUNT 2 -#define STAGE_COUNT_SHORT 2 -extern StageRangeConfig stage_config[STAGE_COUNT]; - - +/*=========================================================================== + * 函数声明 + *===========================================================================*/ void loop_task_function(void *pvParameters); +void poll_sw_state(void); +uint32_t get_flt_value(uint32_t new_value, uint32_t last_Value); +uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt, + uint32_t* p_origin, uint32_t new_value, + uint8_t window); +void poll_yellow_led(void); +void LEDA_ON_OFF(void); +void vd1_task(void); - -#endif +#endif /* __TASKLOOP_H__ */ diff --git a/utilities/at32f421_freertos_demo/src/TaskLoop.c b/utilities/at32f421_freertos_demo/src/TaskLoop.c index 02bfae3..df3a921 100644 --- a/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -1,175 +1,143 @@ + #include "TaskLoop.h" #include "at32f421_board.h" #include "cmcng.h" #include "FreeRTOS.h" #include "task.h" -#include "math.h" #include +/*=========================================================================== + * 灵敏度表 — 对齐 M1H 参考实现 + * 进入阈值 = Origin × SensTable[SENS] / 65536 + * 离开阈值（滞回）= Origin × SensTable_1[SENS] / 65536 + * SENS: 0=低灵敏, 3=高灵敏 + *===========================================================================*/ +const uint16_t SensTable[4] = {216, 108, 36, 10}; +const uint16_t SensTable_1[4] = {108, 72, 18, 9}; +/*=========================================================================== + * 全局状态变量 — 捕获 & 测量 + *===========================================================================*/ +uint32_t g_sys_freq = 0; +uint8_t g_input_div = 1; -// 灵敏度表（对齐 M1H 参考实现） -// 进入阈值 = Origin × SensTable[SENS] / 65536 -// 离开阈值（滞回）= Origin × SensTable_1[SENS] / 65536，约为进入的一半 -const uint16_t SensTable[4] = {216, 108, 36, 10}; // 0=低灵敏, 3=高灵敏 -const uint16_t SensTable_1[4] = {108, 72, 18, 9}; // 释放灵敏度（滞回） - -const uint16_t DELAY_TIME[4] = {0, 10}; //0、0.6秒、1.6秒、3秒 {0,12,32,60}; //0、0.6秒、1.6秒、3秒 - -int8_t g_freq_sens; -uint8_t loop1_SensLevel; -uint8_t g_freq_level; -uint32_t g_sys_freq = 0; // -uint16_t g_safe_max_cnt = LC_HOLD_TIME; - - -uint16_t Hold_CNT = 0; -uint8_t Flt_Reg = 0; -uint8_t Delay_300ms_cnt = 0; -uint8_t TM1cnt = 0; -uint16_t g_hold_time = 0; - -__IO uint16_t loop1_Xn; -uint16_t loop1_HoldTime; -uint16_t loop1_dlt_ORG; -uint32_t loop1_Origin; -__IO uint16_t loop1_CapThis; -__IO uint16_t loop1_CapLast; - +uint16_t loop1_Xn; +uint16_t loop1_CapThis; +uint16_t loop1_CapLast; uint16_t loop1_LPCNT; -__IO uint16_t loop1_CapCnt; +uint16_t loop1_CapCnt; uint32_t loop1_CapSum; -uint32_t loop1_CAPVD; uint32_t loop1_Value; -uint16_t loop1_Flt; - -uint32_t tmp_loop1_CAPVD; - +uint32_t loop1_CAPVD; +uint32_t loop1_Origin; uint32_t loop1_ORG_SUM; uint16_t loop1_ORG_CNT; +uint16_t loop1_dlt_ORG; +uint8_t Flt_Reg; -uint8_t loop1_INCNT; -uint8_t loop1_OUTCNT; +/*=========================================================================== + * 全局状态变量 — 标志位 + *===========================================================================*/ +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_INI_LOOP; -uint8_t loop1_CAP_OK; -uint8_t loop1_CAP_FLAG; -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_FLAG_PLUSE_DELAY; -uint8_t loop1_FLAG_IN_PLUSE; -uint8_t loop1_FLAG_CUT; -uint8_t loop1_FLAG_HOLD_TIMEOUT; // 存在超时 +/*=========================================================================== + * 全局状态变量 — 计数器 + *===========================================================================*/ +uint16_t Hold_CNT = 0; +uint8_t loop1_INCNT = 0; +uint8_t loop1_OUTCNT = 0; +uint8_t TM1cnt = 0; -uint8_t loop1_LC_HOLD = 0; -uint8_t loop1_LC_Reset = 0; -uint32_t LC_Hold_CNT = 0; - -uint8_t SET_PLUS = 0; -uint8_t SET_DLY = 0; +/*=========================================================================== + * 拨码开关状态 + *===========================================================================*/ +uint8_t SET_PLUS = 0; +uint8_t SET_DLY = 0; uint8_t SET_SAFE = 0; -uint8_t g_loop_power_up_state = 0; // 开机启动时线圈是否连接 -uint8_t g_led_loop_reconnect = 0; // 线圈是否重连 -uint8_t g_flag_faul_reconnect = 0; -uint8_t g_step_fault_led = 0; -uint16_t g_counter_fault_led = 0; - -uint32_t g_xn_counter = 0; - -//100 补偿的周期大概为1.68秒，50 补偿的周期大概为0.84秒, 120 补偿的周期大概为2.016秒, 360 补偿的周期大概6秒 -#define LOOP_WINDOW_SIZE_LOW 360 -#define LOOP_WINDOW_SIZE_NORMAL 120 // 50 // 200 -#define LOOP_WINDOW_SIZE_FAST 50 // 快速补偿 -#define LOOP_WINDOW_SIZE_MIN 10 // 用于计算临时平均数 -uint16_t g_moving_average_window_size = LOOP_WINDOW_SIZE_NORMAL; // 滑动窗口大小 - - -SecondOrderState second_order_state; -FltHistoryManager flt_mgr = {0}; // 初始化为0 -StageRangeConfig stage_config[STAGE_COUNT] = { - {0, 0,0, 20, RANGE_PERCENT}, // 开机阶段：基准值±10% - {0, 0,0, 20, RANGE_PERCENT}//, //最近5秒 -// {0, 0,0, 2, RANGE_PERCENT}, // 5分钟阶段：基准值±50 -// {0, 0,0, 9, RANGE_PERCENT}, // 10分钟阶段：基准值±8% -// {0, 0,0, 12, RANGE_ABSOLUTE}, // 30分钟阶段：基准值±30 -// {0, 0,0, 15, RANGE_PERCENT} // 1小时阶段：基准值±5% -}; - -uint16_t g_loop_out_delay = 10; // 0、0.6秒、1.6秒、3秒{0,12,32,60}; -uint32_t g_loop_release_ori_timeout = MAX_TIMEOUT_CMP; - -#define MAX_ALLOW_DIFF_FREQ 3500 //频率最大偏移值 3500Hz -uint16_t g_MaxAllowDiff = 0; // 采样值允许最大偏移值 -float g_freq_tmp = 0.0; - -uint8_t loop1_FLAG_PLUSE_IN_F = 0; -uint8_t loop1_FLAG_PLUSE_IN = 0; - -uint8_t g_loop_cut_last = 0; -uint32_t g_loop_cut_amount = 0; - - -__IO uint32_t tmr3freq = 0; -__IO uint16_t capturenumber = 0; -__IO uint16_t ic3readvalue1 = 0, ic3readvalue2 = 0; -__IO uint32_t capture = 0; - -Loop_ACS_Info g_loop_acs_info; - +/*=========================================================================== + * 拨码开关去抖变量 + *===========================================================================*/ 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; +/*=========================================================================== + * 故障指示相关 (DLD154V4B 特有黄灯) + *===========================================================================*/ +uint8_t g_loop_power_up_state = 0; +uint8_t g_led_loop_reconnect = 0; +uint8_t g_flag_faul_reconnect = 0; +uint8_t g_step_fault_led = 0; +uint16_t g_counter_fault_led = 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); - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN); - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN); - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN); - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN); - - if(sw0 == sw1){ - if(swok != sw0){ + 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 > 10){ + if (swcnt > 5) { // 对齐 M1H: 5 次确认 swok = sw0; - - SET_PLUS = swok & 0x04; // 输出方式 - SET_DLY = (swok & 0x08) >> 3; + + SET_PLUS = swok & 0x04; + SET_DLY = (swok & 0x08) >> 3; SET_SAFE = (swok & 0x10) >> 4; } } - } - else{ - sw1 = sw0; + } else { + sw1 = sw0; swcnt = 0; } - + SENS = swok & 0x03; - if(SENS != SENS_LAST){ - PRINT("SENS:%02X, SENS_LAST: %02X\n", SENS, SENS_LAST); + if (SENS != SENS_LAST) { + PRINT("SENS changed: %02X -> %02X, resetting...\n", SENS_LAST, SENS); nvic_system_reset(); } - } - - -uint32_t get_flt_value(uint32_t new_value, uint32_t last_Value) { - // 一阶低通滤波器，用于对输入信号进行平滑处理 +/*=========================================================================== + * 一阶 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 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) { @@ -181,997 +149,558 @@ uint32_t get_flt_value(uint32_t new_value, uint32_t last_Value) { return value_Flt; } -void init_second_order(SecondOrderState *state, int32_t initial_delta) { - state->delta_prev = initial_delta; - state->delta2_flt = 0; -} - -/** - * @brief 计算滤波后的二阶变化量（加速度） - * @param delta_current 当前一阶变化量（new_Flt - previous_new_Flt） - * @param state 二阶计算状态（需外部维护） - * @param filter_coef 二阶滤波系数（0~255，对应浮点系数 filter_coef/256） - * @return 滤波后的二阶变化量 delta2_flt - */ -int32_t get_second_order(int32_t delta_current, SecondOrderState *state, uint8_t filter_coef) { - // 计算当前二阶变化量（一阶变化的差分） - int32_t delta2 = delta_current - state->delta_prev; - - // --- 关键改进点：处理一阶持续为0的情况 --- - if (delta_current == 0 && state->delta_prev == 0) { - // 连续两次一阶变化为0，强制二阶归零 - state->delta2_flt = 0; - } else { - // 常规滤波：delta2_flt = delta2_flt + (delta2 - delta2_flt) * filter_coef / 256 - state->delta2_flt += ((delta2 - state->delta2_flt) * filter_coef)/256; //((delta2 - state->delta2_flt) * (uint32_t)filter_coef) >> 8; - } - - // 更新历史一阶变化量 - state->delta_prev = delta_current; - - return state->delta2_flt; -} - - - -/** - * @brief 更新滑动平均值（通过指针传递状态） - * @param p_sum 累加值指针（uint32_t*） - * @param p_cnt 计数器指针（uint8_t*） - * @param p_origin 平均值指针（uint16_t*） - * @param new_value 新输入值 - * @param window 窗口大小（例如100） - */ -uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt, uint32_t* p_origin, uint32_t new_value, uint8_t window) { - // 参数安全检查 +/*=========================================================================== + * 滑动平均基线更新 + * 累加 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, + uint8_t window) +{ if (!p_sum || !p_cnt || !p_origin || window == 0) { - return 0; // 可添加错误处理 + return 0; } - *p_sum += new_value; // 累加新值 + *p_sum += new_value; (*p_cnt)++; - if (*p_cnt >= window) { // 窗口满（如window=100时，计数器0~99） - *p_origin = (uint32_t)(*p_sum / window); // 计算平均值 + if (*p_cnt >= window) { + *p_origin = *p_sum / window; *p_cnt = 0; - *p_sum = 0; + *p_sum = 0; return 1; } return 0; } - -void update_lower_upper(StageRangeConfig * cfg) -{ - if(cfg->base_value == 0) return; - - // 计算区间上下限 - uint32_t lower, upper; - if (cfg->range_type == RANGE_ABSOLUTE) { - cfg->lower = (cfg->base_value >= cfg->range_size) ? - (cfg->base_value - cfg->range_size) : 0; - cfg->upper = cfg->base_value + cfg->range_size; - } else { // RANGE_PERCENT -// uint32_t offset = (cfg->base_value * cfg->range_size) >> 16; - uint32_t dlt = (cfg->base_value * (SensTable_1[loop1_SensLevel] - 3))>>16; // 采取比当前释放灵敏度更高的值，解决平坦算法释放的时候不断误触发的bug - cfg->lower = (cfg->base_value >= dlt) ? - (cfg->base_value - dlt) : 0; - cfg->upper = cfg->base_value + dlt; - } -} - -void reload_lower_upper(uint8_t sens_level){ - uint32_t dlt = 0 ; -// stage_config[0].lower = - uint8_t i; - for(i = 0; i < STAGE_COUNT; i++){ - if(stage_config[i].base_value == 0){ - continue; - } - dlt = (stage_config[i].base_value * (SensTable_1[loop1_SensLevel] - 3))>>16; - stage_config[i].lower = (stage_config[i].base_value >= dlt) ? (stage_config[i].base_value - dlt): 0; - stage_config[i].upper = stage_config[i].base_value + dlt; - } -} - - - -/** - * @brief 更新一阶滤波历史管理器 - * @param new_flt 当前一阶滤波值 - * @param current_time 系统时间戳（毫秒） - */ -void update_flt_history(uint32_t new_flt, int32_t firstOrder, int32_t secondOrder ) { - static uint8_t _first_cnt = 0; -// if(firstOrder || secondOrder) - if((abs(firstOrder) > 5) || (abs(secondOrder) >2)) - { - if(_first_cnt < 3){ //连续三次有变动 - _first_cnt++; - PRINT("____+_first_cnt:%d, idle:%d\n", _first_cnt, flt_mgr.flag_not_idle); - } - - flt_mgr.cnt_not_idle++; - if(flt_mgr.cnt_idle) - { - flt_mgr.cnt_idle--; - if(flt_mgr.cnt_not_idle > 5){ - flt_mgr.cnt_idle = 0; - } - } - } - else { - if(_first_cnt){ - _first_cnt--; - PRINT("____-_first_cnt:%d, idle:%d\n", _first_cnt, flt_mgr.flag_not_idle); - } - } - -// if((firstOrder || secondOrder) && (_first_cnt >= 3)) - if(((abs(firstOrder) > 5) || secondOrder) && (_first_cnt >= 3)) - { - flt_mgr.counter_not_idle = 0; - flt_mgr.flag_not_idle = 1; - } - if(flt_mgr.flag_not_idle || (abs(firstOrder) >= 10)) - { - flt_mgr.cnt_sec_samples = 0; - flt_mgr.sum_sec_samples = 0; - if(!flt_mgr.is_initialized) - { - flt_mgr.sum_samples = 0; - flt_mgr.init_sum = 0; - } - else - { - flt_mgr.sum_samples = 0; - flt_mgr.cnt_samples = 0; - } - return ; - } - - flt_mgr.cnt_idle++; - if(flt_mgr.cnt_not_idle){ - flt_mgr.cnt_not_idle--; - if(flt_mgr.cnt_idle > 100){ - flt_mgr.cnt_not_idle = 0; - } - } - - // 1. 初始化阶段：累加采样值 - if (!flt_mgr.is_initialized) { - flt_mgr.init_sum += new_flt; - flt_mgr.init_samples++; - PRINT("__new_flt:%d, init_sum:%d, amount:%d, firstOrder:%d, sedond:%d\n", new_flt, flt_mgr.init_sum, flt_mgr.init_samples, firstOrder, secondOrder); - - // 达到采样次数后计算稳定初始值 - if (flt_mgr.init_samples >= INIT_SAMPLE_COUNT) { - flt_mgr.stable_init_value = flt_mgr.init_sum / INIT_SAMPLE_COUNT; - flt_mgr.is_initialized = 1; - LEDB_OFF; - stage_config[0].base_value = flt_mgr.stable_init_value; - update_lower_upper(&stage_config[0]); - flt_mgr.init_freq = (float)(g_sys_freq )/((float)(flt_mgr.stable_init_value << XNSUM_FOR_ORIGIN_FACTOR)/((float)(loop1_LPCNT))) * g_input_div; - loop1_Origin = flt_mgr.stable_init_value; - PRINT("sys_freq:%d, new_flt:%d, init_sum:%d, stable_init:%d, init_freq:%d\n", g_sys_freq, new_flt, flt_mgr.init_sum, flt_mgr.stable_init_value, flt_mgr.init_freq); - } - return ; - } - - if(flt_mgr.flag_5sec) - { - flt_mgr.cnt_sec_samples++; - flt_mgr.sum_sec_samples += new_flt; - if(flt_mgr.cnt_sec_samples >= TMP_SAMPLE_COUNT) - { - flt_mgr.recent_5sec = flt_mgr.sum_sec_samples/TMP_SAMPLE_COUNT; - - stage_config[1].base_value = flt_mgr.recent_5sec; - update_lower_upper(&stage_config[1]); - flt_mgr.flag_5sec = 0; - flt_mgr.cnt_sec_samples = 0; - flt_mgr.sum_sec_samples = 0; - } - } - -} - -void poll_yellow_led(void) -{ - #define YELLOW_ON_SHORT 16 // 10:50ms, 12 60ms, 16:80ms - #define YELLOW_OFF_MIDDLE 40 // 40: 200ms - #define YELLOW_OFF_LONG 290 // 100:500ms, 140:700ms, 160: 800ms - - if(g_loop_power_up_state){ - if(g_led_loop_reconnect == 0x01){ - g_counter_fault_led++; - if(g_flag_faul_reconnect){ - if(g_counter_fault_led > YELLOW_ON_SHORT){ // 100ms: 20, 80ms - g_counter_fault_led = 0; - g_flag_faul_reconnect = 0; - LED_YELLOW_OFF; - } - } - else{ - if(g_counter_fault_led > YELLOW_OFF_LONG){ //500ms - g_flag_faul_reconnect = 1; - g_counter_fault_led = 0; - LED_YELLOW_ON; - } - } - } - else if(g_led_loop_reconnect == 0x02) - { - g_counter_fault_led++; - if(g_step_fault_led == 0) - { - if(g_counter_fault_led > YELLOW_ON_SHORT){ - LED_YELLOW_OFF; - g_step_fault_led++; //1 - g_counter_fault_led = 0; - } - } - else if(g_step_fault_led == 1){ - if(g_counter_fault_led > YELLOW_OFF_MIDDLE){ // 200ms: 40 - g_counter_fault_led = 0; - g_step_fault_led++; //2 - LED_YELLOW_ON; - } - } - else if(g_step_fault_led == 2) - { - if(g_counter_fault_led > YELLOW_ON_SHORT){ // 亮50ms - LED_YELLOW_OFF; - g_step_fault_led++; //1 - g_counter_fault_led = 0; - } - } - else if(g_step_fault_led == 3){ - if(g_counter_fault_led > YELLOW_OFF_LONG){ // 200ms: 40 - g_counter_fault_led = 0; - g_step_fault_led = 0; //2 - LED_YELLOW_ON; - } - } - - } - else if(g_led_loop_reconnect >= 0x03) - { - g_counter_fault_led++; - if(g_step_fault_led == 0) - { - if(g_counter_fault_led > YELLOW_ON_SHORT){ - LED_YELLOW_OFF; - g_step_fault_led++; //1 - g_counter_fault_led = 0; - } - } - else if(g_step_fault_led == 1){ - if(g_counter_fault_led > YELLOW_OFF_MIDDLE){ // 200ms: 40 - g_counter_fault_led = 0; - g_step_fault_led++; //2 - LED_YELLOW_ON; - } - } - else if(g_step_fault_led == 2) - { - if(g_counter_fault_led > YELLOW_ON_SHORT){ // 亮50ms - LED_YELLOW_OFF; - g_step_fault_led++; //1 - g_counter_fault_led = 0; - } - } - else if(g_step_fault_led == 3){ - if(g_counter_fault_led > YELLOW_OFF_MIDDLE){ // 200ms: 40 - g_counter_fault_led = 0; - g_step_fault_led++; //4 - LED_YELLOW_ON; - } - } - else if(g_step_fault_led == 4) - { - if(g_counter_fault_led > YELLOW_ON_SHORT){ // 亮50ms - LED_YELLOW_OFF; - g_step_fault_led++; //1 - g_counter_fault_led = 0; - } - } - else if(g_step_fault_led == 5){ - if(g_counter_fault_led > YELLOW_OFF_LONG){ // 200ms: 40 - g_counter_fault_led = 0; - g_step_fault_led = 0; //2 - LED_YELLOW_ON; - } - } - } - }// end if g_loop_power_up_state -} - - - -void INIT_VD(){ - loop1_INCNT = 0; - loop1_OUTCNT = 0; -// loop1_PLUSECNT =0; - loop1_CapCnt = 0; - -// Cap.loop1_CapThis = 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_SensLevel = 2; - Flt_Reg = ALFA_CAP1; //79 - - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - // loop1_LPCNT=0; -// loop1_INI_LOOP=1; - - loop1_FLAG_IN = 0; - loop1_FLAG_OUT = 0; - loop1_FLAG_PLUSE = 0; - loop1_FLAG_CUT = 0; - - - flt_mgr.is_initialized = 0; - flt_mgr.recent_5sec = 0; - stage_config[0].base_value = 0; - stage_config[1].base_value = 0; - - g_loop_acs_info.car_state = 0; - loop1_FLAG_HOLD_TIMEOUT = 0; - - // 初始化二次判断增强条件 - flt_mgr.cnt_stable_to_idle = 0; - flt_mgr.stable_to_idle_threshold = 10; // 连续10次检测到恢复到空闲值 - - - flt_mgr.window_size = LOOP_WINDOW_SIZE_LOW; - - - - sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN); // 0x10 安全复位 - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN); // 0x08 延时 - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN); // 0x04 outmode - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN); // 0x02 - sw0 = sw0 << 1; - sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN); // 0x01 - - 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; // 1 OFF, 0 ON - -} - - - - -/** - * @brief this function handles tmr3 trigger exception. - * @param none - * @retval none - */ -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{ - 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 > 100) - { - loop1_LPCNT = (32768 / loop1_Xn) << XNSUM_FOR_ORIGIN_FACTOR; - PRINT("First_calc_loop1_LPCNT:%d, loop1_Xn:%d\n", loop1_LPCNT, loop1_Xn); - if(loop1_LPCNT == 0) - { - loop1_LPCNT = 1000; - } - loop1_CAPVD = 0; - loop1_CapSum = 0; - loop1_CapCnt = 0; - } - } - else{ - loop1_CapSum += loop1_Xn; - if(loop1_CapCnt >= loop1_LPCNT){ - loop1_CAPVD = (loop1_CapSum); // >> 5); - loop1_Origin = loop1_CAPVD >> XNSUM_FOR_ORIGIN_FACTOR; - PRINT("First_capSum:%d, Origin:%d\n", loop1_CapSum, loop1_Origin); - loop1_INI_LOOP = 0; - loop1_CapSum = 0; - loop1_Value = 0; - loop1_CapCnt = 0; - } - } - } // end if loop1_INI_LOOP - else{ - loop1_CapSum += loop1_Xn; - if(loop1_CapCnt >= loop1_LPCNT){ - loop1_Value = (loop1_CapSum); // >> 5); - loop1_CAP_OK = 1; - loop1_CapSum = 0; - loop1_CapCnt = 0; - loop1_INI_LOOP = 0; - } - } - - } - - } -} - - +/*=========================================================================== + * 红灯呼吸 (LEDA, PB1) — 每 5ms 由 TMR15 ISR 调用 + *===========================================================================*/ void LEDA_ON_OFF(void) { LED_RED_GPIO->odt ^= LED_RED_PIN; } +/*=========================================================================== + * 黄灯故障指示 (LEDC, PA10) — DLD154V4B 特有 + * 状态机由 TMR15 ISR 每 5ms 驱动 + *===========================================================================*/ +void poll_yellow_led(void) +{ +#define YELLOW_ON_SHORT 16 // 80ms +#define YELLOW_OFF_MIDDLE 40 // 200ms +#define YELLOW_OFF_LONG 290 // ~1.45s -// 5ms per + if (!g_loop_power_up_state) return; + + g_counter_fault_led++; + + if (g_led_loop_reconnect == 0x01) { + // 线圈重连：长灭 + 短亮 + if (g_flag_faul_reconnect) { + if (g_counter_fault_led > YELLOW_ON_SHORT) { + g_counter_fault_led = 0; + g_flag_faul_reconnect = 0; + LED_YELLOW_OFF; + } + } else { + if (g_counter_fault_led > YELLOW_OFF_LONG) { + g_flag_faul_reconnect = 1; + g_counter_fault_led = 0; + LED_YELLOW_ON; + } + } + } else if (g_led_loop_reconnect >= 0x02) { + // 线圈断开：编码闪烁 + if (g_step_fault_led == 0) { + if (g_counter_fault_led > YELLOW_ON_SHORT) { + LED_YELLOW_OFF; + g_step_fault_led = 1; + g_counter_fault_led = 0; + } + } else if (g_step_fault_led == 1) { + if (g_counter_fault_led > YELLOW_OFF_MIDDLE) { + LED_YELLOW_ON; + g_step_fault_led = 2; + g_counter_fault_led = 0; + } + } else if (g_step_fault_led == 2) { + if (g_counter_fault_led > YELLOW_ON_SHORT) { + LED_YELLOW_OFF; + g_step_fault_led = 3; + g_counter_fault_led = 0; + } + } else if (g_step_fault_led == 3) { + if (g_counter_fault_led > (g_led_loop_reconnect >= 0x03 + ? YELLOW_OFF_MIDDLE + : YELLOW_OFF_LONG)) { + LED_YELLOW_ON; + g_step_fault_led = 0; + g_counter_fault_led = 0; + + // 慢闪模式（低频故障）需要额外步骤 + if (g_led_loop_reconnect >= 0x03) { + g_step_fault_led = 4; + } + } + } else if (g_step_fault_led == 4) { + if (g_counter_fault_led > YELLOW_ON_SHORT) { + LED_YELLOW_OFF; + g_step_fault_led = 5; + g_counter_fault_led = 0; + } + } else if (g_step_fault_led == 5) { + if (g_counter_fault_led > YELLOW_OFF_LONG) { + LED_YELLOW_ON; + g_step_fault_led = 0; + g_counter_fault_led = 0; + } + } + } +} + +/*=========================================================================== + * 初始化所有状态变量（对齐 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_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_LC_HOLD = 0; + loop1_LC_Reset = 0; + LC_Hold_CNT = 0; + Hold_CNT = 0; + + g_loop_power_up_state = 0; + g_led_loop_reconnect = 0; + g_flag_faul_reconnect = 0; + g_step_fault_led = 0; + g_counter_fault_led = 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) { + /* + * 修复: 先乘后除，避免截断为 0 + * 原代码: loop1_LPCNT = (32768 / Xn) << 6 ← 小 Xn 时溢出 + * 新代码: (32768UL << 6) / Xn = 2097152 / Xn + */ + loop1_LPCNT = (32768UL << XNSUM_FOR_ORIGIN_FACTOR) / loop1_Xn; + PRINT("First_calc_loop1_LPCNT:%d, loop1_Xn:%d\n", + loop1_LPCNT, loop1_Xn); + if (loop1_LPCNT == 0) { + loop1_LPCNT = 1000; + } + 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 >> XNSUM_FOR_ORIGIN_FACTOR; + 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; - static uint8_t TM1cnt = 0; - if(tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) - { - if(loop1_FLAG_CUT == 0) - { - if(!flt_mgr.is_initialized) - { - _counter1_init++; - if(_counter1_init >=40) // 30:150ms, 40: 200ms, 50:250ms - { - _counter1_init = 0; - LEDA_ON_OFF(); - } - } - } - if(flt_mgr.flag_flat_release) - { - flt_mgr.cnt_flat_release++; - if(flt_mgr.cnt_flat_release >= flt_mgr.max_cnt_flat_release) - { - flt_mgr.flag_flat_release = 0; - flt_mgr.cnt_flat_release = 0; + + if (tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) { + + /*--- 初始化阶段红灯快闪 (200ms 周期) ---*/ + if (!g_loop_power_up_state) { + _counter1_init++; + if (_counter1_init >= 40) { // 40 × 5ms = 200ms + _counter1_init = 0; + LEDA_ON_OFF(); } } - if(flt_mgr.flag_loop_reconnect == 1) - { - flt_mgr.cnt_loop_reconnect++; - if(flt_mgr.cnt_loop_reconnect >= 200) - { - flt_mgr.flag_loop_reconnect = 2; - flt_mgr.cnt_loop_reconnect = 0; - } - } - if(flt_mgr.flag_not_idle) - { - flt_mgr.counter_not_idle++; - if(flt_mgr.counter_not_idle >= 200) //400) // 5ms per unit, 400 mean 2second, 200 mean 1second - { - flt_mgr.counter_not_idle = 0; - flt_mgr.flag_not_idle = 0; - } - } - if(flt_mgr.flag_5sec == 0) - { - flt_mgr.counter_5sec++; - if(flt_mgr.counter_5sec >= 1000) - { - flt_mgr.counter_5sec = 0; // 5*1000/5=1000 - flt_mgr.flag_5sec = 1; - } - } - - - - //------ + /*--- 50ms tick 分频 ---*/ TM1cnt++; - if(TM1cnt >= 10) //about=50ms - { - TM1cnt = 0; - if(loop1_FLAG_IN) // 线圈1 从无车到有车 - { - loop1_FLAG_OUT = 0; - loop1_OUTCNT = 0; - loop1_FLAG_IN = 0; -// if(g_loop_cng_unit.output_mode == 1) // 进入脉冲 - { - loop1_FLAG_PLUSE_IN_F = 1; - } - loop1_INCNT = 0; - LEDB_ON; - } + if (TM1cnt >= 10) { // 10 × 5ms = 50ms + TM1cnt = 0; - if(loop1_FLAG_PLUSE_IN_F) - { - loop1_INCNT++; - if(loop1_INCNT > DELAY_TIME[1- SET_DLY]) - { - loop1_INCNT = 0; - loop1_FLAG_PLUSE_IN_F = 0; - loop1_FLAG_PLUSE_IN = 1; - } - } + /*================================================================ + * 时序状态机（对齐 M1H Timer1 ISR） + * + * IN_DELAY OUT_DELAY + * [空闲] ──有车──→ [进入延时] ──→ [有车确认] ──离开──→ + * ↑ │ + * └──脉冲结束──── [脉冲输出] ←──── [离开延时] ←──────┘ + * PULSE_DELAY + *================================================================*/ - if(loop1_FLAG_PLUSE_IN) - { - loop1_INCNT++; - if(loop1_INCNT > IN_DELAY) // 500ms - { - loop1_INCNT = 0; - loop1_FLAG_PLUSE_IN = 0; - } - } - - - if(loop1_FLAG_OUT) // 从有车到无车的标志 - { - loop1_OUTCNT++; - if(loop1_OUTCNT > DELAY_TIME[1- SET_DLY]) // - { - loop1_FLAG_OUT = 0; - loop1_FLAG_PLUSE = 1; - loop1_OUTCNT = 0; - LEDB_OFF; + /* FLAG_IN: 进入延时 500ms */ + if (loop1_FLAG_IN) { + loop1_INCNT++; + if (loop1_INCNT > IN_DELAY) { + loop1_FLAG_IN = 0; + loop1_INCNT = 0; } - } + } - if(loop1_FLAG_PLUSE) // Give 500ms - { - loop1_OUTCNT++; - if(loop1_OUTCNT > PULSE_DELAY) - { - loop1_FLAG_PLUSE = 0; - loop1_OUTCNT = 0; - } - } + /* FLAG_OUT: 离开延时 1.9s（或 SET_DLY 时立即脉冲） */ + if (loop1_FLAG_OUT) { + if (SET_DLY) { + /* 延时模式：立即触发脉冲 */ + loop1_FLAG_OUT = 0; + loop1_FLAG_PLUSE = 1; + loop1_OUTCNT = 0; + } else { + loop1_OUTCNT++; + if (loop1_OUTCNT > OUT_DELAY) { + loop1_FLAG_OUT = 0; + loop1_FLAG_PLUSE = 1; + loop1_OUTCNT = 0; + } + } + } + /* FLAG_PLUSE: 脉冲宽度 950ms */ + 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 > g_hold_time) // 超过有限存在时间 - { - loop1_VD_HOLD = 0; - Hold_CNT = 0; - if(loop1_VD_FLAG) - { - // loop1_FLAG_CUT = 1; // - loop1_FLAG_HOLD_TIMEOUT = 1; - g_moving_average_window_size = LOOP_WINDOW_SIZE_FAST; // 进入快速补偿模式 - RLY1_OFF; RLY2_OFF; - // Reset_RstPeripheralAll(); - } - } - } + /* 有限存在超时 */ + 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) - { + /* 安全复位超时 */ + if (loop1_LC_HOLD) { LC_Hold_CNT++; - if(LC_Hold_CNT > g_safe_max_cnt) - { - //TODO: 这里需要验证是否能够实现安全复位的功能 - loop1_LC_Reset = 1; - loop1_INI_LOOP = 1; - loop1_LOOP_OK0 = 0; - LC_Hold_CNT = 0; - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; + if (LC_Hold_CNT > g_safe_max_cnt) { + loop1_LC_Reset = 1; + loop1_INI_LOOP = 1; + loop1_LOOP_OK0 = 0; + LC_Hold_CNT = 0; + loop1_ORG_CNT = 0; + loop1_ORG_SUM = 0; } } } - - - - if(loop1_RF_FLAG) - { - // loop1_LOOP_OK0 = loop1_LOOP_OK; - loop1_LOOP_OK = 1; - loop1_RF_FLAG = 0; - if(loop1_LC_Reset == 0) - { - if(loop1_VD_FLAG) // 继电器2存在 - { + /*================================================================ + * 线圈载波检测 & 继电器输出刷新（每 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{ + else RLY2_OFF; - } - - if(SET_PLUS) - { - if(loop1_VD_FLAG || loop1_FLAG_OUT) // 输出存在信号 - { + + /* 继电器 1（主输出） */ + if (SET_PLUS) { + /* 存在输出模式：有车或离开延时中 */ + if (loop1_VD_FLAG || loop1_FLAG_OUT) RLY1_ON; - } - else{ + else RLY1_OFF; - } - } - else{ - if(loop1_FLAG_PLUSE) // 输出离开脉冲 - { + } else { + /* 脉冲输出模式：脉冲期间吸合 */ + if (loop1_FLAG_PLUSE) RLY1_ON; - } - else{ + else RLY1_OFF; - } } - }// end if loop1_LC_Reset == 0 - } // end if loop1_RF_FLAG - else - { + } + } else { loop1_LOOP_OK0 = loop1_LOOP_OK; - loop1_LOOP_OK = 0; + loop1_LOOP_OK = 0; } - + + /*--- 硬件驱动：呼吸灯 + 故障灯 ---*/ poll_red_pwm(); - poll_yellow_led(); - - + tmr_flag_clear(TMR15, TMR_OVF_FLAG); } } +/*=========================================================================== + * vd1_task — 核心检测算法（对齐 M1H VD1_TASK） + * + * 每次 loop1_CAP_OK 时调用一次。 + * 流程： + * 1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD + * 2. 无车时：基线跟踪 + 进入检测 + * 3. 有车时：离开检测（带滞回） + *===========================================================================*/ +void vd1_task(void) +{ + uint32_t tmp_value; -uint8_t g_flag_stable_delta2_flt = 0; -uint8_t g_flag_reset_delta = 0; + if (loop1_Origin == 0) return; -void vd1_task(void){ - static uint32_t tmp_value = 0; - static uint32_t new_Flt_prev = 0; // 保存上一次滤波值 - int32_t delta_current = 0; - int32_t delta2_flt = 0; - - if(loop1_Origin == 0){ - return; - } - if(g_flag_reset_delta == 0){ - new_Flt_prev = loop1_Origin; - loop1_CAPVD = loop1_Origin; - g_flag_reset_delta = 1; - } - tmp_value = loop1_Value; - loop1_Value >>= XNSUM_FOR_ORIGIN_FACTOR; + /*--- 1. IIR 一阶低通滤波（对齐 M1H 公式） ---*/ + tmp_value = loop1_Value >> XNSUM_FOR_ORIGIN_FACTOR; + loop1_CAPVD = get_flt_value(tmp_value, loop1_CAPVD); - #ifdef DEBUG - flt_mgr.current_freq = g_crm_clocks_freq_struct.sclk_freq/(tmp_value /loop1_LPCNT) * g_input_div; - #endif - - tmp_loop1_CAPVD = loop1_Value; - - loop1_CAPVD = get_flt_value(tmp_loop1_CAPVD, loop1_CAPVD); - - - // 4. 一阶变化量计算（new_Flt - 前次值） - if(new_Flt_prev == 0) - { - new_Flt_prev = loop1_CAPVD; - } - delta_current = (int32_t)(loop1_CAPVD - new_Flt_prev); - - if(abs(delta_current) > 30){ -// PRINT("Xn_:%d, tmp_value:%d, loop1_Value:%d, last_prev:%d, loop1_CAPVD:%d, delta_current:%d, freq:%d, rapid:%d\n",loop1_Xn, tmp_value, tmp_loop1_CAPVD, new_Flt_prev, loop1_CAPVD, delta_current, flt_mgr.current_freq, rapid_change_detected); - } - new_Flt_prev = loop1_CAPVD; // 更新历史值 - - - if(!loop1_VD_FLAG){ - update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, flt_mgr.window_size); + if (!loop1_VD_FLAG) { + /*================================================================ + * 无车状态 + *================================================================*/ - // 5. 调用二阶变化量计算函数 - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); -// if(g_flag_stable_delta2_flt < 2) -// { -// g_flag_stable_delta2_flt++; -// return; -// } - update_flt_history(loop1_CAPVD, delta_current, delta2_flt); - // PRINT("CarOFF_LPCNT:%d, xn:%d, LPCNT:%d, freq:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1: %d, delta_2:%d\n", loop1_LPCNT, loop1_Xn, loop1_LPCNT, flt_mgr.current_freq, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); + /*--- 基线跟踪（仿 TLD-110：有车时冻结） ---*/ + update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, + &loop1_Origin, loop1_CAPVD, 100); - if(!delta_current) // || !delta2_flt) - { - return; - } -// PRINT("CarOFF_5sec:%d, Origin:%d, loop1_CAPVD:%d\n", flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD); + /*--- 进入检测 ---*/ + loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel]) >> 16; - if(!flt_mgr.is_initialized) - { - PRINT("__init_freq:%d, CarOFF_LPCNT:%d, xn:%d, LPCNT:%d, freq:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1: %d, delta_2:%d, freq:%d\n",flt_mgr.init_freq, loop1_LPCNT, loop1_Xn, loop1_LPCNT, flt_mgr.current_freq, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt, flt_mgr.current_freq); + if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) { + PRINT("Car_In, Value:%d, CAPVD:%d, Origin:%d, dlt:%d, freq:%d\n", + tmp_value, loop1_CAPVD, loop1_Origin, + loop1_dlt_ORG, g_crm_clocks_freq_struct.sclk_freq); - return; - } - - if(loop1_LOOP_OK0 == 0){ - return; - } - - loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel & 0x0F]) >> 16; - g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); - - if(loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) // - { - - PRINT("Car_In,tmp_value:%d, Value:%d, loop1_CAPVD:%d, loop1_Origin:%d, loop1_dlt_ORG:%d, delta_1:%d, delta_2:%d, freq:%d\n",tmp_value, tmp_loop1_CAPVD, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq); -// if(g_loop_cng_unit.exist_mode) -// { -// loop1_VD_HOLD = 1; // 有限存在10分钟 -// } - if(flt_mgr.cnt_simple_release) - { - flt_mgr.cnt_simple_release = 0; - } - flt_mgr.cnt_simple_busy++; - if(flt_mgr.cnt_simple_busy < 3){ - return; - } - PRINT("Car_In01,tmp_value:%d, Value:%d, loop1_CAPVD:%d, loop1_Origin:%d, loop1_dlt_ORG:%d, delta_1:%d, delta_2:%d, freq:%d\n",tmp_value, tmp_loop1_CAPVD, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq); - - - flt_mgr.last_loop_Origin = loop1_Origin; // Mark - flt_mgr.last_loop_vd_flag = 1; - flt_mgr.second_loop_ORG_CNT = 0; - flt_mgr.second_loop_ORG_SUM = 0; - flt_mgr.second_loop_Origin = 0; - flt_mgr.second_loop_vd_flag = 0; - - // 重置二次判断增强条件的计数器 - flt_mgr.cnt_stable_to_idle = 0; - - loop1_VD_FLAG = 1; //???????1 - loop1_FLAG_IN = 1; //?????? - + loop1_VD_FLAG = 1; + loop1_FLAG_IN = 1; LEDB_ON; - - if(!SET_SAFE) - { + /* 有限存在计时（非安全复位模式下） */ + if (!SET_SAFE) { loop1_LC_HOLD = 1; - } - else - { + } else { loop1_LC_HOLD = 0; } - if(loop1_LC_Reset) + if (loop1_LC_Reset) loop1_LC_Reset = 0; - } - else{ - if(flt_mgr.cnt_simple_busy) - { - flt_mgr.cnt_simple_busy = 0; - } - } - }// end if !loop1_VD_FLAG - else{ - loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; - g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - new_Flt_prev = loop1_CAPVD; // 更新历史值 - - if(flt_mgr.cnt_simple_busy) - { - flt_mgr.cnt_simple_busy = 0; - } - - if((( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD )) // - { - - PRINT("CAR_OFF0,5sec:%d, tmp_value:%d, Value:%d, Origin:%d, loop1_CAPVD:%d,dlt_ORG:%d, delta_1: %d, delta_2:%d,freq:%d, rapid:%d\n", - flt_mgr.recent_5sec, tmp_value, - tmp_loop1_CAPVD, loop1_Origin, loop1_CAPVD, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq, rapid_change_detected); - - - - - PRINT("CAR_OFF_CONFIRMED,5sec:%d, tmp_value:%d, Value:%d, Origin:%d, loop1_CAPVD:%d,dlt_ORG:%d, delta_1: %d, delta_2:%d,freq:%d\n", - flt_mgr.recent_5sec, tmp_value, - tmp_loop1_CAPVD, loop1_Origin, loop1_CAPVD, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq); - - loop1_VD_FLAG = 0; //???????0 - loop1_FLAG_OUT = 1; //?????? - loop1_VD_HOLD = 0 ; - loop1_LC_HOLD = 0; - - g_loop_acs_info.flag_event = 1; - g_loop_acs_info.car_state = 0; - // g_led_A_state = 0; - LEDB_OFF; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; - Hold_CNT = 0; + } + } else { + /*================================================================ + * 有车状态 + *================================================================*/ - flt_mgr.flag_5sec = 0; - flt_mgr.counter_5sec = 0; - flt_mgr.last_loop_vd_flag = 0; - flt_mgr.second_loop_vd_flag = 0; - flt_mgr.second_loop_ORG_CNT = 0; - flt_mgr.second_loop_ORG_SUM = 0; - } + /*--- 离开检测（滞回阈值 SensTable_1 ≈ SensTable 的 50%） ---*/ + loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; + + if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD) { + PRINT("Car_OFF, Value:%d, CAPVD:%d, Origin:%d, dlt:%d, freq:%d\n", + tmp_value, loop1_CAPVD, loop1_Origin, + loop1_dlt_ORG, g_crm_clocks_freq_struct.sclk_freq); + + loop1_VD_FLAG = 0; + loop1_FLAG_OUT = 1; + loop1_VD_HOLD = 0; + loop1_LC_HOLD = 0; + LEDB_OFF; + + loop1_ORG_CNT = 0; + loop1_ORG_SUM = 0; + Hold_CNT = 0; + } } - } - - - - +/*=========================================================================== + * 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_CAP_OK){ - if(!g_loop_power_up_state){ - g_loop_power_up_state = 1; // 表示连接了线圈 - } - else{ - if(loop1_FLAG_CUT){ - g_led_loop_reconnect |= 0x10; - } - } - } - -// if(loop1_FLAG_HOLD_TIMEOUT){ -// INIT_VD(); -// } - if(loop1_FLAG_CUT){ - loop1_FLAG_CUT = 0; - flt_mgr.flag_loop_reconnect = 1; - } - if(g_led_loop_reconnect & 0x10) - { - if(!loop1_LOOP_OK0) - { - loop1_LOOP_OK0 = 1; - } - } - - if(!loop1_LOOP_OK0){ - for(swcnt = 0; swcnt < 6; swcnt++){ - if(gpio_output_data_bit_read(LED_GREEN_GPIO, LED_GREEN_PIN)){ - LEDB_ON; - } - else{ - LEDB_OFF; - } + + while (1) { + if (loop1_LOOP_OK) { + /*--- 线圈重连检测 ---*/ + if (!loop1_LOOP_OK0) { + /* 线圈恢复：绿灯闪烁提示 */ + int i; + for (i = 0; i < 6; i++) { + LEDB_ON; + vTaskDelay(200); + LEDB_OFF; vTaskDelay(200); - // TODO: 添加看门狗 } - loop1_LOOP_OK0= 1; - LEDB_OFF; + loop1_LOOP_OK0 = 1; } - - if(loop1_CAP_OK){ + + if (loop1_CAP_OK) { loop1_CAP_OK = 0; + + /* 核心检测算法 */ vd1_task(); - + + /* 拨码开关轮询 */ poll_sw_state(); - - if(g_led_loop_reconnect & 0x10){ - g_led_loop_reconnect = (g_led_loop_reconnect & 0x0F) + 1; - if(g_led_loop_reconnect > 3){ + + /* 线圈重连故障指示 */ + if (g_led_loop_reconnect) { + g_led_loop_reconnect &= 0x0F; + g_led_loop_reconnect++; + if (g_led_loop_reconnect > 3) { g_led_loop_reconnect = 3; } - g_counter_fault_led = 0; - g_step_fault_led = 0; - } - else{ - if(!g_led_loop_reconnect){ - // TODO: 如果电感量不达标，是否也要进行故障提示，比如慢闪？ - if(!gpio_output_data_bit_read(LED_YELLOW_GPIO, LED_YELLOW_PIN)){ - LED_YELLOW_OFF; - } - - } } } - - }// end if loop1_LOOP_OK - else{ - loop1_FLAG_CUT = 1; - + } else { + /*--- 线圈断开 ---*/ loop1_VD_FLAG = 0; -// loop1_FLAG_IN = 0; -// loop1_FLAG_OUT = 0; -// loop1_FLAG_PLUSE = 0; - - // 线圈断开，继电器释放，状态为0，车辆状态也为0 - RLY1_OFF; RLY2_OFF; LEDB_OFF; - - if(gpio_output_data_bit_read(LED_YELLOW_GPIO, LED_YELLOW_PIN)){ - LED_YELLOW_ON; + RLY1_OFF; + RLY2_OFF; + LEDB_OFF; + + /* 首次检测到断开：标记故障 */ + if (!g_led_loop_reconnect) { + g_led_loop_reconnect = 0x02; + g_counter_fault_led = 0; + g_step_fault_led = 0; } - else{ - LED_YELLOW_OFF; - } - + vTaskDelay(150); } - - if(g_flag_output) + + /*--- 调试输出（每 2 秒） ---*/ +#ifdef DEBUG { - g_flag_output = 0; -// PRINT("SET_DLY:%02X, SET_SAFE:%02X,xnCounter:%d, ms_counter:%d, Xn:%d, LPCNT:%d, capvd:%d, loop1_Origin:%d, freq:%d, init_freq:%d\n", -// SET_DLY, SET_SAFE, g_xn_counter, g_xn_counter/2000, loop1_Xn, loop1_LPCNT, loop1_CAPVD, loop1_Origin, flt_mgr.current_freq, flt_mgr.init_freq); - g_xn_counter = 0; - -// if(gpio_output_data_bit_read(LED_GREEN_GPIO, LED_GREEN_PIN)){ -// LEDB_ON; -// } -// else{ -// LEDB_OFF; -// } -// -// if(gpio_output_data_bit_read(LED_YELLOW_GPIO, LED_YELLOW_PIN)){ -// LED_YELLOW_ON; -// } -// else{ -// LED_YELLOW_OFF; -// } - + static uint32_t _dbg_cnt = 0; + _dbg_cnt++; + if (_dbg_cnt >= 200) { // 200 × 10ms = 2s + _dbg_cnt = 0; + PRINT("SET_DLY:%d, SET_SAFE:%d, Xn:%d, LPCNT:%d, " + "CAPVD:%d, Origin:%d, VD:%d\n", + SET_DLY, SET_SAFE, loop1_Xn, loop1_LPCNT, + loop1_CAPVD, loop1_Origin, loop1_VD_FLAG); + } } - - +#endif + vTaskDelay(10); } } -