diff --git a/.gitignore b/.gitignore index 8c708c9..0451b15 100644 --- a/.gitignore +++ b/.gitignore @@ -24,3 +24,6 @@ obj/ *.launch.bak *.old *.TMP + +# root-owned upload backups (pending sudo cleanup) +*.rootowned_bak/ diff --git a/BLE/DLD110S_Peripheral_28K/APP/dbn_ble_srv.c b/BLE/DLD110S_Peripheral_28K/APP/dbn_ble_srv.c index 2013261..f134859 100644 --- a/BLE/DLD110S_Peripheral_28K/APP/dbn_ble_srv.c +++ b/BLE/DLD110S_Peripheral_28K/APP/dbn_ble_srv.c @@ -454,11 +454,11 @@ void usart_packet_processing_acs_loop(uint8_t flag) clear_ble_notify_buf(&g_tmp_report_buf); uint8_t i = 0,j; - uint32_t _loop_capvd = g_loop_acs_info.loop_capvd << 5; + uint32_t _loop_capvd = g_loop_acs_info.loop_capvd; g_tmp_report_buf.flag = flag; - g_loop_acs_info.condition = (flt_mgr.cnt_not_idle > 150) ? 15: (flt_mgr.cnt_not_idle / 10); + g_loop_acs_info.condition = (g_env_activity > 150) ? 15 : (g_env_activity / 10); if(loop1_FLAG_CUT){ g_loop_acs_info.condition = 15; } diff --git a/BLE/DLD110S_Peripheral_28K/APP/include/cmcng.h b/BLE/DLD110S_Peripheral_28K/APP/include/cmcng.h index fa472e6..13fef33 100644 --- a/BLE/DLD110S_Peripheral_28K/APP/include/cmcng.h +++ b/BLE/DLD110S_Peripheral_28K/APP/include/cmcng.h @@ -3,6 +3,8 @@ * * Created on: Aug 27, 2024 * Author: Thinkpad + * 2026-07-18: 算法移植 DLD154V4B, 移除 FltHistoryManager / SecondOrderState / + * StageRangeConfig 等旧算法结构 */ #ifndef INCLUDE_CMCNG_H_ @@ -11,122 +13,19 @@ #include #define PRODUCT_MODEL "DLD110S" -#define FIRMWARE_VER "4.00" +#define FIRMWARE_VER "4.20" #define HARDWARE_VER "4.01" #define FIRMWARE_VER_MAIN 4 -#define FIRMWARE_VER_SUB 12 +#define FIRMWARE_VER_SUB 20 #define HARDWARE_VER_MAIN 4 #define HARDWARE_VER_SUB 1 -#define FUNCTION_A 0x01 // 二次判断 -#define FUNCTION_B 0x02 // 二次判断增强条件判断 +/* 功能模式位 (direction_mode 高4位, 协议保留; 新算法不再消费) */ +#define FUNCTION_A 0x01 // 二次判断 (已废弃) +#define FUNCTION_B 0x02 // 二次判断增强条件判断 (已废弃) #define FUNCTION_C 0x04 // #define FUNCTION_D 0x08 // -// 二阶变化量计算状态结构体 -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; // 初始频率 - - 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; - - 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]; - - extern uint16_t g_hold_time; extern char g_flag_debug; @@ -139,7 +38,10 @@ extern uint8_t g_loop_power_up_state; extern uint8_t g_function_mode; +/* 新算法状态 (DLD154V4B 移植) */ +extern uint8_t g_loop_stable; // 稳定期结束标志 (0=收敛中, 1=稳定) +extern uint8_t g_env_activity; // 环境活跃度 (BLE condition 字段来源) + uint32_t mstick(void); -void reload_lower_upper(uint8_t sens_level); #endif /* INCLUDE_CMCNG_H_ */ diff --git a/BLE/DLD110S_Peripheral_28K/APP/peripheral_main.c b/BLE/DLD110S_Peripheral_28K/APP/peripheral_main.c index abfe1f8..400fdcb 100644 --- a/BLE/DLD110S_Peripheral_28K/APP/peripheral_main.c +++ b/BLE/DLD110S_Peripheral_28K/APP/peripheral_main.c @@ -1,12 +1,20 @@ /********************************** (C) COPYRIGHT ******************************* - * File Name : main.c - * Author : WCH - * Version : V1.1 - * Date : 2020/08/06 - * Description : 外设从机应用主函数及任务系统初始化 + * File Name : peripheral_main.c + * Author : WCH / wangfq + * Version : V2.0 (algorithm ported from DLD154V4B) + * Date : 2026/07/18 + * Description : DLD110SV4 单线圈蓝牙车检器 主函数 + * + * 算法说明 (移植自 DLD154V4B TaskLoop V2.0, M1H 精简系): + * - 自适应测量窗: LPCNT = MEASUREMENT_BASE / Xn, 测量窗时长与线圈频率无关 + * - 一阶 IIR (79/256) + 斜率限幅 5% (拒绝 EMI 瞬态尖峰) + * - 稳定期 128 窗快速收敛 (绕过滤波, 小窗口 100) + * - 基线跟踪 窗口 500 + 冻结保护 (超时 + 稳定性双条件强制更新) + * - 进入确认 3 次; 离开: 平坦性三条件判定 (专利 CN200910309382) + * - 有限存在超时 / 安全复位: 全复位重建基线 ********************************************************************************* * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd. - * Attention: This software (modified or not) and binary are used for + * Attention: This software (modified or not) and binary are used for * microcontroller manufactured by Nanjing Qinheng Microelectronics. *******************************************************************************/ @@ -19,17 +27,16 @@ #include "storage.h" #include #include "cmcng.h" - +#include "dbn_ble_srv.h" uint8_t g_flag_bt_state = 0; // 0 蓝牙没有连接，1 有蓝牙连接 -uint32_t g_counter_bt_timeout = 0; uint8_t g_flag_output = 0; uint32_t g_xn_counter = 0; uint8_t g_function_mode = 0; -#define RLY1_ON (R32_PA_OUT |= BV(8)) //(R32_PB_OUT &= ~BV(22)) -#define RLY1_OFF (R32_PA_OUT &= ~BV(8)) //(R32_PB_OUT |= BV(22)) +#define RLY1_ON (R32_PA_OUT |= BV(8)) +#define RLY1_OFF (R32_PA_OUT &= ~BV(8)) #define RLY2_ON (R32_PB_OUT |= BV(23)) #define RLY2_OFF (R32_PB_OUT &= ~BV(23)) #define LEDA_ON (R32_PB_OUT &= ~BV(22)) @@ -47,15 +54,24 @@ const uint8_t MAX_Store_Size = 80; uint8_t g_loop_power_up_state = 0; // 开机启动时线圈是否连接 __IO uint32_t TimingDelayInc; - -#define MAX_CMP_LOOP_UNIT 4 // 5 组 -#define MAX_CMP_DIF 20 // -#define MAX_TIMEOUT_CMP 120000 // 5毫秒 为单位， 表示10分钟， 600秒 - -#define ALFA_CAP1 79 -#define ALFA_CAP2 64 -#define ALFA_ORG1 64 -#define ALFA_ORG2 64 +/*=========================================================================== + * 算法参数 -- 移植自 DLD154V4B (M4 优化版), 适配 CH591R @60MHz + * + * 测量窗: MEASUREMENT_BASE 个系统时钟 tick, 与线圈频率无关 + * 524288(2^19) / 60MHz ~= 8.74ms, 即 vd1_task 有效采样周期 ~8.74ms + * (DLD154V4B 为 131072 @120MHz + 10ms 任务轮询, 时间尺度基本一致) + *===========================================================================*/ +#define MEASUREMENT_BASE 524288L // 2^19, 自适应测量窗目标值 (Value ~= 524288) +#define ALFA_CAP1 79 // IIR a=79/256~=0.31 (@8.74ms -> tau~=24ms) +#define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5% (拒绝 EMI 尖峰) +#define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值才判有车 +#define FREEZE_TIMEOUT 1000 // 基线冻结超时: ~8.7s, 持续偏高且稳定后强制更新 +#define FREEZE_STABILITY_RATE 2 // 冻结稳定性窗口: 参考值 +-2% +#define USE_FLATNESS_EXIT 1 // 1=平坦性三条件离开, 0=简单滞回防抖 +#define WINDOW_ORIGIN 500 // 基线跟踪窗口 (500 窗 ~= 4.4s) +#define STABLE_SAMPLES 128 // 稳定期样本数 (~1.1s) +#define VARIATION_EVENT_TH 40 // BLE variation 事件阈值 (幅值基准 2^19, 对应旧代码的 5) +#define ENV_ACT_TH 30 // 环境活跃度: CAPVD 相邻差绝对值阈值 (待现场验证) uint32_t mstick(void){ return TimingDelayInc; @@ -65,20 +81,6 @@ uint32_t u32CountWdt = 0; uint8_t g_loop_cut_last = 0; uint32_t g_loop_cut_amount = 0; -uint8_t g_flag_stable_delta2_flt = 0; -uint8_t g_flag_reset_delta = 0; - -// 继电器状态防抖计数器（多重确认机制） -static uint8_t relay_on_confirm_cnt = 0; // 吸合确认计数 -static uint8_t relay_off_confirm_cnt = 0; // 释放确认计数 -#define RELAY_ON_CONFIRM_THRESHOLD 3 // 吸合需要连续3次确认 -#define RELAY_OFF_CONFIRM_THRESHOLD 5 // 释放需要连续5次确认（更严格） - -// 快速变化检测（用于识别线圈中间移动） -static int32_t last_delta_for_rapid = 0; // 上一次的一阶变化量 -static uint8_t rapid_change_detected = 0; // 检测到快速变化标志 -static uint16_t rapid_change_duration = 0; // 快速变化持续时间 - /********************************************************************* * GLOBAL TYPEDEFS @@ -103,32 +105,34 @@ 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; +uint8_t TM1cnt = 0; uint16_t g_hold_time = 0; -uint16_t loop1_Xn; -uint16_t loop1_HoldTime; -uint16_t loop1_dlt_ORG; -uint32_t loop1_Origin; -uint16_t loop1_CapLast; - -uint16_t loop1_LPCNT; -uint16_t loop1_CapCnt; -uint32_t loop1_CapSum; -uint32_t loop1_CAPVD; -uint32_t loop1_Value; -uint16_t loop1_Flt; - -uint32_t loop1_ORG_SUM; -uint16_t loop1_ORG_CNT; +/*=========================================================================== + * 捕获 & 测量 + *===========================================================================*/ +uint16_t loop1_Xn; // 相邻下降沿周期 (TMR0 直接给出) +uint32_t loop1_dlt_ORG; // 当前灵敏度阈值 +uint32_t loop1_Origin; // 基线 (无车参考值) +uint16_t loop1_LPCNT; // 每个测量窗累加的 Xn 个数 +uint16_t loop1_CapCnt; // 当前窗口已累加次数 +uint32_t loop1_CapSum; // 当前窗口累加和 +uint32_t loop1_CAPVD; // IIR 滤波后的测量值 +uint32_t loop1_Value; // 一个测量窗的原始 Sigma-Xn +uint32_t loop1_ORG_SUM; // 基线跟踪累加和 +uint16_t loop1_ORG_CNT; // 基线跟踪计数 +/*=========================================================================== + * 计数器 + *===========================================================================*/ uint8_t loop1_INCNT; uint8_t loop1_OUTCNT; +/*=========================================================================== + * 标志位 + *===========================================================================*/ 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; @@ -137,57 +141,57 @@ 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_PLUSE_IN_F = 0; +uint8_t loop1_FLAG_PLUSE_IN = 0; uint8_t loop1_FLAG_CUT; -uint8_t loop1_FLAG_HOLD_TIMEOUT; // 存在超时 +uint8_t loop1_FLAG_HOLD_TIMEOUT; // 有限存在超时 +/*=========================================================================== + * 安全复位 + *===========================================================================*/ uint8_t loop1_LC_HOLD = 0; uint8_t loop1_LC_Reset = 0; uint32_t LC_Hold_CNT = 0; -//const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 37, 27}; //{162, 108, 54, 37, 27}; 9 -//const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 27, 14}; //{108, 81, 32, 27, 14}; 6 -const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 28}; //{162, 108, 54, 37, 27}; -const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 16}; //{108, 81, 32, 27, 14}; -// const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 162, 81, 54}; //{162, 108, 54, 37, 27}; -// const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 54, 32}; //{108, 81, 32, 27, 14}; +/*=========================================================================== + * 新算法状态 (DLD154V4B 移植) + *===========================================================================*/ +uint8_t g_loop_stable = 0; // 线圈数值已稳定 (0=稳定期中, 1=稳定) +uint16_t loop1_stable_cnt = 0; // 稳定期样本计数 +uint8_t loop1_entry_cnt = 0; // 进入确认计数 +uint8_t loop1_cnt_release = 0; // 简单离开防抖计数 (USE_FLATNESS_EXIT=0) +uint16_t loop1_freeze_cnt = 0; // 基线冻结持续计数 +uint32_t loop1_freeze_ref = 0; // 冻结稳定性参考值 +#if USE_FLATNESS_EXIT +uint8_t g_exit_state = 0; // 0=追踪坡面, 1=等待平坦 +uint16_t g_max_slope = 0; // 第一上升坡面最大 |f'| +uint16_t g_max_slope_rate = 0; // 第一上升坡面最大 |f''| +uint16_t g_delta2 = 0; // 一阶平坦阈值 +uint16_t g_delta3 = 0; // 二阶平坦阈值 +int32_t g_prev_capvd = 0; // 上一帧 CAPVD (差分用) +int32_t g_prev_first_deriv = 0; // 上一帧一阶导数 +uint8_t g_slope_flat_cnt = 0; // 斜率趋零连续计数 +uint8_t g_flat_ok_cnt = 0; // 平坦条件满足连续计数 +#endif -#define LOOP_WINDOW_SIZE_NORMAL 50 // 200 -#define LOOP_WINDOW_SIZE_FAST 50 // 快速补偿 -uint16_t g_moving_average_window_size = LOOP_WINDOW_SIZE_NORMAL; // 滑动窗口大小 +/* 环境活跃度 (供 BLE condition 字段上报, 替代旧 flt_mgr.cnt_not_idle) */ +uint8_t g_env_activity = 0; +uint32_t g_env_prev_capvd = 0; -uint32_t g_sys_freq = 0; +/*=========================================================================== + * 出厂灵敏度默认表 (写入 g_loop_sens_list, 运行时以 g_loop_sens_list 为准) + * 进入阈值 = Origin * sens_in / 65536; 离开阈值 = Origin * sens_out / 65536 + *===========================================================================*/ +const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 28}; +const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 16}; -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; +uint16_t g_loop_out_delay = 10; // 离开延时, 50ms 为单位, 由 delay_time 配置 DBN_BLE_State g_dbn_ble_state_acs_enable = {0}; Loop_ACS_Info g_loop_acs_info = {0,}; -#define MAX_ALLOW_DIFF_FREQ 3500 //频率最大偏移值 3500Hz -uint16_t g_MaxAllowDiff = 0; // 采样值允许最大偏移值 float g_freq_tmp = 0.0; -//uint8_t g_filter_step = 0; -//uint16_t g_loop_bad_cnt = 0; // 采样 数值异常计数 - -uint8_t loop1_FLAG_PLUSE_IN_F = 0; -uint8_t loop1_FLAG_PLUSE_IN = 0; - - - /* 用于APP判断文件有效性 */ const uint32_t Address = 0xFFFFFFFF; @@ -205,7 +209,6 @@ void set_factory_param(void) memcpy(&g_loop_cng_unit.sensitvity, loop_cng_default, 7); -// g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT; // max 5 for(i = 0; i < g_loop_sens_list.total; i++) { g_loop_sens_list.sens[i].sens_in = SensTable[i]; @@ -220,7 +223,6 @@ void factory_dev(void){ uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size); if(rBuf == NULL){ -// PRINT("Not enough memory!!"); SYS_ResetExecute(); } for(i = 0; i < MAX_Store_Size; i++){ @@ -232,28 +234,10 @@ void factory_dev(void){ rBuf[2] = 0xC3; rBuf[3] = 0x3C; -// g_freq_sens = 1; i = 4; memcpy(&rBuf[i], &g_loop_cng_unit.sensitvity, 7); i += 7; -// i = Addr_Loop_PlanB_Cng_Offset; -// rBuf[i++] = g_loop_balance_planB.sample_cng.flag; -// rBuf[i++] = g_loop_balance_planB.sample_cng.max_amplitude; -// rBuf[i++] = g_loop_balance_planB.sample_cng.max_amplitude >> 8; -// rBuf[i++] = g_loop_balance_planB.balance_ori_cng.flag; -// rBuf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt; -// rBuf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt >> 8; -// rBuf[i++] = g_loop_balance_planB.release_ori_planB.flag_weight; -// rBuf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude; -// rBuf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude >> 8; -// rBuf[i++] = g_loop_balance_planB.release_ori_planB.timeout; -// -// rBuf[i++] = g_loop_balance_planB.release_change_rate.flag_weight; -// rBuf[i++] = g_loop_balance_planB.release_change_rate.rate_first; -// rBuf[i++] = g_loop_balance_planB.release_change_rate.rate_second; -// rBuf[i++] = g_loop_balance_planB.release_change_rate.mode; - i = Addr_Loop_Sens_List_Offset; if(g_loop_sens_list.total > MAX_LOOP_SENS_AMOUNT) { @@ -301,9 +285,8 @@ void poll_sens_key(void) } if(_last_sens != g_freq_sens){ - //TODO: reboot? + /* 灵敏度阈值每周期由 g_loop_sens_list 实时计算, 无需重载 */ _last_sens = g_freq_sens; - reload_lower_upper(g_freq_sens); } } @@ -311,51 +294,27 @@ void poll_sens_key(void) void para_store_init(void) { - uint8_t i; uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size); if(rBuf == NULL){ -// PRINT("Not enough memory!!"); SYS_ResetExecute(); } memset(rBuf, 0, MAX_Store_Size); read_cfg_from_store(0, rBuf, MAX_Store_Size); -// PRINT("Read_Cng_Store:\n"); -// for(i = 0; i < MAX_Store_Size; i++){ -// PRINT("%02X ", rBuf[i]); -// } -// PRINT("\n"); if(rBuf[0] == 0x33 && rBuf[1] == 0xBB && rBuf[2] == 0xC3 && rBuf[3] == 0x3C){ memcpy(&g_loop_cng_unit, &rBuf[4], sizeof(g_loop_cng_unit)); -// memcpy(&g_loop_balance_planB, &rBuf[Addr_Loop_PlanB_Cng_Offset], sizeof(g_loop_balance_planB)); memcpy(&g_loop_sens_list, &rBuf[Addr_Loop_Sens_List_Offset], sizeof(g_loop_sens_list)); -// g_freq_sens = g_loop_cng_unit.sensitvity & 0x0F; -// if(g_freq_sens > g_loop_sens_list.total ){ -// g_freq_sens = g_loop_sens_list.total; -// } -// if(g_freq_sens){ -// g_freq_sens--; -// } + poll_sens_key(); - g_loop_out_delay = g_loop_cng_unit.delay_time * 2; // 定时器以 50ms 为单位， 延时以100ms为单位 -// g_loop_release_ori_timeout = g_loop_balance_planB.release_ori_planB.timeout * 60 * 1000 / 5; + g_loop_out_delay = g_loop_cng_unit.delay_time * 2; // 定时器以 50ms 为单位, 延时以100ms为单位 g_hold_time = g_loop_cng_unit.exist_mode * 30 * 20; //50ms/per, 30s - // g_hold_time = g_loop_cng_unit.exist_mode * 100; //50ms/per, 5s - // if(g_hold_time < 400){ - // g_hold_time = 400; - // } g_safe_max_cnt = g_loop_cng_unit.loopSafe_Timeout * 10 * (1000 / 50); g_freq_level = g_loop_cng_unit.loopFreq_Level - 1; g_function_mode = g_loop_cng_unit.direction_mode >> 4; - flt_mgr.max_cnt_flat_release = g_loop_cng_unit.delay_time * 20 /2; - if(flt_mgr.max_cnt_flat_release < 40){ // 至少检查400毫秒 - flt_mgr.max_cnt_flat_release = 40; - } - #ifdef DEBUG g_freq_level = 0; #endif @@ -389,9 +348,8 @@ void para_store_init(void) else{ free(rBuf); set_factory_param(); -// DelayMs(10); factory_dev(); -// + DelayMs(10); SYS_ResetExecute(); } @@ -422,12 +380,10 @@ void tm_init(void){ void gpio_uart_init(void) { /* 配置串口0：先配置IO口模式，再配置串口 */ -// GPIOPinRemap(ENABLE, RB_PIN_UART0); #ifdef DEBUG GPIOPinRemap(ENABLE, RB_PIN_UART0); GPIOA_SetBits(GPIO_Pin_14); GPIOA_ModeCfg(GPIO_Pin_14, GPIO_ModeOut_PP_5mA); // TXD-配置推挽输出，注意先让IO口输出高电平 - // UART0_DefInit(); UART0_BaudRateCfg(230400); //115200 R8_UART0_FCR = (2 << 6) | RB_FCR_TX_FIFO_CLR | RB_FCR_RX_FIFO_CLR | RB_FCR_FIFO_EN; // FIFO打开，触发点4字节 @@ -441,7 +397,7 @@ void gpio_uart_init(void) GPIOA_ModeCfg(GPIO_Pin_15|GPIO_Pin_14, GPIO_ModeIN_PU); // 拨动开关，调节灵敏度 #endif - + GPIOA_SetBits(GPIO_Pin_12|GPIO_Pin_13); GPIOA_ModeCfg(GPIO_Pin_12|GPIO_Pin_13, GPIO_ModeOut_PP_5mA); @@ -453,21 +409,14 @@ void gpio_uart_init(void) GPIOB_ModeCfg(GPIO_Pin_22|GPIO_Pin_23, GPIO_ModeOut_PP_5mA); RLY1_OFF; RLY2_OFF; LEDA_OFF; -#if 0 // 中断方式：接收数据后发送出去 - UART0_ByteTrigCfg(UART_7BYTE_TRIG); - trigB = 7; - UART0_INTCfg(ENABLE, RB_IER_RECV_RDY | RB_IER_LINE_STAT); - PFIC_EnableIRQ(UART0_IRQn); -#endif - - } - - +/*=========================================================================== + * 一阶 IIR 低通滤波器 (对齐 M1H 公式) + * CAPVD_new = a * new_value + (1-a) * CAPVD_old, a = Flt_Reg/256 + *===========================================================================*/ 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; @@ -481,939 +430,367 @@ 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） - */ -void update_moving_average(uint32_t* p_sum, uint16_t* p_cnt, uint32_t* p_origin, uint32_t new_value, uint8_t window) { - // 参数安全检查 +/*=========================================================================== + * 滑动平均基线更新 + * 注意: window 必须是 uint16_t (WINDOW_ORIGIN=500 > 255, uint8_t 会静默截断) + *===========================================================================*/ +void 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; // 可添加错误处理 - } - - *p_sum += new_value; // 累加新值 - (*p_cnt)++; - - if (*p_cnt >= window) { // 窗口满（如window=100时，计数器0~99） - *p_origin = (uint32_t)(*p_sum / window); // 计算平均值 - *p_cnt = 0; - *p_sum = 0; - // PRINT("window:%d, new_origin:%d\n", window, *p_origin); - } -} - - -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 * (g_loop_sens_list.sens[g_freq_sens].sens_out - 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 * (g_loop_sens_list.sens[sens_level].sens_out - 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(_first_cnt < 3){ //连续三次有变动 - _first_cnt++; - } - - 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--; - } - } - - if((firstOrder || secondOrder) && (_first_cnt >= 3)) - { - flt_mgr.counter_not_idle = 0; - flt_mgr.flag_not_idle = 1; - } - if(flt_mgr.flag_not_idle) - { - 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++; - - // 达到采样次数后计算稳定初始值 - 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; - LEDA_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 << 5)/((float)(loop1_LPCNT))) + 360; - } - 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; - } - } - -} - - -/** - * @brief 判断新值是否在指定阶段的区间内 - * @param new_flt 新的一阶滤波值 - * @param result 输出匹配结果 - */ -uint8_t check_flt_by_stage(uint16_t new_flt) { - - for (uint8_t i = 1; i < STAGE_COUNT_SHORT; i++) { //offset从1开始，表示开机的稳定参考值不参与释放判断 - StageRangeConfig *cfg = &stage_config[i]; - if (cfg->base_value == 0) continue; // 忽略未初始化的阶段 - - - // 判断是否在区间内 - if (new_flt >= cfg->lower && new_flt <= cfg->upper) { - return (i+1); - } - } - return 0; -} - - -uint8_t check_flat_leave(FltHistoryManager* fltMgr, int16_t firstOrder, int16_t secondOrder, uint16_t datCurrent) -{ - static uint8_t _flag = 0; - if(firstOrder || secondOrder) - { -// if(firstOrder > 0) -// { -// fltMgr->cnt_release_Up++; -// } -// else if(firstOrder < 0) -// { -// fltMgr->min_release_capvd = datCurrent; -// fltMgr->cnt_release_Down++; -// } - - fltMgr->cnt_flat_release = 0; - fltMgr->flag_flat_release = 1; - - return 0; - } - if(fltMgr->flag_flat_release) - { - return 0; - } - - fltMgr->flag_second_judge = 1; - if(g_function_mode & FUNCTION_B){ - // 初始化二次判断的时间计数器 - fltMgr->second_judge_enter_time = 0; - fltMgr->cnt_stable_to_idle = 0; - } - return 0; -} - - - -void vd1_task(void){ - static uint32_t new_Flt_prev = 0; // 保存上一次滤波值 - int32_t delta_current = 0; - int32_t delta2_flt = 0; - - if(g_loop_acs_info.flag_change == 0){ - g_loop_acs_info.loop_capvd = loop1_Value; //记录 有车无车变化的参数 - } - - // if(g_function_mode & FUNCTION_B) - { - if(g_flag_reset_delta == 0){ - new_Flt_prev = loop1_Origin; - loop1_CAPVD = loop1_Origin; - g_flag_reset_delta = 1; - } - } - - loop1_CAPVD = get_flt_value(loop1_Value, 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(g_function_mode & FUNCTION_B){ - // 检测快速变化：如果变化量符号频繁反转且幅度较大，说明是线圈中间移动 - if(abs(delta_current) > 10 && abs(last_delta_for_rapid) > 10) - { - if((delta_current > 0 && last_delta_for_rapid < 0) || - (delta_current < 0 && last_delta_for_rapid > 0)) - { - rapid_change_detected = 1; - rapid_change_duration++; - } - else - { - // 变化方向一致，逐渐清除快速变化标志 - if(rapid_change_duration > 0) - { - rapid_change_duration--; - } - if(rapid_change_duration == 0) - { - rapid_change_detected = 0; - } - } - } - last_delta_for_rapid = delta_current; - } - - // 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(flt_mgr.flag_loop_reconnect) // 处理线圈重连的情况 - { - if(flt_mgr.flag_loop_reconnect >= 2) - { - - if(!flt_mgr.last_loop_vd_flag) - { - LEDA_OFF; - flt_mgr.flag_loop_reconnect = 0; - return; - } - - loop1_dlt_ORG = ((uint32_t)flt_mgr.last_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; - - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - new_Flt_prev = loop1_CAPVD; // 更新历史值 - - // if(( flt_mgr.last_loop_Origin - loop1_dlt_ORG ) < loop1_CAPVD ){ - - // } - if(( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD) // - { // 车离开 - //TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？ - // 1、如果增加平坦算法，会有延时，不能及时落杆； - - if(loop1_VD_FLAG) - { - loop1_FLAG_OUT = 1; - } - loop1_VD_FLAG = 0; - - g_loop_acs_info.car_state = 0; -// LED_LP1_OFF(); - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - loop1_VD_HOLD = 0 ; - Hold_CNT = 0; - - flt_mgr.flag_5sec = 0; - flt_mgr.counter_5sec = 0; - flt_mgr.last_loop_vd_flag = 0; - flt_mgr.flag_loop_reconnect = 0; - - loop1_Origin = flt_mgr.last_loop_Origin; - } // end if(( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD) - else if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD)) - { -// PRINT("FLAT_TRIGER_CAR_OFF_init_re:%d,5sec:%d,5min:%d,10min:%d,30min:%d,60min:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", -// flt_mgr.stable_init_value, flt_mgr.recent_5sec, flt_mgr.recent_5min, flt_mgr.recent_10min, flt_mgr.recent_30min, flt_mgr.recent_60min, -// loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - -// loop1_FLAG_OUT = 1; - if(loop1_VD_FLAG) - { - loop1_FLAG_OUT = 1; - } - loop1_VD_FLAG = 0; - - g_loop_acs_info.car_state = 0; - -// LED_LP1_OFF(); - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - loop1_VD_HOLD = 0 ; - Hold_CNT = 0; - - flt_mgr.flag_5sec = 0; - flt_mgr.counter_5sec = 0; - flt_mgr.last_loop_vd_flag = 0; - flt_mgr.flag_loop_reconnect = 0; - loop1_Origin = flt_mgr.last_loop_Origin; - } // end else if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD)) - else - { - if(!loop1_VD_FLAG) - { - if(( flt_mgr.last_loop_Origin - loop1_dlt_ORG ) < loop1_CAPVD ){ - if(loop1_VD_FLAG) - { - loop1_FLAG_OUT = 1; - } - loop1_VD_FLAG = 0; - g_loop_acs_info.car_state = 0; - - // LED_LP1_OFF(); - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - loop1_VD_HOLD = 0 ; - Hold_CNT = 0; - - flt_mgr.flag_5sec = 0; - flt_mgr.counter_5sec = 0; - flt_mgr.last_loop_vd_flag = 0; - flt_mgr.flag_loop_reconnect = 0; - loop1_Origin = flt_mgr.last_loop_Origin; - - return; - } - - if(loop1_CAPVD < (flt_mgr.last_loop_Origin - loop1_dlt_ORG)) // - {// 有车 - if(loop1_VD_FLAG == 0){ - g_loop_acs_info.flag_change = 1; - } - - loop1_VD_FLAG = 1; - loop1_FLAG_IN = 1; - g_loop_acs_info.car_state = 1; - if(g_loop_cng_unit.exist_mode) //有限存在10分钟 - { - loop1_VD_HOLD = 1 ; - } - - } - } // end if(!loop1_VD_FLAG) - flt_mgr.flag_loop_reconnect = 4; - if(flt_mgr.flag_second_judge) - { - if(!flt_mgr.second_loop_vd_flag) - { - update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size); - - // 5. 调用二阶变化量计算函数 - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); -// PRINT("Second_CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - - if(!delta_current || !delta2_flt) - { - return; - } - if(flt_mgr.second_loop_Origin == 0) - { - return; - } - - flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens & 0x0F].sens_in) >> 16; //loop1_dlt_ORG??:?--26;?--13;?--6 - if(loop1_CAPVD < (flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG)) // - {// 有车 - flt_mgr.second_loop_vd_flag = 1; - } - } // end if(!flt_mgr.second_loop_vd_flag) - else - { - flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; - - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - new_Flt_prev = loop1_CAPVD; // 更新历史值 - // PRINT("CarON_LPCNT:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - - if(( flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG ) < loop1_CAPVD) // - { // 车离开 - //TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？ - // 1、如果增加平坦算法，会有延时，不能及时落杆； - if(loop1_VD_FLAG == 1){ - g_loop_acs_info.flag_change = 1; - } - loop1_VD_FLAG = 0; - loop1_FLAG_OUT = 1; - g_loop_acs_info.car_state = 0; - -// LED_LP1_OFF(); - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - loop1_VD_HOLD = 0 ; - Hold_CNT = 0; - - 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.flag_second_judge = 0; - flt_mgr.second_loop_ORG_CNT = 0; - flt_mgr.second_loop_ORG_SUM = 0; - - loop1_Origin = flt_mgr.second_loop_Origin; - } - } // end else - } // end if(flt_mgr.flag_second_judge) - } // end else - } // end if(flt_mgr.flag_loop_reconnect >= 2) return; } + *p_sum += new_value; + (*p_cnt)++; - - if(!loop1_VD_FLAG){ - - update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, g_moving_average_window_size); - - // 5. 调用二阶变化量计算函数 - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - update_flt_history(loop1_CAPVD, delta_current, delta2_flt); -// PRINT("CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - - if(!delta_current) // || !delta2_flt) - { - return; - } -// PRINT("CarOFF_5sec:%d, Origin:%d, loop1_CAPVD:%d\n", flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD); - - if(!flt_mgr.is_initialized) - { - return; - } - - if(loop1_LOOP_OK0 == 0){ - return; - } - - loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens & 0x0F].sens_in) >> 16; - if(g_flag_bt_state){ - if(g_loop_acs_info.flag_change == 0){ - g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); - if(g_loop_acs_info.variation > 5 || g_loop_acs_info.variation < -5){ - g_loop_acs_info.flag_event = 1; - } - } - } - else{ - g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); - } - - if(loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) // - { - if(g_function_mode & FUNCTION_B){ - 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; - } - - // 改进：增加吸合多重确认机制，避免误触发 - // 如果检测到快速变化（线圈中间移动），不立即吸合 - if(rapid_change_detected && rapid_change_duration < 10) - { - relay_on_confirm_cnt = 0; // 重置确认计数 - return; // 等待稳定后再判断 - } - - relay_on_confirm_cnt++; - if(relay_on_confirm_cnt < RELAY_ON_CONFIRM_THRESHOLD){ - return; // 需要连续多次确认才吸合 - } - relay_on_confirm_cnt = RELAY_ON_CONFIRM_THRESHOLD; // 达到阈值后保持最大值 - - // 检测到车辆进入，重置释放确认计数 - relay_off_confirm_cnt = 0; - - // 重置二次判断增强条件的计数器 - flt_mgr.second_judge_enter_time = 0; - flt_mgr.cnt_stable_to_idle = 0; - } - - 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.flag_second_judge = 0; - - if(loop1_VD_FLAG == 0){ - g_loop_acs_info.flag_change = 1; - } - - loop1_VD_FLAG = 1; //???????1 - loop1_FLAG_IN = 1; //?????? -// PRINT("Car_In,sens_level:%d, counter:%d\n", g_freq_sens, g_tmp_counter); - if(g_loop_cng_unit.exist_mode) - { - loop1_VD_HOLD = 1; // 有限存在10分钟 - } - -// LEDA_ON; - g_loop_acs_info.flag_event = 1; - g_loop_acs_info.car_state = 1; - - // if(g_loop_cng_unit.loopSafe_Timeout > 0) - // { - // loop1_LC_HOLD = 1; - // } - // else - // { - // loop1_LC_HOLD = 0; - // } - if(loop1_LC_Reset) - loop1_LC_Reset = 0; - } - }// end if !loop1_VD_FLAG - else{ - loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; - if(g_loop_acs_info.flag_change == 0) - { - g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); - if(g_loop_acs_info.variation > 5 || g_loop_acs_info.variation < -5){ - g_loop_acs_info.flag_event = 1; - } - } - - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - new_Flt_prev = loop1_CAPVD; // 更新历史值 - - if(g_function_mode & FUNCTION_B){ - if(flt_mgr.cnt_simple_busy) - { - flt_mgr.cnt_simple_busy = 0; - } - } - - if((( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD )) // - { - if(g_function_mode & FUNCTION_B){ - // 改进：如果检测到快速变化，说明可能是线圈中间移动，不是真正的离开 - // 此时不应该释放继电器，而是继续监测 - if(rapid_change_detected) - { - // 快速变化期间，重置释放确认计数，防止误释放 - relay_off_confirm_cnt = 0; - PRINT("RAPID_CHANGE_IGNORE_RELEASE\n"); - return; // 不释放，继续监测 - } - - // 多重确认机制：需要连续多次满足释放条件才真正释放 - relay_off_confirm_cnt++; - if(relay_off_confirm_cnt < RELAY_OFF_CONFIRM_THRESHOLD){ - PRINT("RELEASE_PENDING cnt:%d/%d\n", relay_off_confirm_cnt, RELAY_OFF_CONFIRM_THRESHOLD); - return; // 还未达到确认次数，暂不释放 - } - } - - if(loop1_VD_FLAG == 1){ - g_loop_acs_info.flag_change = 1; - } - loop1_VD_FLAG = 0; //???????0 - loop1_FLAG_OUT = 1; //?????? - loop1_VD_HOLD = 0 ; - loop1_LC_HOLD = 0; - - if(g_function_mode & FUNCTION_B){ - // 释放后重置所有确认计数器 - relay_on_confirm_cnt = 0; - relay_off_confirm_cnt = 0; - rapid_change_detected = 0; - rapid_change_duration = 0; - } - - g_loop_acs_info.flag_event = 1; - g_loop_acs_info.car_state = 0; - // g_led_A_state = 0; -// LEDA_OFF; - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - Hold_CNT = 0; - - 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; - flt_mgr.flag_second_judge = 0; - } - else if(g_function_mode & FUNCTION_A){ - if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD)){ - // PRINT("FLAT_TRIGER_CAR_OFF_init:%d,5sec:%d,5min:%d,10min:%d,30min:%d,60min:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", - // flt_mgr.stable_init_value, flt_mgr.recent_5sec, flt_mgr.recent_5min, flt_mgr.recent_10min, flt_mgr.recent_30min, flt_mgr.recent_60min, - // loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - if(g_function_mode & FUNCTION_B){ - // 平坦算法触发释放也需要多重确认 - relay_off_confirm_cnt++; - if(relay_off_confirm_cnt < RELAY_OFF_CONFIRM_THRESHOLD){ - return; - } - } - - if(loop1_VD_FLAG == 1){ - g_loop_acs_info.flag_change = 1; - } - loop1_VD_FLAG = 0; - loop1_FLAG_OUT = 1; - g_loop_acs_info.car_state = 0; - - // LEDA_OFF; - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - loop1_VD_HOLD = 0 ; - Hold_CNT = 0; - - if(g_function_mode & FUNCTION_B){ - // 释放后重置所有确认计数器 - relay_on_confirm_cnt = 0; - relay_off_confirm_cnt = 0; - rapid_change_detected = 0; - rapid_change_duration = 0; - } - - 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; - flt_mgr.flag_second_judge = 0; - } - else{ // - if(flt_mgr.flag_second_judge){ //有车情况下二次判断 - if(g_function_mode & FUNCTION_B){ - // 条件1：检查是否满足最小时间限制（10秒） - if(flt_mgr.second_judge_enter_time < flt_mgr.second_judge_min_time) - { - // 未达到最小时间，继续监测，不进入二次判断释放逻辑 - // 但仍需要更新二次判断的基准值 - if(!flt_mgr.second_loop_vd_flag) - { - update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size); - } - return; - } - } - - if(!flt_mgr.second_loop_vd_flag) - { - update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size); - - // 5. 调用二阶变化量计算函数 - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - // PRINT("Second_CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - - if(!delta_current || !delta2_flt) - { - return; - } - if(flt_mgr.second_loop_Origin == 0) - { - return; - } - - flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; //loop1_dlt_ORG??:?--26;?--13;?--6 - if(loop1_CAPVD < (flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG)) // - {// 有车 - flt_mgr.second_loop_vd_flag = 1; - if(g_function_mode & FUNCTION_B){ - // 重置稳定计数 - flt_mgr.cnt_stable_to_idle = 0; - } - } - } - else { - flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; - - delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); - new_Flt_prev = loop1_CAPVD; // 更新历史值 - // PRINT("CarON_LPCNT:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt); - - if(( flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG ) < loop1_CAPVD) // - { // 车离开 - if(g_function_mode & FUNCTION_B){ - // 条件2：检查是否连续多次检测到恢复到空闲值 - // 计算当前值与初始空闲值的偏差 - uint32_t deviation_from_init = 0; - if(flt_mgr.stable_init_value > loop1_CAPVD) - { - deviation_from_init = flt_mgr.stable_init_value - loop1_CAPVD; - } - else - { - deviation_from_init = loop1_CAPVD - flt_mgr.stable_init_value; - } - - // 判断是否接近空闲值（偏差小于初始值的5%） - uint32_t idle_threshold = (flt_mgr.stable_init_value * 5) / 100; - if(deviation_from_init <= idle_threshold) - { - flt_mgr.cnt_stable_to_idle++; - PRINT("STABLE_TO_IDLE cnt:%d/%d, dev:%d, thresh:%d\n", - flt_mgr.cnt_stable_to_idle, flt_mgr.stable_to_idle_threshold, - deviation_from_init, idle_threshold); - } - else - { - // 偏差不够，重置计数 - flt_mgr.cnt_stable_to_idle = 0; - return; - } - - // 需要连续多次确认才释放 - if(flt_mgr.cnt_stable_to_idle < flt_mgr.stable_to_idle_threshold) - { - return; // 还未达到稳定次数，暂不释放 - } - } - - //TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？ - // 1、如果增加平坦算法，会有延时，不能及时落杆； - if(loop1_VD_FLAG == 1){ - g_loop_acs_info.flag_change = 1; - } - loop1_VD_FLAG = 0; - loop1_FLAG_OUT = 1; - - g_loop_acs_info.car_state = 0; - // LEDA_OFF; - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - loop1_VD_HOLD = 0 ; - Hold_CNT = 0; - - 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.flag_second_judge = 0; - flt_mgr.second_loop_ORG_CNT = 0; - flt_mgr.second_loop_ORG_SUM = 0; - - if(g_function_mode & FUNCTION_B){ - // 重置二次判断增强条件的计数器 - flt_mgr.second_judge_enter_time = 0; - flt_mgr.cnt_stable_to_idle = 0; - } - - loop1_Origin = flt_mgr.second_loop_Origin; - } - } - } // end if(flt_mgr.flag_second_judge) - } - } //end if(g_function_mode & FUNCTION_A) - + if (*p_cnt >= window) { + *p_origin = (uint32_t)(*p_sum / window); + *p_cnt = 0; + *p_sum = 0; } } +/*=========================================================================== + * 初始化所有检测状态 (上电 / 有限存在超时 / 线圈重连 时调用) + * 注意: 不复位 g_loop_power_up_state / g_loop_cut_amount (BLE 上报用) + *===========================================================================*/ void INIT_VD(void) { + loop1_INI_LOOP = 1; + loop1_LPCNT = 0; 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_CapSum = 0; + loop1_Value = 0; + loop1_CAPVD = 0; + loop1_Origin = 0; 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_PLUSE_IN_F = 0; + loop1_FLAG_PLUSE_IN = 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.second_judge_enter_time = 0; - flt_mgr.second_judge_min_time = 2000; // 10秒（10000ms / 5ms = 2000） - flt_mgr.cnt_stable_to_idle = 0; - flt_mgr.stable_to_idle_threshold = 10; // 连续10次检测到恢复到空闲值 - + loop1_VD_HOLD = 0; + Hold_CNT = 0; + loop1_LC_HOLD = 0; + loop1_LC_Reset = 0; + LC_Hold_CNT = 0; + + /* 新算法状态 */ + g_loop_stable = 0; + loop1_stable_cnt = 0; + loop1_entry_cnt = 0; + loop1_cnt_release = 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 + g_env_activity = 0; + g_env_prev_capvd = 0; + + g_loop_acs_info.car_state = 0; } +/*=========================================================================== + * 车辆离开 / 释放 公共处理 + *===========================================================================*/ +static void loop_release_common(void) +{ + loop1_VD_FLAG = 0; + loop1_FLAG_OUT = 1; + loop1_VD_HOLD = 0; + loop1_LC_HOLD = 0; + Hold_CNT = 0; + LC_Hold_CNT = 0; + loop1_ORG_CNT = 0; + loop1_ORG_SUM = 0; + + g_loop_acs_info.flag_change = 1; + g_loop_acs_info.flag_event = 1; + g_loop_acs_info.car_state = 0; +} + + +/*=========================================================================== + * vd1_task -- 核心检测算法 (移植 DLD154V4B, 每次 CAP_OK 调用, ~8.74ms) + * + * 流程: + * 1. IIR 滤波 + 斜率限幅 + * 2. 稳定期: 小窗口快速收敛基线 + * 3. 无车: 基线跟踪 (冻结保护) + 进入确认 + * 4. 有车: 平坦性三条件离开判定 (带滞回) + *===========================================================================*/ +void vd1_task(void){ + + if (loop1_Origin == 0) return; + + /*--- 1. IIR 滤波 + 斜率限幅 ---*/ + if (loop1_CAPVD == 0) { + loop1_CAPVD = loop1_Value; + } else { + int32_t raw_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD; + int32_t max_step = (int32_t)(loop1_CAPVD * MAX_SLOPE_RATE / 100); + uint32_t clamped_value; + if (max_step < 100) max_step = 100; + if (raw_delta > max_step) raw_delta = max_step; + if (raw_delta < -max_step) raw_delta = -max_step; + clamped_value = (uint32_t)((int32_t)loop1_CAPVD + raw_delta); + loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD); + } + + /*--- 2. 稳定期: 绕过 IIR/斜率限幅, 小窗口快速收敛 ---*/ + if (!g_loop_stable) { + loop1_CAPVD = loop1_Value; + update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, 100); + loop1_stable_cnt++; + if (loop1_stable_cnt >= STABLE_SAMPLES) { + loop1_stable_cnt = 0; + g_loop_stable = 1; + } + return; + } + + /*--- 3. BLE 上报状态刷新 (flag_change 置位期间冻结, 保持事件时刻的值) ---*/ + if (g_loop_acs_info.flag_change == 0) { + int32_t _var = (int32_t)loop1_Origin - (int32_t)loop1_CAPVD; + g_loop_acs_info.loop_capvd = loop1_CAPVD; + if (_var > 32767) _var = 32767; + if (_var < -32768) _var = -32768; + g_loop_acs_info.variation = (int16_t)_var; + if (g_flag_bt_state && (_var > VARIATION_EVENT_TH || _var < -VARIATION_EVENT_TH)) { + g_loop_acs_info.flag_event = 1; + } + } + + /*--- 4. 环境活跃度 (BLE condition 字段, 0~15) ---*/ + { + int32_t d = (int32_t)loop1_CAPVD - (int32_t)g_env_prev_capvd; + if (d < 0) d = -d; + if (g_env_prev_capvd != 0 && d > ENV_ACT_TH) { + if (g_env_activity < 200) g_env_activity++; + } else { + if (g_env_activity) g_env_activity--; + } + g_env_prev_capvd = loop1_CAPVD; + } + + if (!loop1_VD_FLAG) { + /*================================================================ + * 无车状态: 基线跟踪 (冻结保护) + 进入确认 + *================================================================*/ + loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_in) >> 16; + { + int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin; + if (dev < (int32_t)(loop1_dlt_ORG * 4)) { + loop1_freeze_cnt = 0; + update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN); + } else { + /*--- CAPVD 异常偏高: 暂停跟踪, 冻结超时 + 稳定性双条件 ---*/ + 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; + } + } + + /*--- 进入确认: 连续 ENTRY_CONFIRM 次低于阈值才判有车 ---*/ + if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) { + loop1_entry_cnt++; + if (loop1_entry_cnt >= ENTRY_CONFIRM) { + loop1_entry_cnt = 0; + loop1_freeze_cnt = 0; + loop1_freeze_ref = 0; + + loop1_VD_FLAG = 1; + loop1_FLAG_IN = 1; + + if (g_loop_cng_unit.exist_mode) { /* 有限存在 */ + loop1_VD_HOLD = 1; + Hold_CNT = 0; + } + if (g_loop_cng_unit.loopSafe_Timeout) { /* 线圈安全模式: 启动安全复位计时 */ + loop1_LC_HOLD = 1; + LC_Hold_CNT = 0; + } else { + loop1_LC_HOLD = 0; + } + if (loop1_LC_Reset) { + loop1_LC_Reset = 0; + } + + loop1_ORG_CNT = 0; + loop1_ORG_SUM = 0; + + g_loop_acs_info.flag_change = 1; + g_loop_acs_info.flag_event = 1; + g_loop_acs_info.car_state = 1; + +#if USE_FLATNESS_EXIT + /* 重置平坦性状态 (专利 CN200910309382) */ + 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; + g_prev_capvd = (int32_t)loop1_CAPVD; + g_prev_first_deriv = 0; +#endif + } + } else { + if (loop1_entry_cnt > 0) loop1_entry_cnt = 0; + } + } + else { +#if USE_FLATNESS_EXIT + /*================================================================ + * 有车状态: 平坦性三条件判定离开 (专利 CN200910309382) + * 幅值阈值按新测量基准 2^19 缩放 (DLD154V4B: 100/5/2 @2^17) + *================================================================*/ +#define K1 8 +#define K2 8 +#define SLOPE_FLAT_THRESH 350 +#define MIN_DELTA2 18 +#define MIN_DELTA3 7 +#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) { + /*--- 阶段0: 追踪第一上升坡面, 自适应计算平坦阈值 ---*/ + if (abs_fd > (int32_t)g_max_slope) g_max_slope = (uint16_t)abs_fd; + if (abs_sd > (int32_t)g_max_slope_rate) g_max_slope_rate = (uint16_t)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 { + /*--- 阶段1: 幅值回归 + 一阶平坦 + 二阶平坦 三条件确认离开 ---*/ + 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 * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 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) { + g_flat_ok_cnt = 0; + g_exit_state = 0; + loop_release_common(); + } + } else { + g_flat_ok_cnt = 0; + } + } + g_prev_capvd = (int32_t)loop1_CAPVD; + g_prev_first_deriv = first_deriv; +#else + /*================================================================ + * 有车状态: 简单滞回 + 防抖离开 + *================================================================*/ + loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; + + if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD) { + loop1_cnt_release++; + if (loop1_cnt_release >= 3) { + loop1_cnt_release = 0; + loop_release_common(); + } + } else { + if (loop1_cnt_release > 0) loop1_cnt_release = 0; + } +#endif + } +} + + +/*=========================================================================== + * cjq_srv -- 车检器主循环服务 + * - 有限存在超时 / 线圈断开重连: 全复位重建基线 + * (线圈仍被占用时, 新基线落在占用值上, 继电器保持释放 -- 对齐 M1H 行为) + * - CAP_OK 到达时执行核心检测算法 + *===========================================================================*/ void cjq_srv(void){ - + if(loop1_LOOP_OK){ if(loop1_FLAG_HOLD_TIMEOUT){ + /* 有限存在超时: 全复位, 重新经历稳定期重建基线 */ INIT_VD(); + return; } - if(loop1_CAP_OK){ - if(!g_loop_power_up_state){ - g_loop_power_up_state = 1; // // 表示连接了线圈 - } - else{ - if(loop1_FLAG_CUT){ - - } - } - } if(loop1_FLAG_CUT){ - // 线圈断开后重新连上 需要复位 - loop1_FLAG_CUT = 0; -// SYS_ResetExecute(); - if(loop1_VD_FLAG){ - if(g_loop_cng_unit.exist_mode){ - Hold_CNT = 0; - loop1_VD_HOLD = 1; - } - } - flt_mgr.flag_loop_reconnect = 1; - } - if(g_loop_cut_last != loop1_FLAG_CUT){ - g_loop_cut_last = loop1_FLAG_CUT; + /* 线圈断开后重连: 全复位, 重新自适应测量窗并重建基线 */ + g_loop_cut_last = 0; + INIT_VD(); + return; } if(!loop1_LOOP_OK0){ - DelayMs(200); + DelayMs(200); /* 线圈重新起振后等待 200ms 再消费数据 */ loop1_LOOP_OK0 = 1; } if(loop1_CAP_OK){ loop1_CAP_OK = 0; + if(!g_loop_power_up_state){ + g_loop_power_up_state = 1; // 表示连接了线圈 + } vd1_task(); - }// end if loop1_CAP_OK + poll_sens_key(); + } }// end if loop1_LOOP_OK else{ loop1_FLAG_CUT = 1; @@ -1421,8 +798,10 @@ void cjq_srv(void){ loop1_FLAG_IN = 0; loop1_FLAG_OUT = 0; loop1_FLAG_PLUSE = 0; + loop1_FLAG_PLUSE_IN_F = 0; + loop1_FLAG_PLUSE_IN = 0; - // 线圈断开，继电器释放，状态为0，车辆状态也为0 + // 线圈断开: 继电器释放, 车辆状态清零 RLY1_OFF; RLY2_OFF; g_loop_acs_info.relay1_state = 0; g_loop_acs_info.relay2_state = 0; @@ -1436,7 +815,6 @@ void cjq_srv(void){ g_loop_cut_amount = 255; } } - } } @@ -1460,7 +838,6 @@ void Main_Circulation() while(1) { TMOS_SystemProcess(); - // poll_sens_key(); #ifndef DEBUG poll_dbn_ble(); #endif @@ -1476,11 +853,12 @@ void Main_Circulation() #ifdef DEBUG if(g_flag_output){ + uint32_t tmp; g_flag_output = 0; - uint32_t tmp = (g_sys_freq )/(loop1_Xn); - PRINT("Xn: %d, loop1_LPCNT: %d, counter:%d, ms_counter:%d, capvd:%d, loop1_Origin:%d, freq:%d, init_freq:%d\n", - loop1_Xn, loop1_LPCNT, g_xn_counter, g_xn_counter/2000, loop1_CAPVD, loop1_Origin, tmp, flt_mgr.init_freq); - + tmp = (loop1_Xn) ? (g_sys_freq / loop1_Xn) : 0; + PRINT("Xn:%d, LPCNT:%d, edges:%d, CAPVD:%d, Origin:%d, VD:%d, freq:%d\n", + loop1_Xn, loop1_LPCNT, g_xn_counter, loop1_CAPVD, loop1_Origin, loop1_VD_FLAG, tmp); + g_xn_counter = 0; } #endif @@ -1513,12 +891,10 @@ int main(void) for(i = 0; i < 6; i++) { g_dev_number[i] = MACAddr[i]; - } + } g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT; // max INIT_VD(); para_store_init(); -// DelayMs(10); - init_second_order(&second_order_state, 0); tm_init(); @@ -1533,21 +909,28 @@ int main(void) /* @fn TMR0_IRQHandler * -* @brief TMR0中断函数 +* @brief TMR0 输入捕获中断 -- 线圈频率测量 +* FallEdge_To_FallEdge 模式, CAPGetData 直接给出相邻下降沿周期 Xn +* 累加 LPCNT 次 -> 一个测量窗 Value (~= MEASUREMENT_BASE) * * @return none */ __INTERRUPT __HIGH_CODE -void TMR0_IRQHandler(void) // TMR0 定时中断 +void TMR0_IRQHandler(void) // TMR0 捕获中断 { if(TMR0_GetITFlag(TMR0_3_IT_DATA_ACT)){ - loop1_Xn = TMR0_CAPGetData(); + loop1_Xn = (uint16_t)TMR0_CAPGetData(); #ifdef DEBUG g_xn_counter++; #endif + if(loop1_Xn == 0){ + R8_TMR0_INT_FLAG |= 0xff; + return; + } + if(!loop1_RF_FLAG){ loop1_RF_FLAG = 1; } @@ -1555,29 +938,31 @@ void TMR0_IRQHandler(void) // TMR0 loop1_CapCnt++; if(loop1_INI_LOOP){ -// if(g_filter_step < 2){ - if(loop1_LPCNT == 0){ - if(loop1_CapCnt > 200){ - loop1_LPCNT = (32768/loop1_Xn)<<6; - if(loop1_LPCNT == 0){ - loop1_LPCNT = 10; - } - - g_freq_tmp = (float)g_sys_freq/((float)loop1_Xn); - - loop1_CAPVD = 0; - loop1_CapSum = 0; - loop1_CapCnt = 0; + /*--- 初始化阶段 ---*/ + if(loop1_LPCNT == 0){ + if(loop1_CapCnt > 200){ /* 上电/换频后等待振荡稳定 */ + uint32_t lpcnt = (uint32_t)MEASUREMENT_BASE / loop1_Xn; + if(lpcnt == 0 || lpcnt > 65535){ + lpcnt = 100; /* 异常 Xn 保护 */ } - } + loop1_LPCNT = (uint16_t)lpcnt; -// }//end if g_filter_step < 2 + g_freq_tmp = (float)g_sys_freq/((float)loop1_Xn); + + loop1_CAPVD = 0; + loop1_CapSum = 0; + loop1_CapCnt = 0; + } + } else{ + /* 第一个测量窗: 直接作为基线 Origin */ loop1_CapSum += loop1_Xn; if(loop1_CapCnt >= loop1_LPCNT){ - g_loop_acs_info.loop_capvd = (loop1_CapSum >> 5); - loop1_CAPVD = g_loop_acs_info.loop_capvd; + loop1_CAPVD = loop1_CapSum; loop1_Origin = loop1_CAPVD; + if(g_loop_acs_info.flag_change == 0){ + g_loop_acs_info.loop_capvd = loop1_CAPVD; + } loop1_INI_LOOP = 0; loop1_CapSum = 0; loop1_Value = 0; @@ -1586,15 +971,14 @@ void TMR0_IRQHandler(void) // TMR0 } }// end if loop1_INI_LOOP else { + /*--- 正常运行: 累加到测量窗 ---*/ loop1_CapSum += loop1_Xn; if(loop1_CapCnt >= loop1_LPCNT){ - loop1_Value = (loop1_CapSum >> 5); // g_loop_acs_info.loop_capvd = (loop1_CapSum >> 5); - // loop1_Value = g_loop_acs_info.loop_capvd; + loop1_Value = loop1_CapSum; loop1_CAP_OK = 1; loop1_CapSum = 0; loop1_CapCnt = 0; - loop1_INI_LOOP = 0; } } } @@ -1608,16 +992,18 @@ void TMR0_IRQHandler(void) // TMR0 /********************************************************************* * @fn TMR1_IRQHandler * - * @brief TMR1中断函数 + * @brief TMR1 定时中断 5ms/per -- 时序状态机 / 继电器输出 / LED * * @return none */ __INTERRUPT __HIGH_CODE -void TMR1_IRQHandler(void) // TMR1 定时中断 5ms/per +void TMR1_IRQHandler(void) { static uint16_t _counter1_init = 0; +#ifdef DEBUG static uint16_t _cnt = 0; +#endif if(TMR1_GetITFlag(TMR0_3_IT_CYC_END)) { @@ -1627,112 +1013,38 @@ void TMR1_IRQHandler(void) // TMR1 g_flag_output = 1; _cnt = 0; } - #endif if(g_dbn_ble_state_acs_enable.enable && g_flag_bt_state){ g_loop_acs_info.report_counter++; - } + } TimingDelayInc++; + u32CountWdt++; - + /*--- LED: 稳定期 200ms 快闪; 线圈断开 150ms 快闪 ---*/ if(loop1_FLAG_CUT == 0) { - if(!flt_mgr.is_initialized) + if(!g_loop_stable) { _counter1_init++; - if(_counter1_init >=40) // 30:150ms, 40: 200ms, 50:250ms + if(_counter1_init >= 40) // 40: 200ms { _counter1_init = 0; LEDA_ON_OFF(); } } } - if(flt_mgr.flag_flat_release) + else { - flt_mgr.cnt_flat_release++; - if(flt_mgr.cnt_flat_release >= flt_mgr.max_cnt_flat_release) + _counter1_init++; + if(_counter1_init >= 30) // 30: 150ms { - flt_mgr.flag_flat_release = 0; - flt_mgr.cnt_flat_release = 0; + _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 - { - 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; - } - } - - if(loop1_FLAG_CUT) - { - _counter1_init++; - if(_counter1_init >= 30) // 30:150ms, 40: 200ms, 50:250ms - { - _counter1_init = 0; - LEDA_ON_OFF(); - } - } -// else -// { -// if(loop1_LOOP_OK == 0) -// { -// if(g_led_A_cnt >= 100) // 200: 1000ms,1s -// { -// g_led_A_cnt = 0; -// LEDA_ON_OFF(); -// } -// } -// else -// { -// if(loop1_VD_FLAG) // 有车 car busy -// { -// } -// else // No car , idle -// { -// if(g_led_A_cnt >= 100) // 100: 500ms -// { -// g_led_A_cnt = 0; -// LEDA_OFF; -// } -// } -// } -// } - - if(g_function_mode & FUNCTION_B){ - // 更新二次判断进入时间（当有车时累加） - if(loop1_VD_FLAG && flt_mgr.flag_second_judge) - { - flt_mgr.second_judge_enter_time++; - } - } - - - u32CountWdt++; - TM1cnt++; if(TM1cnt >= 10) //about=50ms { @@ -1775,7 +1087,7 @@ void TMR1_IRQHandler(void) // TMR1 if(loop1_FLAG_OUT) // 从有车到无车的标志 { loop1_OUTCNT++; - if(loop1_OUTCNT > g_loop_out_delay) // + if(loop1_OUTCNT > g_loop_out_delay) { loop1_FLAG_OUT = 0; loop1_FLAG_PLUSE = 1; @@ -1804,12 +1116,11 @@ void TMR1_IRQHandler(void) // TMR1 Hold_CNT = 0; if(loop1_VD_FLAG) { - // flt_mgr.flag_second_judge = 1; // 二次判断 - // loop1_FLAG_CUT = 1; // - loop1_FLAG_HOLD_TIMEOUT = 1; - g_moving_average_window_size = LOOP_WINDOW_SIZE_FAST; // 进入快速补偿模式 - RLY1_OFF; RLY2_OFF; - // Reset_RstPeripheralAll(); + /* 有限存在超时: 释放继电器, 主循环全复位重建基线 */ + loop1_FLAG_HOLD_TIMEOUT = 1; + RLY1_OFF; RLY2_OFF; + g_loop_acs_info.relay1_state = 0; + g_loop_acs_info.relay2_state = 0; } } } @@ -1819,20 +1130,29 @@ void TMR1_IRQHandler(void) // TMR1 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; + /* 安全复位: 重新初始化测量, 重建基线 (对齐 M1H) */ + loop1_LC_Reset = 1; + loop1_INI_LOOP = 1; + loop1_LOOP_OK0 = 0; + g_loop_stable = 0; + loop1_stable_cnt = 0; + LC_Hold_CNT = 0; + loop1_ORG_CNT = 0; + loop1_ORG_SUM = 0; +#if USE_FLATNESS_EXIT + g_exit_state = 0; + g_max_slope = 0; + g_max_slope_rate = 0; + g_slope_flat_cnt = 0; + g_flat_ok_cnt = 0; +#endif } } } + /*--- 线圈载波检测 & 继电器输出刷新 (每 5ms) ---*/ if(loop1_RF_FLAG) { - // loop1_LOOP_OK0 = loop1_LOOP_OK; loop1_LOOP_OK = 1; loop1_RF_FLAG = 0; @@ -1844,13 +1164,10 @@ void TMR1_IRQHandler(void) // TMR1 { if(loop1_VD_FLAG | loop1_FLAG_OUT)// 输出存在信号 { - if((flt_mgr.flag_loop_reconnect & 0x03) == 0){ - RLY1_ON; - g_loop_acs_info.relay1_state = 1; - RLY2_ON; - g_loop_acs_info.relay2_state = 1; - } - + RLY1_ON; + g_loop_acs_info.relay1_state = 1; + RLY2_ON; + g_loop_acs_info.relay2_state = 1; } else { @@ -1864,11 +1181,8 @@ void TMR1_IRQHandler(void) // TMR1 { if(loop1_VD_FLAG) // 继电器2存在信号 { - if((flt_mgr.flag_loop_reconnect & 0x03) == 0){ - - RLY2_ON; - g_loop_acs_info.relay2_state = 1; - } + RLY2_ON; + g_loop_acs_info.relay2_state = 1; } else { @@ -1879,23 +1193,20 @@ void TMR1_IRQHandler(void) // TMR1 if(loop1_FLAG_PLUSE_IN) { RLY1_ON; - g_loop_acs_info.relay1_state = 1; + g_loop_acs_info.relay1_state = 1; } else { RLY1_OFF; - g_loop_acs_info.relay1_state = 0; + g_loop_acs_info.relay1_state = 0; } } break; case 2: // 离开脉冲 { if(loop1_VD_FLAG) // 继电器2存在信号 { - if((flt_mgr.flag_loop_reconnect & 0x03) == 0){ - - RLY2_ON; - g_loop_acs_info.relay2_state = 1; - } + RLY2_ON; + g_loop_acs_info.relay2_state = 1; } else { @@ -1917,11 +1228,8 @@ void TMR1_IRQHandler(void) // TMR1 default: { if(loop1_VD_FLAG) // 继电器2存在信号 { - if((flt_mgr.flag_loop_reconnect & 0x03) == 0){ - - RLY2_ON; - g_loop_acs_info.relay2_state = 1; - } + RLY2_ON; + g_loop_acs_info.relay2_state = 1; } else { @@ -1947,5 +1255,4 @@ void TMR1_IRQHandler(void) // TMR1 - /******************************** endfile @ main ******************************/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..3439759 --- /dev/null +++ b/README.md @@ -0,0 +1,89 @@ +# DLD110SV4 鍗曠嚎鍦堣摑鐗欒溅杈嗘娴嬪櫒 + +## 纭欢瑙勬牸 + +| 椤圭洰 | 鍙傛暟 | +|------|------| +| MCU | 娌佹亽 CH591R (RISC-V, BLE 5.1) | +| 涓婚 | 60MHz (PLL) | +| Flash | 192KB (BLE OTA 涓夋寮忓竷灞, 搴旂敤鍖 28KB) | +| RAM | 32KB (搴旂敤鍙敤绾 18KB, BLE 鍗忚鏍堝父椹) | +| 閫氶亾鏁 | 1 璺幆褰㈢嚎鍦 | +| 瀵瑰鎺ュ彛 | BLE (灏忕▼搴忚鍐欏弬鏁 / OTA / 鐘舵佷笂鎶)銆佸弻缁х數鍣 | +| 鎸囩ず鐏 | 绾㈣壊鐢垫簮鐏 (纭欢鐩磋繛)銆佺豢鑹茬姸鎬佺伅 LEDA | +| 鐏垫晱搴 | 婊戝姩寮鍏 3 绾 + BLE 鍙厤缃伒鏁忓害琛 | + +## IO 璧勬簮鍒嗛厤 + +### 绾垮湀娴嬮噺 + +| 寮曡剼 | 鏍囪瘑 | 澶栬 | 鍔熻兘 | +|------|------|------|------| +| PA9 | LP | TMR0 | 绾垮湀淇″彿杈撳叆鎹曡幏 (FallEdge_To_FallEdge) | +| PA12 | F1 / FLEVEL_H | GPIO | 璋冮鐢靛 33nF (0=鎺ュ叆) | +| PA13 | F2 / FLEVEL_L | GPIO | 璋冮鐢靛 10nF (0=鎺ュ叆) | + +棰戠巼妗ｄ綅 (loopFreq_Level): 楂橀 / 涓珮 / 涓綆 / 浣庨, 鐢变袱鍙數瀹圭粍鍚堝垏鎹€ + +### 杈撳嚭涓庢寚绀 + +| 寮曡剼 | 鏍囪瘑 | 鍔熻兘 | +|------|------|------| +| PA8 | RLY1 | 鍔熻兘缁х數鍣: 瀛樺湪 / 杩涘叆鑴夊啿 / 绂诲紑鑴夊啿 (output_mode 閰嶇疆) | +| PB23 | RLY2 | 瀛樺湪缁х數鍣 | +| PB22 | LEDA | 缁跨伅: 鏈夎溅浜 / 鏃犺溅鐏; 绋冲畾鏈 200ms 蹇棯; 鏂嚎 150ms 蹇棯 | + +### 婊戝姩寮鍏 + +| 寮曡剼 | 鏍囪瘑 | 鍔熻兘 | +|------|------|------| +| PA14 | KEY_SA1 | 鐏垫晱搴﹂夋嫨 (涓 SA2 缁勫悎鍑 3 绾) | +| PA15 | KEY_SA2 | 鐏垫晱搴﹂夋嫨 | + +SA1=1/SA2=1 鈫 0 妗(浣); SA1=0/SA2=1 鈫 1 妗(涓); SA1=1/SA2=0 鈫 2 妗(楂)銆 + +## 妫娴嬬畻娉 (V2.0, 绉绘鑷 DLD154V4B) + +- **鑷傚簲娴嬮噺绐**: `LPCNT = 2^19 / Xn`, 娴嬮噺绐楁椂闀 ~8.74ms @60MHz, 涓庣嚎鍦堥鐜囨棤鍏, `Value = 危Xn 鈮 524288` +- **涓闃 IIR** 伪=79/256 + **鏂滅巼闄愬箙 5%** (鎷掔粷 EMI 鐬佸皷宄) +- **绋冲畾鏈**: 涓婄數/澶嶄綅鍚 128 绐 (~1.1s) 灏忕獥鍙(100)蹇熸敹鏁, 缁曡繃婊ゆ尝 +- **鍩虹嚎璺熻釜**: 绐楀彛 500 (~4.4s); **鍐荤粨淇濇姢**: CAPVD 鍋忕 4脳闃堝兼椂鏆傚仠璺熻釜, 鎸佺画 ~8.7s 涓旀尝鍔 卤2% 浠ュ唴鎵嶅己鍒舵帴鍙楁柊鍩虹嚎 (娓╂紓/鎹㈢嚎鍦堣嚜閫傚簲, 鎱㈤熻繘杞︿笉璇惛鏀) +- **杩涘叆鍒ゅ畾**: `CAPVD < Origin - Origin脳sens_in/65536`, 杩炵画 3 娆＄‘璁 +- **绂诲紑鍒ゅ畾**: 骞冲潶鎬т笁鏉′欢 (骞呭煎洖褰 + 涓闃跺钩鍧 + 浜岄樁骞冲潶, 涓撳埄 CN200910309382), 婊炲洖闃堝 sens_out +- **鏈夐檺瀛樺湪瓒呮椂 / 绾垮湀鏂紑閲嶈繛**: 鍏ㄥ浣嶉噸寤哄熀绾, 缁х數鍣ㄤ繚鎸侀噴鏀 +- **瀹夊叏澶嶄綅**: loopSafe_Timeout 闈 0 鏃, 鏈夎溅鎸佺画瓒呮椂鍚庨噸鏂板垵濮嬪寲鍩虹嚎 + +## 宸ョ▼缁撴瀯 + +``` +DLD110SV4/ +鈹溾攢鈹 BLE/ +鈹 鈹溾攢鈹 DLD110S_Peripheral_28K/ # 涓诲簲鐢 (妫娴嬬畻娉 + BLE DBN 鏈嶅姟) +鈹 鈹 鈹溾攢鈹 APP/ +鈹 鈹 鈹 鈹溾攢鈹 peripheral_main.c # 涓绘祦绋 + 妫娴嬬畻娉 + TMR0/TMR1 ISR +鈹 鈹 鈹 鈹溾攢鈹 dbn_ble_srv.c # DBN BLE 鍗忚鏈嶅姟 (灏忕▼搴忓鎺) +鈹 鈹 鈹 鈹溾攢鈹 peripheral.c # BLE 澶栬瑙掕壊绠＄悊 +鈹 鈹 鈹 鈹斺攢鈹 storage.c # 鍙傛暟瀛樺偍 (EEPROM) +鈹 鈹 鈹溾攢鈹 Profile/ # GATT Profile +鈹 鈹 鈹溾攢鈹 Startup / Ld # 鍚姩鏂囦欢 / 閾炬帴鑴氭湰 +鈹 鈹 鈹斺攢鈹 DLD110S_Peripheral_28K.wvproj +鈹 鈹溾攢鈹 592OnlyUpdateApp_IAP_28K/ # OTA IAP 寮曞 +鈹 鈹溾攢鈹 592OnlyUpdateApp_JumpIAP_28K/ # 璺宠浆 IAP +鈹 鈹溾攢鈹 HAL/ # MCU / RTC / KEY / LED +鈹 鈹斺攢鈹 LIB/ # CH59x BLE ROM 搴 +鈹溾攢鈹 SRC/ # StdPeriphDriver / RVMSIS / Startup / Ld +鈹溾攢鈹 docs/ # 纭欢璧勬簮鏂囨。 / CH592 datasheet / devlog +鈹斺攢鈹 tools/ +``` + +## 寮鍙戠幆澧 + +- IDE: MounRiver Studio (wvproj) +- 璋冭瘯涓插彛: UART0 (PA14 TX, 浠 DEBUG 缂栬瘧, 230400bps; 涓 KEY_SA1 澶嶇敤) +- 鐗堟湰: 瑙 `APP/include/cmcng.h` (`FIRMWARE_VER`) + +## 鏂囨。 + +- `docs/DLD110SV4纭欢璧勬簮.md` 鈥 纭欢 IO 璇存槑 +- `docs/devlog.md` 鈥 寮鍙戞棩蹇 +- `docs/CH592DS1.PDF` 鈥 CH592/591 datasheet diff --git a/docs/devlog.md b/docs/devlog.md new file mode 100644 index 0000000..b9861b6 --- /dev/null +++ b/docs/devlog.md @@ -0,0 +1,93 @@ +# DLD110SV4 寮鍙戞棩蹇 + +> MCU: CH591R (RISC-V, BLE) @60MHz | 閫氶亾: 1 璺嚎鍦 | 璋冭瘯鍙: UART0 (DEBUG 缂栬瘧, 230400) + +## 2026-07-18 鈥 妫娴嬬畻娉曠Щ妞 DLD154V4B (V2.0) + +### 鑳屾櫙 + +DLD110SV4 鍘熷浐浠 (V4.x) 浣跨敤鏃т竴浠ｇ畻娉: 涓闃舵护娉 + 浜岄樁鍙樺寲閲 + FltHistoryManager +澶氭椂闂寸獥鍩哄噯 + 浜屾鍒ゆ柇 (FUNCTION_A/B) + 蹇熷彉鍖栨娴嬬瓑澶氬鏈哄埗鍙犲姞, 鍒嗘敮澶嶆潅銆 +鍙傛暟绾犵紶, 鐜板満璋冧紭鍥伴毦銆侱LD154V4B 宸插湪 AT32F421 涓婂畬鎴愮畻娉曠簿绠閲嶆瀯 (瀵归綈 M1H 鍙傝 +瀹炵幇 + M4 浼樺寲), 鏈灏嗗叾鏁翠綋绉绘鍒 CH591R, 鏃х嚎鍦堢畻娉曚唬鐮佸叏閮ㄥ垹闄ゃ + +### 绉绘鍐呭 + +| 妯″潡 | 鏃у疄鐜 | 鏂板疄鐜 (绉绘鍚) | +|------|--------|----------------| +| 娴嬮噺绐 | `LPCNT=(32768/Xn)<<6`, `Value=危Xn>>5` (~65536, 绐楅暱 ~35ms) | `LPCNT=2^19/Xn`, `Value=危Xn` (~524288, 绐楅暱 ~8.74ms) | +| 婊ゆ尝 | IIR 79/256, 鏃犻檺骞 | IIR 79/256 + 鏂滅巼闄愬箙 5% (EMI 灏栧嘲鎷掔粷) | +| 寮鏈烘敹鏁 | INIT_SAMPLE_COUNT=10 绱姞骞冲潎 | 绋冲畾鏈 128 绐, 灏忕獥鍙 100, 缁曡繃 IIR | +| 鍩虹嚎璺熻釜 | 绐楀彛 50, 鏈夎溅鍐荤粨 | 绐楀彛 500, 鍐荤粨淇濇姢 (瓒呮椂 ~8.7s + 绋冲畾鎬 卤2% 鍙屾潯浠) | +| 杩涘叆鍒ゅ畾 | 鍗曟闃堝 (FUNCTION_B 鏃 3 娆) | 杩炵画 3 娆＄‘璁 (ENTRY_CONFIRM) | +| 绂诲紑鍒ゅ畾 | 婊炲洖 + check_flat_leave + 浜屾鍒ゆ柇宓屽 | 骞冲潶鎬т笁鏉′欢 (CN200910309382), 婊炲洖闃堝 sens_out | +| 鏈夐檺瀛樺湪瓒呮椂 | FLAG_HOLD_TIMEOUT 鈫 INIT_VD + 蹇熻ˉ鍋跨獥 | 鍚屾濊矾淇濈暀: 鍏ㄥ浣嶉噸寤哄熀绾 | +| 鏂嚎閲嶈繛 | flt_mgr.flag_loop_reconnect 澶氶樁娈垫仮澶 (淇濈暀鏂紑鍓 Origin 閲嶅垽) | **绠鍖栦负鍏ㄥ浣嶉噸寤哄熀绾** (鏂紑鏃剁户鐢靛櫒宸查噴鏀, 琛屼负瀵归綈 M1H) | +| 瀹夊叏澶嶄綅 | 浠ｇ爜瀛樺湪浣 LC_HOLD 婵娲昏娉ㄩ噴 (瀹為檯涓嶇敓鏁) | **鎭㈠鍚敤**: loopSafe_Timeout 闈 0 鏃惰繘杞﹀惎鍔ㄨ鏃 | +| 鐏垫晱搴﹀垏鎹 | 浠呭紑鏈鸿鍙 (涓诲惊鐜 poll 琚敞閲) | 姣忎釜娴嬮噺绐楄疆璇㈡粦鍔ㄥ紑鍏, 杩愯鏃跺嵆鏃剁敓鏁 | + +### 鍒犻櫎鐨勬棫绠楁硶妯″潡 + +`SecondOrderState` / `get_second_order` / `FltHistoryManager` (flt_mgr) / +`StageRangeConfig` (stage_config) / `update_flt_history` / `check_flt_by_stage` / +`check_flat_leave` / `update_lower_upper` / `reload_lower_upper` / +rapid_change 蹇熷彉鍖栨娴 / relay_on(off)_confirm 澶氶噸纭 / FUNCTION_A/B 鍒嗘敮銆 +`FUNCTION_*` 瀹忎笌 `g_function_mode` 淇濈暀 (BLE 鍗忚 direction_mode 楂 4 浣嶄粛璇诲啓), +浣嗙畻娉曚笉鍐嶆秷璐广 + +### 鍙傛暟閫傞厤 (CH591R @60MHz) + +| 鍙傛暟 | DLD154V4B (@120MHz, 10ms) | DLD110SV4 (@60MHz) | 璇存槑 | +|------|--------------------------|--------------------|------| +| MEASUREMENT_BASE | 131072 (2^17) | 524288 (2^19) | 绐楅暱 2^19/60M 鈮 8.74ms, 鎺ヨ繎 10ms 浠诲姟鑺傛媿 | +| ALFA_CAP1 | 79 (蟿鈮32ms) | 79 (蟿鈮24ms) | | +| WINDOW_ORIGIN | 500 (5s) | 500 (~4.4s) | | +| FREEZE_TIMEOUT | 1000 (10s) | 1000 (~8.7s) | | +| SLOPE_FLAT_THRESH | 100 | 350 | 骞呭煎熀鍑嗘斁澶 4脳銆佺獥鍛ㄦ湡 0.87脳, 鍑缂╂斁 ~3.5脳 | +| MIN_DELTA2 / MIN_DELTA3 | 5 / 2 | 18 / 7 | 鍚屼笂缂╂斁, **寰呯幇鍦洪獙璇** | +| VARIATION_EVENT_TH | 鈥 | 40 | BLE variation 浜嬩欢闃堝, 瀵瑰簲鏃т唬鐮佺殑 5 (骞呭 8脳) | + +### BLE 鎺ュ彛閫傞厤 (dbn_ble_srv.c) + +- `loop_capvd` 涓婃姤: 鏃у间负 `危Xn>>5`, BLE 绔 `<<5` 杩樺師; 鏂板肩洿鎺 `危Xn`, + **鍒犻櫎 `<<5`**銆傞鐜囧叕寮 `freq = g_sys_freq / (loop_capvd / LPCNT) + 360` 缁撴瀯涓嶅彉 + (+360 涓虹‖浠舵牎鍑嗗亸绉, 寰呯幇鍦哄鏍)銆 +- `condition` (绾垮湀鐜璇勪及 0~15): 鏃ф簮 `flt_mgr.cnt_not_idle`; 鏂版簮 `g_env_activity` + (CAPVD 鐩搁偦宸 > ENV_ACT_TH=30 绱姞 / 鍚﹀垯琛板噺, **闃堝煎緟鐜板満楠岃瘉**)銆 +- `variation` = Origin 鈭 CAPVD, 鏈夌鍙, 楗卞拰鍒 卤32767; 骞呭煎熀鍑嗗彉涓 2^19, + **灏忕▼搴忕鑻ユ湁鍥哄畾闃堝奸渶鍚屾鏀惧ぇ 8脳**銆 + +### 椤烘墜淇 + +- `update_moving_average` 鐨 `window` 鍙傛暟 uint8_t 鈫 **uint16_t** (500 浼氳鎴柇涓 244, + DLD154V4B 宸茬煡 pitfall)銆 +- DLD154V4B 鐨 `_stable_cnt` 涓哄嚱鏁板唴 static, 浜屾杩涘叆绋冲畾鏈熸椂涓嶆竻闆 鈫 1 涓牱鏈嵆鍒ょǔ銆 + 绉绘鐗堟敼涓哄叏灞 `loop1_stable_cnt`, 绋冲畾鏈熺粨鏉熶笌 INIT_VD/瀹夊叏澶嶄綅鏃舵竻闆躲 + (**DLD154V4B 渚у悓鏍风殑闅愭偅寰呭洖鏀**) +- TMR0 ISR 澧炲姞 Xn==0 淇濇姢 (闄ら浂) 涓 LPCNT>65535 淇濇姢 (Xn 姣涘埡)銆 +- 杩涘叆鍒ゅ畾鎴愮珛鏃跺钩鍧︽у樊鍒嗗熀鍑 `g_prev_capvd` 浠ュ綋鍓 CAPVD 鎾, 閬垮厤绂诲紑妫娴 + 绗竴甯у嚭鐜拌櫄鍋囧ぇ鏂滅巼銆 + +### 琛屼负鍙樺寲璇存槑 (鐜板満娉ㄦ剰) + +1. **鏂嚎閲嶈繛涓嶅啀灏濊瘯鎭㈠鏂紑鍓嶇殑杞﹁締鐘舵**, 涓寰嬮噸寤哄熀绾 (~1.1s 绋冲畾鏈 + 缁跨伅蹇棯)銆 + 閲嶈繛鏃惰嫢绾垮湀涓婃湁杞, 鍩虹嚎钀藉湪鍗犵敤鍊, 涓嶈緭鍑哄瓨鍦ㄤ俊鍙 鈥 涓庢柇寮鍗抽噴鏀剧殑琛屼负涓鑷淬 +2. 瀹夊叏澶嶄綅 (loopSafe_Timeout) 浠"閰嶇疆瀛樺湪浣嗕笉鐢熸晥"鍙樹负鐪熸鐢熸晥, 鍗囩骇瀛橀噺璁惧鏃 + 娉ㄦ剰璇ュ弬鏁伴厤缃笺 +3. 鐏垫晱搴︽粦鍔ㄥ紑鍏宠繍琛屾椂鍒囨崲鍗虫椂鐢熸晥, 涓嶅啀闇瑕侀噸鏂颁笂鐢点 + +### 寰呯幇鍦洪獙璇 + +- [ ] 鍚勯鐜囨。涓 LPCNT / 瀹炴祴绐楅暱 / CAPVD 骞呭 (绀烘尝鍣 + DEBUG 涓插彛) +- [ ] 骞冲潶鎬х寮闃堝 SLOPE_FLAT_THRESH=350, MIN_DELTA2=18, MIN_DELTA3=7 +- [ ] ENV_ACT_TH=30 (condition 涓婃姤鐨勭伒鏁忕▼搴) +- [ ] BLE 棰戠巼璇绘暟 +360 鍋忕Щ鏄惁浠嶅噯纭 (娴嬮噺鏂规宸插彉) +- [ ] 灏忕▼搴 variation 鏄剧ず骞呭 (鍩哄噯鏀惧ぇ 8脳) + +--- + +## 淇璁板綍 + +| 鐗堟湰 | 鏃堕棿 | 璇存槑 | +|------|------|------| +| V1.0 | 2026-07-18 | 寤轰粨; 绠楁硶绉绘 DLD154V4B V2.0, 鍒犻櫎鏃х嚎鍦堢畻娉; FIRMWARE_VER 4.00鈫4.20 |