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

feat: 移植 CN200910309382 平坦性离开判定

Browse Source 
- 进入: 保持单一阈值法 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次平坦确认后释放, 防大车多峰误触发
- 安全复位时重置平坦性状态
This commit is contained in:
wangfq
2026-06-23 16:17:19 +08:00
parent afd901842b
commit aff6aa9a2b
2 changed files with 109 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
utilities/at32f421_freertos_demo/inc/TaskLoop.h
View File

@@ -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; // 平坦条件满足连续计数
/*===========================================================================
* 全局状态变量 — 计数器

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

@@ -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,23 +673,77 @@ 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 // 平坦连续确认次数
int32_t first_deriv;
int32_t abs_fd, abs_sd;
int32_t 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|
/*--- 离开检测（滞回阈值 SensTable_1 ≈ SensTable 的 50%）
* 增加 cnt_release >= 3 防抖：连续 3 次恢复到阈值以上才释放
* 避免因瞬间噪声导致误落杆 ---*/
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);
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;
@@ -678,14 +753,17 @@ void vd1_task(void)
loop1_ORG_CNT = 0;
loop1_ORG_SUM = 0;
Hold_CNT = 0;
loop1_cnt_release = 0;
g_flat_ok_cnt = 0;
g_exit_state = 0;
}
} else {
/* CAPVD 又掉回阈值以下 → 重置防抖，车还在 */
if (loop1_cnt_release > 0) {
loop1_cnt_release = 0;
g_flat_ok_cnt = 0;
}
}
/* 保存历史用于下一帧差分 */
g_prev_capvd = loop1_CAPVD;
g_prev_first_deriv = first_deriv;
}
}