wangfq/DLD154Pro
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
ee9aba64b8ab24942eb40e3cfdac99d75e14327c
DLD154Pro/BLE/DLD154Pro_Peripheral_28K/APP/peripheral_main.c
T
wangfq ee9aba64b8 feat: LEDB 红色呼吸灯 (PB23, PWM11) 
- 新增 pwm_led_init(): 初始化 PWM11, f≈5.86kHz, 分辨率128级
- 新增四相呼吸状态机 (渐亮/高亮保持/渐灭/低亮保持), ~13.5s周期
- gpio_uart_init(): LEDB 从 GPIO 改为 PWM 控制
- LEDB_ON/OFF 宏保留备用 (不再用于呼吸控制)
- 注: PB23 需在 ISP 烧录时关闭外部复位 (RB_CFG_RESET_EN=0)
2026-07-21 16:40:05 +08:00

1247 lines
43 KiB
C
Raw Blame History

/********************************** (C) COPYRIGHT *******************************
* File Name : peripheral_main.c
* Author : WCH / wangfq
* Version : V2.0 (algorithm ported from DLD154V4B)
* Date : 2026/07/18
* Description : DLD154Pro 单线圈蓝牙车检器 主函数
*
* 算法说明 (移植自 DLD154V4B TaskLoop V2.0, M1H 精简系):
* - 自适应测量窗: LPCNT = MEASUREMENT_BASE / Xn, 测量窗时长与线圈频率无关
* - 一阶 IIR (79/256) + 斜率限幅 5% (拒绝 EMI 瞬态尖峰)
* - 稳定期 128 窗快速收敛 (绕过滤波, 小窗口 100)
* - 基线跟踪 窗口 500 + 冻结保护 (超时 + 稳定性双条件强制更新)
* - 进入确认 3 次; 离开: 平坦性三条件判定 (专利 CN200910309382)
* - 有限存在超时 / 安全复位: 全复位重建基线
*********************************************************************************
* Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd.
* Attention: This software (modified or not) and binary are used for
* microcontroller manufactured by Nanjing Qinheng Microelectronics.
*******************************************************************************/
/******************************************************************************/
/* 头文件包含 */
#include "CONFIG.h"
#include "HAL.h"
#include "gattprofile.h"
#include "peripheral.h"
#include "storage.h"
#include <stdlib.h>
#include "cmcng.h"
#include "dbn_ble_srv.h"
#include "CH59x_pwm.h"
uint8_t g_flag_bt_state = 0; // 0 蓝牙没有连接，1 有蓝牙连接
uint8_t g_function_mode = 0;
uint32_t g_sys_freq = 0;
/*===========================================================================
* IO 宏定义 -- DLD154Pro 硬件资源
*
* PA8 : RLY1 存在继电器 (单刀双掷, 低电平有效)
* PB22 : LEDA 绿色指示灯 (有车亮/无车灭, 低电平有效)
* PB23 : LEDB 红色指示灯 (呼吸灯, 低电平有效)
* PA12 : FLEVEL 线圈调频 (一级, 0=33nF, 1=0nF)
* PA14 : UART0_Tx RS485发送 (协议待定)
* PA15 : UART0_Rx RS485接收 (协议待定)
* PA13 : UART0_RE RS485方向控制 (协议待定)
* PB10 : IN_1 光耦输入 (逻辑待定)
*===========================================================================*/
#define RLY1_ON (R32_PA_OUT |= BV(8))
#define RLY1_OFF (R32_PA_OUT &= ~BV(8))
#define LEDA_ON (R32_PB_OUT &= ~BV(22))
#define LEDA_OFF (R32_PB_OUT |= BV(22))
#define LEDA_ON_OFF() (GPIOB_ReadPortPin(GPIO_Pin_22) ? GPIOB_ResetBits(GPIO_Pin_22) : GPIOB_SetBits(GPIO_Pin_22))
#define LEDB_ON (R32_PB_OUT &= ~BV(23))
#define LEDB_OFF (R32_PB_OUT |= BV(23))
#define FLEVEL_LOW (R32_PA_OUT &= ~BV(12))
#define FLEVEL_HIGH (R32_PA_OUT |= BV(12))
/* RS485 预留 (协议待定) */
#define RS485_TX_PIN GPIO_Pin_14
#define RS485_RX_PIN GPIO_Pin_15
#define RS485_RE_PIN GPIO_Pin_13
#define RS485_RE_TX (GPIOA_SetBits(RS485_RE_PIN)) // RE=1 发送模式
#define RS485_RE_RX (GPIOA_ResetBits(RS485_RE_PIN)) // RE=0 接收模式
/* 光耦输入预留 (逻辑待定) */
#define OPT_IN1 (GPIOB_ReadPortPin(GPIO_Pin_10))
/*===========================================================================
* LEDB 红色呼吸灯 (PB23, PWM11)
* PWM freq ≈ 60MHz / (PWM_CLK_DIV * 128) ≈ 5.86kHz @DIV=80
* 呼吸周期 ≈ (127*2*50ms) + 0.5s + 0.3s ≈ 13.5s
*===========================================================================*/
#define PWM_CLK_DIV 80
#define PWM_CYCLE 128 // 7-bit 分辨率 (0~127)
#define BREATH_STEP_DELAY 1 // 每 50ms 更新一次占空比 (步进平滑)
#define BREATH_FADE_IN 0
#define BREATH_HOLD_HIGH 1
#define BREATH_FADE_OUT 2
#define BREATH_HOLD_LOW 3
static uint8_t g_breath_phase = BREATH_FADE_IN;
static uint8_t g_breath_duty = 0;
static uint8_t g_breath_tick = 0;
const uint8_t MAX_Store_Size = 80;
uint8_t g_loop_power_up_state = 0; // 开机启动时线圈是否连接
uint8_t g_disconnect_active = 0; // 当前线圈是否断开中 (1=断开)
__IO uint32_t TimingDelayInc;
/*===========================================================================
* 算法参数 -- 移植自 DLD154V4B (M4 优化版), 适配 CH591R @60MHz
*
* 测量窗: MEASUREMENT_BASE 个系统时钟 tick, 与线圈频率无关
* 524288(2^19) / 60MHz ~= 8.74ms, 即 vd1_task 有效采样周期 ~8.74ms
* (DLD154V4B 为 131072 @120MHz + 10ms 任务轮询, 时间尺度基本一致)
*===========================================================================*/
#define MEASUREMENT_BASE 524288L // 2^19, 自适应测量窗目标值 (Value ~= 524288)
#define ALFA_CAP1 79 // IIR a=79/256~=0.31 (@8.74ms -> tau~=24ms)
#define MAX_SLOPE_RATE 5 // 斜率限幅: 单次最大变化 5% (拒绝 EMI 尖峰)
#define ENTRY_CONFIRM 3 // 进入确认: 连续 N 次低于阈值才判有车
#define FREEZE_TIMEOUT 1000 // 基线冻结超时: ~8.7s, 持续偏高且稳定后强制更新
#define FREEZE_STABILITY_RATE 2 // 冻结稳定性窗口: 参考值 +-2%
// #define USE_FLATNESS_EXIT 1 // 1=平坦性三条件离开, 0=简单滞回防抖
#define WINDOW_ORIGIN 500 // 基线跟踪窗口 (500 窗 ~= 4.4s)
#define STABLE_SAMPLES 128 // 稳定期样本数 (~1.1s)
#define VARIATION_EVENT_TH 40 // BLE variation 事件阈值 (幅值基准 2^19, 对应旧代码的 5)
#define ENV_ACT_TH 30 // 环境活跃度: CAPVD 相邻差绝对值阈值 (待现场验证)
uint32_t mstick(void){
return TimingDelayInc;
}
uint32_t u32CountWdt = 0;
uint8_t g_loop_cut_last = 0;
uint32_t g_loop_cut_amount = 0;
/*********************************************************************
* GLOBAL TYPEDEFS
*/
__attribute__((aligned(4))) uint32_t MEM_BUF[BLE_MEMHEAP_SIZE / 4];
#if(defined(BLE_MAC)) && (BLE_MAC == TRUE)
const uint8_t MacAddr[6] = {0x84, 0xC2, 0xE4, 0x03, 0x02, 0x02};
#else
uint8_t MACAddr[6] = {0x84, 0xC2, 0xE4, 0x03, 0x02, 0x02};
#endif
uint8_t g_dev_number[6] = ""; // 设备编号 产品编号
char g_flag_debug = 1;
uint8_t g_freq_sens;
uint8_t loop1_SensLevel;
uint8_t g_freq_level;
uint16_t g_safe_max_cnt = LC_HOLD_TIME;
uint16_t Hold_CNT = 0;
uint8_t Flt_Reg = 0;
uint8_t TM1cnt = 0;
uint16_t g_hold_time = 0;
/*===========================================================================
* 捕获 & 测量
*===========================================================================*/
uint16_t loop1_Xn; // 相邻下降沿周期 (TMR0 直接给出)
uint32_t loop1_dlt_ORG; // 当前灵敏度阈值
uint32_t loop1_Origin; // 基线 (无车参考值)
uint16_t loop1_LPCNT; // 每个测量窗累加的 Xn 个数
uint16_t loop1_CapCnt; // 当前窗口已累加次数
uint32_t loop1_CapSum; // 当前窗口累加和
uint32_t loop1_CAPVD; // IIR 滤波后的测量值
uint32_t loop1_Value; // 一个测量窗的原始 Sigma-Xn
uint32_t loop1_ORG_SUM; // 基线跟踪累加和
uint16_t loop1_ORG_CNT; // 基线跟踪计数
/*===========================================================================
* 计数器
*===========================================================================*/
uint8_t loop1_INCNT;
uint8_t loop1_OUTCNT;
/*===========================================================================
* 标志位
*===========================================================================*/
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_FLAG_PLUSE_IN_F = 0;
uint8_t loop1_FLAG_PLUSE_IN = 0;
uint8_t loop1_FLAG_CUT;
uint8_t loop1_FLAG_HOLD_TIMEOUT; // 有限存在超时
/*===========================================================================
* 安全复位
*===========================================================================*/
uint8_t loop1_LC_HOLD = 0;
uint8_t loop1_LC_Reset = 0;
uint32_t LC_Hold_CNT = 0;
/*===========================================================================
* 新算法状态 (DLD154V4B 移植)
*===========================================================================*/
uint8_t g_loop_stable = 0; // 线圈数值已稳定 (0=稳定期中, 1=稳定)
uint16_t loop1_stable_cnt = 0; // 稳定期样本计数
uint8_t loop1_entry_cnt = 0; // 进入确认计数
uint8_t loop1_cnt_release = 0; // 简单离开防抖计数 (USE_FLATNESS_EXIT=0)
uint16_t loop1_freeze_cnt = 0; // 基线冻结持续计数
uint32_t loop1_freeze_ref = 0; // 冻结稳定性参考值
#if USE_FLATNESS_EXIT
uint8_t g_exit_state = 0; // 0=追踪坡面, 1=等待平坦
uint16_t g_max_slope = 0; // 第一上升坡面最大 |f'|
uint16_t g_max_slope_rate = 0; // 第一上升坡面最大 |f''|
uint16_t g_delta2 = 0; // 一阶平坦阈值
uint16_t g_delta3 = 0; // 二阶平坦阈值
int32_t g_prev_capvd = 0; // 上一帧 CAPVD (差分用)
int32_t g_prev_first_deriv = 0; // 上一帧一阶导数
uint8_t g_slope_flat_cnt = 0; // 斜率趋零连续计数
uint8_t g_flat_ok_cnt = 0; // 平坦条件满足连续计数
#endif
/* 环境活跃度 (供 BLE condition 字段上报, 替代旧 flt_mgr.cnt_not_idle) */
uint8_t g_env_activity = 0;
uint32_t g_env_prev_capvd = 0;
/*===========================================================================
* 出厂灵敏度默认表 (写入 g_loop_sens_list, 运行时以 g_loop_sens_list 为准)
* 进入阈值 = Origin * sens_in / 65536; 离开阈值 = Origin * sens_out / 65536
*===========================================================================*/
const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 28};
const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 16};
uint16_t g_loop_out_delay = 10; // 离开延时, 50ms 为单位, 由 delay_time 配置
DBN_BLE_State g_dbn_ble_state_acs_enable = {0};
Loop_ACS_Info g_loop_acs_info = {0,};
float g_freq_tmp = 0.0;
/* 用于APP判断文件有效性 */
const uint32_t Address = 0xFFFFFFFF;
__attribute__((aligned(4))) uint32_t Image_Flag __attribute__((section(".ImageFlag"))) = (uint32_t)&Address;
uint8_t loop_cng_default[7] = {2,1,0,0,4,0,0};
void set_factory_param(void)
{
uint8_t i;
memcpy(&g_loop_cng_unit.sensitvity, loop_cng_default, 7);
for(i = 0; i < g_loop_sens_list.total; i++)
{
g_loop_sens_list.sens[i].sens_in = SensTable[i];
g_loop_sens_list.sens[i].sens_out = SensTable1[i];
}
}
void factory_dev(void){
uint8_t i = 0, k = 0;
uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size);
if(rBuf == NULL){
SYS_ResetExecute();
}
for(i = 0; i < MAX_Store_Size; i++){
rBuf[i] = 0;
}
rBuf[0] = 0x33;
rBuf[1] = 0xBB;
rBuf[2] = 0xC3;
rBuf[3] = 0x3C;
i = 4;
memcpy(&rBuf[i], &g_loop_cng_unit.sensitvity, 7);
i += 7;
i = Addr_Loop_Sens_List_Offset;
if(g_loop_sens_list.total > MAX_LOOP_SENS_AMOUNT)
{
g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT;
}
rBuf[i++] = g_loop_sens_list.total;
for(k = 0; k < MAX_LOOP_SENS_AMOUNT; k++)
{
rBuf[i++] = g_loop_sens_list.sens[k].sens_in;
rBuf[i++] = g_loop_sens_list.sens[k].sens_in >> 8;
rBuf[i++] = g_loop_sens_list.sens[k].sens_out;
rBuf[i++] = g_loop_sens_list.sens[k].sens_out >> 8;
}
write_cfg_to_store(0, rBuf, MAX_Store_Size);
free(rBuf);
}
void para_store_init(void)
{
uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size);
if(rBuf == NULL){
SYS_ResetExecute();
}
memset(rBuf, 0, MAX_Store_Size);
read_cfg_from_store(0, rBuf, MAX_Store_Size);
if(rBuf[0] == 0x33 && rBuf[1] == 0xBB && rBuf[2] == 0xC3 && rBuf[3] == 0x3C){
memcpy(&g_loop_cng_unit, &rBuf[4], sizeof(g_loop_cng_unit));
memcpy(&g_loop_sens_list, &rBuf[Addr_Loop_Sens_List_Offset], sizeof(g_loop_sens_list));
g_loop_out_delay = g_loop_cng_unit.delay_time * 2; // 定时器以 50ms 为单位, 延时以100ms为单位
g_hold_time = g_loop_cng_unit.exist_mode * 20 * 5; //50ms/per, period: 5s
g_safe_max_cnt = g_loop_cng_unit.loopSafe_Timeout * 10 * (1000 / 50);
g_freq_level = g_loop_cng_unit.loopFreq_Level - 1;
g_function_mode = g_loop_cng_unit.direction_mode >> 4;
g_freq_sens = (g_loop_cng_unit.sensitvity > 0) ? (g_loop_cng_unit.sensitvity - 1) : 0;
#ifdef DEBUG
g_freq_level = 0;
#endif
// DLD154Pro: 一级调频, 0=33nF(低频), 1=0nF(高频)
if (g_freq_level == 0) {
FLEVEL_LOW;
} else {
FLEVEL_HIGH;
}
free(rBuf);
}
else{
free(rBuf);
set_factory_param();
factory_dev();
DelayMs(10);
SYS_ResetExecute();
}
}
void tm_init(void){
// Cap 捕获
GPIOA_ResetBits(GPIO_Pin_9);
GPIOA_ModeCfg(GPIO_Pin_9, GPIO_ModeIN_Floating);
TMR0_CapInit(FallEdge_To_FallEdge);
TMR0_CAPTimeoutCfg(0xFFFFFFFF);
TMR0_ClearITFlag(TMR0_3_IT_DATA_ACT);
TMR0_ITCfg(ENABLE, TMR0_3_IT_DATA_ACT);
PFIC_EnableIRQ(TMR0_IRQn);
// TMR1
TMR1_TimerInit(FREQ_SYS / 200); // 设置定时时间 5ms
TMR1_ITCfg(ENABLE, TMR0_3_IT_CYC_END); // 开启中断
PFIC_EnableIRQ(TMR1_IRQn);
}
void gpio_uart_init(void)
{
/*--- RS485 通信 (PA14=TX, PA15=RX, PA13=RE) ---*/
/* 协议待定, 仅初始化引脚, 暂不配置 UART0 */
GPIOA_SetBits(GPIO_Pin_14);
GPIOA_ModeCfg(GPIO_Pin_14, GPIO_ModeOut_PP_5mA); // RS485 TX
GPIOA_ModeCfg(GPIO_Pin_15, GPIO_ModeIN_PU); // RS485 RX
GPIOA_SetBits(GPIO_Pin_13);
GPIOA_ModeCfg(GPIO_Pin_13, GPIO_ModeOut_PP_5mA); // RS485 RE (默认接收模式)
/*--- 光耦输入 (PB10=IN_1) ---*/
/* 逻辑待定, 仅初始化引脚为输入上拉 */
GPIOB_ModeCfg(GPIO_Pin_10, GPIO_ModeIN_PU);
/*--- 线圈调频 (PA12=FLEVEL, 一级) ---*/
GPIOA_SetBits(GPIO_Pin_12);
GPIOA_ModeCfg(GPIO_Pin_12, GPIO_ModeOut_PP_5mA);
FLEVEL_LOW; // 默认接入 33nF
/*--- 继电器 (PA8=RLY1) ---*/
GPIOA_SetBits(GPIO_Pin_8);
GPIOA_ModeCfg(GPIO_Pin_8, GPIO_ModeOut_PP_5mA);
RLY1_OFF;
/*--- LEDA 绿色 (PB22, GPIO, 低电平有效) ---*/
GPIOB_SetBits(GPIO_Pin_22);
GPIOB_ModeCfg(GPIO_Pin_22, GPIO_ModeOut_PP_5mA);
LEDA_OFF;
/*--- LEDB 红色 (PB23, PWM11, 呼吸灯) ---*/
pwm_led_init();
}
/*===========================================================================
* pwm_led_init -- 初始化 LEDB (PB23) 红色呼吸灯 (PWM11)
* PWM 频率 ≈ 60MHz / (80 * 128) ≈ 5.86kHz
* 初始占空比 = 0 (LED 灭)
* 注: PB23 需在 ISP 烧录时关闭外部复位功能 (RB_CFG_RESET_EN=0)
*===========================================================================*/
static void pwm_led_init(void)
{
GPIOB_SetBits(GPIO_Pin_23);
GPIOB_ModeCfg(GPIO_Pin_23, GPIO_ModeOut_PP_5mA);
PWMX_CLKCfg(PWM_CLK_DIV);
PWMX_CycleCfg(PWMX_Cycle_128); // 128 级占空比分辨率
PWMX_ACTOUT(CH_PWM11, 0, Low_Level, ENABLE); // 初始 duty=0, LED 灭
}
/*===========================================================================
* 一阶 IIR 低通滤波器 (对齐 M1H 公式)
* CAPVD_new = a * new_value + (1-a) * CAPVD_old, a = 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 * (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;
}
/*===========================================================================
* 滑动平均基线更新
* 注意: window 必须是 uint16_t (WINDOW_ORIGIN=500 > 255, uint8_t 会静默截断)
*===========================================================================*/
void 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;
}
*p_sum += new_value;
(*p_cnt)++;
if (*p_cnt >= window) {
*p_origin = (uint32_t)(*p_sum / window);
*p_cnt = 0;
*p_sum = 0;
}
}
/*===========================================================================
* 初始化所有检测状态 (上电 / 有限存在超时 / 线圈重连 时调用)
* 注意: 不复位 g_loop_power_up_state / g_loop_cut_amount (BLE 上报用)
*===========================================================================*/
void INIT_VD(void)
{
loop1_INI_LOOP = 1;
loop1_LPCNT = 0;
loop1_INCNT = 0;
loop1_OUTCNT = 0;
loop1_CapCnt = 0;
loop1_CapSum = 0;
loop1_Value = 0;
loop1_CAPVD = 0;
loop1_Origin = 0;
loop1_VD_FLAG = 0;
loop1_RF_FLAG = 0;
loop1_LOOP_OK = 1;
loop1_LOOP_OK0 = 0;
loop1_CAP_OK = 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_FLAG_PLUSE_IN_F = 0;
loop1_FLAG_PLUSE_IN = 0;
loop1_FLAG_CUT = 0;
loop1_FLAG_HOLD_TIMEOUT = 0;
g_disconnect_active = 0;
loop1_VD_HOLD = 0;
Hold_CNT = 0;
loop1_LC_HOLD = 0;
loop1_LC_Reset = 0;
LC_Hold_CNT = 0;
/* 新算法状态 */
g_loop_stable = 0;
loop1_stable_cnt = 0;
loop1_entry_cnt = 0;
loop1_cnt_release = 0;
loop1_freeze_cnt = 0;
loop1_freeze_ref = 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
g_env_activity = 0;
g_env_prev_capvd = 0;
g_loop_acs_info.car_state = 0;
}
/*===========================================================================
* 车辆离开 / 释放 公共处理
*===========================================================================*/
static void loop_release_common(void)
{
loop1_VD_FLAG = 0;
loop1_FLAG_OUT = 1;
loop1_VD_HOLD = 0;
loop1_LC_HOLD = 0;
Hold_CNT = 0;
LC_Hold_CNT = 0;
loop1_ORG_CNT = 0;
loop1_ORG_SUM = 0;
g_loop_acs_info.flag_change = 1;
g_loop_acs_info.flag_event = 1;
g_loop_acs_info.car_state = 0;
}
/*===========================================================================
* vd1_task -- 核心检测算法 (移植 DLD154V4B, 每次 CAP_OK 调用, ~8.74ms)
*
* 流程:
* 1. IIR 滤波 + 斜率限幅
* 2. 稳定期: 小窗口快速收敛基线
* 3. 无车: 基线跟踪 (冻结保护) + 进入确认
* 4. 有车: 平坦性三条件离开判定 (带滞回)
*===========================================================================*/
void vd1_task(void){
if (loop1_Origin == 0) return;
/*--- 1. IIR 滤波 + 斜率限幅 ---*/
if (loop1_CAPVD == 0) {
loop1_CAPVD = loop1_Value;
} else {
int32_t raw_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD;
int32_t max_step = (int32_t)(loop1_CAPVD * MAX_SLOPE_RATE / 100);
uint32_t clamped_value;
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;
clamped_value = (uint32_t)((int32_t)loop1_CAPVD + raw_delta);
loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD);
}
/*--- 2. 稳定期: 绕过 IIR/斜率限幅, 小窗口快速收敛 ---*/
if (!g_loop_stable) {
loop1_CAPVD = loop1_Value;
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, 100);
loop1_stable_cnt++;
if (loop1_stable_cnt >= STABLE_SAMPLES) {
loop1_stable_cnt = 0;
g_loop_stable = 1;
}
return;
}
/*--- 3. BLE 上报状态刷新 (flag_change 置位期间冻结, 保持事件时刻的值) ---*/
if (g_loop_acs_info.flag_change == 0) {
int32_t _var = (int32_t)loop1_Origin - (int32_t)loop1_CAPVD;
g_loop_acs_info.loop_capvd = loop1_CAPVD;
g_loop_acs_info.variation = _var;
if (g_flag_bt_state && (_var > VARIATION_EVENT_TH || _var < -VARIATION_EVENT_TH)) {
g_loop_acs_info.flag_event = 1;
}
}
/*--- 4. 环境活跃度 (BLE condition 字段, 0~15) ---*/
{
int32_t d = (int32_t)loop1_CAPVD - (int32_t)g_env_prev_capvd;
if (d < 0) d = -d;
if (g_env_prev_capvd != 0 && d > ENV_ACT_TH) {
if (g_env_activity < 200) g_env_activity++;
} else {
if (g_env_activity) g_env_activity--;
}
g_env_prev_capvd = loop1_CAPVD;
}
if (!loop1_VD_FLAG) {
/*================================================================
* 无车状态: 基线跟踪 (冻结保护) + 进入确认
*================================================================*/
loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_in) >> 16;
{
int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin;
if (dev < (int32_t)(loop1_dlt_ORG * 4)) {
loop1_freeze_cnt = 0;
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, WINDOW_ORIGIN);
} else {
/*--- CAPVD 异常偏高: 暂停跟踪, 冻结超时 + 稳定性双条件 ---*/
if (loop1_freeze_cnt == 0) {
loop1_freeze_ref = loop1_CAPVD;
} else {
int32_t drift = (int32_t)loop1_CAPVD - (int32_t)loop1_freeze_ref;
if (drift < 0) drift = -drift;
if (drift > (int32_t)(loop1_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
loop1_freeze_cnt = 0;
loop1_freeze_ref = loop1_CAPVD;
}
}
loop1_freeze_cnt++;
if (loop1_freeze_cnt >= FREEZE_TIMEOUT) {
loop1_Origin = loop1_CAPVD; /* 确认稳定的新常态 */
loop1_freeze_cnt = 0;
loop1_freeze_ref = 0;
}
loop1_ORG_CNT = 0;
loop1_ORG_SUM = 0;
}
}
/*--- 进入确认: 连续 ENTRY_CONFIRM 次低于阈值才判有车 ---*/
if (loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) {
loop1_entry_cnt++;
if (loop1_entry_cnt >= ENTRY_CONFIRM) {
loop1_entry_cnt = 0;
loop1_freeze_cnt = 0;
loop1_freeze_ref = 0;
loop1_VD_FLAG = 1;
loop1_FLAG_IN = 1;
if (g_loop_cng_unit.exist_mode) { /* 有限存在 */
loop1_VD_HOLD = 1;
Hold_CNT = 0;
}
if (g_loop_cng_unit.loopSafe_Timeout) { /* 线圈安全模式: 启动安全复位计时 */
loop1_LC_HOLD = 1;
LC_Hold_CNT = 0;
} else {
loop1_LC_HOLD = 0;
}
if (loop1_LC_Reset) {
loop1_LC_Reset = 0;
}
loop1_ORG_CNT = 0;
loop1_ORG_SUM = 0;
g_loop_acs_info.flag_change = 1;
g_loop_acs_info.flag_event = 1;
g_loop_acs_info.car_state = 1;
#if USE_FLATNESS_EXIT
/* 重置平坦性状态 (专利 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;
g_prev_capvd = (int32_t)loop1_CAPVD;
g_prev_first_deriv = 0;
#endif
}
} else {
if (loop1_entry_cnt > 0) loop1_entry_cnt = 0;
}
}
else {
#if USE_FLATNESS_EXIT
/*================================================================
* 有车状态: 平坦性三条件判定离开 (专利 CN200910309382)
* 幅值阈值按新测量基准 2^19 缩放 (DLD154V4B: 100/5/2 @2^17)
*================================================================*/
#define K1 8
#define K2 8
#define SLOPE_FLAT_THRESH 350
#define MIN_DELTA2 18
#define MIN_DELTA3 7
#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) {
/*--- 阶段0: 追踪第一上升坡面, 自适应计算平坦阈值 ---*/
if (abs_fd > (int32_t)g_max_slope) g_max_slope = (uint16_t)abs_fd;
if (abs_sd > (int32_t)g_max_slope_rate) g_max_slope_rate = (uint16_t)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 {
/*--- 阶段1: 幅值回归 + 一阶平坦 + 二阶平坦 三条件确认离开 ---*/
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 * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 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) {
g_flat_ok_cnt = 0;
g_exit_state = 0;
loop_release_common();
}
} else {
g_flat_ok_cnt = 0;
}
}
g_prev_capvd = (int32_t)loop1_CAPVD;
g_prev_first_deriv = first_deriv;
#else
/*================================================================
* 有车状态: 简单滞回 + 防抖离开
*================================================================*/
loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;
if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD) {
loop1_cnt_release++;
if (loop1_cnt_release >= 3) {
loop1_cnt_release = 0;
loop_release_common();
}
} else {
if (loop1_cnt_release > 0) loop1_cnt_release = 0;
}
#endif
}
}
/*===========================================================================
* cjq_srv -- 车检器主循环服务
* - 有限存在超时 / 线圈断开重连: 全复位重建基线
* (线圈仍被占用时, 新基线落在占用值上, 继电器保持释放 -- 对齐 M1H 行为)
* - CAP_OK 到达时执行核心检测算法
*===========================================================================*/
void cjq_srv(void){
if(loop1_LOOP_OK){
if(loop1_FLAG_HOLD_TIMEOUT){
/* 有限存在超时: 全复位, 重新经历稳定期重建基线 */
INIT_VD();
return;
}
if(loop1_FLAG_CUT){
/* 线圈断开后重连: 不丢 VD_FLAG, CAPVD=0 直锁首个样本快速收敛 */
g_disconnect_active = 0;
loop1_FLAG_CUT = 0;
loop1_CAPVD = 0; // 首个样本直锁, 跳过 IIR
loop1_ORG_CNT = 0; // 丢弃断开期间过期数据
loop1_ORG_SUM = 0;
if(loop1_Origin == 0) {
/* Origin 意外丢失: 全复位 (不应发生, 做防御) */
INIT_VD();
}
return;
}
if(!loop1_LOOP_OK0){
DelayMs(200); /* 线圈重新起振后等待 200ms 再消费数据 */
loop1_LOOP_OK0 = 1;
}
if(loop1_CAP_OK){
loop1_CAP_OK = 0;
if(!g_loop_power_up_state){
g_loop_power_up_state = 1; // 表示连接了线圈
}
vd1_task();
}
}// end if loop1_LOOP_OK
else{
loop1_FLAG_CUT = 1;
g_disconnect_active = 1;
// 不丢 VD_FLAG (保留有车状态, 重连后恢复)
loop1_FLAG_IN = 0;
loop1_FLAG_OUT = 0;
loop1_FLAG_PLUSE = 0;
loop1_FLAG_PLUSE_IN_F = 0;
loop1_FLAG_PLUSE_IN = 0;
// 线圈断开: 继电器释放
RLY1_OFF;
g_loop_acs_info.relay1_state = 0;
g_loop_acs_info.car_state = 0;
if(g_loop_cut_last != loop1_FLAG_CUT){
g_loop_cut_last = loop1_FLAG_CUT;
if(g_loop_power_up_state){
g_loop_cut_amount++;
if(g_loop_cut_amount > 3){
g_loop_cut_amount = 3;
}
}
}
}
}
/*********************************************************************
* @fn Main_Circulation
*
* @brief 主循环
*
* @return none
*/
__HIGH_CODE
__attribute__((noinline))
void Main_Circulation()
{
while(1)
{
TMOS_SystemProcess();
poll_dbn_ble();
cjq_srv();
if(g_flag_bt_state == 0){
// 如果蓝牙断开，主动上报需要禁用
if(g_dbn_ble_state_acs_enable.enable){
g_dbn_ble_state_acs_enable.enable = 0;
}
}
}
}
/*********************************************************************
* @fn main
*
* @brief 主函数
*
* @return none
*/
int main(void)
{
uint8_t i;
#if(defined(DCDC_ENABLE)) && (DCDC_ENABLE == TRUE)
PWR_DCDCCfg(ENABLE);
#endif
SetSysClock(CLK_SOURCE_PLL_60MHz);
#if(defined(HAL_SLEEP)) && (HAL_SLEEP == TRUE)
GPIOA_ModeCfg(GPIO_Pin_All, GPIO_ModeIN_PU);
GPIOB_ModeCfg(GPIO_Pin_All, GPIO_ModeIN_PU);
#endif
gpio_uart_init();
g_sys_freq = FREQ_SYS;
GetMACAddress(MACAddr);
for(i = 0; i < 6; i++)
{
g_dev_number[i] = MACAddr[i];
}
g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT; // max
INIT_VD();
para_store_init();
tm_init();
CH59x_BLEInit();
HAL_Init();
GAPRole_PeripheralInit();
Peripheral_Init();
Main_Circulation();
}
/* @fn TMR0_IRQHandler
*
* @brief TMR0 输入捕获中断 -- 线圈频率测量
* FallEdge_To_FallEdge 模式, CAPGetData 直接给出相邻下降沿周期 Xn
* 累加 LPCNT 次 -> 一个测量窗 Value (~= MEASUREMENT_BASE)
*
* @return none
*/
__INTERRUPT
__HIGH_CODE
void TMR0_IRQHandler(void) // TMR0 捕获中断
{
if(TMR0_GetITFlag(TMR0_3_IT_DATA_ACT)){
loop1_Xn = (uint16_t)TMR0_CAPGetData();
if(loop1_Xn == 0){
R8_TMR0_INT_FLAG |= 0xff;
return;
}
if(!loop1_RF_FLAG){
loop1_RF_FLAG = 1;
}
else{
loop1_CapCnt++;
if(loop1_INI_LOOP){
/*--- 初始化阶段 ---*/
if(loop1_LPCNT == 0){
if(loop1_CapCnt > 200){ /* 上电/换频后等待振荡稳定 */
uint32_t lpcnt = (uint32_t)MEASUREMENT_BASE / loop1_Xn;
if(lpcnt == 0 || lpcnt > 65535){
lpcnt = 100; /* 异常 Xn 保护 */
}
loop1_LPCNT = (uint16_t)lpcnt;
g_freq_tmp = (float)g_sys_freq/((float)loop1_Xn);
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;
if(g_loop_acs_info.flag_change == 0){
g_loop_acs_info.loop_capvd = loop1_CAPVD;
}
loop1_INI_LOOP = 0;
loop1_CapSum = 0;
loop1_Value = 0;
loop1_CapCnt = 0;
}
}
}// end if loop1_INI_LOOP
else {
/*--- 正常运行: 累加到测量窗 ---*/
loop1_CapSum += loop1_Xn;
if(loop1_CapCnt >= loop1_LPCNT){
loop1_Value = loop1_CapSum;
loop1_CAP_OK = 1;
loop1_CapSum = 0;
loop1_CapCnt = 0;
}
}
}
R8_TMR0_INT_FLAG |= 0xff;
}
}
/*********************************************************************
* @fn TMR1_IRQHandler
*
* @brief TMR1 定时中断 5ms/per -- 时序状态机 / 继电器输出 / LED
*
* @return none
*/
__INTERRUPT
__HIGH_CODE
void TMR1_IRQHandler(void)
{
static uint16_t _counter1_init = 0;
if(TMR1_GetITFlag(TMR0_3_IT_CYC_END))
{
if(g_dbn_ble_state_acs_enable.enable && g_flag_bt_state){
g_loop_acs_info.report_counter++;
}
TimingDelayInc++;
u32CountWdt++;
/*--- LED: 断开中 200ms 快闪; 稳定期 200ms 慢闪 ---*/
if(g_disconnect_active || !g_loop_power_up_state)
{
/* 线圈断开中 / 上电从未连接 → 200ms 快闪 (故障指示) */
_counter1_init++;
if(_counter1_init >= 40) // 40: 200ms
{
_counter1_init = 0;
LEDA_ON_OFF();
}
}
else if(!g_loop_stable)
{
/* 稳定期 → 200ms 慢闪 (自检中) */
_counter1_init++;
if(_counter1_init >= 40) // 40: 200ms
{
_counter1_init = 0;
LEDA_ON_OFF();
}
}
TM1cnt++;
if(TM1cnt >= 10) //about=50ms
{
TM1cnt = 0;
if(loop1_FLAG_IN) // 线圈1 从无车到有车
{
loop1_FLAG_OUT = 0;
loop1_OUTCNT = 0;
loop1_FLAG_IN = 0;
if(g_loop_cng_unit.output_mode == 1) // 进入脉冲
{
loop1_FLAG_PLUSE_IN_F = 1;
}
loop1_INCNT = 0;
LEDA_ON;
}
if(loop1_FLAG_PLUSE_IN_F)
{
loop1_INCNT++;
if(loop1_INCNT > g_loop_out_delay)
{
loop1_INCNT = 0;
loop1_FLAG_PLUSE_IN_F = 0;
loop1_FLAG_PLUSE_IN = 1;
}
}
if(loop1_FLAG_PLUSE_IN)
{
loop1_INCNT++;
if(loop1_INCNT > IN_DELAY) // 500ms
{
loop1_INCNT = 0;
loop1_FLAG_PLUSE_IN = 0;
}
}
if(loop1_FLAG_OUT) // 从有车到无车的标志
{
loop1_OUTCNT++;
if(loop1_OUTCNT > g_loop_out_delay)
{
loop1_FLAG_OUT = 0;
loop1_FLAG_PLUSE = 1;
loop1_OUTCNT = 0;
LEDA_OFF;
}
}
if(loop1_FLAG_PLUSE) // Give 500ms
{
loop1_OUTCNT++;
if(loop1_OUTCNT > PLUSE_DELAY)
{
loop1_FLAG_PLUSE = 0;
loop1_OUTCNT = 0;
}
}
if(loop1_VD_HOLD)
{
Hold_CNT++;
if(Hold_CNT > g_hold_time) // 超过有限存在时间
{
loop1_VD_HOLD = 0;
Hold_CNT = 0;
if(loop1_VD_FLAG)
{
/* 有限存在超时: 释放继电器, 主循环全复位重建基线 */
loop1_FLAG_HOLD_TIMEOUT = 1;
RLY1_OFF;
g_loop_acs_info.relay1_state = 0;
}
}
}
if(loop1_LC_HOLD)
{
LC_Hold_CNT++;
if(LC_Hold_CNT > g_safe_max_cnt)
{
/* 安全复位: 重新初始化测量, 重建基线 (对齐 M1H) */
loop1_LC_Reset = 1;
loop1_INI_LOOP = 1;
loop1_LOOP_OK0 = 0;
g_loop_stable = 0;
loop1_stable_cnt = 0;
LC_Hold_CNT = 0;
loop1_ORG_CNT = 0;
loop1_ORG_SUM = 0;
#if USE_FLATNESS_EXIT
g_exit_state = 0;
g_max_slope = 0;
g_max_slope_rate = 0;
g_slope_flat_cnt = 0;
g_flat_ok_cnt = 0;
#endif
}
}
/*--- LEDB 红色呼吸灯 (PWM11, 50ms tick) ---*/
g_breath_tick++;
switch (g_breath_phase) {
case BREATH_FADE_IN: // 渐亮 0 → max, 每300ms一步
if (g_breath_tick >= BREATH_STEP_DELAY) {
g_breath_tick = 0;
g_breath_duty++;
PWM11_ActDataWidth(g_breath_duty);
if (g_breath_duty >= PWM_CYCLE - 1) {
g_breath_phase = BREATH_HOLD_HIGH;
g_breath_tick = 0;
}
}
break;
case BREATH_HOLD_HIGH: // 最亮保持 ~0.5s
if (g_breath_tick >= 10) {
g_breath_phase = BREATH_FADE_OUT;
g_breath_tick = 0;
}
break;
case BREATH_FADE_OUT: // 渐灭 max → 0, 每300ms一步
if (g_breath_tick >= BREATH_STEP_DELAY) {
g_breath_tick = 0;
g_breath_duty--;
PWM11_ActDataWidth(g_breath_duty);
if (g_breath_duty == 0) {
g_breath_phase = BREATH_HOLD_LOW;
g_breath_tick = 0;
}
}
break;
case BREATH_HOLD_LOW: // 最暗保持 ~0.3s
if (g_breath_tick >= 6) {
g_breath_phase = BREATH_FADE_IN;
g_breath_tick = 0;
}
break;
}
}
/*--- 线圈载波检测 & 继电器输出刷新 (每 5ms) ---*/
if(loop1_RF_FLAG)
{
loop1_LOOP_OK = 1;
loop1_RF_FLAG = 0;
if(loop1_LC_Reset == 0)
{
switch(g_loop_cng_unit.output_mode)
{ // 输出方式
case 0: // 存在输出
{
if(loop1_VD_FLAG | loop1_FLAG_OUT)// 输出存在信号
{
RLY1_ON;
g_loop_acs_info.relay1_state = 1;
}
else
{
RLY1_OFF;
g_loop_acs_info.relay1_state = 0;
}
} break;
case 1: // 进入脉冲
{
if(loop1_FLAG_PLUSE_IN)
{
RLY1_ON;
g_loop_acs_info.relay1_state = 1;
}
else
{
RLY1_OFF;
g_loop_acs_info.relay1_state = 0;
}
} break;
case 2: // 离开脉冲
{
if(loop1_FLAG_PLUSE) // 输出离开脉冲
{
RLY1_ON;
g_loop_acs_info.relay1_state = 1;
}
else
{
RLY1_OFF;
g_loop_acs_info.relay1_state = 0;
}
} break;
default: {
// DLD154Pro 单继电器, 默认存在模式
if(loop1_VD_FLAG)
{
RLY1_ON;
g_loop_acs_info.relay1_state = 1;
}
else
{
RLY1_OFF;
g_loop_acs_info.relay1_state = 0;
}
} break;
}
}// end if loop1_LC_Reset == 0
} // end if loop1_RF_FLAG
else
{
loop1_LOOP_OK0 = loop1_LOOP_OK;
loop1_LOOP_OK = 0;
}
TMR1_ClearITFlag(TMR0_3_IT_CYC_END); // 清除中断标志
}
}
/******************************** endfile @ main ******************************/
Reference in New Issue View Git Blame Copy Permalink