diff --git a/utilities/at32f421_freertos_demo/inc/TaskLoop.h b/utilities/at32f421_freertos_demo/inc/TaskLoop.h index a807e6d..ea9fcc8 100644 --- a/utilities/at32f421_freertos_demo/inc/TaskLoop.h +++ b/utilities/at32f421_freertos_demo/inc/TaskLoop.h @@ -31,6 +31,12 @@ *===========================================================================*/ #define ALFA_CAP1 79 // IIR 指数平滑 α = 79/256 ≈ 0.31 +/*=========================================================================== + * 离开检测模式 + * 1 = 平坦性三条件判定 (CN200910309382), 0 = 简单 cnt_release 防抖 + *===========================================================================*/ +#define USE_FLATNESS_EXIT 1 + /*=========================================================================== * 频率测量参数 * MEASUREMENT_BASE: 自适应测量窗口目标值 (≈131072) @@ -85,6 +91,7 @@ extern uint8_t g_loop_stable; // 线圈数值已稳定 (0=稳定中, /* 离开防抖计数器(连续 CAPVD 恢复到阈值以上才释放) */ extern uint8_t loop1_cnt_release; // 离开防抖计数 +#if USE_FLATNESS_EXIT extern uint8_t g_exit_state; // 离开检测: 0=追踪斜率, 1=等待平坦 extern uint16_t g_max_slope; // 第一上升坡面最大 |f'| extern uint16_t g_max_slope_rate; // 第一上升坡面最大 |f''| @@ -94,6 +101,7 @@ 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; // 平坦条件满足连续计数 +#endif /*=========================================================================== * 全局状态变量 — 计数器 diff --git a/utilities/at32f421_freertos_demo/src/TaskLoop.c b/utilities/at32f421_freertos_demo/src/TaskLoop.c index 89f70bc..ced6631 100644 --- a/utilities/at32f421_freertos_demo/src/TaskLoop.c +++ b/utilities/at32f421_freertos_demo/src/TaskLoop.c @@ -52,6 +52,7 @@ uint8_t loop1_FLAG_PLUSE; uint8_t loop1_SensLevel; uint8_t loop1_cnt_release; uint8_t g_loop_stable; +#if USE_FLATNESS_EXIT uint8_t g_exit_state; uint16_t g_max_slope; uint16_t g_max_slope_rate; @@ -61,6 +62,7 @@ int32_t g_prev_capvd; int32_t g_prev_first_deriv; uint8_t g_slope_flat_cnt; uint8_t g_flat_ok_cnt; +#endif /*=========================================================================== * 全局状态变量 — 计数器 @@ -332,6 +334,7 @@ void INIT_VD(void) loop1_FLAG_PLUSE = 0; loop1_cnt_release = 0; g_loop_stable = 0; +#if USE_FLATNESS_EXIT g_exit_state = 0; g_max_slope = 0; g_max_slope_rate = 0; @@ -341,6 +344,7 @@ void INIT_VD(void) g_prev_first_deriv = 0; g_slope_flat_cnt = 0; g_flat_ok_cnt = 0; +#endif loop1_LC_HOLD = 0; loop1_LC_Reset = 0; @@ -552,9 +556,11 @@ void TMR15_GLOBAL_IRQHandler(void) loop1_INI_LOOP = 1; loop1_LOOP_OK0 = 0; g_loop_stable = 0; +#if USE_FLATNESS_EXIT g_exit_state = 0; g_max_slope = 0; g_max_slope_rate = 0; +#endif LC_Hold_CNT = 0; loop1_ORG_CNT = 0; loop1_ORG_SUM = 0; @@ -675,6 +681,7 @@ void vd1_task(void) loop1_ORG_SUM = 0; /* 重置平坦性状态(专利 CN200910309382) */ +#if USE_FLATNESS_EXIT g_exit_state = 0; // 开始追踪第一上升坡面斜率 g_max_slope = 0; g_max_slope_rate = 0; @@ -682,61 +689,47 @@ void vd1_task(void) g_delta3 = 0; g_slope_flat_cnt = 0; g_flat_ok_cnt = 0; +#endif } } else { +#if USE_FLATNESS_EXIT /*================================================================ * 有车状态 — 平坦性判定离开(专利 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 // 平坦连续确认次数 +#define K1 8 +#define K2 8 +#define SLOPE_FLAT_THRESH 100 +#define MIN_DELTA2 5 +#define MIN_DELTA3 2 +#define FLAT_CONFIRM_CNT 3 - int32_t first_deriv; - int32_t abs_fd, abs_sd; - int32_t second_deriv; + 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) { - /*--- 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; - - /* 检测斜率是否已趋平坦 */ 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 { - /*--- Phase 2: 平坦性三条件判定 ---*/ int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin; - int32_t cond1 = (dev >= 0) ? dev : -dev; // |f - f_b| - + int32_t cond1 = (dev >= 0) ? dev : -dev; 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++; @@ -744,12 +737,10 @@ void vd1_task(void) 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; @@ -760,10 +751,32 @@ void vd1_task(void) g_flat_ok_cnt = 0; } } - - /* 保存历史用于下一帧差分 */ g_prev_capvd = loop1_CAPVD; g_prev_first_deriv = first_deriv; +#else + /*================================================================ + * 有车状态 — 简单 cnt_release 防抖离开 + *================================================================*/ + loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; + + 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); + 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; + loop1_cnt_release = 0; + } + } else { + if (loop1_cnt_release > 0) loop1_cnt_release = 0; + } +#endif } }