1175 lines
37 KiB
C
1175 lines
37 KiB
C
|
||
#include "TaskLoop.h"
|
||
#include "at32f421_board.h"
|
||
#include "cmcng.h"
|
||
#include "FreeRTOS.h"
|
||
#include "task.h"
|
||
#include "math.h"
|
||
#include <stdlib.h>
|
||
|
||
|
||
|
||
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);
|
||
}
|
||
}
|
||
|