轨迹处理系列:轨迹中寻找波峰波谷

在角度轨迹中寻找角度最大的值,即为第一个波峰或者第一个波谷,笛卡尔坐标系中,通过atan2获取的角度最大并不一定是在拐弯,直线上的轻微抖动都会形成一个类似拐弯的角度,比如行驶的角度为  【160°,162°,167°,170°,168°,166°】,其实这是一条只想,波峰出现在170°的位置,

//窗口滤波寻找峰值 峰谷
char  findPeaksAndTroughs(int p,int *out_p) { 
 
    int  start = 1;      // Starting index to search 
    //0123  4  5678
    const unsigned char cnt =0;
		static int flag_first=0;
		static int _buff[5];
		int sum;
    const int _buff_max=5;
		if(flag_first==0)//如果是第一次进来
			{
			 flag_first=1;
			 for(int i=0, sum =0;i< _buff_max;i++)//认为所有的元素都是一样的值
			 {
						 _buff[i]= p;
			      
        }
			 return -1;
		}else if(flag_first++ < 5)
		{
			
			for(int i=0;i<(_buff_max-1);i++)
				 {
							 _buff[i]= _buff[i+1];//buff[1]变成buff[0],buff[2]变成buff[1],依次类推
				 }
			   _buff[4]= p;//将最新值赋值给buff[9]
         return -1;
				
		}else 
		{
		  flag_first =6;
	  	for(int i=0;i<(_buff_max-1);i++)
			 {
						 _buff[i]= _buff[i+1];//buff[1]变成buff[0],buff[2]变成buff[1],依次类推
			 }
		   _buff[4]= p;//将最新值赋值给buff[8]
			int middle = _buff[2];
			int lastAverage = (_buff[0] +_buff[1])/2;
			int nextAverage = (_buff[3] +_buff[4])/2;
			static char  _flg_signal = 0;//互斥信号量 
			if(middle > lastAverage && middle > nextAverage && middle > 0 && lastAverage>0 &&nextAverage>0)//peak 波峰
				{
				 if( _flg_signal   == 1)_flg_signal =0;//互斥信号量 只运行一次
				 	
				   _flg_signal -- ;
					 if( _flg_signal  < -1){
					 	   _flg_signal =-1;
					 	   return -1;
					 	}
			    *out_p = middle;
					  return 1;	
				}   
		else 	if(middle < lastAverage && middle < nextAverage  && middle < 0 && lastAverage<0 &&nextAverage<0)//throughs 波菇
			{
			  if( _flg_signal   == -1)_flg_signal =0;
			   _flg_signal ++ ;
				 if( _flg_signal   > 1){
				 	_flg_signal =1;
					 	return -1;
			   	}
			  *out_p = middle;
			   return  0;	 
			}
		
		}
  
 return -1;
} 


sitemap