findeleceye.c

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

/*
*************************************************
*************FindElecEye.c***********************
***************CYB 06.09.20**********************
*************************************************
*/
#include "config.h"
#include "system.h"
#include "reg52.h"
#include "FindElecEye.h"
#include <string.h>
#include "TmrTsk.h"
#include  "tools.h"
#include  "TskMan.h"
//#include "flashdrv.h"
#include "GpsTool.h"
#include "hardware.h"
#include "TestWin.h"
#include "debug.h"
#define  EleceyePeriod   _SECOND,3
static BOOLEAN isGetInfoDot = FALSE;  
static BOOLEAN isGetElecEye = FALSE;
static INFODOT  *Infodotgps ;
static POS_STRUCT  *ElecEyegps;
static POS_STRUCT code *eleceyedata;

static INFODOT code *infodot;
static INT8U EleceyeTmrID;
static DATUM_STRUCT Gpsdata;
static INT16U CurRroadIndex = 0;
static INT16U CurRroadID = 0;

static code INT8U Latzones[] =  {0x05, 
                                 0x18, 0x1C, 0x48,0x57,0x18,0x1D,0x1F,0x55,
                                 0x18, 0x1D, 0x1F,0x55,0x18,0x1D,0x5A,0x53, 
                                 0x18, 0x1D, 0x5A,0x53,0x18,0x1E,0x31,0x51, 
                                 0x18, 0x1E, 0x31,0x51,0x18,0x1F,0x08,0x4F,
                                 0x18, 0x1F, 0x08,0x4F,0x18,0x1F,0x43,0x4D
                                };
static code INT8U Longzones[] = {0x05,
                                 0x76,0x04,0x3A,0x29,0x76,0x05,0x21,0x4F,
                                 0x76,0x05,0x21,0x4F,0x76,0x06,0x09,0x10,
                                 0x76,0x06,0x09,0x10,0x76,0x06,0x54,0x36,
                                 0x76,0x06,0x54,0x36,0x76,0x07,0x3B,0x5C, 
                                 0x76,0x07,0x3B,0x5C,0x76,0x08,0x23,0x1E
                                };
static code INT8U ValidZone[] = {0xFF,0xFF,0xFF,0x00,0xFF,
                                 0x01,0x02,0x03,0x04,0xFF,
                                 0x05,0x06,0x07,0x08,0x09,
                                 0xFF,0xFF,0x0a,0x0b,0xFF,
                                 0xFF,0xFF,0x0c,0xFF,0xFF
                                };
static code OFFSET_STRUCT Dotoffset[] = {0x0000,0x0003,
                                         0x0003,0x0001,
                                         0x0004,0x0004,
                                         0x0008,0x0003,
                                         0x000b,0x0004,
                                         0x000f,0x0003,
                                         0x0012,0x0007,
                                         0x0019,0x0002,
                                         0x001b,0x0004,
                                         0x001f,0x0005,
                                         0x0024,0x0002,
                                         0x0026,0x0003,
                                         0x0029,0x0002
                                };
static code OFFSET_STRUCT Elecoffset[] = {0x0000,0x0005,
                                          0x0005,0x0003,
                                          0x0008,0x0003,
                                          0x000b,0x0001,
                                          0x000c,0x0001,
                                          0x000d,0x0001,
                                          0x000e,0x0001,
                                          0x000f,0x0004,
                                          0x0013,0x0003,
                                          0x0016,0x0001
                                 };
static code INFODOT Infodots[]  = {
0x04,0x000f,0x0007,0x00c1,0xFFFF,0x00FF,0xFFFF,0x005E,0xFFFF,0x18,0x1C,0x4A,0x51,0x76,0x06,0x5B,0x3D,
0x04,0x002b,0x0007,0x005c,0xFFFF,0x00c6,0x0007,0x012e,0xFFFF,0x18,0x1D,0x0C,0x03,0x76,0x07,0x03,0x00,
0x04,0x002d,0x0007,0x0078,0xFFFF,0x00e1,0x0007,0x0133,0xFFFF,0x18,0x1D,0x1C,0x16,0x76,0x07,0x11,0x5E,
0x03,0x0015,0x0003,0x0063,0x0000,0x00c9,0x0003,0xFFFF,0xFFFF,0x18,0x1D,0x48,0x10,0x76,0x04,0x48,0x5D,
0x02,0x005d,0x0000,0x0111,0x0000,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x33,0x16,0x76,0x05,0x2D,0x14,
0x03,0x004c,0x0000,0x0111,0x0000,0x00a2,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x32,0x35,0x76,0x05,0x3A,0x3C,
0x03,0x004b,0x0000,0x00FF,0x0000,0x00a3,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x36,0x37,0x76,0x05,0x4A,0x0E,
0x04,0x015c,0xFFFF,0x004b,0x0000,0x00FF,0x0000,0x009c,0xFFFF,0x18,0x1D,0x3A,0x39,0x76,0x05,0x56,0x3F,
0x03,0x0156,0xFFFF,0x004b,0x0000,0x00FF,0x0000,0xFFFF,0xFFFF,0x18,0x1D,0x49,0x36,0x76,0x06,0x2C,0x05,
0x03,0x0043,0x0000,0x00FF,0x0000,0x0099,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x4D,0x1C,0x76,0x06,0x3A,0x15,
0x03,0x0042,0x0000,0x00f6,0x0000,0x0098,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x57,0x0C,0x76,0x06,0x4F,0x54,
0x03,0x002d,0x0007,0x00e1,0x0007,0x0080,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x29,0x31,0x76,0x07,0x1F,0x0B,
0x04,0x015d,0x0007,0x00e1,0x0007,0x00FF,0xFFFF,0x002d,0xFFFF,0x18,0x1D,0x34,0x3C,0x76,0x07,0x27,0x05,
0x04,0x015e,0x0007,0x00aa,0x0007,0x0050,0xFFFF,0x0104,0xFFFF,0x18,0x1D,0x46,0x4F,0x76,0x07,0x24,0x3E,
0x04,0x015e,0x0007,0x00aa,0x0007,0x0050,0xFFFF,0x0104,0xFFFF,0x18,0x1D,0x56,0x53,0x76,0x07,0x21,0x11,
0x03,0x0025,0x0003,0x00d9,0x0003,0x005a,0x0002,0xFFFF,0xFFFF,0x18,0x1E,0x2B,0x31,0x76,0x05,0x15,0x1A,
0x04,0x005a,0x0002,0x010e,0x0002,0x000f,0x0004,0x00c3,0x0004,0x18,0x1E,0x2B,0x28,0x76,0x05,0x20,0x03,
0x02,0x015a,0x0004,0x007b,0x0001,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0x18,0x1E,0x23,0x15,0x76,0x05,0x1E,0x48,
0x04,0x0000,0x0005,0x00b4,0x0005,0x005a,0x0002,0x010e,0x0002,0x18,0x1E,0x2B,0x31,0x76,0x05,0x37,0x4C,
0x04,0x0000,0xFFFF,0x00b4,0xffff,0x005a,0x0002,0x010e,0x0002,0x18,0x1E,0x2B,0x28,0x76,0x05,0x4B,0x2B,
0x03,0x0000,0x0005,0x005a,0x0001,0x010e,0x0001,0xFFFF,0xFFFF,0x18,0x1E,0x1C,0x61,0x76,0x05,0x37,0x19,
0x03,0x0000,0xffff,0x0060,0x0001,0x0108,0x0001,0xFFFF,0xFFFF,0x18,0x1E,0x1E,0x16,0x76,0x05,0x4B,0x33,
0x03,0x011f,0x0002,0x0032,0xffff,0x00bc,0x0002,0xFFFF,0xFFFF,0x18,0x1E,0x29,0x21,0x76,0x05,0x5E,0x28,
0x03,0x0008,0x0002,0x0061,0x0001,0x0115,0x0001,0xFFFF,0xFFFF,0x18,0x1E,0x1C,0x46,0x76,0x05,0x5D,0x21,
0x03,0x0115,0x0001,0x003f,0x0001,0x00d0,0xffff,0xFFFF,0xFFFF,0x18,0x1E,0x1A,0x14,0x76,0x06,0x07,0x31,
0x03,0x0003,0x0006,0x00ef,0x0001,0x0050,0x0001,0xFFFF,0xFFFF,0x18,0x1E,0x22,0x42,0x76,0x06,0x16,0x0c,
0x03,0x0000,0xFFFF,0x005a,0x0001,0x010e,0x0001,0xFFFF,0xFFFF,0x18,0x1E,0x27,0x41,0x76,0x06,0x39,0x5B,
0x02,0x0042,0x0000,0x00f6,0x0000,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0x18,0x1D,0x5D,0x24,0x76,0x06,0x5E,0x10,
0x02,0x0042,0x0000,0x00f6,0x0000,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0x18,0x1E,0x02,0x25,0x76,0x07,0x0E,0x29,
0x04,0x015e,0x0007,0x00aa,0x0007,0x0041,0x0000,0x00f5,0x0000,0x18,0x1E,0x08,0x4A,0x76,0x07,0x1D,0x22,
0x04,0x0050,0x0000,0x0104,0x0000,0x015c,0xFFFF,0x0098,0xFFFF,0x18,0x1E,0x0E,0x1D,0x76,0x07,0x29,0x44,
0x04,0x0009,0x0008,0x00bd,0x0008,0x005f,0x0000,0x0113,0x0000,0x18,0x1E,0x0E,0x00,0x76,0x07,0x45,0x19,
0x02,0x005f,0x0000,0x0113,0x0000,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0x18,0x1E,0x0D,0x20,0x76,0x07,0x50,0x34,
0x02,0x005f,0x0000,0x0113,0x0000,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0x18,0x1E,0x0D,0x04,0x76,0x07,0x54,0x4E,
0x04,0x005f,0x0000,0x0113,0x0000,0x0005,0xFFFF,0x00b4,0xFFFF,0x18,0x1E,0x0B,0x0A,0x76,0x08,0x05,0x41,
0x04,0x005f,0x0000,0x0113,0x0000,0x001e,0xFFFF,0x00d2,0xFFFF,0x18,0x1E,0x0A,0x36,0x76,0x08,0x0A,0x07,
0x03,0x015e,0x0007,0x00b4,0x0007,0x010e,0xFFFF,0xFFFF,0xFFFF,0x18,0x1F,0x05,0x1C,0x76,0x06,0x50,0x05,
0x04,0x0159,0x0007,0x0098,0x0007,0x003e,0xFFFF,0x010e,0xFFFF,0x18,0x1E,0x56,0x1D,0x76,0x06,0x53,0x04,
0x04,0x012e,0x0007,0x0087,0x0007,0x00e9,0xFFFF,0x0046,0xFFFF,0x18,0x1E,0x42,0x05,0x76,0x06,0x5E,0x1F,
0x03,0x00f7,0x0001,0x00b4,0x0001,0x013b,0xFFFF,0xFFFF,0xFFFF,0x18,0x1E,0x32,0x57,0x76,0x06,0x63,0x13,
0x04,0x00f7,0x0001,0x00b4,0xFFFF,0x0086,0x0007,0x013a,0x0007,0x18,0x1E,0x35,0x4F,0x76,0x07,0x05,0x0D,
0x04,0x0135,0x0007,0x008f,0x0007,0x003a,0xFFFF,0x00ee,0xFFFF,0x18,0x1F,0x22,0x4A,0x76,0x06,0x45,0x13, 
0x03,0x0152,0x0007,0x00aa,0x0007,0x004d,0xFFFF,0xFFFF,0xFFFF,0x18,0x1F,0x0F,0x30,0x76,0x06,0x4F,0x21
                                };
static code POS_STRUCT Eleceyes[]   ={0x18,0x1D,0x4B,0x39,0x76,0x04,0x4C,0x1B, 
                                      0x18,0x1D,0x39,0x1F,0x76,0x05,0x52,0x31, 
                                      0x18,0x1D,0x4F,0x09,0x76,0x06,0x3D,0x20, 
                                      0x18,0x1E,0x05,0x52,0x76,0x07,0x15,0x16, 
                                      0x18,0x1E,0x0D,0x57,0x76,0x07,0x45,0x5D,
                                      0x18,0x1E,0x1C,0x24,0x76,0x05,0x5F,0x19, 
                                      0x18,0x1E,0x22,0x24,0x76,0x06,0x13,0x5C, 
                                      0x18,0x1E,0x34,0x35,0x76,0x07,0x02,0x09, 
                                      0x18,0x1E,0x2B,0x31,0x76,0x05,0x1D,0x11, 
                                      0x18,0x1E,0x2B,0x27,0x76,0x05,0x37,0x14, 
                                      0x18,0x1E,0x1C,0x24,0x76,0x05,0x5F,0x19,
                                      0x18,0x1D,0x4B,0x39,0x76,0x04,0x4C,0x1B,
                                      0x18,0x1E,0x2B,0x31,0x76,0x05,0x1D,0x11,
                                      0x18,0x1E,0x2B,0x27,0x76,0x05,0x37,0x14, 
                                      0x18,0x1E,0x22,0x24,0x76,0x06,0x13,0x5C, 
                                      0x18,0x1E,0x34,0x35,0x76,0x07,0x02,0x09,
                                      0x18,0x1E,0x05,0x52,0x76,0x07,0x15,0x16, 
                                      0x18,0x1D,0x38,0x3F,0x76,0x07,0x27,0x15, 
                                      0x18,0x1C,0x51,0x60,0x76,0x06,0x5E,0x58, 
                                      0x18,0x1E,0x0D,0x57,0x76,0x07,0x45,0x5D 
                                };

                     
                                       
/**********************************************************
                查找纬度区间
***********************************************************/
static INT8U FindLatZone(INT8U *lat)
{
    INT8U i;
	INT8U nums;
    ZONE_STRUCT latzone;
      
   	nums = Latzones[0];
	memcpy((INT8U*)&latzone,&Latzones[1],8);
    //latzone = (ZONE_STRUCT*)&Latzones[1];
#if DEBUG_ELECDOG > 0
    printf("\n");
	printf("纬度区间个数:"); 
	puthex(nums);
#endif 

	for( i = 0 ; i<nums; i++)
	{
       if(CmpString(lat, latzone.downorleft, 4) == STR_GREAT)
	   {
	      if(CmpString(lat, latzone.uporright, 4) == STR_LESS)
 		  {
		       return i;
		  }
		  
		  //latzone += sizeof(ZONE_STRUCT);
           memcpy((INT8U*)&latzone,&Latzones[1+(i+1)*8],8);
          
	   }
	   else
	   {
	          return 0xff;
	   }
   }
}
/**********************************************************
                查找经度区间
***********************************************************/
static INT8U FindLongtZone(INT8U *longt)
{
     INT8U i;
	 INT8U nums;
     ZONE_STRUCT longzone;
     
	 nums = Longzones[0];
     //longzone =(ZONE_STRUCT*)&Longzones[1];
     memcpy((INT8U*)&longzone, &Longzones[1],8);	 
	 for( i = 0; i<nums;i++)
	 {
	    if(CmpString(longt, longzone.downorleft, 4)==STR_GREAT)
		{
		    if(CmpString(longt,longzone.uporright, 4)==STR_LESS)
			{
			     return i;   
			}
            
		  // longzone += sizeof(ZONE_STRUCT);
		    memcpy((INT8U*)&longzone, &Longzones[1+(i+1)*8],8);
        		   
		}
		else
		{
		         return 0xff;
		}
	 }
}
/**********************************************************
                获取路号（标定有电子眼的道路）
***********************************************************/
static INT16U GetRoadNumber(DATUM_STRUCT *gpsdata,INFODOT *infodot)
{
      INT8U i;
      INT16U dirs;

      if( !(infodot->Havemonitor))                             
 	  {
	     return 0xffff;
	  }
      dirs = (gpsdata->direction -1)*3;
	  
	  for(i=0 ; i<infodot->direc_nums ; i++)
	  {
	     if(dirs >= (infodot->road[i].direction-10) && dirs <= (infodot->road[i].direction+10))
         {
		    return infodot->road[i].roadindex;
		  }
	  }

	  return 0xffff;
}
/**********************************************************
                获取道路ID（标定限速的道路）
***********************************************************/
static INT16U GetRoadID(DATUM_STRUCT *gpsdata,INFODOT *infodot)
{
     INT8U i;
	 INT16U dirs;

	 if( !(infodot->Veclimited ))                             
 	 {
	     return 0xffff;
	 }
     
     dirs = (gpsdata->direction -1)*3;

     for(i=0 ; i<infodot->direc_nums ; i++)
	 {
	     if(dirs >= (infodot->road[i].direction-10) && dirs <= (infodot->road[i].direction+10))
         {
		    return infodot->road[i].roadID;
		 }
	 }

      return 0xffff;

}

/**********************************************************
                搜索路口信息点
***********************************************************/
static INT32U SearchInfoDot(DATUM_STRUCT *gpsdata,INT8U index_lat, INT8U index_longt)
{
      INT8U i;
      INT8U index;
	  INT8U valid;
	  INT16U pos;
	  INT16U roadindex;
	  INT16U roadID;
	  INT16U nums;
	  INT32U len_32;
	  INT32U roadinfo;
#if DEBUG_ELECDOG >0      
	  INT8U  temp[4];
#endif 
	  roadindex = 0xfffe;
      roadID = 0xfffe;