1952 lines
65 KiB
C
1952 lines
65 KiB
C
/********************************** (C) COPYRIGHT *******************************
|
||
* File Name : main.c
|
||
* Author : WCH
|
||
* Version : V1.1
|
||
* Date : 2020/08/06
|
||
* Description : 外设从机应用主函数及任务系统初始化
|
||
*********************************************************************************
|
||
* 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"
|
||
|
||
|
||
|
||
uint8_t g_flag_bt_state = 0; // 0 蓝牙没有连接，1 有蓝牙连接
|
||
uint32_t g_counter_bt_timeout = 0;
|
||
uint8_t g_flag_output = 0;
|
||
uint32_t g_xn_counter = 0;
|
||
uint8_t g_function_mode = 0;
|
||
|
||
#define RLY1_ON (R32_PA_OUT |= BV(8)) //(R32_PB_OUT &= ~BV(22))
|
||
#define RLY1_OFF (R32_PA_OUT &= ~BV(8)) //(R32_PB_OUT |= BV(22))
|
||
#define RLY2_ON (R32_PB_OUT |= BV(23))
|
||
#define RLY2_OFF (R32_PB_OUT &= ~BV(23))
|
||
#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 FLEVEL_H_LOW (R32_PA_OUT &= ~BV(12))
|
||
#define FLEVEL_H_HIGH (R32_PA_OUT |= BV(12))
|
||
#define FLEVEL_L_LOW (R32_PA_OUT &= ~BV(13))
|
||
#define FLEVEL_L_HIGH (R32_PA_OUT |= BV(13))
|
||
|
||
#define KEY_SA1 (GPIOA_ReadPortPin(GPIO_Pin_14))
|
||
#define KEY_SA2 (GPIOA_ReadPortPin(GPIO_Pin_15))
|
||
|
||
const uint8_t MAX_Store_Size = 80;
|
||
uint8_t g_loop_power_up_state = 0; // 开机启动时线圈是否连接
|
||
__IO uint32_t TimingDelayInc;
|
||
|
||
|
||
#define MAX_CMP_LOOP_UNIT 4 // 5 组
|
||
#define MAX_CMP_DIF 20 //
|
||
#define MAX_TIMEOUT_CMP 120000 // 5毫秒 为单位， 表示10分钟， 600秒
|
||
|
||
#define ALFA_CAP1 79
|
||
#define ALFA_CAP2 64
|
||
#define ALFA_ORG1 64
|
||
#define ALFA_ORG2 64
|
||
|
||
uint32_t mstick(void){
|
||
return TimingDelayInc;
|
||
}
|
||
|
||
uint32_t u32CountWdt = 0;
|
||
uint8_t g_loop_cut_last = 0;
|
||
uint32_t g_loop_cut_amount = 0;
|
||
|
||
uint8_t g_flag_stable_delta2_flt = 0;
|
||
uint8_t g_flag_reset_delta = 0;
|
||
|
||
// 继电器状态防抖计数器（多重确认机制）
|
||
static uint8_t relay_on_confirm_cnt = 0; // 吸合确认计数
|
||
static uint8_t relay_off_confirm_cnt = 0; // 释放确认计数
|
||
#define RELAY_ON_CONFIRM_THRESHOLD 3 // 吸合需要连续3次确认
|
||
#define RELAY_OFF_CONFIRM_THRESHOLD 5 // 释放需要连续5次确认（更严格）
|
||
|
||
// 快速变化检测（用于识别线圈中间移动）
|
||
static int32_t last_delta_for_rapid = 0; // 上一次的一阶变化量
|
||
static uint8_t rapid_change_detected = 0; // 检测到快速变化标志
|
||
static uint16_t rapid_change_duration = 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 Delay_300ms_cnt = 0;
|
||
uint8_t TM1cnt = 0;
|
||
uint16_t g_hold_time = 0;
|
||
|
||
uint16_t loop1_Xn;
|
||
uint16_t loop1_HoldTime;
|
||
uint16_t loop1_dlt_ORG;
|
||
uint32_t loop1_Origin;
|
||
uint16_t loop1_CapLast;
|
||
|
||
uint16_t loop1_LPCNT;
|
||
uint16_t loop1_CapCnt;
|
||
uint32_t loop1_CapSum;
|
||
uint32_t loop1_CAPVD;
|
||
uint32_t loop1_Value;
|
||
uint16_t loop1_Flt;
|
||
|
||
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_CAP_FLAG;
|
||
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_DELAY;
|
||
uint8_t loop1_FLAG_IN_PLUSE;
|
||
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;
|
||
|
||
//const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 37, 27}; //{162, 108, 54, 37, 27}; 9
|
||
//const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 27, 14}; //{108, 81, 32, 27, 14}; 6
|
||
const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 28}; //{162, 108, 54, 37, 27};
|
||
const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 16}; //{108, 81, 32, 27, 14};
|
||
// const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 162, 81, 54}; //{162, 108, 54, 37, 27};
|
||
// const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 54, 32}; //{108, 81, 32, 27, 14};
|
||
|
||
#define LOOP_WINDOW_SIZE_NORMAL 50 // 200
|
||
#define LOOP_WINDOW_SIZE_FAST 50 // 快速补偿
|
||
uint16_t g_moving_average_window_size = LOOP_WINDOW_SIZE_NORMAL; // 滑动窗口大小
|
||
|
||
uint32_t g_sys_freq = 0;
|
||
|
||
SecondOrderState second_order_state;
|
||
FltHistoryManager flt_mgr = {0}; // 初始化为0
|
||
StageRangeConfig stage_config[STAGE_COUNT] = {
|
||
{0, 0,0, 20, RANGE_PERCENT}, // 开机阶段：基准值±10%
|
||
{0, 0,0, 20, RANGE_PERCENT}//, //最近5秒
|
||
// {0, 0,0, 2, RANGE_PERCENT}, // 5分钟阶段：基准值±50
|
||
// {0, 0,0, 9, RANGE_PERCENT}, // 10分钟阶段：基准值±8%
|
||
// {0, 0,0, 12, RANGE_ABSOLUTE}, // 30分钟阶段：基准值±30
|
||
// {0, 0,0, 15, RANGE_PERCENT} // 1小时阶段：基准值±5%
|
||
};
|
||
|
||
uint16_t g_loop_out_delay = 10; // 0、0.6秒、1.6秒、3秒{0,12,32,60};
|
||
uint32_t g_loop_release_ori_timeout = MAX_TIMEOUT_CMP;
|
||
|
||
DBN_BLE_State g_dbn_ble_state_acs_enable = {0};
|
||
Loop_ACS_Info g_loop_acs_info = {0,};
|
||
|
||
#define MAX_ALLOW_DIFF_FREQ 3500 //频率最大偏移值 3500Hz
|
||
uint16_t g_MaxAllowDiff = 0; // 采样值允许最大偏移值
|
||
float g_freq_tmp = 0.0;
|
||
|
||
//uint8_t g_filter_step = 0;
|
||
//uint16_t g_loop_bad_cnt = 0; // 采样 数值异常计数
|
||
|
||
uint8_t loop1_FLAG_PLUSE_IN_F = 0;
|
||
uint8_t loop1_FLAG_PLUSE_IN = 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);
|
||
|
||
// g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT; // max 5
|
||
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){
|
||
// PRINT("Not enough memory!!");
|
||
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;
|
||
|
||
// g_freq_sens = 1;
|
||
i = 4;
|
||
memcpy(&rBuf[i], &g_loop_cng_unit.sensitvity, 7);
|
||
i += 7;
|
||
|
||
// i = Addr_Loop_PlanB_Cng_Offset;
|
||
// rBuf[i++] = g_loop_balance_planB.sample_cng.flag;
|
||
// rBuf[i++] = g_loop_balance_planB.sample_cng.max_amplitude;
|
||
// rBuf[i++] = g_loop_balance_planB.sample_cng.max_amplitude >> 8;
|
||
// rBuf[i++] = g_loop_balance_planB.balance_ori_cng.flag;
|
||
// rBuf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt;
|
||
// rBuf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt >> 8;
|
||
// rBuf[i++] = g_loop_balance_planB.release_ori_planB.flag_weight;
|
||
// rBuf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude;
|
||
// rBuf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude >> 8;
|
||
// rBuf[i++] = g_loop_balance_planB.release_ori_planB.timeout;
|
||
//
|
||
// rBuf[i++] = g_loop_balance_planB.release_change_rate.flag_weight;
|
||
// rBuf[i++] = g_loop_balance_planB.release_change_rate.rate_first;
|
||
// rBuf[i++] = g_loop_balance_planB.release_change_rate.rate_second;
|
||
// rBuf[i++] = g_loop_balance_planB.release_change_rate.mode;
|
||
|
||
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 poll_sens_key(void)
|
||
{
|
||
static uint8_t _last_sens = 3;
|
||
|
||
#if DEBUG
|
||
g_freq_sens = 2; // 默认最高灵敏度
|
||
#else
|
||
if(KEY_SA1){
|
||
if(KEY_SA2){
|
||
g_freq_sens = 0;
|
||
}
|
||
else {
|
||
g_freq_sens = 2;
|
||
}
|
||
}
|
||
else{
|
||
if (KEY_SA2) {
|
||
g_freq_sens = 1;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
if(_last_sens == 3){
|
||
_last_sens = g_freq_sens;
|
||
}
|
||
|
||
if(_last_sens != g_freq_sens){
|
||
//TODO: reboot?
|
||
_last_sens = g_freq_sens;
|
||
reload_lower_upper(g_freq_sens);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
void para_store_init(void)
|
||
{
|
||
uint8_t i;
|
||
uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size);
|
||
|
||
if(rBuf == NULL){
|
||
// PRINT("Not enough memory!!");
|
||
SYS_ResetExecute();
|
||
}
|
||
memset(rBuf, 0, MAX_Store_Size);
|
||
|
||
read_cfg_from_store(0, rBuf, MAX_Store_Size);
|
||
// PRINT("Read_Cng_Store:\n");
|
||
// for(i = 0; i < MAX_Store_Size; i++){
|
||
// PRINT("%02X ", rBuf[i]);
|
||
// }
|
||
// PRINT("\n");
|
||
|
||
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_balance_planB, &rBuf[Addr_Loop_PlanB_Cng_Offset], sizeof(g_loop_balance_planB));
|
||
memcpy(&g_loop_sens_list, &rBuf[Addr_Loop_Sens_List_Offset], sizeof(g_loop_sens_list));
|
||
// g_freq_sens = g_loop_cng_unit.sensitvity & 0x0F;
|
||
// if(g_freq_sens > g_loop_sens_list.total ){
|
||
// g_freq_sens = g_loop_sens_list.total;
|
||
// }
|
||
// if(g_freq_sens){
|
||
// g_freq_sens--;
|
||
// }
|
||
poll_sens_key();
|
||
|
||
g_loop_out_delay = g_loop_cng_unit.delay_time * 2; // 定时器以 50ms 为单位， 延时以100ms为单位
|
||
// g_loop_release_ori_timeout = g_loop_balance_planB.release_ori_planB.timeout * 60 * 1000 / 5;
|
||
g_hold_time = g_loop_cng_unit.exist_mode * 30 * 20; //50ms/per, 30s
|
||
// g_hold_time = g_loop_cng_unit.exist_mode * 100; //50ms/per, 5s
|
||
// if(g_hold_time < 400){
|
||
// g_hold_time = 400;
|
||
// }
|
||
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;
|
||
|
||
flt_mgr.max_cnt_flat_release = g_loop_cng_unit.delay_time * 20 /2;
|
||
if(flt_mgr.max_cnt_flat_release < 40){ // 至少检查400毫秒
|
||
flt_mgr.max_cnt_flat_release = 40;
|
||
}
|
||
|
||
#ifdef DEBUG
|
||
g_freq_level = 0;
|
||
#endif
|
||
|
||
switch(g_freq_level)
|
||
{
|
||
case 0: {
|
||
// 高频
|
||
FLEVEL_H_LOW; FLEVEL_L_LOW;
|
||
} break;
|
||
case 1: {
|
||
// 中高
|
||
FLEVEL_H_LOW; FLEVEL_L_HIGH;
|
||
} break;
|
||
case 2: {
|
||
// 中低
|
||
FLEVEL_H_HIGH; FLEVEL_L_LOW;
|
||
} break;
|
||
case 3: {
|
||
// 低频
|
||
FLEVEL_H_HIGH; FLEVEL_L_HIGH;
|
||
} break;
|
||
default:{
|
||
// 默认低频
|
||
g_freq_level = 3;
|
||
FLEVEL_H_HIGH; FLEVEL_L_HIGH;
|
||
} break;
|
||
}
|
||
free(rBuf);
|
||
}
|
||
else{
|
||
free(rBuf);
|
||
set_factory_param();
|
||
// DelayMs(10);
|
||
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)
|
||
{
|
||
/* 配置串口0：先配置IO口模式，再配置串口 */
|
||
// GPIOPinRemap(ENABLE, RB_PIN_UART0);
|
||
#ifdef DEBUG
|
||
GPIOPinRemap(ENABLE, RB_PIN_UART0);
|
||
GPIOA_SetBits(GPIO_Pin_14);
|
||
GPIOA_ModeCfg(GPIO_Pin_14, GPIO_ModeOut_PP_5mA); // TXD-配置推挽输出，注意先让IO口输出高电平
|
||
// UART0_DefInit();
|
||
|
||
UART0_BaudRateCfg(230400); //115200
|
||
R8_UART0_FCR = (2 << 6) | RB_FCR_TX_FIFO_CLR | RB_FCR_RX_FIFO_CLR | RB_FCR_FIFO_EN; // FIFO打开，触发点4字节
|
||
R8_UART0_LCR = RB_LCR_WORD_SZ;
|
||
R8_UART0_IER = RB_IER_TXD_EN;
|
||
R8_UART0_DIV = 1;
|
||
|
||
GPIOA_ModeCfg(GPIO_Pin_15, GPIO_ModeIN_PU); // 拨动开关，调节灵敏度
|
||
#else
|
||
|
||
GPIOA_ModeCfg(GPIO_Pin_15|GPIO_Pin_14, GPIO_ModeIN_PU); // 拨动开关，调节灵敏度
|
||
|
||
#endif
|
||
|
||
|
||
GPIOA_SetBits(GPIO_Pin_12|GPIO_Pin_13);
|
||
GPIOA_ModeCfg(GPIO_Pin_12|GPIO_Pin_13, GPIO_ModeOut_PP_5mA);
|
||
FLEVEL_H_LOW; FLEVEL_L_LOW;
|
||
|
||
GPIOA_SetBits(GPIO_Pin_8);
|
||
GPIOA_ModeCfg(GPIO_Pin_8, GPIO_ModeOut_PP_5mA);
|
||
GPIOB_SetBits(GPIO_Pin_22|GPIO_Pin_23);
|
||
GPIOB_ModeCfg(GPIO_Pin_22|GPIO_Pin_23, GPIO_ModeOut_PP_5mA);
|
||
RLY1_OFF; RLY2_OFF; LEDA_OFF;
|
||
|
||
#if 0 // 中断方式：接收数据后发送出去
|
||
UART0_ByteTrigCfg(UART_7BYTE_TRIG);
|
||
trigB = 7;
|
||
UART0_INTCfg(ENABLE, RB_IER_RECV_RDY | RB_IER_LINE_STAT);
|
||
PFIC_EnableIRQ(UART0_IRQn);
|
||
#endif
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
void init_second_order(SecondOrderState *state, int32_t initial_delta) {
|
||
state->delta_prev = initial_delta;
|
||
state->delta2_flt = 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算滤波后的二阶变化量（加速度）
|
||
* @param delta_current 当前一阶变化量（new_Flt - previous_new_Flt）
|
||
* @param state 二阶计算状态（需外部维护）
|
||
* @param filter_coef 二阶滤波系数（0~255，对应浮点系数 filter_coef/256）
|
||
* @return 滤波后的二阶变化量 delta2_flt
|
||
*/
|
||
int32_t get_second_order(int32_t delta_current, SecondOrderState *state, uint8_t filter_coef) {
|
||
// 计算当前二阶变化量（一阶变化的差分）
|
||
int32_t delta2 = delta_current - state->delta_prev;
|
||
|
||
// --- 关键改进点：处理一阶持续为0的情况 ---
|
||
if (delta_current == 0 && state->delta_prev == 0) {
|
||
// 连续两次一阶变化为0，强制二阶归零
|
||
state->delta2_flt = 0;
|
||
} else {
|
||
// 常规滤波：delta2_flt = delta2_flt + (delta2 - delta2_flt) * filter_coef / 256
|
||
state->delta2_flt += ((delta2 - state->delta2_flt) * filter_coef)/256; //((delta2 - state->delta2_flt) * (uint32_t)filter_coef) >> 8;
|
||
}
|
||
|
||
// 更新历史一阶变化量
|
||
state->delta_prev = delta_current;
|
||
|
||
return state->delta2_flt;
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @brief 更新滑动平均值（通过指针传递状态）
|
||
* @param p_sum 累加值指针（uint32_t*）
|
||
* @param p_cnt 计数器指针（uint8_t*）
|
||
* @param p_origin 平均值指针（uint16_t*）
|
||
* @param new_value 新输入值
|
||
* @param window 窗口大小（例如100）
|
||
*/
|
||
void 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; // 可添加错误处理
|
||
}
|
||
|
||
*p_sum += new_value; // 累加新值
|
||
(*p_cnt)++;
|
||
|
||
if (*p_cnt >= window) { // 窗口满（如window=100时，计数器0~99）
|
||
*p_origin = (uint32_t)(*p_sum / window); // 计算平均值
|
||
*p_cnt = 0;
|
||
*p_sum = 0;
|
||
// PRINT("window:%d, new_origin:%d\n", window, *p_origin);
|
||
}
|
||
}
|
||
|
||
|
||
void update_lower_upper(StageRangeConfig * cfg)
|
||
{
|
||
if(cfg->base_value == 0) return;
|
||
|
||
// 计算区间上下限
|
||
uint32_t lower, upper;
|
||
if (cfg->range_type == RANGE_ABSOLUTE) {
|
||
cfg->lower = (cfg->base_value >= cfg->range_size) ?
|
||
(cfg->base_value - cfg->range_size) : 0;
|
||
cfg->upper = cfg->base_value + cfg->range_size;
|
||
} else { // RANGE_PERCENT
|
||
// uint32_t offset = (cfg->base_value * cfg->range_size) >> 16;
|
||
uint32_t dlt = (cfg->base_value * (g_loop_sens_list.sens[g_freq_sens].sens_out - 3))>>16; // 采取比当前释放灵敏度更高的值，解决平坦算法释放的时候不断误触发的bug
|
||
cfg->lower = (cfg->base_value >= dlt) ?
|
||
(cfg->base_value - dlt) : 0;
|
||
cfg->upper = cfg->base_value + dlt;
|
||
}
|
||
}
|
||
|
||
void reload_lower_upper(uint8_t sens_level){
|
||
uint32_t dlt = 0 ;
|
||
// stage_config[0].lower =
|
||
uint8_t i;
|
||
for(i = 0; i < STAGE_COUNT; i++){
|
||
if(stage_config[i].base_value == 0){
|
||
continue;
|
||
}
|
||
dlt = (stage_config[i].base_value * (g_loop_sens_list.sens[sens_level].sens_out - 3))>>16;
|
||
stage_config[i].lower = (stage_config[i].base_value >= dlt) ? (stage_config[i].base_value - dlt): 0;
|
||
stage_config[i].upper = stage_config[i].base_value + dlt;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @brief 更新一阶滤波历史管理器
|
||
* @param new_flt 当前一阶滤波值
|
||
* @param current_time 系统时间戳（毫秒）
|
||
*/
|
||
void update_flt_history(uint32_t new_flt, int32_t firstOrder, int32_t secondOrder ) {
|
||
static uint8_t _first_cnt = 0;
|
||
if(firstOrder || secondOrder){
|
||
if(_first_cnt < 3){ //连续三次有变动
|
||
_first_cnt++;
|
||
}
|
||
|
||
flt_mgr.cnt_not_idle++;
|
||
if(flt_mgr.cnt_idle)
|
||
{
|
||
flt_mgr.cnt_idle--;
|
||
if(flt_mgr.cnt_not_idle > 5){
|
||
flt_mgr.cnt_idle = 0;
|
||
}
|
||
}
|
||
}
|
||
else {
|
||
if(_first_cnt){
|
||
_first_cnt--;
|
||
}
|
||
}
|
||
|
||
if((firstOrder || secondOrder) && (_first_cnt >= 3))
|
||
{
|
||
flt_mgr.counter_not_idle = 0;
|
||
flt_mgr.flag_not_idle = 1;
|
||
}
|
||
if(flt_mgr.flag_not_idle)
|
||
{
|
||
flt_mgr.cnt_sec_samples = 0;
|
||
flt_mgr.sum_sec_samples = 0;
|
||
if(!flt_mgr.is_initialized)
|
||
{
|
||
flt_mgr.sum_samples = 0;
|
||
flt_mgr.init_sum = 0;
|
||
}
|
||
else
|
||
{
|
||
flt_mgr.sum_samples = 0;
|
||
flt_mgr.cnt_samples = 0;
|
||
}
|
||
return ;
|
||
}
|
||
|
||
flt_mgr.cnt_idle++;
|
||
if(flt_mgr.cnt_not_idle){
|
||
flt_mgr.cnt_not_idle--;
|
||
if(flt_mgr.cnt_idle > 100){
|
||
flt_mgr.cnt_not_idle = 0;
|
||
}
|
||
}
|
||
|
||
// 1. 初始化阶段：累加采样值
|
||
if (!flt_mgr.is_initialized) {
|
||
flt_mgr.init_sum += new_flt;
|
||
flt_mgr.init_samples++;
|
||
|
||
// 达到采样次数后计算稳定初始值
|
||
if (flt_mgr.init_samples >= INIT_SAMPLE_COUNT) {
|
||
flt_mgr.stable_init_value = flt_mgr.init_sum / INIT_SAMPLE_COUNT;
|
||
flt_mgr.is_initialized = 1;
|
||
LEDA_OFF;
|
||
stage_config[0].base_value = flt_mgr.stable_init_value;
|
||
update_lower_upper(&stage_config[0]);
|
||
flt_mgr.init_freq = (float)(g_sys_freq )/((float)(flt_mgr.stable_init_value << 5)/((float)(loop1_LPCNT))) + 360;
|
||
}
|
||
return ;
|
||
}
|
||
|
||
if(flt_mgr.flag_5sec)
|
||
{
|
||
flt_mgr.cnt_sec_samples++;
|
||
flt_mgr.sum_sec_samples += new_flt;
|
||
if(flt_mgr.cnt_sec_samples >= TMP_SAMPLE_COUNT)
|
||
{
|
||
flt_mgr.recent_5sec = flt_mgr.sum_sec_samples/TMP_SAMPLE_COUNT;
|
||
|
||
stage_config[1].base_value = flt_mgr.recent_5sec;
|
||
update_lower_upper(&stage_config[1]);
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.cnt_sec_samples = 0;
|
||
flt_mgr.sum_sec_samples = 0;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 判断新值是否在指定阶段的区间内
|
||
* @param new_flt 新的一阶滤波值
|
||
* @param result 输出匹配结果
|
||
*/
|
||
uint8_t check_flt_by_stage(uint16_t new_flt) {
|
||
|
||
for (uint8_t i = 1; i < STAGE_COUNT_SHORT; i++) { //offset从1开始，表示开机的稳定参考值不参与释放判断
|
||
StageRangeConfig *cfg = &stage_config[i];
|
||
if (cfg->base_value == 0) continue; // 忽略未初始化的阶段
|
||
|
||
|
||
// 判断是否在区间内
|
||
if (new_flt >= cfg->lower && new_flt <= cfg->upper) {
|
||
return (i+1);
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
uint8_t check_flat_leave(FltHistoryManager* fltMgr, int16_t firstOrder, int16_t secondOrder, uint16_t datCurrent)
|
||
{
|
||
static uint8_t _flag = 0;
|
||
if(firstOrder || secondOrder)
|
||
{
|
||
// if(firstOrder > 0)
|
||
// {
|
||
// fltMgr->cnt_release_Up++;
|
||
// }
|
||
// else if(firstOrder < 0)
|
||
// {
|
||
// fltMgr->min_release_capvd = datCurrent;
|
||
// fltMgr->cnt_release_Down++;
|
||
// }
|
||
|
||
fltMgr->cnt_flat_release = 0;
|
||
fltMgr->flag_flat_release = 1;
|
||
|
||
return 0;
|
||
}
|
||
if(fltMgr->flag_flat_release)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
fltMgr->flag_second_judge = 1;
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 初始化二次判断的时间计数器
|
||
fltMgr->second_judge_enter_time = 0;
|
||
fltMgr->cnt_stable_to_idle = 0;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
void vd1_task(void){
|
||
static uint32_t new_Flt_prev = 0; // 保存上一次滤波值
|
||
int32_t delta_current = 0;
|
||
int32_t delta2_flt = 0;
|
||
|
||
if(g_loop_acs_info.flag_change == 0){
|
||
g_loop_acs_info.loop_capvd = loop1_Value; //记录 有车无车变化的参数
|
||
}
|
||
|
||
// if(g_function_mode & FUNCTION_B)
|
||
{
|
||
if(g_flag_reset_delta == 0){
|
||
new_Flt_prev = loop1_Origin;
|
||
loop1_CAPVD = loop1_Origin;
|
||
g_flag_reset_delta = 1;
|
||
}
|
||
}
|
||
|
||
loop1_CAPVD = get_flt_value(loop1_Value, loop1_CAPVD);
|
||
|
||
// 4. 一阶变化量计算（new_Flt - 前次值）
|
||
if(new_Flt_prev == 0)
|
||
{
|
||
new_Flt_prev = loop1_CAPVD;
|
||
}
|
||
delta_current = (int32_t)(loop1_CAPVD - new_Flt_prev);
|
||
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 检测快速变化：如果变化量符号频繁反转且幅度较大，说明是线圈中间移动
|
||
if(abs(delta_current) > 10 && abs(last_delta_for_rapid) > 10)
|
||
{
|
||
if((delta_current > 0 && last_delta_for_rapid < 0) ||
|
||
(delta_current < 0 && last_delta_for_rapid > 0))
|
||
{
|
||
rapid_change_detected = 1;
|
||
rapid_change_duration++;
|
||
}
|
||
else
|
||
{
|
||
// 变化方向一致，逐渐清除快速变化标志
|
||
if(rapid_change_duration > 0)
|
||
{
|
||
rapid_change_duration--;
|
||
}
|
||
if(rapid_change_duration == 0)
|
||
{
|
||
rapid_change_detected = 0;
|
||
}
|
||
}
|
||
}
|
||
last_delta_for_rapid = delta_current;
|
||
}
|
||
|
||
// if(abs(delta_current) > 30){
|
||
// PRINT("Xn_:%d, tmp_value:%d, loop1_Value:%d, last_prev:%d, loop1_CAPVD:%d, delta_current:%d, freq:%d, rapid:%d\n",loop1_Xn, tmp_value, tmp_loop1_CAPVD, new_Flt_prev, loop1_CAPVD, delta_current, flt_mgr.current_freq, rapid_change_detected);
|
||
// }
|
||
|
||
new_Flt_prev = loop1_CAPVD; // 更新历史值
|
||
|
||
if(flt_mgr.flag_loop_reconnect) // 处理线圈重连的情况
|
||
{
|
||
if(flt_mgr.flag_loop_reconnect >= 2)
|
||
{
|
||
|
||
if(!flt_mgr.last_loop_vd_flag)
|
||
{
|
||
LEDA_OFF;
|
||
flt_mgr.flag_loop_reconnect = 0;
|
||
return;
|
||
}
|
||
|
||
loop1_dlt_ORG = ((uint32_t)flt_mgr.last_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;
|
||
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
new_Flt_prev = loop1_CAPVD; // 更新历史值
|
||
|
||
// if(( flt_mgr.last_loop_Origin - loop1_dlt_ORG ) < loop1_CAPVD ){
|
||
|
||
// }
|
||
if(( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD) //
|
||
{ // 车离开
|
||
//TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？
|
||
// 1、如果增加平坦算法，会有延时，不能及时落杆；
|
||
|
||
if(loop1_VD_FLAG)
|
||
{
|
||
loop1_FLAG_OUT = 1;
|
||
}
|
||
loop1_VD_FLAG = 0;
|
||
|
||
g_loop_acs_info.car_state = 0;
|
||
// LED_LP1_OFF();
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_VD_HOLD = 0 ;
|
||
Hold_CNT = 0;
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.flag_loop_reconnect = 0;
|
||
|
||
loop1_Origin = flt_mgr.last_loop_Origin;
|
||
} // end if(( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD)
|
||
else if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD))
|
||
{
|
||
// PRINT("FLAT_TRIGER_CAR_OFF_init_re:%d,5sec:%d,5min:%d,10min:%d,30min:%d,60min:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n",
|
||
// flt_mgr.stable_init_value, flt_mgr.recent_5sec, flt_mgr.recent_5min, flt_mgr.recent_10min, flt_mgr.recent_30min, flt_mgr.recent_60min,
|
||
// loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
|
||
// loop1_FLAG_OUT = 1;
|
||
if(loop1_VD_FLAG)
|
||
{
|
||
loop1_FLAG_OUT = 1;
|
||
}
|
||
loop1_VD_FLAG = 0;
|
||
|
||
g_loop_acs_info.car_state = 0;
|
||
|
||
// LED_LP1_OFF();
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_VD_HOLD = 0 ;
|
||
Hold_CNT = 0;
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.flag_loop_reconnect = 0;
|
||
loop1_Origin = flt_mgr.last_loop_Origin;
|
||
} // end else if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD))
|
||
else
|
||
{
|
||
if(!loop1_VD_FLAG)
|
||
{
|
||
if(( flt_mgr.last_loop_Origin - loop1_dlt_ORG ) < loop1_CAPVD ){
|
||
if(loop1_VD_FLAG)
|
||
{
|
||
loop1_FLAG_OUT = 1;
|
||
}
|
||
loop1_VD_FLAG = 0;
|
||
g_loop_acs_info.car_state = 0;
|
||
|
||
// LED_LP1_OFF();
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_VD_HOLD = 0 ;
|
||
Hold_CNT = 0;
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.flag_loop_reconnect = 0;
|
||
loop1_Origin = flt_mgr.last_loop_Origin;
|
||
|
||
return;
|
||
}
|
||
|
||
if(loop1_CAPVD < (flt_mgr.last_loop_Origin - loop1_dlt_ORG)) //
|
||
{// 有车
|
||
if(loop1_VD_FLAG == 0){
|
||
g_loop_acs_info.flag_change = 1;
|
||
}
|
||
|
||
loop1_VD_FLAG = 1;
|
||
loop1_FLAG_IN = 1;
|
||
g_loop_acs_info.car_state = 1;
|
||
if(g_loop_cng_unit.exist_mode) //有限存在10分钟
|
||
{
|
||
loop1_VD_HOLD = 1 ;
|
||
}
|
||
|
||
}
|
||
} // end if(!loop1_VD_FLAG)
|
||
flt_mgr.flag_loop_reconnect = 4;
|
||
if(flt_mgr.flag_second_judge)
|
||
{
|
||
if(!flt_mgr.second_loop_vd_flag)
|
||
{
|
||
update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size);
|
||
|
||
// 5. 调用二阶变化量计算函数
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
// PRINT("Second_CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
|
||
if(!delta_current || !delta2_flt)
|
||
{
|
||
return;
|
||
}
|
||
if(flt_mgr.second_loop_Origin == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens & 0x0F].sens_in) >> 16; //loop1_dlt_ORG??:?--26;?--13;?--6
|
||
if(loop1_CAPVD < (flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG)) //
|
||
{// 有车
|
||
flt_mgr.second_loop_vd_flag = 1;
|
||
}
|
||
} // end if(!flt_mgr.second_loop_vd_flag)
|
||
else
|
||
{
|
||
flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;
|
||
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
new_Flt_prev = loop1_CAPVD; // 更新历史值
|
||
// PRINT("CarON_LPCNT:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
|
||
if(( flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG ) < loop1_CAPVD) //
|
||
{ // 车离开
|
||
//TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？
|
||
// 1、如果增加平坦算法，会有延时，不能及时落杆；
|
||
if(loop1_VD_FLAG == 1){
|
||
g_loop_acs_info.flag_change = 1;
|
||
}
|
||
loop1_VD_FLAG = 0;
|
||
loop1_FLAG_OUT = 1;
|
||
g_loop_acs_info.car_state = 0;
|
||
|
||
// LED_LP1_OFF();
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_VD_HOLD = 0 ;
|
||
Hold_CNT = 0;
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.second_loop_vd_flag = 0;
|
||
flt_mgr.flag_second_judge = 0;
|
||
flt_mgr.second_loop_ORG_CNT = 0;
|
||
flt_mgr.second_loop_ORG_SUM = 0;
|
||
|
||
loop1_Origin = flt_mgr.second_loop_Origin;
|
||
}
|
||
} // end else
|
||
} // end if(flt_mgr.flag_second_judge)
|
||
} // end else
|
||
} // end if(flt_mgr.flag_loop_reconnect >= 2)
|
||
return;
|
||
}
|
||
|
||
|
||
|
||
if(!loop1_VD_FLAG){
|
||
|
||
update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, g_moving_average_window_size);
|
||
|
||
// 5. 调用二阶变化量计算函数
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
update_flt_history(loop1_CAPVD, delta_current, delta2_flt);
|
||
// PRINT("CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
|
||
if(!delta_current) // || !delta2_flt)
|
||
{
|
||
return;
|
||
}
|
||
// PRINT("CarOFF_5sec:%d, Origin:%d, loop1_CAPVD:%d\n", flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD);
|
||
|
||
if(!flt_mgr.is_initialized)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if(loop1_LOOP_OK0 == 0){
|
||
return;
|
||
}
|
||
|
||
loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens & 0x0F].sens_in) >> 16;
|
||
if(g_flag_bt_state){
|
||
if(g_loop_acs_info.flag_change == 0){
|
||
g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD);
|
||
if(g_loop_acs_info.variation > 5 || g_loop_acs_info.variation < -5){
|
||
g_loop_acs_info.flag_event = 1;
|
||
}
|
||
}
|
||
}
|
||
else{
|
||
g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD);
|
||
}
|
||
|
||
if(loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) //
|
||
{
|
||
if(g_function_mode & FUNCTION_B){
|
||
if(flt_mgr.cnt_simple_release)
|
||
{
|
||
flt_mgr.cnt_simple_release = 0;
|
||
}
|
||
flt_mgr.cnt_simple_busy++;
|
||
if(flt_mgr.cnt_simple_busy < 3){
|
||
return;
|
||
}
|
||
|
||
// 改进：增加吸合多重确认机制，避免误触发
|
||
// 如果检测到快速变化（线圈中间移动），不立即吸合
|
||
if(rapid_change_detected && rapid_change_duration < 10)
|
||
{
|
||
relay_on_confirm_cnt = 0; // 重置确认计数
|
||
return; // 等待稳定后再判断
|
||
}
|
||
|
||
relay_on_confirm_cnt++;
|
||
if(relay_on_confirm_cnt < RELAY_ON_CONFIRM_THRESHOLD){
|
||
return; // 需要连续多次确认才吸合
|
||
}
|
||
relay_on_confirm_cnt = RELAY_ON_CONFIRM_THRESHOLD; // 达到阈值后保持最大值
|
||
|
||
// 检测到车辆进入，重置释放确认计数
|
||
relay_off_confirm_cnt = 0;
|
||
|
||
// 重置二次判断增强条件的计数器
|
||
flt_mgr.second_judge_enter_time = 0;
|
||
flt_mgr.cnt_stable_to_idle = 0;
|
||
}
|
||
|
||
flt_mgr.last_loop_Origin = loop1_Origin; // Mark
|
||
flt_mgr.last_loop_vd_flag = 1;
|
||
flt_mgr.second_loop_ORG_CNT = 0;
|
||
flt_mgr.second_loop_ORG_SUM = 0;
|
||
flt_mgr.second_loop_Origin = 0;
|
||
flt_mgr.second_loop_vd_flag = 0;
|
||
flt_mgr.flag_second_judge = 0;
|
||
|
||
if(loop1_VD_FLAG == 0){
|
||
g_loop_acs_info.flag_change = 1;
|
||
}
|
||
|
||
loop1_VD_FLAG = 1; //???????1
|
||
loop1_FLAG_IN = 1; //??????
|
||
// PRINT("Car_In,sens_level:%d, counter:%d\n", g_freq_sens, g_tmp_counter);
|
||
if(g_loop_cng_unit.exist_mode)
|
||
{
|
||
loop1_VD_HOLD = 1; // 有限存在10分钟
|
||
}
|
||
|
||
// LEDA_ON;
|
||
g_loop_acs_info.flag_event = 1;
|
||
g_loop_acs_info.car_state = 1;
|
||
|
||
// if(g_loop_cng_unit.loopSafe_Timeout > 0)
|
||
// {
|
||
// loop1_LC_HOLD = 1;
|
||
// }
|
||
// else
|
||
// {
|
||
// loop1_LC_HOLD = 0;
|
||
// }
|
||
if(loop1_LC_Reset)
|
||
loop1_LC_Reset = 0;
|
||
}
|
||
}// end if !loop1_VD_FLAG
|
||
else{
|
||
loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;
|
||
if(g_loop_acs_info.flag_change == 0)
|
||
{
|
||
g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD);
|
||
if(g_loop_acs_info.variation > 5 || g_loop_acs_info.variation < -5){
|
||
g_loop_acs_info.flag_event = 1;
|
||
}
|
||
}
|
||
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
new_Flt_prev = loop1_CAPVD; // 更新历史值
|
||
|
||
if(g_function_mode & FUNCTION_B){
|
||
if(flt_mgr.cnt_simple_busy)
|
||
{
|
||
flt_mgr.cnt_simple_busy = 0;
|
||
}
|
||
}
|
||
|
||
if((( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD )) //
|
||
{
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 改进：如果检测到快速变化，说明可能是线圈中间移动，不是真正的离开
|
||
// 此时不应该释放继电器，而是继续监测
|
||
if(rapid_change_detected)
|
||
{
|
||
// 快速变化期间，重置释放确认计数，防止误释放
|
||
relay_off_confirm_cnt = 0;
|
||
PRINT("RAPID_CHANGE_IGNORE_RELEASE\n");
|
||
return; // 不释放，继续监测
|
||
}
|
||
|
||
// 多重确认机制：需要连续多次满足释放条件才真正释放
|
||
relay_off_confirm_cnt++;
|
||
if(relay_off_confirm_cnt < RELAY_OFF_CONFIRM_THRESHOLD){
|
||
PRINT("RELEASE_PENDING cnt:%d/%d\n", relay_off_confirm_cnt, RELAY_OFF_CONFIRM_THRESHOLD);
|
||
return; // 还未达到确认次数，暂不释放
|
||
}
|
||
}
|
||
|
||
if(loop1_VD_FLAG == 1){
|
||
g_loop_acs_info.flag_change = 1;
|
||
}
|
||
loop1_VD_FLAG = 0; //???????0
|
||
loop1_FLAG_OUT = 1; //??????
|
||
loop1_VD_HOLD = 0 ;
|
||
loop1_LC_HOLD = 0;
|
||
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 释放后重置所有确认计数器
|
||
relay_on_confirm_cnt = 0;
|
||
relay_off_confirm_cnt = 0;
|
||
rapid_change_detected = 0;
|
||
rapid_change_duration = 0;
|
||
}
|
||
|
||
g_loop_acs_info.flag_event = 1;
|
||
g_loop_acs_info.car_state = 0;
|
||
// g_led_A_state = 0;
|
||
// LEDA_OFF;
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
Hold_CNT = 0;
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.second_loop_vd_flag = 0;
|
||
flt_mgr.second_loop_ORG_CNT = 0;
|
||
flt_mgr.second_loop_ORG_SUM = 0;
|
||
flt_mgr.flag_second_judge = 0;
|
||
}
|
||
else if(g_function_mode & FUNCTION_A){
|
||
if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD)){
|
||
// PRINT("FLAT_TRIGER_CAR_OFF_init:%d,5sec:%d,5min:%d,10min:%d,30min:%d,60min:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n",
|
||
// flt_mgr.stable_init_value, flt_mgr.recent_5sec, flt_mgr.recent_5min, flt_mgr.recent_10min, flt_mgr.recent_30min, flt_mgr.recent_60min,
|
||
// loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 平坦算法触发释放也需要多重确认
|
||
relay_off_confirm_cnt++;
|
||
if(relay_off_confirm_cnt < RELAY_OFF_CONFIRM_THRESHOLD){
|
||
return;
|
||
}
|
||
}
|
||
|
||
if(loop1_VD_FLAG == 1){
|
||
g_loop_acs_info.flag_change = 1;
|
||
}
|
||
loop1_VD_FLAG = 0;
|
||
loop1_FLAG_OUT = 1;
|
||
g_loop_acs_info.car_state = 0;
|
||
|
||
// LEDA_OFF;
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_VD_HOLD = 0 ;
|
||
Hold_CNT = 0;
|
||
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 释放后重置所有确认计数器
|
||
relay_on_confirm_cnt = 0;
|
||
relay_off_confirm_cnt = 0;
|
||
rapid_change_detected = 0;
|
||
rapid_change_duration = 0;
|
||
}
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.second_loop_vd_flag = 0;
|
||
flt_mgr.second_loop_ORG_CNT = 0;
|
||
flt_mgr.second_loop_ORG_SUM = 0;
|
||
flt_mgr.flag_second_judge = 0;
|
||
}
|
||
else{ //
|
||
if(flt_mgr.flag_second_judge){ //有车情况下二次判断
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 条件1：检查是否满足最小时间限制（10秒）
|
||
if(flt_mgr.second_judge_enter_time < flt_mgr.second_judge_min_time)
|
||
{
|
||
// 未达到最小时间，继续监测，不进入二次判断释放逻辑
|
||
// 但仍需要更新二次判断的基准值
|
||
if(!flt_mgr.second_loop_vd_flag)
|
||
{
|
||
update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size);
|
||
}
|
||
return;
|
||
}
|
||
}
|
||
|
||
if(!flt_mgr.second_loop_vd_flag)
|
||
{
|
||
update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size);
|
||
|
||
// 5. 调用二阶变化量计算函数
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
// PRINT("Second_CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
|
||
if(!delta_current || !delta2_flt)
|
||
{
|
||
return;
|
||
}
|
||
if(flt_mgr.second_loop_Origin == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16; //loop1_dlt_ORG??:?--26;?--13;?--6
|
||
if(loop1_CAPVD < (flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG)) //
|
||
{// 有车
|
||
flt_mgr.second_loop_vd_flag = 1;
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 重置稳定计数
|
||
flt_mgr.cnt_stable_to_idle = 0;
|
||
}
|
||
}
|
||
}
|
||
else {
|
||
flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;
|
||
|
||
delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
|
||
new_Flt_prev = loop1_CAPVD; // 更新历史值
|
||
// PRINT("CarON_LPCNT:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
|
||
|
||
if(( flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG ) < loop1_CAPVD) //
|
||
{ // 车离开
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 条件2：检查是否连续多次检测到恢复到空闲值
|
||
// 计算当前值与初始空闲值的偏差
|
||
uint32_t deviation_from_init = 0;
|
||
if(flt_mgr.stable_init_value > loop1_CAPVD)
|
||
{
|
||
deviation_from_init = flt_mgr.stable_init_value - loop1_CAPVD;
|
||
}
|
||
else
|
||
{
|
||
deviation_from_init = loop1_CAPVD - flt_mgr.stable_init_value;
|
||
}
|
||
|
||
// 判断是否接近空闲值（偏差小于初始值的5%）
|
||
uint32_t idle_threshold = (flt_mgr.stable_init_value * 5) / 100;
|
||
if(deviation_from_init <= idle_threshold)
|
||
{
|
||
flt_mgr.cnt_stable_to_idle++;
|
||
PRINT("STABLE_TO_IDLE cnt:%d/%d, dev:%d, thresh:%d\n",
|
||
flt_mgr.cnt_stable_to_idle, flt_mgr.stable_to_idle_threshold,
|
||
deviation_from_init, idle_threshold);
|
||
}
|
||
else
|
||
{
|
||
// 偏差不够，重置计数
|
||
flt_mgr.cnt_stable_to_idle = 0;
|
||
return;
|
||
}
|
||
|
||
// 需要连续多次确认才释放
|
||
if(flt_mgr.cnt_stable_to_idle < flt_mgr.stable_to_idle_threshold)
|
||
{
|
||
return; // 还未达到稳定次数，暂不释放
|
||
}
|
||
}
|
||
|
||
//TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？
|
||
// 1、如果增加平坦算法，会有延时，不能及时落杆；
|
||
if(loop1_VD_FLAG == 1){
|
||
g_loop_acs_info.flag_change = 1;
|
||
}
|
||
loop1_VD_FLAG = 0;
|
||
loop1_FLAG_OUT = 1;
|
||
|
||
g_loop_acs_info.car_state = 0;
|
||
// LEDA_OFF;
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
loop1_VD_HOLD = 0 ;
|
||
Hold_CNT = 0;
|
||
|
||
flt_mgr.flag_5sec = 0;
|
||
flt_mgr.counter_5sec = 0;
|
||
flt_mgr.last_loop_vd_flag = 0;
|
||
flt_mgr.second_loop_vd_flag = 0;
|
||
flt_mgr.flag_second_judge = 0;
|
||
flt_mgr.second_loop_ORG_CNT = 0;
|
||
flt_mgr.second_loop_ORG_SUM = 0;
|
||
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 重置二次判断增强条件的计数器
|
||
flt_mgr.second_judge_enter_time = 0;
|
||
flt_mgr.cnt_stable_to_idle = 0;
|
||
}
|
||
|
||
loop1_Origin = flt_mgr.second_loop_Origin;
|
||
}
|
||
}
|
||
} // end if(flt_mgr.flag_second_judge)
|
||
}
|
||
} //end if(g_function_mode & FUNCTION_A)
|
||
|
||
}
|
||
}
|
||
|
||
|
||
void INIT_VD(void)
|
||
{
|
||
loop1_INCNT = 0;
|
||
loop1_OUTCNT = 0;
|
||
// loop1_PLUSECNT =0;
|
||
loop1_CapCnt = 0;
|
||
|
||
// Cap.loop1_CapThis = 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_LPCNT=0;
|
||
// loop1_INI_LOOP=1;
|
||
|
||
loop1_FLAG_IN = 0;
|
||
loop1_FLAG_OUT = 0;
|
||
loop1_FLAG_PLUSE = 0;
|
||
loop1_FLAG_CUT = 0;
|
||
|
||
|
||
flt_mgr.is_initialized = 0;
|
||
flt_mgr.recent_5sec = 0;
|
||
stage_config[0].base_value = 0;
|
||
stage_config[1].base_value = 0;
|
||
|
||
g_loop_acs_info.car_state = 0;
|
||
loop1_FLAG_HOLD_TIMEOUT = 0;
|
||
|
||
// 初始化二次判断增强条件
|
||
flt_mgr.second_judge_enter_time = 0;
|
||
flt_mgr.second_judge_min_time = 2000; // 10秒（10000ms / 5ms = 2000）
|
||
flt_mgr.cnt_stable_to_idle = 0;
|
||
flt_mgr.stable_to_idle_threshold = 10; // 连续10次检测到恢复到空闲值
|
||
|
||
}
|
||
|
||
|
||
void cjq_srv(void){
|
||
|
||
if(loop1_LOOP_OK){
|
||
if(loop1_FLAG_HOLD_TIMEOUT){
|
||
INIT_VD();
|
||
}
|
||
if(loop1_CAP_OK){
|
||
if(!g_loop_power_up_state){
|
||
g_loop_power_up_state = 1; // // 表示连接了线圈
|
||
}
|
||
else{
|
||
if(loop1_FLAG_CUT){
|
||
|
||
}
|
||
}
|
||
}
|
||
if(loop1_FLAG_CUT){
|
||
// 线圈断开后重新连上 需要复位
|
||
loop1_FLAG_CUT = 0;
|
||
// SYS_ResetExecute();
|
||
if(loop1_VD_FLAG){
|
||
if(g_loop_cng_unit.exist_mode){
|
||
Hold_CNT = 0;
|
||
loop1_VD_HOLD = 1;
|
||
}
|
||
}
|
||
flt_mgr.flag_loop_reconnect = 1;
|
||
}
|
||
if(g_loop_cut_last != loop1_FLAG_CUT){
|
||
g_loop_cut_last = loop1_FLAG_CUT;
|
||
}
|
||
if(!loop1_LOOP_OK0){
|
||
DelayMs(200);
|
||
loop1_LOOP_OK0 = 1;
|
||
}
|
||
if(loop1_CAP_OK){
|
||
loop1_CAP_OK = 0;
|
||
vd1_task();
|
||
}// end if loop1_CAP_OK
|
||
}// end if loop1_LOOP_OK
|
||
else{
|
||
loop1_FLAG_CUT = 1;
|
||
loop1_VD_FLAG = 0;
|
||
loop1_FLAG_IN = 0;
|
||
loop1_FLAG_OUT = 0;
|
||
loop1_FLAG_PLUSE = 0;
|
||
|
||
// 线圈断开，继电器释放，状态为0，车辆状态也为0
|
||
RLY1_OFF; RLY2_OFF;
|
||
g_loop_acs_info.relay1_state = 0;
|
||
g_loop_acs_info.relay2_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 >= 255){
|
||
g_loop_cut_amount = 255;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
/*********************************************************************
|
||
* @fn Main_Circulation
|
||
*
|
||
* @brief 主循环
|
||
*
|
||
* @return none
|
||
*/
|
||
__HIGH_CODE
|
||
__attribute__((noinline))
|
||
void Main_Circulation()
|
||
{
|
||
|
||
while(1)
|
||
{
|
||
TMOS_SystemProcess();
|
||
// poll_sens_key();
|
||
#ifndef DEBUG
|
||
poll_dbn_ble();
|
||
#endif
|
||
|
||
cjq_srv();
|
||
|
||
if(g_flag_bt_state == 0){
|
||
// 如果蓝牙断开，主动上报需要禁用
|
||
if(g_dbn_ble_state_acs_enable.enable){
|
||
g_dbn_ble_state_acs_enable.enable = 0;
|
||
}
|
||
}
|
||
|
||
#ifdef DEBUG
|
||
if(g_flag_output){
|
||
g_flag_output = 0;
|
||
uint32_t tmp = (g_sys_freq )/(loop1_Xn);
|
||
PRINT("Xn: %d, loop1_LPCNT: %d, counter:%d, ms_counter:%d, capvd:%d, loop1_Origin:%d, freq:%d, init_freq:%d\n",
|
||
loop1_Xn, loop1_LPCNT, g_xn_counter, g_xn_counter/2000, loop1_CAPVD, loop1_Origin, tmp, flt_mgr.init_freq);
|
||
|
||
g_xn_counter = 0;
|
||
}
|
||
#endif
|
||
}
|
||
}
|
||
|
||
|
||
/*********************************************************************
|
||
* @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();
|
||
// DelayMs(10);
|
||
init_second_order(&second_order_state, 0);
|
||
|
||
tm_init();
|
||
|
||
CH59x_BLEInit();
|
||
HAL_Init();
|
||
GAPRole_PeripheralInit();
|
||
Peripheral_Init();
|
||
Main_Circulation();
|
||
}
|
||
|
||
|
||
|
||
/* @fn TMR0_IRQHandler
|
||
*
|
||
* @brief TMR0中断函数
|
||
*
|
||
* @return none
|
||
*/
|
||
__INTERRUPT
|
||
__HIGH_CODE
|
||
void TMR0_IRQHandler(void) // TMR0 定时中断
|
||
{
|
||
if(TMR0_GetITFlag(TMR0_3_IT_DATA_ACT)){
|
||
loop1_Xn = TMR0_CAPGetData();
|
||
|
||
#ifdef DEBUG
|
||
g_xn_counter++;
|
||
#endif
|
||
|
||
if(!loop1_RF_FLAG){
|
||
loop1_RF_FLAG = 1;
|
||
}
|
||
else{
|
||
loop1_CapCnt++;
|
||
|
||
if(loop1_INI_LOOP){
|
||
// if(g_filter_step < 2){
|
||
if(loop1_LPCNT == 0){
|
||
if(loop1_CapCnt > 200){
|
||
loop1_LPCNT = (32768/loop1_Xn)<<6;
|
||
if(loop1_LPCNT == 0){
|
||
loop1_LPCNT = 10;
|
||
}
|
||
|
||
g_freq_tmp = (float)g_sys_freq/((float)loop1_Xn);
|
||
|
||
loop1_CAPVD = 0;
|
||
loop1_CapSum = 0;
|
||
loop1_CapCnt = 0;
|
||
}
|
||
}
|
||
|
||
// }//end if g_filter_step < 2
|
||
else{
|
||
loop1_CapSum += loop1_Xn;
|
||
if(loop1_CapCnt >= loop1_LPCNT){
|
||
g_loop_acs_info.loop_capvd = (loop1_CapSum >> 5);
|
||
loop1_CAPVD = g_loop_acs_info.loop_capvd;
|
||
loop1_Origin = 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 >> 5); // g_loop_acs_info.loop_capvd = (loop1_CapSum >> 5);
|
||
// loop1_Value = g_loop_acs_info.loop_capvd;
|
||
loop1_CAP_OK = 1;
|
||
loop1_CapSum = 0;
|
||
loop1_CapCnt = 0;
|
||
loop1_INI_LOOP = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
R8_TMR0_INT_FLAG |= 0xff;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/*********************************************************************
|
||
* @fn TMR1_IRQHandler
|
||
*
|
||
* @brief TMR1中断函数
|
||
*
|
||
* @return none
|
||
*/
|
||
__INTERRUPT
|
||
__HIGH_CODE
|
||
void TMR1_IRQHandler(void) // TMR1 定时中断 5ms/per
|
||
{
|
||
static uint16_t _counter1_init = 0;
|
||
static uint16_t _cnt = 0;
|
||
|
||
if(TMR1_GetITFlag(TMR0_3_IT_CYC_END))
|
||
{
|
||
#ifdef DEBUG
|
||
_cnt++;
|
||
if(_cnt >= 400){
|
||
g_flag_output = 1;
|
||
_cnt = 0;
|
||
}
|
||
|
||
#endif
|
||
|
||
if(g_dbn_ble_state_acs_enable.enable && g_flag_bt_state){
|
||
g_loop_acs_info.report_counter++;
|
||
}
|
||
|
||
TimingDelayInc++;
|
||
|
||
|
||
if(loop1_FLAG_CUT == 0)
|
||
{
|
||
if(!flt_mgr.is_initialized)
|
||
{
|
||
_counter1_init++;
|
||
if(_counter1_init >=40) // 30:150ms, 40: 200ms, 50:250ms
|
||
{
|
||
_counter1_init = 0;
|
||
LEDA_ON_OFF();
|
||
}
|
||
}
|
||
}
|
||
if(flt_mgr.flag_flat_release)
|
||
{
|
||
flt_mgr.cnt_flat_release++;
|
||
if(flt_mgr.cnt_flat_release >= flt_mgr.max_cnt_flat_release)
|
||
{
|
||
flt_mgr.flag_flat_release = 0;
|
||
flt_mgr.cnt_flat_release = 0;
|
||
}
|
||
}
|
||
|
||
if(flt_mgr.flag_loop_reconnect == 1)
|
||
{
|
||
flt_mgr.cnt_loop_reconnect++;
|
||
if(flt_mgr.cnt_loop_reconnect >= 200)
|
||
{
|
||
flt_mgr.flag_loop_reconnect = 2;
|
||
flt_mgr.cnt_loop_reconnect = 0;
|
||
}
|
||
}
|
||
if(flt_mgr.flag_not_idle)
|
||
{
|
||
flt_mgr.counter_not_idle++;
|
||
if(flt_mgr.counter_not_idle >= 200) //400) // 5ms per unit, 400 mean 2second
|
||
{
|
||
flt_mgr.counter_not_idle = 0;
|
||
flt_mgr.flag_not_idle = 0;
|
||
}
|
||
}
|
||
if(flt_mgr.flag_5sec == 0)
|
||
{
|
||
flt_mgr.counter_5sec++;
|
||
if(flt_mgr.counter_5sec >= 1000)
|
||
{
|
||
flt_mgr.counter_5sec = 0; // 5*1000/5=1000
|
||
flt_mgr.flag_5sec = 1;
|
||
}
|
||
}
|
||
|
||
if(loop1_FLAG_CUT)
|
||
{
|
||
_counter1_init++;
|
||
if(_counter1_init >= 30) // 30:150ms, 40: 200ms, 50:250ms
|
||
{
|
||
_counter1_init = 0;
|
||
LEDA_ON_OFF();
|
||
}
|
||
}
|
||
// else
|
||
// {
|
||
// if(loop1_LOOP_OK == 0)
|
||
// {
|
||
// if(g_led_A_cnt >= 100) // 200: 1000ms,1s
|
||
// {
|
||
// g_led_A_cnt = 0;
|
||
// LEDA_ON_OFF();
|
||
// }
|
||
// }
|
||
// else
|
||
// {
|
||
// if(loop1_VD_FLAG) // 有车 car busy
|
||
// {
|
||
// }
|
||
// else // No car , idle
|
||
// {
|
||
// if(g_led_A_cnt >= 100) // 100: 500ms
|
||
// {
|
||
// g_led_A_cnt = 0;
|
||
// LEDA_OFF;
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
if(g_function_mode & FUNCTION_B){
|
||
// 更新二次判断进入时间（当有车时累加）
|
||
if(loop1_VD_FLAG && flt_mgr.flag_second_judge)
|
||
{
|
||
flt_mgr.second_judge_enter_time++;
|
||
}
|
||
}
|
||
|
||
|
||
u32CountWdt++;
|
||
|
||
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)
|
||
{
|
||
// flt_mgr.flag_second_judge = 1; // 二次判断
|
||
// loop1_FLAG_CUT = 1; //
|
||
loop1_FLAG_HOLD_TIMEOUT = 1;
|
||
g_moving_average_window_size = LOOP_WINDOW_SIZE_FAST; // 进入快速补偿模式
|
||
RLY1_OFF; RLY2_OFF;
|
||
// Reset_RstPeripheralAll();
|
||
}
|
||
}
|
||
}
|
||
|
||
if(loop1_LC_HOLD)
|
||
{
|
||
LC_Hold_CNT++;
|
||
if(LC_Hold_CNT > g_safe_max_cnt)
|
||
{
|
||
//TODO: 这里需要验证是否能够实现安全复位的功能
|
||
loop1_LC_Reset = 1;
|
||
loop1_INI_LOOP = 1;
|
||
loop1_LOOP_OK0 = 0;
|
||
LC_Hold_CNT = 0;
|
||
loop1_ORG_CNT = 0;
|
||
loop1_ORG_SUM = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(loop1_RF_FLAG)
|
||
{
|
||
// loop1_LOOP_OK0 = loop1_LOOP_OK;
|
||
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)// 输出存在信号
|
||
{
|
||
if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
|
||
RLY1_ON;
|
||
g_loop_acs_info.relay1_state = 1;
|
||
RLY2_ON;
|
||
g_loop_acs_info.relay2_state = 1;
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
RLY1_OFF;
|
||
g_loop_acs_info.relay1_state = 0;
|
||
RLY2_OFF;
|
||
g_loop_acs_info.relay2_state = 0;
|
||
}
|
||
} break;
|
||
case 1: // 进入脉冲
|
||
{
|
||
if(loop1_VD_FLAG) // 继电器2存在信号
|
||
{
|
||
if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
|
||
|
||
RLY2_ON;
|
||
g_loop_acs_info.relay2_state = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
RLY2_OFF;
|
||
g_loop_acs_info.relay2_state = 0;
|
||
}
|
||
|
||
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_VD_FLAG) // 继电器2存在信号
|
||
{
|
||
if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
|
||
|
||
RLY2_ON;
|
||
g_loop_acs_info.relay2_state = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
RLY2_OFF;
|
||
g_loop_acs_info.relay2_state = 0;
|
||
}
|
||
|
||
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: {
|
||
if(loop1_VD_FLAG) // 继电器2存在信号
|
||
{
|
||
if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
|
||
|
||
RLY2_ON;
|
||
g_loop_acs_info.relay2_state = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
RLY2_OFF;
|
||
g_loop_acs_info.relay2_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 ******************************/
|