#include "process.h" #include "osif.h" #include "gpio.h" #include "adxl312.h" #include "uart.h" #include "crc16.h" #include "string.h" #include "math.h" #include "cfg.h" uint16_t g_blinkLedTime; /*RUN LED闪烁频率控制时间*/ uint16_t g_blinkLedTgtTime; /*RUN LED目标闪烁频率*/ //uint32_t g_alg_run_count; //uint8_t g_bvopen_delay; uint8_t g_bvopen; // 0: close , 1: open uint8_t g_vo_delay; uint8_t g_voiceid; #define DEFAULT_VO_PERIOD (500) /* 5秒*/ uint16_t g_VoiceOutTime; /*语音播报频率控制时间*/ #define BV_OPEN (1) #define BV_CLOSE (0) #define STATUS_DETECTINTERVAL (60) /*60ms * 8 == 500ms */ #define BV_DELAY_COUNT (50) /*50次, 500ms */ static uint8_t g_bvopen_delay; static circle_buffer cbuffer; static filter_avg_t filter_avg; static axis_info_t axis_info; static detect_info_t detect_info; static int16_t accx = 0; static int16_t accy = 0; static int16_t accz = 0; static int16_t acc_x = 0; static int16_t acc_y = 0; static int16_t acc_z = 0; uint8_t g_autocalibration = 0; uint16_t g_samplecount = 0; uint8_t g_send_raw = 0; uint8_t g_send_filtered = 0; uint32_t g_send_sequence = 0; uint8_t g_vo_alarm = 0; /* 是否触发语音输出*/ static uint16_t g_readaccfailed_count = 0; ALG_CONTEXT g_alg_context; void cbuffer_pushdata(int16_t acc_x, int16_t acc_y, int16_t acc_z); //每10ms 调用一次 void timer_callback(void) { if (g_blinkLedTime < 0xFFFF) { g_blinkLedTime++; } if(g_VoiceOutTime < 0xFFFF){ g_VoiceOutTime++; } //底阀打开状态 if(LOW == GET_K3_STS()){ if(g_bvopen_delay <= BV_DELAY_COUNT){ g_bvopen_delay++; }else{ //g_alg_run_count = 10*60*100; //10 min if(BV_CLOSE == g_bvopen){ //g_alg_context.state = ALG_Start; g_bvopen =BV_OPEN; //g_VoiceOutTime = DEFAULT_VO_PERIOD-100; } //g_alg_context.rundelay_count = RUNDELAY_COUNT; } }else{ if(g_bvopen_delay > 0){ g_bvopen_delay--; }else{ g_bvopen = BV_CLOSE; //if(g_alg_context.rundelay_count > 0){ // g_alg_context.rundelay_count--; //} } } //433 语音输出 if(g_vo_delay > 0){ g_vo_delay--; } // 算法触发开阀延时 if(g_alg_context.valveclose_delay > 0){ g_alg_context.valveclose_delay--; } //read acc data if(0 == ADXL312_ReadAcc(&accx, &accy, &accz)){ cbuffer_pushdata(accx, accy, accz); g_readaccfailed_count = 0; }else{ g_readaccfailed_count++; if(g_readaccfailed_count > 300){ //重启，尝度恢复加速度读取 NVIC_SystemReset(); }else if(g_readaccfailed_count > 3){ //运行指示灯，闪烁加快 g_blinkLedTgtTime = BLINK_LED_MINT; } } } void Process_Init(void) { g_blinkLedTime = 0; g_blinkLedTgtTime = BLINK_LED_DFTT; //g_alg_run_count = 0; g_bvopen_delay = 0; g_bvopen = BV_CLOSE; g_vo_delay = 0; g_voiceid = 0; g_vo_alarm = 0; /* acc data */ g_send_raw = 0; g_send_filtered = 0; g_alg_context.state = ALG_None; //g_alg_context.rundelay_count = 0; g_alg_context.valveclose_delay = 0; cbuffer.read_index = 0; cbuffer.write_index = 0; filter_avg.count = 0; g_autocalibration=0; g_samplecount = 0; g_readaccfailed_count = 0; } void Process_RunLedPrd(void) { /*周期性地检查LED闪烁,运行指示灯闪烁.*/ if (g_blinkLedTime >= g_blinkLedTgtTime) { g_blinkLedTime = 0; LED3_TOGGLE; } /*语音输出*/ if(g_VoiceOutTime >= DEFAULT_VO_PERIOD){ g_VoiceOutTime = 0; if((g_vo_alarm == 1) && (g_vo_delay == 0)){ g_vo_delay = VO_COUNT_DFTT; } } //if(g_bvopen == BV_OPEN){ // LED4_ON; //}else{ // LED4_OFF; //} //433 语音输出 if(g_vo_delay > 0){ Trigger_VoiceOn(g_voiceid); }else{ Trigger_VoiceOff(); } } void sv_open(void) { LED1_ON; } void sv_close(void) { LED1_OFF; } void cbuffer_pushdata(int16_t acc_x, int16_t acc_y, int16_t acc_z) { cbuffer.data[0][cbuffer.write_index] = acc_x; cbuffer.data[1][cbuffer.write_index] = acc_y; cbuffer.data[2][cbuffer.write_index] = acc_z; cbuffer.write_index++; if(cbuffer.write_index == ACC_DATASIZE){ cbuffer.write_index = 0; } } int cbuffer_popdata(int16_t* acc_x, int16_t* acc_y, int16_t* acc_z) { if(cbuffer.write_index == cbuffer.read_index){ return 1; } *acc_x = cbuffer.data[0][cbuffer.read_index]; *acc_y = cbuffer.data[1][cbuffer.read_index]; *acc_z = cbuffer.data[2][cbuffer.read_index]; cbuffer.read_index++; if(cbuffer.read_index == ACC_DATASIZE){ cbuffer.read_index = 0; } return 0; } void autocaculate_mtnoise(axis_info_t* paxis_info) { //复用测速 buffer // 以防精度不够用 double 类型 double accx_average = 0.0; double accy_average = 0.0; double accz_average = 0.0; double accx_sd = 0.0; double accy_sd = 0.0; double accz_sd = 0.0; uint16_t i=0; int tmp = 0; LED4_ON; detect_info.info[g_samplecount].x = paxis_info->x; detect_info.info[g_samplecount].y = paxis_info->y; detect_info.info[g_samplecount].z = paxis_info->z; g_samplecount++; if(DETECT_CNT == g_samplecount){ for(i=0; i < DETECT_CNT; i++){ accx_average += detect_info.info[i].x; accy_average += detect_info.info[i].y; accz_average += detect_info.info[i].z; } accx_average = accx_average/DETECT_CNT; accy_average = accy_average/DETECT_CNT; accz_average = accz_average/DETECT_CNT; for(i=0; i < DETECT_CNT; i++){ accx_sd += pow(detect_info.info[i].x - accx_average, 2)/DETECT_CNT; accy_sd += pow(detect_info.info[i].y - accy_average, 2)/DETECT_CNT; accz_sd += pow(detect_info.info[i].z - accz_average, 2)/DETECT_CNT; } accx_sd = pow(accx_sd, 0.5); accy_sd = pow(accy_sd, 0.5); accz_sd = pow(accz_sd, 0.5); //3 sigma //设备内参， 不加入出厂设置 config->xaxis_threshold = 3*fabs(accx_sd); config->yaxis_threshold = 3*fabs(accy_sd); config->zaxis_threshold = 3*fabs(accz_sd); //截取小数点后3位 tmp = (config->xaxis_threshold+0.0005)*1000; config->xaxis_threshold = 1.0*tmp/1000; tmp = (config->yaxis_threshold+0.0005)*1000; config->yaxis_threshold = 1.0*tmp/1000; tmp = (config->zaxis_threshold+0.0005)*1000; config->zaxis_threshold = 1.0*tmp/1000; SaveConfig(); g_samplecount=0; g_autocalibration=0; LED4_OFF; } } void alg_PreProcess(void) { static uint8_t _bvopen = BV_CLOSE; //状态切换 if(_bvopen != g_bvopen){ _bvopen = g_bvopen; if(BV_OPEN == g_bvopen){ LED4_ON; if(ALG_None == g_alg_context.state){ g_alg_context.state = ALG_Start; } }else{ LED4_OFF; if(ALG_Triggering != g_alg_context.state){ g_alg_context.state = ALG_None; } g_vo_alarm = 0; } } } void alg_start(void) { g_alg_context.state = ALG_Calibrate; g_alg_context.axis_offset.x = 0.0; g_alg_context.axis_offset.y = 0.0; g_alg_context.axis_offset.z = 0.0; g_alg_context.calibrate_count = 0; g_alg_context.valveclose_delay = 0; g_alg_context.valveclose_times = 0; detect_info.count = 0; memset(detect_info.info, 0x00, sizeof(axis_info_t)*DETECT_CNT); //filter_avg.count = 0; //printf("alg_start \r"); } void alg_calibrate(axis_info_t* pAxis_info){ g_alg_context.axis_offset.x += pAxis_info->x; g_alg_context.axis_offset.y += pAxis_info->y; g_alg_context.axis_offset.z += pAxis_info->z; g_alg_context.calibrate_count++; if(g_alg_context.calibrate_count == CALIBRATE_COUNT){ g_alg_context.axis_offset.x /= CALIBRATE_COUNT; g_alg_context.axis_offset.y /= CALIBRATE_COUNT; g_alg_context.axis_offset.z /= CALIBRATE_COUNT; g_alg_context.state = ALG_Detecting; g_alg_context.calibrate_count = 0; //printf("X=%4.3f g Y=%4.3f g Z=%4.3f g \r", g_alg_context.axis_offset.x, g_alg_context.axis_offset.y, g_alg_context.axis_offset.z); } } void alg_detect(axis_info_t* paxis_info) { uint16_t i=0; uint16_t _trigger=0; static uint16_t x_keepcount =0; static uint16_t y_keepcount =0; static uint16_t z_keepcount =0; float accx_sum = 0.0; float accy_sum = 0.0; float accz_sum = 0.0; //消除零偏 paxis_info->x -= g_alg_context.axis_offset.x; paxis_info->y -= g_alg_context.axis_offset.y; paxis_info->z -= g_alg_context.axis_offset.z; //机械滤波 if(fabs(paxis_info->x) <= MECHANICAL_NOISE_THRESHOLD){ paxis_info->x = 0.0; } if(fabs(paxis_info->y) <= MECHANICAL_NOISE_THRESHOLD){ paxis_info->y = 0.0; } if(fabs(paxis_info->z) <= MECHANICAL_NOISE_THRESHOLD){ paxis_info->z = 0.0; } detect_info.info[detect_info.count].x = paxis_info->x; detect_info.info[detect_info.count].y = paxis_info->y; detect_info.info[detect_info.count].z = paxis_info->z; for(i=0; i < DETECT_CNT; i++){ accx_sum += detect_info.info[i].x*0.01; accy_sum += detect_info.info[i].y*0.01; accz_sum += detect_info.info[i].z*0.01; } accx_sum *= 9.8; accy_sum *= 9.8; accz_sum *= 9.8; //printf("X=%4.3f g Y=%4.3f g Z=%4.3f g \r", accx_sum, accy_sum, accz_sum); //加速度累加值大于了 5Km/h, 需要持续 2s 去震动 if(fabs(accx_sum) >= DETECT_THRESHOLD){ x_keepcount++; if(KEEP_CNT == x_keepcount){ _trigger = 1; } }else if(fabs(accx_sum) >= DETECT_THRESHOLD_DOWN){ if(x_keepcount>0){ x_keepcount++; } if(KEEP_CNT == x_keepcount){ _trigger = 1; } }else{ x_keepcount=0; } if(fabs(accy_sum) >= DETECT_THRESHOLD){ y_keepcount++; if(KEEP_CNT == y_keepcount){ _trigger = 1; } }else if(fabs(accy_sum) >= DETECT_THRESHOLD_DOWN){ if(y_keepcount>0){ y_keepcount++; } if(KEEP_CNT == y_keepcount){ _trigger = 1; } }else{ y_keepcount = 0; } if(fabs(accz_sum) >= DETECT_THRESHOLD){ z_keepcount++; if(KEEP_CNT == z_keepcount){ _trigger = 1; } }else if(fabs(accz_sum) >= DETECT_THRESHOLD_DOWN){ if(z_keepcount>0){ z_keepcount++; } if(KEEP_CNT == z_keepcount){ _trigger = 1; } }else{ z_keepcount = 0; } if(1 == _trigger){ g_alg_context.state = ALG_Triggering; g_alg_context.valveclose_delay = CLOSEVALVE_DELAY; sv_open(); //打开电磁阀，泄气 g_alg_context.valveclose_times =0; g_alg_context.valveclose_interval = CLOSEVALVE_INTERVAL; g_VoiceOutTime = DEFAULT_VO_PERIOD-10; g_vo_alarm = 1; g_voiceid = VOICE_ID_0; x_keepcount = 0; y_keepcount = 0; z_keepcount = 0; } detect_info.count++; if(detect_info.count == DETECT_CNT) { detect_info.count = 0; } } void alg_Triggering(void) { //时间到强制关阀 //底阀关闭时,就可以关阀了 //if(BV_CLOSE == g_bvopen){ // sv_close(); // g_alg_context.state = ALG_Finished; // g_vo_alarm = 0; //}else{ if(0 == g_alg_context.valveclose_delay) { /* if(1 != g_vo_alarm){ g_VoiceOutTime = DEFAULT_VO_PERIOD-50; g_vo_alarm = 1; } */ sv_close(); if(g_alg_context.valveclose_interval > 0){ g_alg_context.valveclose_interval--; }else{ if(BV_CLOSE == g_bvopen){ //sv_close(); g_alg_context.state = ALG_Finished; g_vo_alarm = 0; g_vo_delay = VO_COUNT_DFTT; g_voiceid = VOICE_ID_1; return ; } g_alg_context.valveclose_delay = CLOSEVALVE_DELAY; sv_open(); //打开电磁阀，泄气 g_alg_context.valveclose_times++; g_alg_context.valveclose_interval = CLOSEVALVE_INTERVAL; } } if(g_alg_context.valveclose_times >= 3){ sv_close(); g_alg_context.state = ALG_Finished; if(1 != g_vo_alarm){ g_VoiceOutTime = DEFAULT_VO_PERIOD-10; g_vo_alarm = 1; } } //} } void alg_Finished(void) { //if(g_alg_context.rundelay_count == 0){ g_alg_context.state = ALG_None; //} } void alg_filter(axis_info_t* paxis_info) { uint16_t i=0; float x_sum = 0.0; float y_sum = 0.0; float z_sum = 0.0; filter_avg.info[filter_avg.count].x = paxis_info->x; filter_avg.info[filter_avg.count].y = paxis_info->y; filter_avg.info[filter_avg.count].z = paxis_info->z; for(i=0; i < FILTER_CNT; i++){ x_sum += filter_avg.info[i].x; y_sum += filter_avg.info[i].y; z_sum += filter_avg.info[i].z; } paxis_info->x = x_sum/FILTER_CNT; paxis_info->y = y_sum/FILTER_CNT; paxis_info->z = z_sum/FILTER_CNT; filter_avg.count++; if(filter_avg.count == FILTER_CNT){ filter_avg.count = 0; } } static uint16_t d_crc; static uint8_t d_buffer[20] = {0}; void Process_Alg(void) { alg_PreProcess(); if(0 == cbuffer_popdata(&acc_x, &acc_y, &acc_z)){ axis_info.x = acc_x*2.9/1000; axis_info.y = acc_y*2.9/1000; axis_info.z = acc_z*2.9/1000; //uint32_t start = OSIF_GetMilliseconds(); //printf("alg:%d ms st: %d \r", start, g_alg_context.state); //if(1 == g_autocalibration ){ // autocaculate_mtnoise(&axis_info); //}else{ alg_filter(&axis_info); switch(g_alg_context.state){ case ALG_Start: alg_start(); break; case ALG_Calibrate: alg_calibrate(&axis_info); break; case ALG_Detecting: alg_detect(&axis_info); break; case ALG_Triggering: alg_Triggering(); break; case ALG_Finished: alg_Finished(); break; default: break; } // } #if 1 if(g_send_raw){ d_buffer[0] = 0xAA; d_buffer[1] = 0xAA; g_send_sequence++; d_buffer[2] = (g_send_sequence >> 24)&0xff; d_buffer[3] = (g_send_sequence >> 16)&0xff; d_buffer[4] = (g_send_sequence >> 8)&0xff; d_buffer[5] = (g_send_sequence >> 0)&0xff; d_buffer[6] = (acc_x>>8)&0xFF; d_buffer[7] = (acc_x)&0xFF; d_buffer[8] = (acc_y>>8)&0xFF; d_buffer[9] = (acc_y)&0xFF; d_buffer[10] = (acc_z>>8)&0xFF; d_buffer[11] = (acc_z)&0xFF; d_crc = crc16(d_buffer, 12); d_buffer[12] = (uint8_t)((d_crc>>8) & 0xff); d_buffer[13] = (uint8_t)(d_crc & 0xff); Uart1_TransmitData(d_buffer, 14); } #endif } }