wangfq/DLD154V4B
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: USE_FLATNESS_EXIT 宏开关 — 平坦性/简单防抖可切换

Browse Source 
- TaskLoop.h 新增 #define USE_FLATNESS_EXIT 1
- #if USE_FLATNESS_EXIT 包裹所有平坦性状态变量和逻辑
- #else 回退到 cnt_release>=3 简单防抖
- 置 0 即可切回传统离开判定
This commit is contained in:
wangfq
2026-06-23 17:24:46 +08:00
parent b6ef3c4c4f
commit 86d09eb953
2 changed files with 49 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
utilities/at32f421_freertos_demo/src/TaskLoop.c
View File

@@ -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
}
}