152 lines
4.0 KiB
C++
152 lines
4.0 KiB
C++
#include "fm_parse_gps.h"
|
|
|
|
bool FMParseGPS::ParseRMC(char szSentence[], NMEA_INFO* data, int nPacketSize)
|
|
{
|
|
Q_UNUSED(nPacketSize);
|
|
|
|
// qDebug() << szSentence;
|
|
|
|
char szItem[NMEA_TOKEN_SIZE]={0,};
|
|
long nTemp;
|
|
|
|
// 의미없으니 하지 말자
|
|
// DWORD dwCheckSum = 0;
|
|
//for (i = 1; i < strlen(szSentence) && szSentence[i] != '*'; i++)
|
|
// dwCheckSum ^= szSentence[i];
|
|
|
|
//NMEA_INFO* data = &_gpsData[index];
|
|
|
|
//0th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
|
|
// 1th
|
|
char szUtc[11] = {0,};
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
strncpy(szUtc, szItem, sizeof(szUtc));
|
|
|
|
// 2th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
char cStatus = szItem[0];
|
|
|
|
// 3th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
data->Latitude = atof(szItem);
|
|
|
|
// 4th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
unsigned char cLatitudeDir = szItem[0];
|
|
|
|
// 5th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
data->Longitude = atof(szItem);
|
|
|
|
// 6th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
unsigned char cLongitudeDir = szItem[0];
|
|
|
|
// 7th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
double speed = atof(szItem);
|
|
|
|
// 8th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
double track = atof(szItem);
|
|
|
|
// 9th
|
|
char szDate[7] = {0,};
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
strncpy(szDate, szItem, sizeof(szDate));
|
|
|
|
// 10th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
// double Mag = atof(szItem); // Magnetic variation
|
|
|
|
// 11th
|
|
szSentence = _GetNextToken(szSentence, szItem);
|
|
// unsigned char cMagDir = szItem[0];
|
|
|
|
// Set GPS Info
|
|
if (strlen(szUtc) >= 6)
|
|
{
|
|
data->nHour = (szUtc[0] - 48) * 10 + (szUtc[1] - 48);
|
|
data->nMin = (szUtc[2] - 48) * 10 + (szUtc[3] - 48);
|
|
data->nSec = (szUtc[4] - 48) * 10 + (szUtc[5] - 48);
|
|
}
|
|
if (strlen(szDate) >= 6)
|
|
{
|
|
data->nDay = (szDate[0] - 48) * 10 + (szDate[1] - 48);
|
|
data->nMonth = (szDate[2] - 48) * 10 + (szDate[3] - 48);
|
|
data->nYear = (szDate[4] - 48) * 10 + (szDate[5] - 48);
|
|
if (data->nYear > 80)
|
|
{
|
|
data->nYear += 1900;
|
|
}
|
|
else
|
|
{
|
|
data->nYear += 2000;
|
|
}
|
|
}
|
|
// printf("date:%d-%d-%d %d:%d:%d\n",data->nYear,data->nMonth,data->nDay,data->nHour,data->nMin,data->nSec);
|
|
data->nStatus = (cStatus == 'A') ? 1 : 0;
|
|
|
|
data->Latitude = data->Latitude / 100.0;
|
|
nTemp = (long)data->Latitude;
|
|
data->Latitude = (double)nTemp + (double)(data->Latitude - nTemp) * 100.0 / 60.0;
|
|
if (cLatitudeDir == 'S')
|
|
{
|
|
data->Latitude *= -1;
|
|
}
|
|
|
|
data->Longitude = data->Longitude / 100.0;
|
|
nTemp = (long)data->Longitude;
|
|
data->Longitude = (double)nTemp + (double)(data->Longitude - nTemp) * 100.0 / 60.0;
|
|
|
|
if (cLongitudeDir == 'W')
|
|
{
|
|
data->Longitude *= -1;
|
|
}
|
|
|
|
data->Speed = speed * 1.852; // 1 knots는 1해리를 1시간에 가는 속력으로 1해리는 1852m
|
|
|
|
data->nAngle = (uint8_t) track;
|
|
// // data->nAngle = (long) track;
|
|
// if((data->nAngle-track>5 || data->nAngle-track<-5) ||
|
|
// (_nLastAngle[1]==_nLastAngle[0] && _nLastAngle[0] == track ))
|
|
// {
|
|
// data->nAngle = (uint8_t) track;
|
|
// }
|
|
// else
|
|
// {
|
|
// _nLastAngle[1] = _nLastAngle[0];
|
|
// _nLastAngle[0] = (long)track;
|
|
// }
|
|
|
|
return true;
|
|
}
|
|
char* FMParseGPS::_GetNextToken(char* lpSentence, char* lpToken, int iTokenSize)
|
|
{
|
|
lpToken[0] = '\0';
|
|
if (lpSentence == NULL || lpSentence[0] == '\0')
|
|
{
|
|
return NULL;
|
|
}
|
|
if (lpSentence[0] == ',')
|
|
{
|
|
return lpSentence + 1;
|
|
}
|
|
iTokenSize--;
|
|
|
|
while( *lpSentence != ',' &&
|
|
*lpSentence != '\0' &&
|
|
*lpSentence != '*'&& iTokenSize > 0)
|
|
{
|
|
*lpToken = *lpSentence;
|
|
lpToken++;
|
|
lpSentence++;
|
|
iTokenSize--;
|
|
}
|
|
lpSentence++;
|
|
*lpToken = '\0';
|
|
return lpSentence;
|
|
}
|