T640_DTU/USER/GPS.c

 /**********************************************************************************
 * File Name: GPS.c
 * Function Describe:device for GPS	
 * Relate Module:
***********************************************************************************/
#define THIS_FILE_ID 112
/* Includes ------------------------------------------------------------------*/
#include "GPS.h"
#include "includes.h"

SUT_GPS_INF sutGpsInfo;
const static unsigned char SetBDEnCmd[]={
	0xB5,0x62,0x06,0x3E,0x2C,0x00,0x00,0x00,0x20,0x05,0x00,0x08,0x10,0x00,0x01,0x00,0x01,0x01,0x01,0x01,0x03,0x00,0x00,0x00,0x01,0x01,0x03,
	0x08,0x10,0x00,0x01,0x00,0x01,0x01,0x05,0x00,0x03,0x00,0x00,0x00,0x01,0x01,0x06,0x08,0x0E,0x00,0x00,0x00,0x01,0x01,0xFD,0x25,0xB5,0x62,
	0x06,0x17,0x14,0x00,0x00,0x40,0x00,0x02,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x75,0x50,0xB5,
	0x62,0x05,0x01,0x02,0x00,0x06,0x17,0x25,0x4E
};
void GpsInit(void)
{

	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC, ENABLE);
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_InitStructure.GPIO_Pin = GPS_PWR_PIN;
	GPIO_Init(GPS_PWR_PORT, &GPIO_InitStructure);
	
	memset(&sutGpsInfo,0,sizeof(SUT_GPS_INF));
	GPS_PWR_DIS;
	FEED_EXTWATCHDOG();//喂狗
	IWDG_ReloadCounter();//喂狗
	os_dly_wait(50);
	FEED_EXTWATCHDOG();//喂狗
	IWDG_ReloadCounter();//喂狗
	GPS_PWR_EN;
	RunMake(THIS_FILE_ID);
	os_dly_wait(1);
	Uart2Init();
#if GPS_TYPE==GPS_TYPE_UBLOX	//UBlox方案才需要发如下指令给GPS模块，启动北斗功能
	os_dly_wait(50); 
	FEED_EXTWATCHDOG();//喂狗
	IWDG_ReloadCounter();//喂狗
	Uart2Send((char *)SetBDEnCmd,sizeof(SetBDEnCmd));
	os_dly_wait(50); 
	FEED_EXTWATCHDOG();//喂狗
	IWDG_ReloadCounter();//喂狗
	Uart2Send((char *)SetBDEnCmd,sizeof(SetBDEnCmd));
#endif 
}

int HEX_2_INT32(int hex_val)
{
	char tmp_buf[13] = {0};
	int val;
	sprintf(tmp_buf, "%x", hex_val);
	val = atoi (tmp_buf);
	return val;
}
static unsigned char translate_digitAscii_to_bcd(char *ascii_number, unsigned char len, char *bcd_number)
{
	int i, j;
	char temp[10] = "";  /* extra + and NULL */
	unsigned char bcd_index = 0;          /* Index into output bcd_number */
	
	int num_len = len;
	
	
	/* Translate or skip over invalid characters */
	for (i = 0, j = 0; i < num_len; i++)
	{
		temp[j++] = ascii_number[i] & 0x0F;
	}
	
	ascii_number[len] = 0;
	/* Now that temp has the bcd codes in natural order... Squish them together
	* and reverse the order per bcd coding.
	*/
	for (i = 0; i < j; i+=2)
		bcd_number[bcd_index++] = ((char)(temp[i] << 4)) | temp[i+1];
	
	bcd_number[len] = 0;
	
	return 0;
}
/*=====================================
UTC time:hms   status       latitue         north?    longitue          east?   ??(?)   ???  UTC time      ???      ??   ??      checksum
$GPRMC    hhmmss.sss      A/V      ddmm.mmmm   N/S   dddmm.mmmm  E/W      speed             aspect        ddmmyy      000-180            E/W        A/D/E/N     *hh
For example: $GPRMC,024813.640,A,3158.4608,N,11848.3737,E,10.05,324.27,150706,,,A*50
*/

#define GPRMC_UTC1_TOKEN_INDEX						0
#define GPRMC_STATUS_TOKEN_INDEX					1
#define GPRMC_LATITUE_TOKEN_INDEX					2
#define GPRMC_NORTH_TOKEN_INDEX						3
#define GPRMC_longitue_TOKEN_INDEX				4
#define GPRMC_EAST_TOKEN_INDEX						5
#define GPRMC_SPEED_TOKEN_INDEX						6
#define GPRMC_ASPECT_TOKEN_INDEX					7
#define GPRMC_UTC2_TOKEN_INDEX						8


/*
从str找到第一个字符为ch的位置返回其位置
位置从0开始计算
*/
int FindChIndex(char *str,char ch)
{
	char *p=str;
	int i=0; 
	while(*p){
		if(*p==ch)return i;
		i++;
		p++;
	}
}
/*************************************************************************************
*process_gps_number
获取卫星个数
$GPGSA,A,3,33,14,22,27,31,32,08,35,21,,,,1.6,1.1,1.2*38
$BDGSA,A,3,01,02,03,04,08,09,13,06,,,,,1.6,1.1,1.2*26
**************************************************************************************/
void process_gps_number(char *buf,uint32_t size)
{
	int flag=0;
	int i,j,num;
	char ch,ch1,ch2;
	if(buf[1]=='G' && buf[2]=='P')flag=1;
	else if(buf[1]=='B' && buf[2]=='D')	flag=2;
	else return;
	i=11;j=0;num=0;
	while(i<size && j<11){
		ch=buf[i];
		if(ch==','){
			i++;
			j++;
		}else if(ch>='0' && ch<='9'){
			i+=2;
			num++;
		}else{//非法
			break;
		}
	}
	if(flag==1){
		sutGpsInfo.GPSNum=num;
	}else if(flag==2){
		sutGpsInfo.BDNum=num;
	}

}
/*************************************************************************************
*process_gps_data
**************************************************************************************/
void process_gps_data(char *buf,uint32_t size)
{

	char tmp_char[16] = {0};
	char  bcd_tmp[8] = {0};
	char *token[16] = {NULL};
	char size_each_token[16] = {0};
    unsigned char hh,hl;
	unsigned char token_index = 0;
	char ch;
	int i = 0;
	int index = 0;
	int dig_bit_num = 0;
	int ptr_each = 0;
	unsigned short speed;
	unsigned short aspect;

	double speed_jie = 0.0;
	
	token[token_index] = buf;
	RunMake(THIS_FILE_ID);	
	while(index < size )
	{
		if(buf[index] == ',')
		{
			size_each_token[token_index] = ptr_each;
			if(ptr_each == 0)
				token[token_index] = NULL;
			ptr_each = 0;
			token_index++;
			token[token_index] = buf+index+1;  //skip this ',' 
			if(*(token[token_index]+1) == '*')
			{
				size_each_token[token_index] = 1;	     //cut of *hh\r\n
			}        
		}
		else
		{
			ptr_each++;    
		}
		index++;
	}
	RunMake(THIS_FILE_ID);
   
	//UTC time:hms    
	//014216 = 01:42:16
	if(token[GPRMC_UTC1_TOKEN_INDEX])   
	{ //,040831.00,  一般长度为9
		if(size_each_token[0]<6 && size_each_token[0]>10){
			//printf("GPS Error1 %d\r\n",size_each_token[0]);
			return;
		}
		strncpy((char*)tmp_char,(char*)token[0],size_each_token[0]);
		translate_digitAscii_to_bcd(tmp_char,6,bcd_tmp);  //??.xx
	  sutGpsInfo.hour=bcd_tmp[0];
		sutGpsInfo.minu = bcd_tmp[1];
		sutGpsInfo.sec = bcd_tmp[2];
	}  	
	RunMake(THIS_FILE_ID);
		//UTC2
	//250219 = 19-02-25
	if(token[GPRMC_UTC2_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		memset(bcd_tmp , 0 ,sizeof(bcd_tmp));    	   
		if(size_each_token[GPRMC_UTC2_TOKEN_INDEX]!=6){
			//printf("GPS Error2\r\n");
			return;
		}
		strncpy((char*)tmp_char,(char*)token[GPRMC_UTC2_TOKEN_INDEX],size_each_token[GPRMC_UTC2_TOKEN_INDEX]);
		translate_digitAscii_to_bcd(tmp_char,6,bcd_tmp);  
		
		sutGpsInfo.day = bcd_tmp[0];
		sutGpsInfo.month= bcd_tmp[1];
		sutGpsInfo.year = bcd_tmp[2];
	}
		
		
	RunMake(THIS_FILE_ID);
		//Fix status
	if(token[GPRMC_STATUS_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		if(size_each_token[1]!=1){
			SlwTrace(DEBUG,"GpsDataError!\r\n");
			return;
		} 
		strncpy((char*)tmp_char,(char*)token[1],size_each_token[1]);
		if(*tmp_char == 'A'){
			sutGpsInfo.isGpsValid = 1;
		}else{
			sutGpsInfo.isGpsValid = 0;
			sutGpsInfo.Altitude=0;
			sutGpsInfo.Latitude=0;
			sutGpsInfo.Longitude=0;
			sutGpsInfo.Aspect=0;
			sutGpsInfo.Speed=0;
			return;
		}
	}
	RunMake(THIS_FILE_ID);	
    //latitude ??
	/*=====================================
	UTC time:hms   status       latitue         north?    longitue          east?   ??(?)   ???  UTC time      ???      ??   ??      checksum
	$GPRMC    hhmmss.ss      A/V      ddmm.mmmm   N/S   dddmm.mmmm  E/W      speed             aspect        ddmmyy      000-180            E/W        A/D/E/N     *hh
	For example: $GPRMC,024813.640,A,3158.4608,N,11848.3737,E,010.5,324.7,150706,,,A*50

	*/
	if(token[GPRMC_LATITUE_TOKEN_INDEX])   
	{
		ch=*token[GPRMC_NORTH_TOKEN_INDEX];
		char *p=token[GPRMC_LATITUE_TOKEN_INDEX];
		//Ublox格式：ddmm.mmmmm
		//中科微电格式：ddmm.mmmm
		int ddmm=atoi(p);
		int mmmmm=atoi((p+5));
		int dd=ddmm/100;
		int mm=ddmm%100;
		//double fmm=(double)mm+(double)mmmmm/100000.0;
		//sutGpsInfo.latitude=(double)dd+fmm/60.0;
		sutGpsInfo.Latitude=dd*1000000;//度 放大1000000倍，整数部分
		mm*=1000000;
#if GPS_TYPE==GPS_TYPE_UBLOX
		mmmmm*=10;
#else
		mmmmm*=100;
#endif
		mm+=mmmmm;//分 放大1000000倍
		dd=mm/60;//度 放大1000000倍  小数部分
		sutGpsInfo.Latitude+=dd;//度 放大1000000倍
		if(ch=='S'){
			sutGpsInfo.South=1;
		}else{
			sutGpsInfo.South=0;
		}
		//sprintf(sutGpsInfo.vv1,"%d%d.%d\r\n",dd,mm,mmmm);

		
	}
	
	//longitude ??
	if(token[GPRMC_longitue_TOKEN_INDEX])   
	{//dddmm.mmmmm,E/W
#if 0
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		memset(bcd_tmp , 0 ,sizeof(bcd_tmp));    
		RunMake(THIS_FILE_ID);
		strncpy((char*)tmp_char,(char*)token[4],size_each_token[4]);
		for(i = 5; i < 8;i++)
			tmp_char[i] = tmp_char[i+1];
		tmp_char[8] = 0;
		translate_digitAscii_to_bcd(tmp_char,8,bcd_tmp);  //??.xx
		RunMake(THIS_FILE_ID);	
		//east or west    
		if(token[GPRMC_EAST_TOKEN_INDEX])
		{
			memset(tmp_char , 0 ,sizeof(tmp_char));    
			strncpy((char*)tmp_char,(char*)token[5],size_each_token[5]);
			sutGpsInfo.EWFlag=*tmp_char;
			sutGpsInfo.longitude = (bcd_tmp[3] | (bcd_tmp[2] << 8) |(bcd_tmp[1] << 16) |(bcd_tmp[0] << 24));   
		}
#else
		ch=*token[GPRMC_EAST_TOKEN_INDEX];
		if(ch=='W')sutGpsInfo.West=1;
		else sutGpsInfo.West=0;
		
		char *p=token[GPRMC_longitue_TOKEN_INDEX];
		int dddmm=atoi(p);
		int mmmmm=atoi((p+6));
		int ddd=dddmm/100;
		int mm=dddmm%100;
		//double fmm=(double)mm+(double)mmmmm/100000.0;
		//sutGpsInfo.longitude=(double)ddd+fmm/60.0;
		
		sutGpsInfo.Longitude=ddd*1000000;//度 放大1000000倍  整数部分
		mm*=1000000;
#if GPS_TYPE==GPS_TYPE_UBLOX
		mmmmm*=10;
#else
		mmmmm*=100;
#endif
		mm+=mmmmm;//分 放大1000000倍
		ddd=mm/60;//度 放大1000000倍  小数部分
		sutGpsInfo.Longitude+=ddd;
		
#endif
	}
	
	//Speed
	//0.094
	if(token[GPRMC_SPEED_TOKEN_INDEX])   
	{
		RunMake(THIS_FILE_ID);
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		strncpy((char*)tmp_char,(char*)token[GPRMC_SPEED_TOKEN_INDEX],size_each_token[GPRMC_SPEED_TOKEN_INDEX]);
		index = 0;
		while(index < size_each_token[GPRMC_SPEED_TOKEN_INDEX])
		{
			if(tmp_char[index] == '.')break;
				index++;
		}
		dig_bit_num = size_each_token[GPRMC_SPEED_TOKEN_INDEX]-index-1;
		RunMake(THIS_FILE_ID);
		speed_jie = atoi(tmp_char);
		if(dig_bit_num>0)speed_jie += (tmp_char[index+1]-0x30)*0.1;
		//sutGpsInfo.Speed=(int)(speed_jie/0.1852);// 1/10 km/h
		sutGpsInfo.Speed=(int)(speed_jie*18.52);// 1/10 km
	}
	RunMake(THIS_FILE_ID);
	//Aspect ??
	if(token[GPRMC_ASPECT_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		if(size_each_token[GPRMC_ASPECT_TOKEN_INDEX]==0){
			sutGpsInfo.Aspect=0;
		}else{
			strncpy((char*)tmp_char,(char*)token[GPRMC_ASPECT_TOKEN_INDEX],size_each_token[GPRMC_ASPECT_TOKEN_INDEX]);
			aspect=atoi(tmp_char);
			sutGpsInfo.Aspect= aspect;
		}
		
	}else sutGpsInfo.Aspect=0;
	RunMake(THIS_FILE_ID);
}		

#ifdef GPS_DEBUG_VALID
static unsigned char sucTimeH,sucTimeL;
static unsigned long susTime=(7*3600+30*60+0);
static unsigned long sucLatitue=0x02237542;//;
static unsigned long sucLongitue=0x11401316;//;
#endif
/*************************************************************************************
process_nema
**************************************************************************************/
void process_nema(char *nema)  
{
	static int susTime=61200;
	uint8_t sucTimeH,sucTimeL;
	static unsigned char sucCt=0;
	int len;
	len=strlen(nema);
	//$GPRMC  or $GNRMC or $GPGSA or $BDGSA
	if(nema[0]!='$')return;
	if(nema[3]=='R' && nema[4]=='M' && nema[5]=='C'){
		//获取时间、经纬度、速度、方位
		process_gps_data(&nema[7],len);
	}
//	else if(nema[3]=='G' && nema[4]=='S' && nema[5]=='A'){
//		//获取使用卫星个数
//		process_gps_number(&nema[0],len);
//	}
	
	
	
#ifdef GPS_DEBUG_VALID
		SlwTrace(DEBUG,"GPS_DEBUG_VALID!!!");
		if(sutGpsInfo.isGpsValid==0){
			sutGpsInfo.isGpsValid=1;
			sutGpsInfo.latitue=sucLatitue;
			sutGpsInfo.longitue=sucLongitue;
			sutGpsInfo.speed=0x4321;
			sutGpsInfo.aspect=0x7654;
			sutGpsInfo.StatisticsMile=0x123456;
			sutGpsInfo.year=0x15;
			sutGpsInfo.month=0x07;
			sutGpsInfo.day=0x26;
			
			susTime++;
			sutGpsInfo.hour=(unsigned char)(susTime/3600);
			sutGpsInfo.minu=(unsigned char)((susTime%3600)/60);
			sutGpsInfo.sec=(unsigned char)(susTime%60);
			sucTimeH=sutGpsInfo.hour/10;
			sucTimeL=sutGpsInfo.hour%10;
			sutGpsInfo.hour=(sucTimeH<<4)+sucTimeL;
			
			sucTimeH=sutGpsInfo.minu/10;
			sucTimeL=sutGpsInfo.minu%10;
			sutGpsInfo.minu=(sucTimeH<<4)+sucTimeL;
			
			sucTimeH=sutGpsInfo.sec/10;
			sucTimeL=sutGpsInfo.sec%10;
			sutGpsInfo.sec=(sucTimeH<<4)+sucTimeL;
		}
#endif

}

/*
UTC（BCD） 时间转换为 北京时间
	 unsigned short Years; 
	 char Months;
	 char Days;
	 char Hours;
	 char Minute;
	 char Seconds;
*/
void GpsConversionTime(void)
{
	unsigned short t;
	if(sutGpsInfo.year==0){
		sutGpsInfo.Years=0;
		sutGpsInfo.Months=0;
		sutGpsInfo.Days=0;
		sutGpsInfo.Hours=0;
		sutGpsInfo.Minute=0;
		sutGpsInfo.Seconds=0;
		return;
	}
#if 0	//test
	sutGpsInfo.year=0x19;
	sutGpsInfo.month=0x02;
	sutGpsInfo.day=0x28;
	sutGpsInfo.hour=0x20;
#endif
	t=sutGpsInfo.year>>4;
	t*=10;
	t+=(sutGpsInfo.year&0x0f);
	sutGpsInfo.Years=2000+t;
	
	t=sutGpsInfo.month>>4;
	t*=10;
	t+=(sutGpsInfo.month&0x0f);
	sutGpsInfo.Months=t;
	
	t=sutGpsInfo.day>>4;
	t*=10;
	t+=(sutGpsInfo.day&0x0f);
	sutGpsInfo.Days=t;
	
	t=sutGpsInfo.hour>>4;
	t*=10;
	t+=(sutGpsInfo.hour&0x0f);
	sutGpsInfo.Hours=t;
	
	t=sutGpsInfo.minu>>4;
	t*=10;
	t+=(sutGpsInfo.minu&0x0f);
	sutGpsInfo.Minute=t;
	
	t=sutGpsInfo.sec>>4;
	t*=10;
	t+=(sutGpsInfo.sec&0x0f);
	sutGpsInfo.Seconds=t;
	
	//转换为北京时间
	sutGpsInfo.Hours+=8;
	if(sutGpsInfo.Hours>23){
		sutGpsInfo.Hours-=24;
		if(++sutGpsInfo.Days>MonthDays(sutGpsInfo.Years,sutGpsInfo.Months)){
			sutGpsInfo.Days=1;
			if(++sutGpsInfo.Months>12){
				sutGpsInfo.Months=1;
				sutGpsInfo.Years++;
			}
		}
	}
	
}