From aff6aa9a2b2ef6b3cf00e9f769011c7247d5961e Mon Sep 17 00:00:00 2001 From: wangfq Date: Tue, 23 Jun 2026 16:17:19 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E7=A7=BB=E6=A4=8D=20CN200910309382=20?= =?UTF-8?q?=E5=B9=B3=E5=9D=A6=E6=80=A7=E7=A6=BB=E5=BC=80=E5=88=A4=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 进入: 保持单一阈值法 f-f_b > Δ1 - Phase 1 (g_exit_state=0): 追踪第一上升坡面 max |f'|, |f''| - Phase 2 (g_exit_state=1): 三条件平坦性 |f-f_b| < Δ1 (SensTable_1) && |f'| < Δ2 && |f''| < Δ3 - Δ2 = max_slope / 8, Δ3 = max_slope_rate / 8 (动态阈值) - 连续3次平坦确认后释放, 防大车多峰误触发 - 安全复位时重置平坦性状态 --- .../at32f421_freertos_demo/inc/TaskLoop.h | 9 ++ .../at32f421_freertos_demo/src/TaskLoop.c | 122 ++++++++++++++---- 2 files changed, 109 insertions(+), 22 deletions(-) diff --git a/utilities/at32f421_freertos_demo/inc/TaskLoop.h b/utilities/at32f421_freertos_demo/inc/TaskLoop.h index 90aaa36..a807e6d 100644 --- a/utilities/at32f421_freertos_demo/inc/TaskLoop.h +++ b/utilities/at32f421_freertos_demo/inc/TaskLoop.h @@ -85,6 +85,15 @@ extern uint8_t g_loop_stable; // 线圈数值已稳定 (0=稳定中, /* 离开防抖计数器（连续 CAPVD 恢复到阈值以上才释放） */ extern uint8_t loop1_cnt_release; // 离开防抖计数 +extern uint8_t g_exit_state; // 离开检测: 0=追踪斜率, 1=等待平坦 +extern uint16_t g_max_slope; // 第一上升坡面最大 |f'| +extern uint16_t g_max_slope_rate; // 第一上升坡面最大 |f''| +extern uint16_t g_delta2; // Δ2: 一阶平坦阈值 +extern uint16_t g_delta3; // Δ3: 二阶平坦阈值 +extern int32_t g_prev_capvd; // 上一帧 CAPVD (差分用) +extern int32_t g_prev_first_deriv; // 上一帧一阶导数 +extern uint8_t g_slope_flat_cnt; // 斜率趋零连续计数 +extern uint8_t g_flat_ok_cnt; // 平坦条件满足连续计数 /*=========================================================================== * 全局状态变量 — 计数器 diff --git a/utilities/at32f421_freertos_demo/src/TaskLoop.c b/utilities/at32f421_freertos_demo/src/TaskLoop.c index e022ccc..89f70bc 100644 --- a/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -52,6 +52,15 @@ uint8_t loop1_FLAG_PLUSE; uint8_t loop1_SensLevel; uint8_t loop1_cnt_release; uint8_t g_loop_stable; +uint8_t g_exit_state; +uint16_t g_max_slope; +uint16_t g_max_slope_rate; +uint16_t g_delta2; +uint16_t g_delta3; +int32_t g_prev_capvd; +int32_t g_prev_first_deriv; +uint8_t g_slope_flat_cnt; +uint8_t g_flat_ok_cnt; /*=========================================================================== * 全局状态变量 — 计数器 @@ -323,6 +332,15 @@ void INIT_VD(void) loop1_FLAG_PLUSE = 0; loop1_cnt_release = 0; g_loop_stable = 0; + 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; loop1_LC_HOLD = 0; loop1_LC_Reset = 0; @@ -534,6 +552,9 @@ void TMR15_GLOBAL_IRQHandler(void) loop1_INI_LOOP = 1; loop1_LOOP_OK0 = 0; g_loop_stable = 0; + g_exit_state = 0; + g_max_slope = 0; + g_max_slope_rate = 0; LC_Hold_CNT = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; @@ -652,40 +673,97 @@ void vd1_task(void) loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; - loop1_cnt_release = 0; // 重置离开防抖 + + /* 重置平坦性状态（专利 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; } } else { /*================================================================ - * 有车状态 + * 有车状态 — 平坦性判定离开（专利 CN200910309382） + * + * Phase 1 (g_exit_state=0): 追踪第一上升坡面最大 |f'| 和 |f''| + * Phase 2 (g_exit_state=1): 三条件平坦性判定 + * |f-f_b| < Δ1 && |f'| < Δ2 && |f''| < Δ3 *================================================================*/ +#define K1 8 // 斜率除数 +#define K2 8 // 斜率变化率除数 +#define SLOPE_FLAT_THRESH 100 // 斜率趋零阈值 +#define MIN_DELTA2 5 // Δ2 最小值 +#define MIN_DELTA3 2 // Δ3 最小值 +#define FLAT_CONFIRM_CNT 3 // 平坦连续确认次数 - /*--- 离开检测（滞回阈值 SensTable_1 ≈ SensTable 的 50%） - * 增加 cnt_release >= 3 防抖：连续 3 次恢复到阈值以上才释放 - * 避免因瞬间噪声导致误落杆 ---*/ - loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; + int32_t first_deriv; + int32_t abs_fd, abs_sd; + int32_t second_deriv; - if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD) { - loop1_cnt_release++; - if (loop1_cnt_release >= 3) { - PRINT("Car_OFF, Value:%d, CAPVD:%d, Origin:%d, dlt:%d\n", - loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG); + /* 计算一阶/二阶导数 */ + 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; - loop1_VD_FLAG = 0; - loop1_FLAG_OUT = 1; - loop1_VD_HOLD = 0; - loop1_LC_HOLD = 0; + if (g_exit_state == 0) { + /*--- Phase 1: 追踪第一上升坡面最大斜率 ---*/ + if (abs_fd > g_max_slope) g_max_slope = abs_fd; + if (abs_sd > g_max_slope_rate) g_max_slope_rate = abs_sd; - loop1_ORG_CNT = 0; - loop1_ORG_SUM = 0; - Hold_CNT = 0; - loop1_cnt_release = 0; + /* 检测斜率是否已趋平坦 */ + if (abs_fd < SLOPE_FLAT_THRESH) { + g_slope_flat_cnt++; + if (g_slope_flat_cnt >= 3) { + /* 计算动态阈值 Δ2, Δ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; + PRINT("FlatThresh: d2=%d d3=%d maxF'=%d maxF''=%d\n", + g_delta2, g_delta3, g_max_slope, g_max_slope_rate); + } + } else { + g_slope_flat_cnt = 0; } } else { - /* CAPVD 又掉回阈值以下 → 重置防抖，车还在 */ - if (loop1_cnt_release > 0) { - loop1_cnt_release = 0; + /*--- Phase 2: 平坦性三条件判定 ---*/ + int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin; + int32_t cond1 = (dev >= 0) ? dev : -dev; // |f - f_b| + + loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; + + if (cond1 < (int32_t)loop1_dlt_ORG && abs_fd < (int32_t)g_delta2 + && abs_sd < (int32_t)g_delta3) { + g_flat_ok_cnt++; + if (g_flat_ok_cnt >= FLAT_CONFIRM_CNT) { + PRINT("Car_OFF_FLAT, CAPVD:%d Origin:%d d1:%d d2:%d d3:%d f':%d f'':%d\n", + loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, + g_delta2, g_delta3, first_deriv, second_deriv); + + loop1_VD_FLAG = 0; + loop1_FLAG_OUT = 1; + loop1_VD_HOLD = 0; + loop1_LC_HOLD = 0; + + loop1_ORG_CNT = 0; + loop1_ORG_SUM = 0; + Hold_CNT = 0; + g_flat_ok_cnt = 0; + g_exit_state = 0; + } + } else { + g_flat_ok_cnt = 0; } } + + /* 保存历史用于下一帧差分 */ + g_prev_capvd = loop1_CAPVD; + g_prev_first_deriv = first_deriv; } }