wangfq/DLD154V4B
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
86d09eb953e0aac1b2d7cac7897f890a0fd8e5db
DLD154V4B/utilities/at32f421_freertos_demo/src/TaskLoop.c
wangfq 86d09eb953 feat: USE_FLATNESS_EXIT 宏开关 — 平坦性/简单防抖可切换 
- TaskLoop.h 新增 #define USE_FLATNESS_EXIT 1
- #if USE_FLATNESS_EXIT 包裹所有平坦性状态变量和逻辑
- #else 回退到 cnt_release>=3 简单防抖
- 置 0 即可切回传统离开判定
2026-06-23 17:24:46 +08:00

857 lines
30 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#include "TaskLoop.h"
#include "at32f421_board.h"
#include "cmcng.h"
#include "FreeRTOS.h"
#include "task.h"
#include <stdlib.h>
/*===========================================================================
* 灵敏度表 — 对齐 M1H 参考实现
* 进入阈值 = Origin × SensTable[SENS] / 65536
* 离开阈值（滞回）= Origin × SensTable_1[SENS] / 65536
* SENS: 0=低灵敏, 3=高灵敏
*===========================================================================*/
const uint16_t SensTable[4] = {216, 108, 36, 10};
const uint16_t SensTable_1[4] = {108, 72, 18, 9};
/*===========================================================================
* 全局状态变量 — 捕获 & 测量
*===========================================================================*/
uint32_t g_sys_freq = 0;
uint8_t g_input_div = 1;
uint16_t loop1_Xn;
uint16_t loop1_CapThis;
uint16_t loop1_CapLast;
uint16_t loop1_LPCNT;
uint16_t loop1_CapCnt;
uint32_t loop1_CapSum;
uint32_t loop1_Value;
uint32_t loop1_CAPVD;
uint32_t loop1_Origin;
uint32_t loop1_ORG_SUM;
uint16_t loop1_ORG_CNT;
uint16_t loop1_dlt_ORG;
uint8_t Flt_Reg;
/*===========================================================================
* 全局状态变量 — 标志位
*===========================================================================*/
uint8_t loop1_INI_LOOP;
uint8_t loop1_CAP_OK;
uint8_t loop1_VD_FLAG;
uint8_t loop1_VD_HOLD;
uint8_t loop1_RF_FLAG;
uint8_t loop1_LOOP_OK;
uint8_t loop1_LOOP_OK0;
uint8_t loop1_FLAG_IN;
uint8_t loop1_FLAG_OUT;
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;
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;
#endif
/*===========================================================================
* 全局状态变量 — 计数器
*===========================================================================*/
uint16_t Hold_CNT = 0;
uint8_t loop1_INCNT = 0;
uint8_t loop1_OUTCNT = 0;
uint8_t TM1cnt = 0;
/*===========================================================================
* 拨码开关状态
*===========================================================================*/
uint8_t SET_PLUS = 0;
uint8_t SET_DLY = 0;
uint8_t SET_SAFE = 0;
/*===========================================================================
* 拨码开关去抖变量
*===========================================================================*/
uint8_t sw0, swok, sw1, swcnt, SENS = 0, SENS_LAST = 0;
/*===========================================================================
* 安全复位相关
*===========================================================================*/
uint8_t loop1_LC_HOLD = 0;
uint8_t loop1_LC_Reset = 0;
uint32_t LC_Hold_CNT = 0;
uint16_t g_safe_max_cnt = LC_HOLD_TIME;
/*===========================================================================
* 故障指示相关 — 黄灯
*===========================================================================*/
uint8_t g_loop_power_up_state = 0;
uint8_t g_disconnect_count = 0;
uint8_t g_disconnect_active = 0;
uint8_t g_fault_phase = 0;
uint16_t g_fault_tick = 0;
/*===========================================================================
* 调试计数器
*===========================================================================*/
uint32_t g_xn_counter = 0;
/*===========================================================================
* 拨码开关轮询（5 次去抖，对齐 M1H）
*===========================================================================*/
void poll_sw_state(void)
{
sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN) << 4;
sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN) << 3;
sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN) << 2;
sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN) << 1;
sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN) << 0;
if (sw0 == sw1) {
if (swok != sw0) {
swcnt++;
if (swcnt > 5) { // 对齐 M1H: 5 次确认
swok = sw0;
SET_PLUS = swok & 0x04;
SET_DLY = (swok & 0x08) >> 3;
SET_SAFE = (swok & 0x10) >> 4;
}
}
} else {
sw1 = sw0;
swcnt = 0;
}
SENS = swok & 0x03;
if (SENS != SENS_LAST) {
PRINT("SENS changed: %02X -> %02X, resetting...\n", SENS_LAST, SENS);
nvic_system_reset();
}
}
/*===========================================================================
* 一阶 IIR 低通滤波器（对齐 M1H 公式）
* CAPVD_new = α × new_value + (1-α) × CAPVD_old
* α = Flt_Reg / 256 = 79/256 ≈ 0.3086
*===========================================================================*/
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;
if (new_value > last_Value) {
value_Flt = last_Value + scaled_delta;
} else {
value_Flt = last_Value - scaled_delta;
}
return value_Flt;
}
/*===========================================================================
* 滑动平均基线更新
* 累加 new_value，当计数达到 window 时计算平均值并重置
* 返回 1 表示本轮更新了平均值
*===========================================================================*/
uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt,
uint32_t* p_origin, uint32_t new_value,
uint8_t window)
{
if (!p_sum || !p_cnt || !p_origin || window == 0) {
return 0;
}
*p_sum += new_value;
(*p_cnt)++;
if (*p_cnt >= window) {
*p_origin = *p_sum / window;
*p_cnt = 0;
*p_sum = 0;
return 1;
}
return 0;
}
/*===========================================================================
* 红灯呼吸 (LEDA, PB1) — 每 5ms 由 TMR15 ISR 调用
*===========================================================================*/
void LEDA_ON_OFF(void)
{
LED_RED_GPIO->odt ^= LED_RED_PIN;
}
/*===========================================================================
* 绿灯指示 — LEDA (PA9)
*
* 由 TMR15 ISR 每 5ms 驱动一次，是绿灯的唯一控制入口。
*
* 模式：
* 自检/稳定中 (loop1_INI_LOOP || 线圈未连 || !g_loop_stable):
* → 慢闪 (200ms 亮 / 200ms 灭)
* 正常工作:
* → 有车亮 (VD_FLAG==1), 无车灭
*===========================================================================*/
void poll_green_led(void)
{
#define GREEN_SLOW_HALF 40 // 200ms (40 × 5ms)
static uint16_t _slow_tick = 0;
/*--- 线圈断开中 → 灭（黄灯快闪负责故障指示） ---*/
if (g_disconnect_active) {
LEDA_OFF;
_slow_tick = 0;
return;
}
/*--- 自检阶段：慢闪 ---*/
if (loop1_INI_LOOP || !g_loop_power_up_state || !g_loop_stable) {
_slow_tick++;
if (_slow_tick >= GREEN_SLOW_HALF) {
_slow_tick = 0;
LED_GREEN_GPIO->odt ^= LED_GREEN_PIN; // toggle
}
return;
}
/*--- 正常工作：车辆存在指示 ---*/
_slow_tick = 0;
if (loop1_VD_FLAG)
LEDA_ON;
else
LEDA_OFF;
}
/*===========================================================================
* 黄灯故障指示 — LEDC (PA10)
*
* 由 TMR15 ISR 每 5ms 驱动一次。
*
* 模式判定：
* 当前线圈断开 或 上电后从未接线圈:
* → 快闪 (200ms 亮 / 200ms 灭)
* 线圈已恢复连接, 且上电后有断开记录 (g_disconnect_count >= 1):
* → N 短闪 (80ms亮/200ms灭 × N, 1.2s间隔, N=断开次数≤3)
* 线圈正常连接, 无断开记录:
* → 灭
*===========================================================================*/
void poll_yellow_led(void)
{
#define FLT_ON_SHORT 16 // 80ms (16 × 5ms)
#define FLT_GAP_SHORT 40 // 200ms
#define FLT_GAP_LONG 240 // 1.2s (240 × 5ms)
#define FLT_FAST_HALF 40 // 200ms 快闪半周期
g_fault_tick++;
/*--- 模式1: 快闪 — 当前断开中 或 从无线圈上电 ---*/
if (g_disconnect_active || !g_loop_power_up_state) {
if (g_fault_tick >= FLT_FAST_HALF) {
g_fault_tick = 0;
LED_YELLOW_GPIO->odt ^= LED_YELLOW_PIN; // toggle
}
return;
}
uint8_t N = g_disconnect_count;
if (N == 0) {
/*--- 模式2: 无断开记录 → 灭 ---*/
LED_YELLOW_OFF;
g_fault_tick = 0;
g_fault_phase = 0;
return;
}
/*--- 模式3: N 短闪 (连接中, 但有断开历史) ---*/
if (N > 3) N = 3;
uint8_t phases_total = N * 2; // N 个 ON_short + N 个 GAP_short
uint16_t duration;
if (g_fault_phase < phases_total) {
duration = (g_fault_phase & 1) ? FLT_GAP_SHORT : FLT_ON_SHORT;
if (g_fault_phase & 1)
LED_YELLOW_OFF;
else
LED_YELLOW_ON;
} else {
/* LONG 间隔 */
duration = FLT_GAP_LONG;
LED_YELLOW_OFF;
}
if (g_fault_tick >= duration) {
g_fault_tick = 0;
g_fault_phase++;
if (g_fault_phase > phases_total)
g_fault_phase = 0;
}
}
/*===========================================================================
* 初始化所有状态变量（对齐 M1H INIT_VD）
*===========================================================================*/
void INIT_VD(void)
{
loop1_INCNT = 0;
loop1_OUTCNT = 0;
loop1_CapCnt = 0;
loop1_CapLast = 0;
loop1_LPCNT = 0;
loop1_INI_LOOP = 1;
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_FLAG_IN = 0;
loop1_FLAG_OUT = 0;
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;
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
loop1_LC_HOLD = 0;
loop1_LC_Reset = 0;
LC_Hold_CNT = 0;
Hold_CNT = 0;
g_loop_power_up_state = 0;
g_disconnect_count = 0;
g_disconnect_active = 0;
g_fault_phase = 0;
g_fault_tick = 0;
/* 读取拨码初始状态 */
sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN) << 4;
sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN) << 3;
sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN) << 2;
sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN) << 1;
sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN) << 0;
swok = sw0;
sw1 = sw0;
SENS = swok & 0x03;
SENS_LAST = SENS;
swcnt = 0;
switch (SENS) {
case 0x00: loop1_SensLevel = 0; break;
case 0x02: loop1_SensLevel = 1; break;
case 0x01: loop1_SensLevel = 2; break;
case 0x03: loop1_SensLevel = 3; break;
default: loop1_SensLevel = 0; break;
}
SET_PLUS = swok & 0x04;
SET_DLY = (swok & 0x08) >> 3;
SET_SAFE = (swok & 0x10) >> 4;
}
/*===========================================================================
* TMR3 输入捕获中断 — 线圈频率测量（对齐 M1H CAP0 ISR）
*
* PA7 → TIM3_CH2, 上升沿触发。
* 每次捕获中断读取 CCR2，计算相邻边沿差 Xn。
* 累加 LPCNT 次后产生一个测量窗口的 Value。
*===========================================================================*/
void TMR3_GLOBAL_IRQHandler(void)
{
if (tmr_interrupt_flag_get(TMR3, TMR_C2_FLAG) != RESET) {
tmr_flag_clear(TMR3, TMR_C2_FLAG);
loop1_CapThis = tmr_channel_value_get(TMR3, TMR_SELECT_CHANNEL_2);
if (!loop1_RF_FLAG) {
/* 首次捕获，记录基准 */
loop1_CapLast = loop1_CapThis;
loop1_RF_FLAG = 1;
} else {
/* 计算相邻边沿周期差（16bit 溢出回绕处理） */
if (loop1_CapThis > loop1_CapLast) {
loop1_Xn = loop1_CapThis - loop1_CapLast;
} else {
loop1_Xn = (0x10000 - loop1_CapLast) + loop1_CapThis;
}
g_xn_counter++;
loop1_CapLast = loop1_CapThis;
loop1_CapCnt++;
if (loop1_INI_LOOP) {
/*--- 初始化阶段：计算自适应测量窗口大小 ---*/
if (loop1_LPCNT == 0) {
if (loop1_CapCnt > 10) {
/*
* 自适应测量窗口: MEASUREMENT_BASE / Xn
* 使归一化 Value ≈ 131072，无需后续 >>6
*/
loop1_LPCNT = MEASUREMENT_BASE / loop1_Xn;
PRINT("First_calc_loop1_LPCNT:%d, loop1_Xn:%d\n",
loop1_LPCNT, loop1_Xn);
if (loop1_LPCNT == 0) {
loop1_LPCNT = 100;
}
loop1_CAPVD = 0;
loop1_CapSum = 0;
loop1_CapCnt = 0;
}
} else {
/* 第一个窗口：直接作为基线 Origin */
loop1_CapSum += loop1_Xn;
if (loop1_CapCnt >= loop1_LPCNT) {
loop1_CAPVD = loop1_CapSum;
loop1_Origin = loop1_CAPVD; // 不再 >>6
PRINT("First_capSum:%d, Origin:%d\n",
loop1_CapSum, loop1_Origin);
loop1_INI_LOOP = 0;
loop1_CapSum = 0;
loop1_Value = 0;
loop1_CapCnt = 0;
}
}
} else {
/*--- 正常运行：累加到测量窗口 ---*/
loop1_CapSum += loop1_Xn;
if (loop1_CapCnt >= loop1_LPCNT) {
loop1_Value = loop1_CapSum;
loop1_CAP_OK = 1;
loop1_CapSum = 0;
loop1_CapCnt = 0;
loop1_INI_LOOP = 0;
}
}
}
}
}
/*===========================================================================
* TMR15 定时器中断 — 5ms tick，主状态机（对齐 M1H Timer1 ISR）
*
* 职责：
* 1. 50ms tick 计数器（TM1cnt ÷ 10）
* 2. 进入/离开/脉冲时序状态机
* 3. 有限存在超时 / 安全复位超时
* 4. 线圈载波检测 (RF_FLAG) 及继电器输出刷新
* 5. 呼吸灯、故障灯驱动
*===========================================================================*/
void TMR15_GLOBAL_IRQHandler(void)
{
static uint16_t _counter1_init = 0;
if (tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) {
/*--- 初始化阶段红灯快闪 (200ms 周期) ---*/
if (!g_loop_power_up_state) {
_counter1_init++;
if (_counter1_init >= 40) { // 40 × 5ms = 200ms
_counter1_init = 0;
LEDA_ON_OFF();
}
}
/*--- 50ms tick 分频 ---*/
TM1cnt++;
if (TM1cnt >= 10) { // 10 × 5ms = 50ms
TM1cnt = 0;
/*================================================================
* 时序状态机（对齐 M1H Timer1 ISR）
*
* IN_DELAY OUT_DELAY
* [空闲] ──有车──→ [进入延时] ──→ [有车确认] ──离开──→
* ↑ │
* └──脉冲结束──── [脉冲输出] ←──── [离开延时] ←──────┘
* PULSE_DELAY
*================================================================*/
/* FLAG_IN: 进入延时 500ms */
if (loop1_FLAG_IN) {
loop1_INCNT++;
if (loop1_INCNT > IN_DELAY) {
loop1_FLAG_IN = 0;
loop1_INCNT = 0;
}
}
/* FLAG_OUT: 离开延时 1.9s（或 SET_DLY 时立即脉冲） */
if (loop1_FLAG_OUT) {
if (SET_DLY) {
/* 延时模式：立即触发脉冲 */
loop1_FLAG_OUT = 0;
loop1_FLAG_PLUSE = 1;
loop1_OUTCNT = 0;
} else {
loop1_OUTCNT++;
if (loop1_OUTCNT > OUT_DELAY) {
loop1_FLAG_OUT = 0;
loop1_FLAG_PLUSE = 1;
loop1_OUTCNT = 0;
}
}
}
/* FLAG_PLUSE: 脉冲宽度 950ms */
if (loop1_FLAG_PLUSE) {
loop1_OUTCNT++;
if (loop1_OUTCNT > PULSE_DELAY) {
loop1_FLAG_PLUSE = 0;
loop1_OUTCNT = 0;
}
}
/* 有限存在超时 */
if (loop1_VD_HOLD) {
Hold_CNT++;
if (Hold_CNT > HOLD_TIME) {
loop1_VD_HOLD = 0;
Hold_CNT = 0;
if (loop1_VD_FLAG) {
/* 超时：强制释放 */
RLY1_OFF;
RLY2_OFF;
}
}
}
/* 安全复位超时 */
if (loop1_LC_HOLD) {
LC_Hold_CNT++;
if (LC_Hold_CNT > g_safe_max_cnt) {
loop1_LC_Reset = 1;
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;
}
}
}
/*================================================================
* 线圈载波检测 & 继电器输出刷新（每 5ms）
* RF_FLAG 由 TMR3 ISR 置位，此处消费后清除。
*================================================================*/
if (loop1_RF_FLAG) {
loop1_LOOP_OK = 1;
loop1_RF_FLAG = 0;
if (loop1_LC_Reset == 0) {
/* 继电器 2（辅助/方向） */
if (loop1_VD_FLAG)
RLY2_ON;
else
RLY2_OFF;
/* 继电器 1（主输出） */
if (SET_PLUS) {
/* 存在输出模式：有车或离开延时中 */
if (loop1_VD_FLAG || loop1_FLAG_OUT)
RLY1_ON;
else
RLY1_OFF;
} else {
/* 脉冲输出模式：脉冲期间吸合 */
if (loop1_FLAG_PLUSE)
RLY1_ON;
else
RLY1_OFF;
}
}
} else {
loop1_LOOP_OK0 = loop1_LOOP_OK;
loop1_LOOP_OK = 0;
}
/*--- 硬件驱动：呼吸灯 + 指示灯 ---*/
poll_red_pwm();
poll_green_led();
poll_yellow_led();
tmr_flag_clear(TMR15, TMR_OVF_FLAG);
}
}
/*===========================================================================
* vd1_task — 核心检测算法（对齐 M1H VD1_TASK）
*
* 每次 loop1_CAP_OK 时调用一次。
* 流程：
* 1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD
* 2. 无车时：基线跟踪 + 进入检测
* 3. 有车时：离开检测（带滞回）
*===========================================================================*/
void vd1_task(void)
{
#define STABLE_SAMPLES 128 // 稳定期样本数 (~128ms)
static uint16_t _stable_cnt = 0;
if (loop1_Origin == 0) return;
/*--- 1. IIR 一阶低通滤波 ---*/
/* 重连后 CAPVD=0：跳过 IIR，直接锁定首个有效值 */
if (loop1_CAPVD == 0) {
loop1_CAPVD = loop1_Value;
} else {
loop1_CAPVD = get_flt_value(loop1_Value, loop1_CAPVD);
}
/*--- 2. 稳定期：只跟踪基线，不检测车辆 ---*/
if (!g_loop_stable) {
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
&loop1_Origin, loop1_CAPVD, 100);
_stable_cnt++;
if (_stable_cnt >= STABLE_SAMPLES) {
g_loop_stable = 1;
PRINT("Loop stable, Origin:%d\n", loop1_Origin);
}
return;
}
if (!loop1_VD_FLAG) {
/*================================================================
* 无车状态
*================================================================*/
/*--- 基线跟踪（仿 TLD-110：有车时冻结） ---*/
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
&loop1_Origin, loop1_CAPVD, 100);
/*--- 进入检测 ---*/
loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel]) >> 16;
if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) {
PRINT("Car_In, Value:%d, CAPVD:%d, Origin:%d, dlt:%d\n",
loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG);
loop1_VD_FLAG = 1;
loop1_FLAG_IN = 1;
/* 有限存在计时（非安全复位模式下） */
if (!SET_SAFE) {
loop1_LC_HOLD = 1;
} else {
loop1_LC_HOLD = 0;
}
if (loop1_LC_Reset)
loop1_LC_Reset = 0;
loop1_ORG_CNT = 0;
loop1_ORG_SUM = 0;
/* 重置平坦性状态（专利 CN200910309382） */
#if USE_FLATNESS_EXIT
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;
#endif
}
} else {
#if USE_FLATNESS_EXIT
/*================================================================
* 有车状态 — 平坦性判定离开（专利 CN200910309382）
*================================================================*/
#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, 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) {
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) {
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 {
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 * 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;
#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
}
}
/*===========================================================================
* loop_task_function — FreeRTOS 主任务（对齐 M1H main 循环）
*===========================================================================*/
void loop_task_function(void *pvParameters)
{
g_sys_freq = g_crm_clocks_freq_struct.sclk_freq;
INIT_VD();
while (1) {
if (loop1_LOOP_OK) {
/*--- 线圈重连 ---*/
if (!loop1_LOOP_OK0) {
loop1_LOOP_OK0 = 1;
g_disconnect_active = 0; // 重连，清除断开标记
loop1_CAPVD = 0; // 强制 IIR 从首个有效值重新收敛
}
if (loop1_CAP_OK) {
loop1_CAP_OK = 0;
/* 首次成功测量 → 标记线圈已连接 */
if (!g_loop_power_up_state) {
g_loop_power_up_state = 1;
}
/* 核心检测算法 */
vd1_task();
/* 拨码开关轮询 */
poll_sw_state();
}
} else {
/*--- 线圈断开 ---*/
/* 注意: 不清除 loop1_VD_FLAG，保留断开前的检测状态 */
RLY1_OFF;
RLY2_OFF;
/*
* 断开计数规则:
* - 从未连接过上电 (g_loop_power_up_state == 0): 不计入
* - g_disconnect_active == 0: 新的断开 → 计数+1
* - g_disconnect_active == 1: 同一次断开，不重复计数
*/
if (g_loop_power_up_state && !g_disconnect_active) {
g_disconnect_active = 1;
if (g_disconnect_count < 3) {
g_disconnect_count++;
}
g_fault_phase = 0;
g_fault_tick = 0;
}
vTaskDelay(150);
}
/*--- 调试输出（每 2 秒） ---*/
#ifdef DEBUG
{
static uint32_t _dbg_cnt = 0;
_dbg_cnt++;
if (_dbg_cnt >= 200) { // 200 × 10ms = 2s
_dbg_cnt = 0;
PRINT("SET_DLY:%d, SAFE:%d, Xn:%d, LPCNT:%d, "
"CAPVD:%d, Origin:%d, VD:%d\n",
SET_DLY, SET_SAFE, loop1_Xn, loop1_LPCNT,
loop1_CAPVD, loop1_Origin, loop1_VD_FLAG);
}
}
#endif
vTaskDelay(10);
}
}
Reference in New Issue View Git Blame Copy Permalink