gpslib.c

GPS的一个任务

#include <vxWorks.h>
#include <ioLib.h>
#include <stdio.h>
#include <intLib.h>
#include <string.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <taskLib.h>
#include <time.h>
#include <rebootLib.h>
#include <selectLib.h>
#include <math.h>
#include <private/semLibP.h>

#include "util.h"
#include "gisclient.h"
#include "sysMesLib.h"
#include "gsmcdmaLib.h"
#include "gpsLib.h"
#include "iisLib.h"
#include "gpsPdu.h"
#include "breakPoint.h"

#include "gpsMileage.h"
#include "../iniLib/iniFileLib.h"
#include "ioDetecLib.h"
#include "adcLib.h"

#include "tcpConnLib.h"

#include "sysInfo.h"
#include "cardReaderLib.h"

#undef rPCOND
#undef rPDATD
#define rPCOND		(*(volatile unsigned *)0x01D2001C)
#define rPDATD		(*(volatile unsigned *)0x1d20020)

#ifndef MAX
#define MAX(a, b)       ((a) > (b)? (a): (b))
#endif


#define PI 3.14159265		/*圆周率,用于根据两点经纬度求距离*/
#define R 6.371229*1e6		/*用于根据两点经纬度求距离*/
#define MILETIME  30            /*用于保存累计历程保存的时间间隔*/

char GPSDEVNAME[] = "/pty/gpsS";
char gpsTaskName[] = "tGpsTask";
char GGA_MSG[] = "$GPGGA";
char RMC_MSG[] = "$GPRMC";
char GSV_MSG[] = "$GPGSV";
static int gpsFd;
int gpsstop = FALSE;

char *gpsData=0;
char *jkGpsData=0;
int positionValid = 0;

int gpsTaskId;
int saveGpsTaskId;
int sendGpsTaskId;

float gpsLongitude;
float gpsLatitude;
float oldGpsLong;/*记录上一点的经度*/
float oldGpsLat;/*记录上一点的纬度*/

int runFlag = 0;/*记录运行状态*/

/*铁闪使用*/
SEM_ID framSem;
int stopZero = 0;/*记录是不是第一次速度等于0, 用于铁闪*/
int sendGpsFlag = 0;/*标识GPS是否处理完成*/
void *pSaveGpsMsgQueue;
void *pSendGpsMsgQueue;
float latitude,longitude;
char gpsRunBuf[1300];
char timeBuf[70];
ULONG gpsMileageTime;
ULONG overSpeedtime;
ULONG stopTime;
int checkSysTime = 0;

char gpsNumber[10];  /*buffer for save Total number of satellites in view, 00 to 12*/

int currentSpeed=0;


#undef NOGPS
static void gpsTask();
static void saveGpsTask();
static void sendGpsTask();

int gpsLibInited = FALSE;
int gpsNeedUpTime = FALSE;

void gpsTimeout()
{
	extern BOOL systemOk;
	systemOk = FALSE;
}


/*gpsLibInit - init gps lib, spawn gps task read gps message and handle
*
*
*/
void gpsLibInit()
{
	gpsFd = open(GPSDEVNAME, O_RDWR, 0);
	ioctl(gpsFd, FIOOPTIONS, OPT_LINE);
	gpsData = (char *)malloc(100);
	jkGpsData = (char *)malloc(100);
	gpsMileageTime = tickGet();
	overSpeedtime = tickGet();
	stopTime = tickGet();
	gpsTaskId = taskSpawn(gpsTaskName, 50, VX_FP_TASK, 40*1024, (FUNCPTR)gpsTask, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
	while(!gpsLibInited)
		taskDelay(20);
	DEBUG_PRINT(("GPS task init ok.\n"));
}
/*计算两点经纬度之间的距离*/
double countDistance(double oldLong, double oldLat, double newLong, double newLat){
	double out;
	double x;
	double y;
	x=(newLong-oldLong)*PI*R*cos(((oldLat+newLat)/2)*PI/180)/180;
	y=(newLat-oldLat)*PI*R/180;
	out=sqrt(x*x+y*y);
	return out;
}
STATUS sysTimeSetByGps(char *gpsmsg)
{
	char paramBuffer[40];
	char timebuf[4];
	int year, mon, day, hour, min, sec;

	DEBUG_PRINT(("校准时间\r\n"));

	/*UTC date of position fix, ddmmyy format*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(gpsmsg, 9, paramBuffer);
	memset(timebuf, 0, sizeof(timebuf));
	/*get date*/
	timebuf[0] = paramBuffer[0];
	timebuf[1] = paramBuffer[1];
	day  = atoi(timebuf);
	/*get mon*/
	timebuf[0] = paramBuffer[2];
	timebuf[1] = paramBuffer[3];
	mon  = atoi(timebuf);
	/*get year*/
	timebuf[0] = paramBuffer[4];
	timebuf[1] = paramBuffer[5];
	year  = atoi(timebuf)+100;

	/*UTC time of position fix, hhmmss format*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(gpsmsg, 1, paramBuffer);
	/*get hour*/
	timebuf[0] = paramBuffer[0];
	timebuf[1] = paramBuffer[1];
	hour  = atoi(timebuf);
	/*get min*/
	timebuf[0] = paramBuffer[2];
	timebuf[1] = paramBuffer[3];
	min  = atoi(timebuf);
	/*get sec*/
	timebuf[0] = paramBuffer[4];
	timebuf[1] = paramBuffer[5];
	sec  = atoi(timebuf);

	return sysTimeSetUtc(year, mon, day, hour, min, sec);
}


STATUS gpsMsgPackTcp(char *oldGpsMsg, char *newGpsMsg, double *pangle, double *pspeed, int *ppositionValid)
{
	char paramBuffer[40];
	char temp1, temp2;
	long mile;
	int st;
	double st1;
	char temp3[10];
	/*intLock();*/

	newGpsMsg[0] = 0;
	/*UTC time of position fix, hhmmss format*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 1, paramBuffer);
	if (strlen(paramBuffer) < 6 )
		strncat(newGpsMsg, "000000", 6);
	else
		strncat(newGpsMsg, paramBuffer, 6);

	/*Status, A = Valid position, V = NAV receiver warning*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 2, paramBuffer);
	*ppositionValid = paramBuffer[0] == 'A' ? 1 : 0;
	strcat(newGpsMsg, *ppositionValid?"A":"V");

	/*Latitude, ddmm.mmmm format (leading zeros will be transmitted)*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 3, paramBuffer);
	if (strlen(paramBuffer)>=9)
	{
		strncat(newGpsMsg, paramBuffer, 9);
	}
	else if (strlen(paramBuffer)<9)
	{
		strncat(newGpsMsg, "0000.0000", 9 - strlen(paramBuffer));
		strcat(newGpsMsg, paramBuffer);
	}

	memset(temp3, '\0', 10);
	memcpy(temp3, paramBuffer, 2);
	gpsLatitude = atoi(temp3);
	memset(temp3, '\0', 10);
	memcpy(temp3, &paramBuffer[2], 7);
	gpsLatitude =gpsLatitude + atof(temp3) / 60;
	if(oldGpsLat == 0)
		oldGpsLat = gpsLatitude;

	/*Latitude hemisphere, N or S*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 4, paramBuffer);
	if (strlen(paramBuffer) == 1)
		strcat(newGpsMsg, paramBuffer);
	else
		strcat(newGpsMsg, "N");

	/*Longitude, dddmm.mmmm format (leading zeros will be transmitted)*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 5, paramBuffer);
	if (strlen(paramBuffer)>=10)
		strncat(newGpsMsg, paramBuffer, 10);
	else if (strlen(paramBuffer)<10)
	{
		strncat(newGpsMsg, "00000.0000", 10 - strlen(paramBuffer));
		strcat(newGpsMsg, paramBuffer);
	}

	memset(temp3, '\0', 10);
	memcpy(temp3, paramBuffer, 3);
	gpsLongitude = atoi(temp3);
	memset(temp3, '\0', 10);
	memcpy(temp3, &paramBuffer[3], 7);
	gpsLongitude = gpsLongitude + atof(temp3) / 60;
	if(oldGpsLong == 0)
		oldGpsLong = gpsLongitude;

	/*Longitude hemisphere, E or W*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 6, paramBuffer);
	if (strlen(paramBuffer) == 1)
		strcat(newGpsMsg, paramBuffer);
	else
		strcat(newGpsMsg, "E");

	/*Speed over ground, 000.0 to 999.9 knots (leading zeros will be transmitted)*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 7, paramBuffer);
	if (strlen(paramBuffer) == 0)
		*pspeed = 0.0;
	else
		*pspeed = atof(paramBuffer);
	if ((*pspeed>=300.0) || (*pspeed < 0))
		*pspeed = 0.0;
        /*DEBUG_PRINT(("=========pspeed=%f\n\r",*pspeed));*/
	if(*pspeed > 2.69978 && positionValid){
		mile = countDistance(oldGpsLong,oldGpsLat,gpsLongitude,gpsLatitude);
		if(mile < 500 && positionValid){
			gpsMileage = gpsMileage + mile;
			oldGpsLong = gpsLongitude;
			oldGpsLat = gpsLatitude;
		}else{
			oldGpsLong = 0;
			oldGpsLat = 0;
		}
	}else{
		oldGpsLong = 0;
		oldGpsLat = 0;
		/*当定不到位或者是速度小于5公里每小时的时候将上传的速度设为0*/
		if(*pspeed < 1.5)
		    *pspeed = 0.0;
	}
	memset(paramBuffer, 0, sizeof(paramBuffer));
	sprintf(paramBuffer, "%05.1f", *pspeed);
	strcat(newGpsMsg, paramBuffer);
	/*strcat(newGpsMsg, "020.0");*/

	/*UTC date of position fix, ddmmyy format*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 9, paramBuffer);
	if (strlen(paramBuffer) < 6)
		strcpy(paramBuffer, "000000");
	temp1 = paramBuffer[0];
	temp2 = paramBuffer[1];
	paramBuffer[0] = paramBuffer[4];
	paramBuffer[1] = paramBuffer[5];
	paramBuffer[4] = temp1;
	paramBuffer[5] = temp2;
	strcat(newGpsMsg, paramBuffer);

	/*Course over ground, 000.0 to 359.9 degrees, true (leading zeros will be transmitted)*/
	memset(paramBuffer, 0, sizeof(paramBuffer));
	getParam(oldGpsMsg, 8, paramBuffer);
	if (strlen(paramBuffer) == 0)
		*pangle = 0.0;
	else
		*pangle = atof(paramBuffer);
	if ((*pangle > 360.00) || (*pangle < 0.0))
		*pangle = 0.0;
	memset(paramBuffer, 0, sizeof(paramBuffer));
	sprintf(paramBuffer, "%06.2f", *pangle);
	strcat(newGpsMsg, paramBuffer);
	/*strcat(newGpsMsg, "359.00");*/
	return strlen(newGpsMsg) == 45 ? OK : ERROR;

}


/*组织GPS信息格式参见移动车管家*/
void busPduPack(char *gpsPdu)
{
	/*strcpy(gpsData, "061825V2432.0733N10627.2544E000.0040331309.62");*/
	int  ispeed;
	int  ilen;
	char GpsMsg[100];
	char SendGpsMsg[100];
	char temp[20],htemp[20];
	command_t *pCommNode;
	memset(SendGpsMsg,0,100);
	memset(GpsMsg,0,100);		
	/*经度*/
	memset(htemp,0,20);
	memset(temp,0,20);
	strncat(temp,&gpsPdu[17],5);
	strncat(temp,&gpsPdu[23],3);
	/*DEBUG_PRINT(("经度=%s\n\r",temp));*/	
	ascii_2_hex(temp,htemp,8);
	GpsMsg[0] = 0x01;
	GpsMsg[1] = 0x10;
	GpsMsg[2] = 0x04;
	GpsMsg[3] = 0x00;
	GpsMsg[4] = htemp[0];
	GpsMsg[5] = htemp[1];
	GpsMsg[6] = htemp[2];
	GpsMsg[7] = htemp[3];
	/*纬度*/
	memset(htemp,0,20);
	memset(temp,0,20);
	temp[0] = '0';
	strncat(&temp[1],&gpsPdu[7],4);
	strncat(&temp[3],&gpsPdu[12],3);
	/*DEBUG_PRINT(("经度=%s\n\r",temp));*/	
	ascii_2_hex(temp,htemp,8);
	GpsMsg[8] = 0x02;
	GpsMsg[9] = 0x10;
	GpsMsg[10] = 0x04;
	GpsMsg[11] = 0x00;
	GpsMsg[12] = htemp[0];
	GpsMsg[13] = htemp[1];
	GpsMsg[14] = htemp[2];
	GpsMsg[15] = htemp[3];
	/*方向*/
	memset(htemp,0,20);
	memset(temp,0,20);
	temp[0] = '0';
	strncat(&temp[1],&gpsPdu[39],3);	
	/*DEBUG_PRINT(("方向=%s\n\r",temp));*/	
	ascii_2_hex(temp,htemp,4);
	GpsMsg[16] = 0x03;
	GpsMsg[17] = 0x10;
	GpsMsg[18] = 0x02;
	GpsMsg[19] = 0x00;
	GpsMsg[20] = htemp[0];
	GpsMsg[21] = htemp[1];
	/*速度*/
	memset(htemp,0,20);
	memset(temp,0,20);	
	strncat(&temp[0],&gpsPdu[28],5);	
	/*DEBUG_PRINT(("速度%s\n\r",temp));*/
	ispeed = atoi(temp)*1.852;
	memset(temp,0,20);
	sprintf(temp,"%04d",ispeed);	
	ascii_2_hex(temp,htemp,4);	
	GpsMsg[22] = 0x04;
	GpsMsg[23] = 0x10;
	GpsMsg[24] = 0x02;
	GpsMsg[25] = 0x00;
	GpsMsg[26] = htemp[0];
	GpsMsg[27] = htemp[1];		
	/*时间串*/
	memset(htemp,0,20);
	memset(temp,0,20);	
	strncat(temp,&gpsPdu[33],6);
	strncat(temp,&gpsPdu[0],6);