BvAutoController/User/process.c

#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_trigger_vo;

#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;	  /*  是否触发语音输出*/ 

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_trigger_vo > 0){
		g_trigger_vo--;
	
	}
	
	// 算法触发开阀延时
	if(g_alg_context.valveclose_delay > 0){
			g_alg_context.valveclose_delay--;
	}
	
	//read acc data
  ADXL312_ReadAcc(&accx, &accy, &accz);
	cbuffer_pushdata(accx, accy, accz);

}

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_trigger_vo = 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;
	
	
}

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_trigger_vo == 0)){
				g_trigger_vo = VO_COUNT_DFTT;
			}
		
	}

	
	if(g_bvopen == BV_OPEN){
			LED4_ON;
	}else{
			LED4_OFF;
	}
	
	
		//433  语音输出
	if(g_trigger_vo > 0){
		
		if(GPIO_DRV_GetPinsOutput(LED2_GPIO)& (1<<LED2_PIN))
		{
				
		}else{
				LED2_ON;
		}
		
	
	}else{
		
		if(GPIO_DRV_GetPinsOutput(LED2_GPIO)&(1<<LED2_PIN))
		{
			LED2_OFF;
		}
	
	}
	
	
}

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;

		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);
			SaveConfig();
			
			g_samplecount=0;
			g_autocalibration=0;
			
		}
	
		
		

}


void alg_PreProcess(void)
{
		static uint8_t _bvopen = BV_CLOSE;
		
		//状态切换
		if(_bvopen != g_bvopen){
			
				_bvopen = g_bvopen;
				if(BV_OPEN == g_bvopen){
						if(ALG_None == g_alg_context.state){
								g_alg_context.state = ALG_Start;
						}
						
				}else{
					  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) <= config->xaxis_threshold){
				paxis_info->x = 0.0;
		}
		if(fabs(paxis_info->y) <= config->yaxis_threshold){
				paxis_info->y = 0.0;
		}
		if(fabs(paxis_info->z) <= config->zaxis_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;
			
				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;
							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;
			
				alg_filter(&axis_info);
				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{
			
				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  
		
		}
	
}