refactor: 精简检测算法，对齐 M1H/TLD-110 参考实现
- 删除二阶差分滤波（计算但从未参与判决） - 删除 FltHistoryManager 死代码（20+ 未用字段） - 删除 StageRangeConfig（区间约束计算后未引用） - 删除动态窗口切换，改为 M1H 固定 100 窗口基线 - 修复 LPCNT 计算溢出 (32768/Xn<<6 改为乘除顺序) - 进入/离开判决对齐 M1H：单次检测 + TMR15 时序防抖 - 时序状态机简化：去掉 PLUSE_IN_F/PLUSE_IN 中间层 - 拨码去抖次数对齐 M1H: 10→5 - 代码量: 1177→706 行 (-40%)
This commit is contained in:
wangfq
@@ -1,13 +1,13 @@
|
|||||||
/**
|
/**
|
||||||
**************************************************************************
|
**************************************************************************
|
||||||
* @file TaskLoop.h
|
* @file TaskLoop.h
|
||||||
* @version v1.0
|
* @version v2.0
|
||||||
* @date 2025-09-08
|
* @date 2025-09-08 (original), 2026-06-23 (simplified)
|
||||||
* @brief 地感功能的实现
|
* @brief 地感功能实现 — 精简版，对齐 M1H/TLD-110 参考实现
|
||||||
**************************************************************************
|
**************************************************************************
|
||||||
* Copyright notice & Disclaimer
|
* Copyright notice & Disclaimer
|
||||||
* * Create 2025-09-08 by wangfq
|
* * Create 2025-09-08 by wangfq
|
||||||
* *
|
* * Simplify 2026-06-23: 去除二阶滤波、StageRangeConfig 等未使用模块
|
||||||
**************************************************************************
|
**************************************************************************
|
||||||
*/
|
*/
|
||||||
|
|
||||||
@@ -15,160 +15,126 @@
|
|||||||
#define __TASKLOOP_H__
|
#define __TASKLOOP_H__
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
// 时序参数（每 tick ≈50ms），对齐 M1H 参考实现
|
/*===========================================================================
|
||||||
#define HOLD_TIME (5 * 1200) // 有限存在保持（约5分钟）
|
* 时序参数（每 tick ≈50ms，由 TMR15 5ms×10 产生），对齐 M1H 参考实现
|
||||||
#define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约4分钟）
|
*===========================================================================*/
|
||||||
|
#define HOLD_TIME (5 * 1200) // 有限存在保持（约 5 分钟）
|
||||||
|
#define LC_HOLD_TIME (4 * 1200) // 安全复位时间（约 4 分钟）
|
||||||
#define IN_DELAY 10 // 进入防抖 500ms
|
#define IN_DELAY 10 // 进入防抖 500ms
|
||||||
#define OUT_DELAY 38 // 离开防抖 1.9s
|
#define OUT_DELAY 38 // 离开防抖 1.9s
|
||||||
#define PULSE_DELAY 19 // 脉冲宽度 950ms
|
#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
|
* 灵敏度表 — 对齐 M1H
|
||||||
#define ALFA_ORG1 64
|
* 进入阈值 = Origin × SensTable[SENS] / 65536
|
||||||
#define ALFA_ORG2 64
|
* 离开阈值 = 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_INI_LOOP; // 初始化阶段
|
||||||
extern uint8_t loop1_VD_HOLD;
|
extern uint8_t loop1_CAP_OK; // 新测量数据就绪
|
||||||
extern uint8_t loop1_RF_FLAG;
|
extern uint8_t loop1_VD_FLAG; // 有车标志
|
||||||
extern uint8_t loop1_LOOP_OK;
|
extern uint8_t loop1_VD_HOLD; // 有限存在计时启用
|
||||||
extern uint8_t loop1_LOOP_OK0;
|
extern uint8_t loop1_RF_FLAG; // 本 tick 收到线圈振荡边沿
|
||||||
extern uint8_t loop1_FLAG_IN;
|
extern uint8_t loop1_LOOP_OK; // 线圈连接正常
|
||||||
extern uint8_t loop1_FLAG_OUT;
|
extern uint8_t loop1_LOOP_OK0; // 线圈连接正常（上周期）
|
||||||
extern uint8_t loop1_FLAG_PLUSE;
|
extern uint8_t loop1_FLAG_IN; // 进入延时中
|
||||||
extern uint8_t loop1_FLAG_PLUSE_DELAY;
|
extern uint8_t loop1_FLAG_OUT; // 离开延时中
|
||||||
extern uint8_t loop1_FLAG_IN_PLUSE;
|
extern uint8_t loop1_FLAG_PLUSE; // 脉冲输出中
|
||||||
extern uint8_t loop1_FLAG_CUT;
|
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 ; // <20><>ʼ<EFBFBD><CABC>Ϊ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 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 /* __TASKLOOP_H__ */
|
||||||
#endif
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user