?? gps.c
字號:
#include "pin.h"
#include "public.h"
#include "uart.h"
#include "commrtos.h"
#include "app.h"
#include "ParaManage.h"
double GPStrans (INT32U gpsData)
{
return ((float)(gpsData))*1.0/1000000;
}
INT32U GetDistance(double StartLat,double StartLong,double EndLat,double EndLong)
{
double fPhimean,fdLambda,fdPhi,fAlpha,fRho,fNu,fR,fz,fTemp,Distance;//,Bearing;
double D2R = 0.017453;
double a = 6378137.0;
double e2 = 0.006739496742337;
INT32U tmp;
fdLambda = (StartLong - EndLong) * D2R;
fdPhi = (StartLat - EndLat) * D2R;
fPhimean = ((StartLat + EndLat) / 2.0) * D2R;
fTemp = 1 - e2 * (pow(sin(fPhimean),2));
fRho = (a * (1 - e2)) / pow(fTemp, 1.5);
fNu = a / (sqrt(1 - e2 * (sin(fPhimean) * sin(fPhimean))));
fz = sqrt(pow(sin(fdPhi/2.0),2)+cos(EndLat*D2R)*cos(StartLat*D2R)*pow(sin(fdLambda/2.0),2)) ;
fz = 2 * asin(fz);
fAlpha = cos(EndLat * D2R) * sin(fdLambda) * 1 / sin(fz);
fAlpha = asin(fAlpha);
fR = (fRho * fNu) / ((fRho * pow(sin(fAlpha),2)) + (fNu * pow(cos(fAlpha),2)));
Distance = (fz * fR);
tmp = (INT32U) Distance;
return tmp;
}
//字符轉換成浮點型(經度和緯度)
void TransData (INT8U *StrData,INT8U Num)
{
typedef union
{
INT32U l;
INT8U buf[4];
} ULL;
ULL ll;
char buf[20];
INT8U j,dotloc;
INT32U L1;
INT32U L2;
INT32U L3;
j=0;
dotloc = 0;
memset(buf,0,20);
do
{
buf[j] = StrData[j];
j++;
if(StrData[j]=='.')
{
dotloc = j;
break;
}
if(j>5) break;
}while(StrData[j]!='.');
L1 = atol(buf); //3201
L3 = (L1%100); //*100000/6; 分
L1 = (L1/100)*1000000; // 度*1000000
memset(buf,0,20);
j = 0;
do
{
buf[j] = StrData[dotloc+1+j];
j++;
if(j>5) break;
}while(StrData[j] != 0);
L2 = atol(buf);//83115
L2 = (L3*1000000 + L2*10);//83115*(1/100000)*1000000/(10)
L2 = L2/60;
ll.l = L1+L2;
if (ll.l != 0) //數據轉化錯誤為零時，不更新GPS數據
{
if (Num == 10)
{
TzGpsData.Lanti[3] = ll.buf[0];
TzGpsData.Lanti[2] = ll.buf[1];
TzGpsData.Lanti[1] = ll.buf[2];
TzGpsData.Lanti[0] = ll.buf[3];
GPSlatNEW = ll.l;
}
else if (Num == 11)
{
TzGpsData.Longi[3] = ll.buf[0];
TzGpsData.Longi[2] = ll.buf[1];
TzGpsData.Longi[1] = ll.buf[2];
TzGpsData.Longi[0] = ll.buf[3];
GPSlonNEW = ll.l;
}
}
}
//分析GPRMC數據
//輸入 GPSData GPS數據包
//返回 0－－失敗
// 1－－成功
INT8U anlyGPS (INT8U *GPSData)
{
float f;
INT8U tmp;
INT16U i,j;
INT8U *Pi;
INT8U *PPi;
INT8U buf[20];
// INT8U GPSData[]="GPRMC,080515.000,V,3201.8318,N,11849.5894,E,12.34,56.78,081106,,N*19";
//$GPRMC,024530.00 ,V, , , , , , ,271206,,,N*7D
// $GPRMC,024535.00 ,V, , , , , , ,271206,,,N*78
i=0;
j=0;
do
{
if (j++ > 20)
{
GPSposFlg++ ;
TzGpsDataB.State.GpsState = 0;
TestSendStr("f");
return FALSE;
}
}while (GPSData[i++] != ',');
if (GPSData[i] != ',')
{
memcpy ((char *)&buf[0],(char *)(&GPSData[i]),6);
for (j=0; j<6; j++)
{
if ((buf[j] < 0x30) || (buf[j] > 0x39))
{
GPSposFlg++;
TzGpsDataB.State.GpsState = 0;
TestSendStr("e");
return FALSE;
}
}
memcpy ((char *)(&GPSRecData.GPSTime[0]),(char *)(&GPSData[i]),6); //拷貝GPS時間
}
else
{
GPSposFlg++;
// TestSendStr("!");
TzGpsDataB.State.GpsState = 0;
return FALSE;
}
j=0;
do
{
if (j++ > 20)
{
GPSposFlg++;
TzGpsDataB.State.GpsState = 0;
return FALSE;
}
}while (GPSData[i++] != ',');
if (GPSData[i] != ',')
{
if (GPSData[i] == 'A')
{
GPSposFlg = 0;
TzGpsDataB.State.GpsState = 1;
SetPin(GPSVALID);
}
else if (GPSData[i] == 'V')
{
GPSposFlg++;
// TzGpsDataB.State.GpsState = 0;
// ClrPin(GPSVALID);
}
else
{
GPSposFlg++;
TzGpsDataB.State.GpsState = 0;
return FALSE;
}
}
else
{
GPSposFlg++;
TzGpsDataB.State.GpsState = 0;
return FALSE;
}
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
if (GPSData[i] != ',')
{
// if (TzGpsDataB.State.GpsState)
if (0 == GPSposFlg)
{
memset (&GPSRecData.GPSLanti[0],0,11);
memcpy ((char *)&buf[0],(char *)(&GPSData[i]),10);
for (j=0; j<10; j++)
{
if ((buf[j] < 0x30) || (buf[j] > 0x39))
{
if (buf[j] != '.')
return FALSE;
}
}
memcpy ((char *)(&GPSRecData.GPSLanti[0]),(char *)(&GPSData[i]),10); //GPS緯度
TransData (&GPSRecData.GPSLanti[0],10);
}
j = 0;
do{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
else
{
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
if (GPSData[i] != ',')
{
if (GPSData[i] == 'N')
{
TzGpsDataB.State.Lan = 1;
}
else if (GPSData[i] == 'S')
{
TzGpsDataB.State.Lan = 0;
}
else
return FALSE;
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ','); //南北緯度
}
else
{
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
if (GPSData[i] != ',')
{
// if (TzGpsDataB.State.GpsState)
if (0 == GPSposFlg)
{
memset (&GPSRecData.GPSLongi[0],0,12);
memcpy (&buf[0],&GPSData[i],11); //經度
for (j=0; j<10; j++)
{
if ((buf[j] < 0x30) || (buf[j] > 0x39))
{
if (buf[j] != '.')
return FALSE;
}
}
memcpy (&GPSRecData.GPSLongi[0],&GPSData[i],11); //經度
TransData (&GPSRecData.GPSLongi[0],11);
}
j=0;
do{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
else
{
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
if (GPSData[i] != ',')
{
if (GPSData[i] == 'E')
{
TzGpsDataB.State.Lon = 1;
}
else if (GPSData[i] == 'W')
{
TzGpsDataB.State.Lon = 0;
}
else
return FALSE;
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ','); //東西經度
}
else
{
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
if (GPSData[i] != ',')
{
// if (TzGpsDataB.State.GpsState)
if (0 == GPSposFlg)
{
Pi = &GPSData[i];
PPi = &buf[0];
j = 0;
do{
*PPi = *Pi++;
if ((*PPi < 0x30) || (*PPi > 0x39))
{
if (*PPi != '.')
return FALSE;
}
PPi++;
if (j++ > 20)
return FALSE;
}while (*Pi != ','); //速度
*PPi = 0;
f = atof((char *)&buf[0]);
tmp = (INT8U)(f*1.852); //1knot = 1.852km
TzGpsData.Speed = tmp;
if (tmp > 200)
{
TzGpsData.Speed = 0;
}
if(TzGpsData.Speed > Para.OverSpeed)
{
TzGpsDataB.State.ChaoSu = 1;
}
else
{
TzGpsDataB.State.ChaoSu = 0;
}
}
j=0;
do{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
else
{
j=0;
do
{
if (j++ > 20)
return FALSE;
}while (GPSData[i++] != ',');
}
if (GPSData[i] != ',')
{
// if (TzGpsDataB.State.GpsState)
if (0 == GPSposFlg)
{
Pi = &GPSData[i];
PPi = &buf[0];
j = 0;
do{
*PPi = *Pi++;
if ((*PPi < 0x30) || (*PPi > 0x39))
{
if (*PPi != '.')
return FALSE;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -