findeleceye.c

在ST单片机上实现GPS交通警示器功能.

#if DEBUG_ELECDOG >0       
      printf("\n");
	  printf("纬度区间序号:"); 
	  puthex(index_lat);

	  printf("\n");
      printf("经度区间序号:"); 
	  puthex(index_longt);
#endif

      index = Longzones[0] * index_lat + index_longt;
      valid = ValidZone[index];
	  
	  if ( valid != 0xff)
	  {
	      pos   = Dotoffset[valid].pos;
	  }
	  else
	  {
	       return roadindex;
	  }

	  infodot = &Infodots[pos];
      nums = Dotoffset[valid].nums;
   
      
	  for(i=0 ; i<nums; i++)
 	  {
	       if(isGetInfoDot == TRUE)
		   {
		       break;
		   }
           
	       len_32 = CalculateGpsSpace(gpsdata->latitude, gpsdata->longitude, infodot->gpsdata.latitude, infodot->gpsdata.longitude);
#if DEBUG_ELECDOG >0		   
		   printf("\n");
           printf("len_32:");
		   puthex(temp[0]);
		   puthex(temp[1]);
		   puthex(temp[2]);
		   puthex(temp[3]);
           printf("\n");
#endif
		  
		   if (len_32 <100)
           {
		        if ( isGetInfoDot == FALSE)
				{
				     isGetInfoDot = TRUE;
                     Infodotgps =infodot;
					 break;
				}
		   }
		   
		   infodot = &Infodots[++pos];
#if DEBUG_ELECDOG >0
           memcpy(temp, (INT8U*)&len_32, 4);
		   
		   printf("\n");
           printf("i:"); 
		   puthex(i);
#endif
           
          



         
           
		   
	  }
#if DEBUG_ELECDOG >0
      printf("\n");
	  printf("路口位置:");
	  printf_hex(Infodotgps->gpsdata.latitude,8);
      printf("\n");
#endif
	  
	  if (isGetInfoDot == TRUE)
	  {
	     if ( CalculateGpsSpace(gpsdata->latitude, gpsdata->longitude, Infodotgps->gpsdata.latitude, Infodotgps->gpsdata.longitude)>=100)
   	     {
		     isGetInfoDot = FALSE;
             roadindex = GetRoadNumber(gpsdata,Infodotgps);
             roadID    = GetRoadID(gpsdata,Infodotgps);
  		 }
	  }
#if DEBUG_ELECDOG >0      
      printf("\n");
	  printf("isGetInfoDot:"); 
	  puthex(isGetInfoDot);
      printf("\n");
#endif 
     
      roadinfo =  roadindex;

	  return roadinfo<<16 + roadID;
}
/**********************************************************
                查找路口信息
	 index1 : 纬度区间下标
	 index2 : 经度区间下标
	 index3 : 路号 当roadindex = 0xfffe,表示未搜索到路口信息点
	               当roadindex = 0xffff,表示走到安全的路上（没有电子眼的路）
				   当roadindex != 0xffff || index3 != 0xfffe, 表示走到一条新的有效路
***********************************************************/
INT32U  FindInfoDot(DATUM_STRUCT *gpsdata)
{
      INT8U latindex,longtindex;
	  INT32U roadinfo;
               
      roadinfo = 0xfffefffe;
	  latindex   = FindLatZone(gpsdata->latitude);
     

   	  if(latindex != 0xff)
	  {
	       longtindex = FindLongtZone(gpsdata->longitude);
	  
	       if(longtindex != 0xff)
	       {
	          roadinfo = SearchInfoDot( gpsdata ,latindex, longtindex);
			  
	       }
	   }

      return roadinfo;
}
/**********************************************************
                查找一条路上的电子眼
	 
***********************************************************/
INT8U  FindElecEye(DATUM_STRUCT *gpsdata,INT16U roadindex)
{
     INT8U i;
     INT16U nums;
	 INT16U pos;
	 
     
	 if( roadindex == 0xffff)
	 {
	      CurRroadIndex = 0xffff; 
	      return 0xff;
	 }
     else if (roadindex == 0xfffe)
	 {
          if ( CurRroadIndex == 0xffff)
		  {
		       return 0xff;
		  }
	      pos = Elecoffset[CurRroadIndex].pos; 
	      eleceyedata = &Eleceyes[pos];
          nums = Elecoffset[CurRroadIndex].nums;
	 }
	 else 
	 {
           
	       CurRroadIndex = roadindex;
	       pos = Elecoffset[CurRroadIndex].pos; 
	       eleceyedata = &Eleceyes[pos];
           nums = Elecoffset[CurRroadIndex].nums;
	 }

	 for( i = 0; i<nums; i++)
	 {
	    if(isGetElecEye == TRUE)
		{
		      break;
		}

	    if(CalculateGpsSpace(gpsdata->latitude, gpsdata->longitude,eleceyedata->latitude, eleceyedata->longitude)< 200)
  	    {
             if ( isGetElecEye == FALSE)
			 {
				     isGetElecEye = TRUE;
                     ElecEyegps = eleceyedata;
					 return i;
			 }
 
		}

         eleceyedata = &Eleceyes[++pos];

        
	 }
     
	 if (isGetElecEye == TRUE)
	 {
	     if ( CalculateGpsSpace(gpsdata->latitude, gpsdata->longitude, ElecEyegps->latitude, ElecEyegps->longitude)>=200)
   	     {
		     isGetElecEye = FALSE;
              			 
 		 }
	 }

          return 0xff;
      
}
/**********************************************************
                查找一条路上的限速路段
	 
***********************************************************/
INT8U  FindVecLimited(DATUM_STRUCT *gpsdata,INT16U roadID)
{
     if( roadID == 0xffff)
	 {
	      CurRroadID = 0xffff; 
	      return 0xff;
	 }
     else if (roadID == 0xfffe)
	 {
          if ( CurRroadID == 0xffff)
		  {
		       return 0xff;
		  }
	      pos = Elecoffset[CurRroadID].pos; 
	      eleceyedata = &Eleceyes[pos];
          nums = Elecoffset[CurRroadIndex].nums;
	 }
	 else 
	 {
           
	       CurRroadIndex = roadindex;
	       pos = Elecoffset[CurRroadIndex].pos; 
	       eleceyedata = &Eleceyes[pos];
           nums = Elecoffset[CurRroadIndex].nums;
	 }

       
}
static void EleceyeTmrProc(void)
{
     INT32U  roadinfo;
     INT8U  Eyeindex;
      
     StartTmr(EleceyeTmrID,EleceyePeriod);

#if USE_SIMDATA > 0
    	GetSimuData(&Gpsdata);
#else
        GetDatumData(&Gpsdata);
#endif 
#if DEBUG_ELECDOG >0         
	//	GetSimuData(&Gpsdata);
        
         printf("行使数据\n");
         printf_hex((INT8U*)&Gpsdata,11);
         printf("\n");
                
		 roadinfo =  FindInfoDot(&Gpsdata);
         
		 dispfindroad();
	     printf("\n"); 
		 puthex((INT8U)roadinfo);
#endif 
        

         if( (INT16U)(roadinfo>>16) != 0xfffe)
         {
             SendTskMsg(HandsetTskID, MSG_ROAD_FOUND, (INT16U)(roadinfo>>16));
         }
         
         Eyeindex = FindElecEye(&Gpsdata, (INT16U)(roadinfo>>16));


		 if(Eyeindex != 0xff)
		 {
		   //发报警信息到哈车宝
              SendTskMsg(HandsetTskID, MSG_ELECEYE_FOUND, 0);
		 }
}

void EleceyeInit(void)
{
      EleceyeTmrID = InstallTmr(AlarmTskID, 0,EleceyeTmrProc);
      StartTmr(EleceyeTmrID,EleceyePeriod);
}

INT8U GetCurRoadindex(void)
{
      return CurRroadIndex;
}