信标避障

lylylyly

信标用线性CCD避障，怎么求不出障碍的坐标，求解


///////定位障碍///////
void  beacon_Barrier()
{
  uint8 i;
  uint16 n=0;
  uint8 data1[1000];//存放目标点的坐标
  uint32 sum1=0;
   lastmid=mid;

  for(i=0;i<128;i++)
  {
    if(Pixel[i]>threshold1)//Pixel
    {
      data1[n++]=i;
      if(n>=1000)
        n=0;
    }
    for(i=0;i<n;i++)
    {
       sum1+=data1[i];
    }
  mid=(uint16)sum1/n;
  if(mid==0)
  {
    count_beacon1++;
//    if(count_beacon1>=5)
//    {
//      beacon_barrier=0;
//    }
//    }
//     else{
//        count_beacon1=0;
//         beacon_barrier=mid;
//  }
     beacon_barrier=mid;
  }


}