wangfq
5aa8edc1ea
- Loop154_States 新增 misc_counter (50ms 自由运行不复位) - 7处时间戳操作改用 misc_counter: passtime_start/last_exit_tick/misc_value - report_counter 还原原始清零逻辑, 仅管上报间隔 - 删除 last_report_tick 这样 report_counter 只管上报调度, misc_counter 只管时间戳, 互不干扰, 即使 report_counter 清零也不影响车间距/通过时间计算
744 lines
28 KiB
C
744 lines
28 KiB
C
|
||
#include "TaskLoop.h"
|
||
#include "at32f421_board.h"
|
||
#include "cmcng.h"
|
||
#include "FreeRTOS.h"
|
||
#include "task.h"
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
|
||
/*===========================================================================
|
||
* 灵敏度表 — 对齐 DLD154V4B / M1H
|
||
* 进入阈值 = Origin × SensTable[SENS] / 65536
|
||
* 离开阈值 = Origin × SensTable_1[SENS] / 65536 （滞回 ~50%）
|
||
* SENS: 0=低灵敏, 3=高灵敏
|
||
*===========================================================================*/
|
||
const uint16_t SensTable[4] = {216, 108, 36, 10};
|
||
const uint16_t SensTable_1[4] = {108, 72, 18, 9};
|
||
|
||
/*===========================================================================
|
||
* 全局状态
|
||
*===========================================================================*/
|
||
Loop154_States g_loop_states = {0};
|
||
uint32_t g_sys_freq = 0;
|
||
uint8_t g_input_div = 1;
|
||
uint32_t g_safe_max_cnt = LC_HOLD_TIME;
|
||
|
||
/*===========================================================================
|
||
* 一阶 IIR 低通滤波器（对齐 DLD154V4B）
|
||
* CAPVD_new = α·new_value + (1-α)·CAPVD_old, α = Flt_Reg/256
|
||
*===========================================================================*/
|
||
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 * ALFA_CAP1) >> 8;
|
||
|
||
if (new_value > last_Value) {
|
||
value_Flt = last_Value + scaled_delta;
|
||
} else {
|
||
value_Flt = last_Value - scaled_delta;
|
||
}
|
||
return value_Flt;
|
||
}
|
||
|
||
/*===========================================================================
|
||
* 滑动平均基线更新
|
||
*===========================================================================*/
|
||
uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt,
|
||
uint32_t* p_origin, uint32_t new_value,
|
||
uint16_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;
|
||
}
|
||
|
||
/*===========================================================================
|
||
* 继电器控制
|
||
*===========================================================================*/
|
||
void set_loops_relay_on(uint8_t loop_num)
|
||
{
|
||
switch(loop_num) {
|
||
case 0: RLY1_ON; break;
|
||
case 1: RLY2_ON; break;
|
||
case 2: RLY3_ON; break;
|
||
case 3: RLY4_ON; break;
|
||
}
|
||
}
|
||
|
||
void set_loops_relay_off(uint8_t loop_num)
|
||
{
|
||
switch(loop_num) {
|
||
case 0: RLY1_OFF; break;
|
||
case 1: RLY2_OFF; break;
|
||
case 2: RLY3_OFF; break;
|
||
case 3: RLY4_OFF; break;
|
||
}
|
||
}
|
||
|
||
/*===========================================================================
|
||
* 单通道初始化
|
||
*===========================================================================*/
|
||
void init_vd_single(Loop154_Unit *unit)
|
||
{
|
||
unit->INCNT = 0;
|
||
unit->OUTCNT = 0;
|
||
unit->loop_CapCnt = 0;
|
||
unit->loop_CapLast = 0;
|
||
unit->loop_LPCNT = 0;
|
||
unit->loop_INI_LOOP = 1;
|
||
|
||
unit->loop_VD_FLAG = 0;
|
||
unit->loop_RF_FLAG = 0;
|
||
unit->loop_LOOP_OK = 1;
|
||
unit->loop_LOOP_OK0 = 0;
|
||
unit->loop_CAP_OK = 0;
|
||
unit->loop_CAPVD = 0;
|
||
|
||
unit->loop_SensLevel = 2; // 默认中灵敏度
|
||
unit->Flt_Reg = ALFA_CAP1;
|
||
|
||
unit->loop_entry_cnt = 0;
|
||
unit->loop_freeze_cnt = 0;
|
||
unit->loop_freeze_ref = 0;
|
||
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
|
||
unit->loop_FLAG_IN = 0;
|
||
unit->loop_FLAG_OUT = 0;
|
||
unit->loop_FLAG_PLUSE = 0;
|
||
unit->loop_cnt_release = 0;
|
||
unit->loop_stable = 0;
|
||
|
||
#if USE_FLATNESS_EXIT
|
||
unit->exit_state = 0;
|
||
unit->max_slope = 0;
|
||
unit->max_slope_rate = 0;
|
||
unit->delta2 = 0;
|
||
unit->delta3 = 0;
|
||
unit->prev_capvd = 0;
|
||
unit->prev_first_deriv = 0;
|
||
unit->slope_flat_cnt = 0;
|
||
unit->flat_ok_cnt = 0;
|
||
#endif
|
||
|
||
unit->LC_HOLD = 0;
|
||
unit->LC_Reset = 0;
|
||
unit->LC_Hold_CNT = 0;
|
||
unit->Hold_CNT = 0;
|
||
|
||
unit->power_up_state = 0;
|
||
unit->disconnect_count = 0;
|
||
unit->disconnect_active = 0;
|
||
unit->fault_phase = 0;
|
||
unit->fault_tick = 0;
|
||
|
||
unit->SET_PLUS = 1; // 存在输出
|
||
unit->SET_DLY = 0;
|
||
unit->SET_SAFE = 1;
|
||
|
||
unit->stable_cnt = 0;
|
||
unit->xn_counter = 0;
|
||
}
|
||
|
||
void INIT_VDs(Loop154_States *loops)
|
||
{
|
||
uint8_t i;
|
||
for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
|
||
loops->loop_unit[i].loop_num = i;
|
||
init_vd_single(&loops->loop_unit[i]);
|
||
}
|
||
}
|
||
|
||
/*===========================================================================
|
||
* TMR3 输入捕获中断 — 四路线圈频率测量（对齐 DLD154V4B CAP0 ISR）
|
||
*
|
||
* 每个通道独立捕获，使用 MEASUREMENT_BASE 自适应测量窗口。
|
||
* LPCNT = MEASUREMENT_BASE / Xn，使 Value ≈ 131072，无需后续 >> 移位。
|
||
*===========================================================================*/
|
||
void TMR3_GLOBAL_IRQHandler(void)
|
||
{
|
||
#define PROCESS_CHANNEL(ch_flag, ch_select, idx) \
|
||
if (tmr_interrupt_flag_get(TMR3, ch_flag) != RESET) { \
|
||
tmr_flag_clear(TMR3, ch_flag); \
|
||
uint16_t cap_this = tmr_channel_value_get(TMR3, ch_select); \
|
||
Loop154_Unit *unit = &g_loop_states.loop_unit[idx]; \
|
||
if (!unit->loop_RF_FLAG) { \
|
||
unit->loop_CapLast = cap_this; \
|
||
unit->loop_RF_FLAG = 1; \
|
||
} else { \
|
||
uint16_t xn; \
|
||
if (cap_this > unit->loop_CapLast) { \
|
||
xn = cap_this - unit->loop_CapLast; \
|
||
} else { \
|
||
xn = (0x10000 - unit->loop_CapLast) + cap_this; \
|
||
} \
|
||
unit->loop_Xn = xn; \
|
||
unit->xn_counter++; \
|
||
unit->loop_CapLast = cap_this; \
|
||
unit->loop_CapCnt++; \
|
||
if (unit->loop_INI_LOOP) { \
|
||
if (unit->loop_LPCNT == 0) { \
|
||
if (unit->loop_CapCnt > 10) { \
|
||
unit->loop_LPCNT = MEASUREMENT_BASE / xn; \
|
||
PRINT("Loop%d LPCNT:%d Xn:%d\n", idx+1, unit->loop_LPCNT, xn); \
|
||
if (unit->loop_LPCNT == 0) unit->loop_LPCNT = 100; \
|
||
unit->loop_CAPVD = 0; \
|
||
unit->loop_CapSum = 0; \
|
||
unit->loop_CapCnt = 0; \
|
||
} \
|
||
} else { \
|
||
unit->loop_CapSum += xn; \
|
||
if (unit->loop_CapCnt >= unit->loop_LPCNT) { \
|
||
unit->loop_CAPVD = unit->loop_CapSum; \
|
||
unit->loop_Origin = unit->loop_CAPVD; \
|
||
PRINT("Loop%d Origin:%d\n", idx+1, unit->loop_Origin); \
|
||
unit->loop_INI_LOOP = 0; \
|
||
unit->loop_CapSum = 0; \
|
||
unit->loop_Value = 0; \
|
||
unit->loop_CapCnt = 0; \
|
||
} \
|
||
} \
|
||
} else { \
|
||
unit->loop_CapSum += xn; \
|
||
if (unit->loop_CapCnt >= unit->loop_LPCNT) { \
|
||
unit->loop_Value = unit->loop_CapSum; \
|
||
unit->loop_CAP_OK = 1; \
|
||
unit->loop_CapSum = 0; \
|
||
unit->loop_CapCnt = 0; \
|
||
unit->loop_INI_LOOP = 0; \
|
||
} \
|
||
} \
|
||
} \
|
||
}
|
||
|
||
PROCESS_CHANNEL(TMR_C1_FLAG, TMR_SELECT_CHANNEL_1, 0)
|
||
PROCESS_CHANNEL(TMR_C2_FLAG, TMR_SELECT_CHANNEL_2, 1)
|
||
PROCESS_CHANNEL(TMR_C3_FLAG, TMR_SELECT_CHANNEL_3, 2)
|
||
PROCESS_CHANNEL(TMR_C4_FLAG, TMR_SELECT_CHANNEL_4, 3)
|
||
|
||
#undef PROCESS_CHANNEL
|
||
}
|
||
|
||
/*===========================================================================
|
||
* TMR15 定时器中断 — 5ms tick，主状态机（对齐 DLD154V4B Timer1 ISR）
|
||
*
|
||
* 职责：
|
||
* 1. 50ms tick 分频
|
||
* 2. 四通道 进入/离开/脉冲 时序状态机
|
||
* 3. 有限存在超时 / 安全复位超时
|
||
* 4. 线圈载波检测 (RF_FLAG) 及继电器输出刷新
|
||
* 5. 呼吸灯、指示灯驱动
|
||
*===========================================================================*/
|
||
void TMR15_GLOBAL_IRQHandler(void)
|
||
{
|
||
static uint8_t TM1cnt = 0;
|
||
uint8_t i;
|
||
|
||
if (tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) {
|
||
|
||
/*--- 50ms tick 分频 ---*/
|
||
TM1cnt++;
|
||
if (TM1cnt >= 10) {
|
||
TM1cnt = 0;
|
||
|
||
/*--- 上报计数器 (50ms tick) ---*/
|
||
g_loop_states.report_counter++;
|
||
g_loop_states.misc_counter++;
|
||
|
||
for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
|
||
Loop154_Unit *unit = &g_loop_states.loop_unit[i];
|
||
|
||
/* FLAG_IN: 进入延时 500ms */
|
||
if (unit->loop_FLAG_IN) {
|
||
unit->INCNT++;
|
||
if (unit->INCNT > IN_DELAY) {
|
||
unit->loop_FLAG_IN = 0;
|
||
unit->INCNT = 0;
|
||
}
|
||
}
|
||
|
||
/* FLAG_OUT: 离开延时 → 脉冲 */
|
||
if (unit->loop_FLAG_OUT) {
|
||
if (!unit->SET_DLY) {
|
||
unit->OUTCNT++;
|
||
if (unit->OUTCNT > unit->relay_delay) {
|
||
unit->loop_FLAG_OUT = 0;
|
||
unit->loop_FLAG_PLUSE = 1;
|
||
unit->OUTCNT = 0;
|
||
}
|
||
} else {
|
||
unit->loop_FLAG_OUT = 0;
|
||
unit->loop_FLAG_PLUSE = 1;
|
||
unit->OUTCNT = 0;
|
||
}
|
||
}
|
||
|
||
/* FLAG_PLUSE: 脉冲宽度 500ms */
|
||
if (unit->loop_FLAG_PLUSE) {
|
||
unit->OUTCNT++;
|
||
if (unit->OUTCNT > PULSE_DELAY) {
|
||
unit->loop_FLAG_PLUSE = 0;
|
||
unit->OUTCNT = 0;
|
||
}
|
||
}
|
||
|
||
/* 有限存在超时 — 通道重启，重新建立基线 */
|
||
if (unit->loop_VD_HOLD) {
|
||
unit->Hold_CNT++;
|
||
if (unit->Hold_CNT > unit->hold_time) {
|
||
unit->loop_VD_HOLD = 0;
|
||
unit->Hold_CNT = 0;
|
||
set_loops_relay_off(unit->loop_num);
|
||
unit->loop_VD_FLAG = 0;
|
||
unit->loop_FLAG_IN = 0;
|
||
unit->loop_FLAG_OUT = 0;
|
||
unit->loop_FLAG_PLUSE = 0;
|
||
unit->LC_HOLD = 0;
|
||
unit->LC_Reset = 1;
|
||
unit->loop_INI_LOOP = 1;
|
||
unit->loop_LOOP_OK0 = 0;
|
||
unit->loop_stable = 0;
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
unit->last_exit_tick = g_loop_states.misc_counter;
|
||
#if USE_FLATNESS_EXIT
|
||
unit->exit_state = 0;
|
||
unit->max_slope = 0;
|
||
unit->max_slope_rate = 0;
|
||
#endif
|
||
}
|
||
}
|
||
|
||
/* 安全复位超时 */
|
||
if (unit->LC_HOLD) {
|
||
unit->LC_Hold_CNT++;
|
||
if (unit->LC_Hold_CNT > g_safe_max_cnt) {
|
||
unit->LC_Reset = 1;
|
||
unit->loop_INI_LOOP = 1;
|
||
unit->loop_LOOP_OK0 = 0;
|
||
unit->loop_stable = 0;
|
||
#if USE_FLATNESS_EXIT
|
||
unit->exit_state = 0;
|
||
unit->max_slope = 0;
|
||
unit->max_slope_rate = 0;
|
||
#endif
|
||
unit->LC_Hold_CNT = 0;
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*================================================================
|
||
* 线圈载波检测 & 继电器输出刷新（每 5ms）
|
||
*================================================================*/
|
||
for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
|
||
Loop154_Unit *unit = &g_loop_states.loop_unit[i];
|
||
|
||
if (unit->loop_RF_FLAG) {
|
||
unit->loop_LOOP_OK = 1;
|
||
unit->loop_RF_FLAG = 0;
|
||
|
||
if (unit->LC_Reset == 0) {
|
||
if (unit->SET_PLUS) {
|
||
/* 存在输出模式：有车或离开延时中 */
|
||
if (unit->loop_VD_FLAG || unit->loop_FLAG_OUT)
|
||
set_loops_relay_on(unit->loop_num);
|
||
else
|
||
set_loops_relay_off(unit->loop_num);
|
||
} else {
|
||
/* 脉冲输出模式：脉冲期间吸合 */
|
||
if (unit->loop_FLAG_PLUSE)
|
||
set_loops_relay_on(unit->loop_num);
|
||
else
|
||
set_loops_relay_off(unit->loop_num);
|
||
}
|
||
}
|
||
} else {
|
||
unit->loop_LOOP_OK0 = unit->loop_LOOP_OK;
|
||
unit->loop_LOOP_OK = 0;
|
||
}
|
||
|
||
/*--- LED 指示 ---*/
|
||
poll_green_led(unit);
|
||
}
|
||
|
||
/*--- 呼吸灯 ---*/
|
||
poll_red_pwm();
|
||
|
||
tmr_flag_clear(TMR15, TMR_OVF_FLAG);
|
||
}
|
||
}
|
||
|
||
/*===========================================================================
|
||
* 绿灯指示 — 每通道独立
|
||
*
|
||
* 模式：
|
||
* 自检/稳定中 (loop_INI_LOOP || 未连 || !loop_stable):
|
||
* → 慢闪 (200ms 亮/灭)
|
||
* 正常工作:
|
||
* → 有车亮, 无车灭
|
||
*===========================================================================*/
|
||
void poll_green_led(Loop154_Unit *unit)
|
||
{
|
||
#define GREEN_SLOW_HALF 40 // 200ms (40 × 5ms)
|
||
|
||
static uint16_t _slow_tick[LOOP_CAPTURE_MAX] = {0};
|
||
uint8_t idx = unit->loop_num;
|
||
|
||
if (unit->disconnect_active) {
|
||
at32_led_off((led_type)idx);
|
||
_slow_tick[idx] = 0;
|
||
return;
|
||
}
|
||
|
||
if (unit->loop_INI_LOOP || !unit->power_up_state || !unit->loop_stable) {
|
||
_slow_tick[idx]++;
|
||
if (_slow_tick[idx] >= GREEN_SLOW_HALF) {
|
||
_slow_tick[idx] = 0;
|
||
at32_led_toggle((led_type)idx);
|
||
}
|
||
return;
|
||
}
|
||
|
||
_slow_tick[idx] = 0;
|
||
if (unit->loop_VD_FLAG)
|
||
at32_led_on((led_type)idx);
|
||
else
|
||
at32_led_off((led_type)idx);
|
||
}
|
||
|
||
/*===========================================================================
|
||
* vd1_task_per_channel — 核心检测算法（移植自 DLD154V4B vd1_task）
|
||
*
|
||
* 每次 loop_CAP_OK 时调用。
|
||
* 流程：
|
||
* 1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD
|
||
* 2. 稳定期：只跟踪基线，不检测车辆 (STABLE_SAMPLES=128)
|
||
* 3. 无车时：基线跟踪 + 进入检测（带基线污染保护）
|
||
* 4. 有车时：平坦性离开 (USE_FLATNESS_EXIT=1) 或 cnt_release 防抖
|
||
*===========================================================================*/
|
||
void vd1_task_per_channel(Loop154_Unit *unit)
|
||
{
|
||
if (unit->loop_Origin == 0) return;
|
||
|
||
/*================================================================
|
||
* 1. IIR 滤波 + 斜率限幅 (M4 优化)
|
||
* ALFA_CAP1=79 @10ms → τ≈32ms
|
||
*================================================================*/
|
||
if (unit->loop_CAPVD == 0) {
|
||
unit->loop_CAPVD = unit->loop_Value;
|
||
} else {
|
||
int32_t raw_delta = (int32_t)unit->loop_Value - (int32_t)unit->loop_CAPVD;
|
||
int32_t max_step = (int32_t)(unit->loop_CAPVD * MAX_SLOPE_RATE / 100);
|
||
if (max_step < 100) max_step = 100;
|
||
if (raw_delta > max_step) raw_delta = max_step;
|
||
if (raw_delta < -max_step) raw_delta = -max_step;
|
||
uint32_t clamped_value = (uint32_t)((int32_t)unit->loop_CAPVD + raw_delta);
|
||
unit->loop_CAPVD = get_flt_value(clamped_value, unit->loop_CAPVD);
|
||
}
|
||
|
||
/*--- 2. 稳定期：绕过 IIR 和斜率限幅，小窗口快速收敛 ---*/
|
||
if (!unit->loop_stable) {
|
||
unit->loop_CAPVD = unit->loop_Value;
|
||
|
||
update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT,
|
||
&unit->loop_Origin, unit->loop_CAPVD, 100);
|
||
unit->stable_cnt++;
|
||
if (unit->stable_cnt >= STABLE_SAMPLES) {
|
||
unit->loop_stable = 1;
|
||
PRINT("Loop%d stable, Origin:%d\n", unit->loop_num + 1, unit->loop_Origin);
|
||
}
|
||
return;
|
||
}
|
||
|
||
if (!unit->loop_VD_FLAG) {
|
||
/*================================================================
|
||
* 无车状态
|
||
*================================================================*/
|
||
|
||
unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable[unit->loop_SensLevel]) >> 16;
|
||
{
|
||
int32_t dev = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_Origin;
|
||
if (dev < (int32_t)(unit->loop_dlt_ORG * 4)) {
|
||
unit->loop_freeze_cnt = 0;
|
||
update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT,
|
||
&unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN);
|
||
} else {
|
||
if (unit->loop_freeze_cnt == 0) {
|
||
unit->loop_freeze_ref = unit->loop_CAPVD;
|
||
} else {
|
||
int32_t drift = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_freeze_ref;
|
||
if (drift < 0) drift = -drift;
|
||
if (drift > (int32_t)(unit->loop_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
|
||
unit->loop_freeze_cnt = 0;
|
||
unit->loop_freeze_ref = unit->loop_CAPVD;
|
||
}
|
||
}
|
||
unit->loop_freeze_cnt++;
|
||
if (unit->loop_freeze_cnt >= FREEZE_TIMEOUT) {
|
||
unit->loop_Origin = unit->loop_CAPVD;
|
||
unit->loop_freeze_cnt = 0;
|
||
unit->loop_freeze_ref = 0;
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
PRINT("Loop%d Baseline timeout, new Origin:%d\n",
|
||
unit->loop_num + 1, unit->loop_Origin);
|
||
} else {
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值 ---*/
|
||
if (unit->loop_CAPVD < (unit->loop_Origin - unit->loop_dlt_ORG)) {
|
||
unit->loop_entry_cnt++;
|
||
if (unit->loop_entry_cnt >= ENTRY_CONFIRM) {
|
||
PRINT("Loop%d Car_In, Value:%d CAPVD:%d Origin:%d dlt:%d\n",
|
||
unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD,
|
||
unit->loop_Origin, unit->loop_dlt_ORG);
|
||
|
||
unit->loop_VD_FLAG = 1;
|
||
unit->loop_FLAG_IN = 1;
|
||
unit->loop_entry_cnt = 0;
|
||
unit->loop_freeze_cnt = 0;
|
||
unit->loop_freeze_ref = 0;
|
||
|
||
/* 主动上报: 车流量+1, 继电器+1, 车间距, 记录进场时间戳 */
|
||
unit->flow_count++;
|
||
unit->relay_count++;
|
||
if (unit->last_exit_tick > 0) {
|
||
unit->misc_value = g_loop_states.misc_counter - unit->last_exit_tick;
|
||
} else {
|
||
unit->misc_value = 0; // 首次检测, 车间距为0
|
||
}
|
||
unit->passtime_start = g_loop_states.misc_counter; // 50ms tick
|
||
if (unit->hold_time > 0) {
|
||
unit->loop_VD_HOLD = 1;
|
||
}
|
||
at32_led_on((led_type)unit->loop_num);
|
||
|
||
if (!unit->SET_SAFE) {
|
||
unit->LC_HOLD = 1;
|
||
} else {
|
||
unit->LC_HOLD = 0;
|
||
}
|
||
if (unit->LC_Reset)
|
||
unit->LC_Reset = 0;
|
||
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
|
||
#if USE_FLATNESS_EXIT
|
||
unit->exit_state = 0;
|
||
unit->max_slope = 0;
|
||
unit->max_slope_rate = 0;
|
||
unit->delta2 = 0;
|
||
unit->delta3 = 0;
|
||
unit->slope_flat_cnt = 0;
|
||
unit->flat_ok_cnt = 0;
|
||
#endif
|
||
}
|
||
} else {
|
||
if (unit->loop_entry_cnt > 0) unit->loop_entry_cnt = 0;
|
||
}
|
||
} else {
|
||
#if USE_FLATNESS_EXIT
|
||
/*================================================================
|
||
* 有车状态 — 平坦性判定离开 (CN200910309382)
|
||
*================================================================*/
|
||
int32_t first_deriv, abs_fd, abs_sd, second_deriv;
|
||
|
||
first_deriv = (int32_t)unit->loop_CAPVD - unit->prev_capvd;
|
||
second_deriv = first_deriv - unit->prev_first_deriv;
|
||
abs_fd = (first_deriv >= 0) ? first_deriv : -first_deriv;
|
||
abs_sd = (second_deriv >= 0) ? second_deriv : -second_deriv;
|
||
|
||
if (unit->exit_state == 0) {
|
||
/* 追踪第一上升坡面最大斜率 */
|
||
if (abs_fd > unit->max_slope) unit->max_slope = abs_fd;
|
||
if (abs_sd > unit->max_slope_rate) unit->max_slope_rate = abs_sd;
|
||
if (abs_fd < SLOPE_FLAT_THRESH) {
|
||
unit->slope_flat_cnt++;
|
||
if (unit->slope_flat_cnt >= 3) {
|
||
unit->delta2 = unit->max_slope / K1;
|
||
unit->delta3 = unit->max_slope_rate / K2;
|
||
if (unit->delta2 < MIN_DELTA2) unit->delta2 = MIN_DELTA2;
|
||
if (unit->delta3 < MIN_DELTA3) unit->delta3 = MIN_DELTA3;
|
||
unit->exit_state = 1;
|
||
unit->flat_ok_cnt = 0;
|
||
}
|
||
} else {
|
||
unit->slope_flat_cnt = 0;
|
||
}
|
||
} else {
|
||
int32_t dev = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_Origin;
|
||
int32_t cond1 = (dev >= 0) ? dev : -dev;
|
||
unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable_1[unit->loop_SensLevel]) >> 16;
|
||
if (cond1 < (int32_t)unit->loop_dlt_ORG && abs_fd < (int32_t)unit->delta2
|
||
&& abs_sd < (int32_t)unit->delta3) {
|
||
unit->flat_ok_cnt++;
|
||
if (unit->flat_ok_cnt >= FLAT_CONFIRM_CNT) {
|
||
PRINT("Loop%d Car_OFF_FLAT, CAPVD:%d Origin:%d d1:%d d2:%d d3:%d f':%d f'':%d\n",
|
||
unit->loop_num + 1, unit->loop_CAPVD, unit->loop_Origin,
|
||
unit->loop_dlt_ORG, unit->delta2, unit->delta3,
|
||
first_deriv, second_deriv);
|
||
unit->loop_VD_FLAG = 0;
|
||
unit->loop_FLAG_OUT = 1;
|
||
unit->loop_VD_HOLD = 0;
|
||
unit->LC_HOLD = 0;
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
unit->Hold_CNT = 0;
|
||
unit->flat_ok_cnt = 0;
|
||
unit->exit_state = 0;
|
||
|
||
/* 主动上报: 计算通过时间 = 当前-进场 (50ms单位) */
|
||
unit->misc_value = g_loop_states.misc_counter - unit->passtime_start;
|
||
unit->last_exit_tick = g_loop_states.misc_counter;
|
||
unit->relay_count++; // 离开时继电器翻转
|
||
}
|
||
} else {
|
||
unit->flat_ok_cnt = 0;
|
||
}
|
||
}
|
||
unit->prev_capvd = unit->loop_CAPVD;
|
||
unit->prev_first_deriv = first_deriv;
|
||
#else
|
||
/*================================================================
|
||
* 有车状态 — cnt_release 防抖离开
|
||
*================================================================*/
|
||
unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable_1[unit->loop_SensLevel]) >> 16;
|
||
|
||
if ((unit->loop_Origin - unit->loop_dlt_ORG) < unit->loop_CAPVD) {
|
||
unit->loop_cnt_release++;
|
||
if (unit->loop_cnt_release >= 3) {
|
||
PRINT("Loop%d Car_OFF, Value:%d CAPVD:%d Origin:%d dlt:%d\n",
|
||
unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD,
|
||
unit->loop_Origin, unit->loop_dlt_ORG);
|
||
unit->loop_VD_FLAG = 0;
|
||
unit->loop_FLAG_OUT = 1;
|
||
unit->loop_VD_HOLD = 0;
|
||
unit->LC_HOLD = 0;
|
||
unit->loop_ORG_CNT = 0;
|
||
unit->loop_ORG_SUM = 0;
|
||
unit->Hold_CNT = 0;
|
||
unit->loop_cnt_release = 0;
|
||
|
||
/* 主动上报: 通过时间 + 继电器 */
|
||
unit->misc_value = g_loop_states.misc_counter - unit->passtime_start;
|
||
unit->last_exit_tick = g_loop_states.misc_counter;
|
||
unit->relay_count++;
|
||
}
|
||
} else {
|
||
if (unit->loop_cnt_release > 0) unit->loop_cnt_release = 0;
|
||
}
|
||
#endif
|
||
}
|
||
}
|
||
|
||
/*===========================================================================
|
||
* loop_task_function — FreeRTOS 主任务（对齐 DLD154V4B main 循环）
|
||
* 10ms 周期，独立处理每路线圈。
|
||
*===========================================================================*/
|
||
void loop_task_function(void *pvParameters)
|
||
{
|
||
uint8_t i;
|
||
|
||
g_sys_freq = g_crm_clocks_freq_struct.sclk_freq;
|
||
INIT_VDs(&g_loop_states);
|
||
|
||
while (1) {
|
||
for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
|
||
Loop154_Unit *unit = &g_loop_states.loop_unit[i];
|
||
|
||
if (unit->loop_LOOP_OK) {
|
||
/* 线圈重连 */
|
||
if (!unit->loop_LOOP_OK0) {
|
||
unit->loop_LOOP_OK0 = 1;
|
||
unit->disconnect_active = 0;
|
||
unit->loop_CAPVD = 0; // 强制 IIR 重新收敛
|
||
}
|
||
|
||
if (unit->loop_CAP_OK) {
|
||
taskENTER_CRITICAL();
|
||
unit->loop_CAP_OK = 0;
|
||
taskEXIT_CRITICAL();
|
||
|
||
if (!unit->power_up_state) {
|
||
unit->power_up_state = 1;
|
||
}
|
||
|
||
/* 核心检测算法 */
|
||
vd1_task_per_channel(unit);
|
||
}
|
||
} else {
|
||
/* 线圈断开 */
|
||
set_loops_relay_off(unit->loop_num);
|
||
|
||
if (unit->power_up_state && !unit->disconnect_active) {
|
||
unit->disconnect_active = 1;
|
||
if (unit->disconnect_count < 3) {
|
||
unit->disconnect_count++;
|
||
}
|
||
unit->fault_phase = 0;
|
||
unit->fault_tick = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
wdt_feed();
|
||
vTaskDelay(10); // 10ms tick, M4 优化: 双路 IIR + 进入确认 + 斜率限幅
|
||
}
|
||
}
|
||
|
||
/*===========================================================================
|
||
* LED 指示灯后台任务 — 线圈未连接时的闪烁指示
|
||
*===========================================================================*/
|
||
void led_indicator_task_function(void *pvParameters)
|
||
{
|
||
uint8_t i;
|
||
static uint8_t led_step[LOOP_CAPTURE_MAX] = {0};
|
||
|
||
while (1) {
|
||
for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
|
||
Loop154_Unit *unit = &g_loop_states.loop_unit[i];
|
||
|
||
if (unit->loop_LOOP_OK0 == 0) {
|
||
if (led_step[i] % 2 == 0) {
|
||
at32_led_toggle((led_type)i);
|
||
}
|
||
if (unit->loop_stable) {
|
||
unit->loop_LOOP_OK0 = 1;
|
||
continue;
|
||
}
|
||
led_step[i]++;
|
||
if (led_step[i] > 16) {
|
||
led_step[i] = 0;
|
||
unit->loop_LOOP_OK0 = 1;
|
||
at32_led_off((led_type)i);
|
||
}
|
||
}
|
||
}
|
||
vTaskDelay(200);
|
||
}
|
||
}
|
||
|
||
|