#include "TaskLoop.h" #include "at32f421_board.h" #include "cmcng.h" #include "FreeRTOS.h" #include "task.h" #include "math.h" #include const uint16_t SensTable[4] = {300, 60, 32, 21 }; const uint16_t SensTable_1[4] = {60, 32, 21, 14 }; //释放灵敏度 const uint16_t DELAY_TIME[4] = {0, 10}; //0、0.6秒、1.6秒、3秒 {0,12,32,60}; //0、0.6秒、1.6秒、3秒 int8_t g_freq_sens; uint8_t loop1_SensLevel; uint8_t g_freq_level; uint32_t g_sys_freq = 0; // 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; __IO uint16_t loop1_Xn; uint16_t loop1_HoldTime; uint16_t loop1_dlt_ORG; uint32_t loop1_Origin; __IO uint16_t loop1_CapThis; __IO uint16_t loop1_CapLast; uint16_t loop1_LPCNT; __IO uint16_t loop1_CapCnt; uint32_t loop1_CapSum; uint32_t loop1_CAPVD; uint32_t loop1_Value; uint16_t loop1_Flt; uint32_t tmp_loop1_CAPVD; 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; uint8_t SET_PLUS = 0; uint8_t SET_DLY = 0; uint8_t SET_SAFE = 0; uint8_t g_loop_power_up_state = 0; // 开机启动时线圈是否连接 uint8_t g_led_loop_reconnect = 0; // 线圈是否重连 uint8_t g_flag_faul_reconnect = 0; uint8_t g_step_fault_led = 0; uint16_t g_counter_fault_led = 0; uint32_t g_xn_counter = 0; //100 补偿的周期大概为1.68秒，50 补偿的周期大概为0.84秒, 120 补偿的周期大概为2.016秒, 360 补偿的周期大概6秒 #define LOOP_WINDOW_SIZE_LOW 360 #define LOOP_WINDOW_SIZE_NORMAL 120 // 50 // 200 #define LOOP_WINDOW_SIZE_FAST 50 // 快速补偿 #define LOOP_WINDOW_SIZE_MIN 10 // 用于计算临时平均数 uint16_t g_moving_average_window_size = LOOP_WINDOW_SIZE_NORMAL; // 滑动窗口大小 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; #define MAX_ALLOW_DIFF_FREQ 3500 //频率最大偏移值 3500Hz uint16_t g_MaxAllowDiff = 0; // 采样值允许最大偏移值 float g_freq_tmp = 0.0; uint8_t loop1_FLAG_PLUSE_IN_F = 0; uint8_t loop1_FLAG_PLUSE_IN = 0; uint8_t g_loop_cut_last = 0; uint32_t g_loop_cut_amount = 0; __IO uint32_t tmr3freq = 0; __IO uint16_t capturenumber = 0; __IO uint16_t ic3readvalue1 = 0, ic3readvalue2 = 0; __IO uint32_t capture = 0; Loop_ACS_Info g_loop_acs_info; uint8_t sw0, swok, sw1, swcnt, SENS = 0, SENS_LAST = 0; void poll_sw_state(void) { sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN); sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN); sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN); sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN); sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN); if(sw0 == sw1){ if(swok != sw0){ swcnt++; if(swcnt > 10){ swok = sw0; SET_PLUS = swok & 0x04; // 输出方式 SET_DLY = (swok & 0x08) >> 3; SET_SAFE = (swok & 0x10) >> 4; } } } else{ sw1 = sw0; swcnt = 0; } SENS = swok & 0x03; if(SENS != SENS_LAST){ PRINT("SENS:%02X, SENS_LAST: %02X\n", SENS, SENS_LAST); nvic_system_reset(); } } 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） */ uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt, uint32_t* p_origin, uint32_t new_value, uint8_t window) { // 参数安全检查 if (!p_sum || !p_cnt || !p_origin || window == 0) { return 0; // 可添加错误处理 } *p_sum += new_value; // 累加新值 (*p_cnt)++; if (*p_cnt >= window) { // 窗口满（如window=100时，计数器0~99） *p_origin = (uint32_t)(*p_sum / window); // 计算平均值 *p_cnt = 0; *p_sum = 0; return 1; } return 0; } 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 * (SensTable_1[loop1_SensLevel] - 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 * (SensTable_1[loop1_SensLevel] - 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((abs(firstOrder) > 5) || (abs(secondOrder) >2)) { if(_first_cnt < 3){ //连续三次有变动 _first_cnt++; PRINT("____+_first_cnt:%d, idle:%d\n", _first_cnt, flt_mgr.flag_not_idle); } 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--; PRINT("____-_first_cnt:%d, idle:%d\n", _first_cnt, flt_mgr.flag_not_idle); } } // if((firstOrder || secondOrder) && (_first_cnt >= 3)) if(((abs(firstOrder) > 5) || secondOrder) && (_first_cnt >= 3)) { flt_mgr.counter_not_idle = 0; flt_mgr.flag_not_idle = 1; } if(flt_mgr.flag_not_idle || (abs(firstOrder) >= 10)) { 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++; PRINT("__new_flt:%d, init_sum:%d, amount:%d, firstOrder:%d, sedond:%d\n", new_flt, flt_mgr.init_sum, flt_mgr.init_samples, firstOrder, secondOrder); // 达到采样次数后计算稳定初始值 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 << XNSUM_FOR_ORIGIN_FACTOR)/((float)(loop1_LPCNT))) * g_input_div; loop1_Origin = flt_mgr.stable_init_value; PRINT("sys_freq:%d, new_flt:%d, init_sum:%d, stable_init:%d, init_freq:%d\n", g_sys_freq, new_flt, flt_mgr.init_sum, flt_mgr.stable_init_value, flt_mgr.init_freq); } 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; } } } void poll_yellow_led(void) { #define YELLOW_ON_SHORT 16 // 10:50ms, 12 60ms, 16:80ms #define YELLOW_OFF_MIDDLE 40 // 40: 200ms #define YELLOW_OFF_LONG 290 // 100:500ms, 140:700ms, 160: 800ms if(g_loop_power_up_state){ if(g_led_loop_reconnect == 0x01){ g_counter_fault_led++; if(g_flag_faul_reconnect){ if(g_counter_fault_led > YELLOW_ON_SHORT){ // 100ms: 20, 80ms g_counter_fault_led = 0; g_flag_faul_reconnect = 0; LED_YELLOW_OFF; } } else{ if(g_counter_fault_led > YELLOW_OFF_LONG){ //500ms g_flag_faul_reconnect = 1; g_counter_fault_led = 0; LED_YELLOW_ON; } } } else if(g_led_loop_reconnect == 0x02) { g_counter_fault_led++; if(g_step_fault_led == 0) { if(g_counter_fault_led > YELLOW_ON_SHORT){ LED_YELLOW_OFF; g_step_fault_led++; //1 g_counter_fault_led = 0; } } else if(g_step_fault_led == 1){ if(g_counter_fault_led > YELLOW_OFF_MIDDLE){ // 200ms: 40 g_counter_fault_led = 0; g_step_fault_led++; //2 LED_YELLOW_ON; } } else if(g_step_fault_led == 2) { if(g_counter_fault_led > YELLOW_ON_SHORT){ // 亮50ms LED_YELLOW_OFF; g_step_fault_led++; //1 g_counter_fault_led = 0; } } else if(g_step_fault_led == 3){ if(g_counter_fault_led > YELLOW_OFF_LONG){ // 200ms: 40 g_counter_fault_led = 0; g_step_fault_led = 0; //2 LED_YELLOW_ON; } } } else if(g_led_loop_reconnect >= 0x03) { g_counter_fault_led++; if(g_step_fault_led == 0) { if(g_counter_fault_led > YELLOW_ON_SHORT){ LED_YELLOW_OFF; g_step_fault_led++; //1 g_counter_fault_led = 0; } } else if(g_step_fault_led == 1){ if(g_counter_fault_led > YELLOW_OFF_MIDDLE){ // 200ms: 40 g_counter_fault_led = 0; g_step_fault_led++; //2 LED_YELLOW_ON; } } else if(g_step_fault_led == 2) { if(g_counter_fault_led > YELLOW_ON_SHORT){ // 亮50ms LED_YELLOW_OFF; g_step_fault_led++; //1 g_counter_fault_led = 0; } } else if(g_step_fault_led == 3){ if(g_counter_fault_led > YELLOW_OFF_MIDDLE){ // 200ms: 40 g_counter_fault_led = 0; g_step_fault_led++; //4 LED_YELLOW_ON; } } else if(g_step_fault_led == 4) { if(g_counter_fault_led > YELLOW_ON_SHORT){ // 亮50ms LED_YELLOW_OFF; g_step_fault_led++; //1 g_counter_fault_led = 0; } } else if(g_step_fault_led == 5){ if(g_counter_fault_led > YELLOW_OFF_LONG){ // 200ms: 40 g_counter_fault_led = 0; g_step_fault_led = 0; //2 LED_YELLOW_ON; } } } }// end if g_loop_power_up_state } void INIT_VD(){ 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.cnt_stable_to_idle = 0; flt_mgr.stable_to_idle_threshold = 10; // 连续10次检测到恢复到空闲值 flt_mgr.window_size = LOOP_WINDOW_SIZE_LOW; sw0 = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN); // 0x10 安全复位 sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN); // 0x08 延时 sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN); // 0x04 outmode sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN); // 0x02 sw0 = sw0 << 1; sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN); // 0x01 swok = sw0; sw1 = sw0; SENS = swok & 0x03; SENS_LAST = SENS; swcnt = 0; switch(SENS) { case 0x00: loop1_SensLevel = 0; break; case 0x02: loop1_SensLevel = 1; break; case 0x01: loop1_SensLevel = 2; break; case 0x03: loop1_SensLevel = 3; break; default: loop1_SensLevel = 0; break; } SET_PLUS = swok & 0x04; // 输出方式 SET_DLY = (swok & 0x08) >> 3; SET_SAFE = (swok & 0x10) >> 4; // 1 OFF, 0 ON } /** * @brief this function handles tmr3 trigger exception. * @param none * @retval none */ void TMR3_GLOBAL_IRQHandler(void) { if(tmr_interrupt_flag_get(TMR3, TMR_C2_FLAG) != RESET) { tmr_flag_clear(TMR3, TMR_C2_FLAG); loop1_CapThis = tmr_channel_value_get(TMR3, TMR_SELECT_CHANNEL_2); if(!loop1_RF_FLAG){ loop1_CapLast = loop1_CapThis; loop1_RF_FLAG = 1; } else{ if(loop1_CapThis > loop1_CapLast){ loop1_Xn = (loop1_CapThis - loop1_CapLast); } else{ loop1_Xn = ((0x10000 - loop1_CapLast) + loop1_CapThis); } g_xn_counter++; loop1_CapLast = loop1_CapThis; loop1_CapCnt++; if(loop1_INI_LOOP){ if(loop1_LPCNT == 0){ if(loop1_CapCnt > 100) { loop1_LPCNT = (32768 / loop1_Xn) << XNSUM_FOR_ORIGIN_FACTOR; PRINT("First_calc_loop1_LPCNT:%d, loop1_Xn:%d\n", loop1_LPCNT, loop1_Xn); if(loop1_LPCNT == 0) { loop1_LPCNT = 1000; } loop1_CAPVD = 0; loop1_CapSum = 0; loop1_CapCnt = 0; } } else{ loop1_CapSum += loop1_Xn; if(loop1_CapCnt >= loop1_LPCNT){ loop1_CAPVD = (loop1_CapSum); // >> 5); loop1_Origin = loop1_CAPVD >> XNSUM_FOR_ORIGIN_FACTOR; PRINT("First_capSum:%d, Origin:%d\n", loop1_CapSum, loop1_Origin); 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); loop1_CAP_OK = 1; loop1_CapSum = 0; loop1_CapCnt = 0; loop1_INI_LOOP = 0; } } } } } void LEDA_ON_OFF(void) { LED_RED_GPIO->odt ^= LED_RED_PIN; } // 5ms per void TMR15_GLOBAL_IRQHandler(void) { static uint16_t _counter1_init = 0; static uint8_t TM1cnt = 0; if(tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) { 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, 200 mean 1second { 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; } } //------ 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 > DELAY_TIME[1- SET_DLY]) { 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 > DELAY_TIME[1- SET_DLY]) // { 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_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) { if(loop1_VD_FLAG) // 继电器2存在 { RLY2_ON; } else{ RLY2_OFF; } if(SET_PLUS) { if(loop1_VD_FLAG || loop1_FLAG_OUT) // 输出存在信号 { RLY1_ON; } else{ RLY1_OFF; } } else{ if(loop1_FLAG_PLUSE) // 输出离开脉冲 { RLY1_ON; } else{ RLY1_OFF; } } }// end if loop1_LC_Reset == 0 } // end if loop1_RF_FLAG else { loop1_LOOP_OK0 = loop1_LOOP_OK; loop1_LOOP_OK = 0; } poll_red_pwm(); poll_yellow_led(); tmr_flag_clear(TMR15, TMR_OVF_FLAG); } } uint8_t g_flag_stable_delta2_flt = 0; uint8_t g_flag_reset_delta = 0; void vd1_task(void){ static uint32_t tmp_value = 0; static uint32_t new_Flt_prev = 0; // 保存上一次滤波值 int32_t delta_current = 0; int32_t delta2_flt = 0; if(loop1_Origin == 0){ return; } if(g_flag_reset_delta == 0){ new_Flt_prev = loop1_Origin; loop1_CAPVD = loop1_Origin; g_flag_reset_delta = 1; } tmp_value = loop1_Value; loop1_Value >>= XNSUM_FOR_ORIGIN_FACTOR; #ifdef DEBUG flt_mgr.current_freq = g_crm_clocks_freq_struct.sclk_freq/(tmp_value /loop1_LPCNT) * g_input_div; #endif tmp_loop1_CAPVD = loop1_Value; loop1_CAPVD = get_flt_value(tmp_loop1_CAPVD, 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(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(!loop1_VD_FLAG){ update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, flt_mgr.window_size); // 5. 调用二阶变化量计算函数 delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); // if(g_flag_stable_delta2_flt < 2) // { // g_flag_stable_delta2_flt++; // return; // } update_flt_history(loop1_CAPVD, delta_current, delta2_flt); // PRINT("CarOFF_LPCNT:%d, xn:%d, LPCNT:%d, freq:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1: %d, delta_2:%d\n", loop1_LPCNT, loop1_Xn, loop1_LPCNT, flt_mgr.current_freq, 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) { PRINT("__init_freq:%d, CarOFF_LPCNT:%d, xn:%d, LPCNT:%d, freq:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1: %d, delta_2:%d, freq:%d\n",flt_mgr.init_freq, loop1_LPCNT, loop1_Xn, loop1_LPCNT, flt_mgr.current_freq, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt, flt_mgr.current_freq); return; } if(loop1_LOOP_OK0 == 0){ return; } loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel & 0x0F]) >> 16; g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); if(loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG)) // { PRINT("Car_In,tmp_value:%d, Value:%d, loop1_CAPVD:%d, loop1_Origin:%d, loop1_dlt_ORG:%d, delta_1:%d, delta_2:%d, freq:%d\n",tmp_value, tmp_loop1_CAPVD, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq); // if(g_loop_cng_unit.exist_mode) // { // loop1_VD_HOLD = 1; // 有限存在10分钟 // } 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; } PRINT("Car_In01,tmp_value:%d, Value:%d, loop1_CAPVD:%d, loop1_Origin:%d, loop1_dlt_ORG:%d, delta_1:%d, delta_2:%d, freq:%d\n",tmp_value, tmp_loop1_CAPVD, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq); 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.cnt_stable_to_idle = 0; loop1_VD_FLAG = 1; //???????1 loop1_FLAG_IN = 1; //?????? LEDA_ON; if(!SET_SAFE) { loop1_LC_HOLD = 1; } else { loop1_LC_HOLD = 0; } if(loop1_LC_Reset) loop1_LC_Reset = 0; } else{ if(flt_mgr.cnt_simple_busy) { flt_mgr.cnt_simple_busy = 0; } } }// end if !loop1_VD_FLAG else{ loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16; g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD); delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF); new_Flt_prev = loop1_CAPVD; // 更新历史值 if(flt_mgr.cnt_simple_busy) { flt_mgr.cnt_simple_busy = 0; } if((( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD )) // { PRINT("CAR_OFF0,5sec:%d, tmp_value:%d, Value:%d, Origin:%d, loop1_CAPVD:%d,dlt_ORG:%d, delta_1: %d, delta_2:%d,freq:%d, rapid:%d\n", flt_mgr.recent_5sec, tmp_value, tmp_loop1_CAPVD, loop1_Origin, loop1_CAPVD, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq, rapid_change_detected); PRINT("CAR_OFF_CONFIRMED,5sec:%d, tmp_value:%d, Value:%d, Origin:%d, loop1_CAPVD:%d,dlt_ORG:%d, delta_1: %d, delta_2:%d,freq:%d\n", flt_mgr.recent_5sec, tmp_value, tmp_loop1_CAPVD, loop1_Origin, loop1_CAPVD, loop1_dlt_ORG, delta_current, delta2_flt, flt_mgr.current_freq); loop1_VD_FLAG = 0; //???????0 loop1_FLAG_OUT = 1; //?????? loop1_VD_HOLD = 0 ; loop1_LC_HOLD = 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; } } } void loop_task_function(void *pvParameters) { g_sys_freq = g_crm_clocks_freq_struct.sclk_freq; INIT_VD(); while(1) { if(loop1_LOOP_OK){ if(loop1_CAP_OK){ if(!g_loop_power_up_state){ g_loop_power_up_state = 1; // 表示连接了线圈 } else{ if(loop1_FLAG_CUT){ g_led_loop_reconnect |= 0x10; } } } // if(loop1_FLAG_HOLD_TIMEOUT){ // INIT_VD(); // } if(loop1_FLAG_CUT){ loop1_FLAG_CUT = 0; flt_mgr.flag_loop_reconnect = 1; } if(g_led_loop_reconnect & 0x10) { if(!loop1_LOOP_OK0) { loop1_LOOP_OK0 = 1; } } if(!loop1_LOOP_OK0){ for(swcnt = 0; swcnt < 6; swcnt++){ if(gpio_output_data_bit_read(LED_GREEN_GPIO, LED_GREEN_PIN)){ LEDA_ON; } else{ LEDA_OFF; } vTaskDelay(200); // TODO: 添加看门狗 } loop1_LOOP_OK0= 1; LEDA_OFF; } if(loop1_CAP_OK){ loop1_CAP_OK = 0; vd1_task(); poll_sw_state(); if(g_led_loop_reconnect & 0x10){ g_led_loop_reconnect = (g_led_loop_reconnect & 0x0F) + 1; if(g_led_loop_reconnect > 3){ g_led_loop_reconnect = 3; } g_counter_fault_led = 0; g_step_fault_led = 0; } else{ if(!g_led_loop_reconnect){ // TODO: 如果电感量不达标，是否也要进行故障提示，比如慢闪？ if(!gpio_output_data_bit_read(LED_YELLOW_GPIO, LED_YELLOW_PIN)){ LED_YELLOW_OFF; } } } } }// 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; LEDA_OFF; if(gpio_output_data_bit_read(LED_YELLOW_GPIO, LED_YELLOW_PIN)){ LED_YELLOW_ON; } else{ LED_YELLOW_OFF; } vTaskDelay(150); } if(g_flag_output) { g_flag_output = 0; // PRINT("SET_DLY:%02X, SET_SAFE:%02X,xnCounter:%d, ms_counter:%d, Xn:%d, LPCNT:%d, capvd:%d, loop1_Origin:%d, freq:%d, init_freq:%d\n", // SET_DLY, SET_SAFE, g_xn_counter, g_xn_counter/2000, loop1_Xn, loop1_LPCNT, loop1_CAPVD, loop1_Origin, flt_mgr.current_freq, flt_mgr.init_freq); g_xn_counter = 0; // if(gpio_output_data_bit_read(LED_GREEN_GPIO, LED_GREEN_PIN)){ // LEDA_ON; // } // else{ // LEDA_OFF; // } // // if(gpio_output_data_bit_read(LED_YELLOW_GPIO, LED_YELLOW_PIN)){ // LED_YELLOW_ON; // } // else{ // LED_YELLOW_OFF; // } } vTaskDelay(10); } }