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

在角度轨迹中寻找角度最大的值，即为第一个波峰或者第一个波谷，笛卡尔坐标系中，通过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;
}