From e3a27af24fa654a7fa27ba8e20933ca3dbd72c26 Mon Sep 17 00:00:00 2001 From: wangfq Date: Sat, 18 Jul 2026 23:30:12 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=A3=80=E6=B5=8B=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E7=A7=BB=E6=A4=8D=20DLD154V4B=20V2.0,=20=E5=88=A0=E9=99=A4?= =?UTF-8?q?=E6=97=A7=E7=BA=BF=E5=9C=88=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 自适应测量窗 2^19 (~8.74ms @60MHz), IIR 79/256 + 斜率限幅 5% - 稳定期 128 窗快速收敛; 基线跟踪窗 500 + 冻结保护(超时+稳定性双条件) - 进入确认 3 次; 平坦性三条件离开 (CN200910309382), 阈值按新幅值基准缩放 - 删除 FltHistoryManager/SecondOrderState/StageRangeConfig/二次判断/FUNCTION_A/B - 断线重连/有限存在超时: 全复位重建基线; 安全复位恢复启用; 灵敏度开关运行时生效 - BLE: loop_capvd 去掉 <<5, condition 改用 g_env_activity, variation 幅值基准 8x - 修复 update_moving_average window uint8 截断; 稳定期计数器改全局可复位 - FIRMWARE_VER 4.00 -> 4.20; 新增 README.md 与 docs/devlog.md --- .gitignore | 3 + BLE/DLD110S_Peripheral_28K/APP/dbn_ble_srv.c | 4 +- .../APP/include/cmcng.h | 120 +- .../APP/peripheral_main.c | 1693 +++++------------ README.md | 89 + docs/devlog.md | 93 + 6 files changed, 698 insertions(+), 1304 deletions(-) create mode 100644 README.md create mode 100644 docs/devlog.md 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; // г޳ʱһױ仯 һױ仯²ֵһβ׽ֵIJ - uint16_t cnt_release_Down; // г޳ʱһױ仯 ʱܵ߳ĹӰIJǰֺͺֲ - 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 ˲IJֵ +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; // 00.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) { /* ô0IOģʽô */ -// 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) { - // 㵱ǰױ仯һױ仯IJ֣ - 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++) { //offset1ʼʾȶοֵͷж - 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; //¼ г޳仯IJ - } - - // 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) + 快速变化检测等多套机制叠加, 分支复杂、 +参数纠缠, 现场调优困难。DLD154V4B 已在 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 |