void GetPosition(int fd, struct TGPS *GPS)
{
int Count, GotGGA, GotRMC;
char Buffer[200];
char Character;
printf("NMEA...\r\n");
GotGGA = 0;
GotRMC = 0;
Count = -1;
while (!GotGGA || !GotRMC)
{
read(fd, &Character, 1);
// printf ("%c", Character);
if (Character == '$')
{
Count = 0;
}
else if (Count > 180)
{
Count = -1;
}
if ((Count >= 0) && (Count <= 180))
{
if (Character != '\r')
{
Buffer[Count++] = Character;
}
if (Character == '\n')
{
Buffer[Count] = '\0';
if (GPSChecksumOK(Buffer, Count))
{
ProcessLine(GPS, Buffer, &GotGGA, &GotRMC);
}
Count = -1;
}
}
}
}
void ProcessLine(struct i2c_info *bb, struct TGPS *GPS, char *Buffer, int Count)
{
static SystemTimeHasBeenSet=0;
float utc_time, latitude, longitude, hdop, altitude;
int lock, satellites;
char active, ns, ew, units, timestring[16], speedstring[16], *course, *date, restofline[80], *ptr;
long Hours, Minutes, Seconds;
if (GPSChecksumOK(Buffer, Count))
{
satellites = 0;
if (strncmp(Buffer+3, "GGA", 3) == 0)
{
if (sscanf(Buffer+7, "%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &utc_time, &latitude, &ns, &longitude, &ew, &lock, &satellites, &hdop, &altitude, &units) >= 1)
{
// $GPGGA,124943.00,5157.01557,N,00232.66381,W,1,09,1.01,149.3,M,48.6,M,,*42
if (satellites >= 4)
{
GPS->Time = utc_time;
Hours = GPS->Time / 10000;
Minutes = (GPS->Time / 100) % 100;
Seconds = GPS->Time % 100;
GPS->Seconds = Hours * 3600 + Minutes * 60 + Seconds;
GPS->Latitude = FixPosition(latitude);
if (ns == 'S') GPS->Latitude = -GPS->Latitude;
GPS->Longitude = FixPosition(longitude);
if (ew == 'W') GPS->Longitude = -GPS->Longitude;
GPS->Altitude = altitude;
}
GPS->Satellites = satellites;
}
if (Config.EnableGPSLogging)
{
WriteLog("gps.txt", Buffer);
}
}
else if (strncmp(Buffer+3, "RMC", 3) == 0)
{
speedstring[0] = '\0';
if (sscanf(Buffer+7, "%[^,],%c,%f,%c,%f,%c,%[^,],%s", timestring, &active, &latitude, &ns, &longitude, &ew, speedstring, restofline) >= 7)
{
// $GPRMC,124943.00,A,5157.01557,N,00232.66381,W,0.039,,200314,,,A*6C
ptr = restofline;
course = strsep(&ptr, ",");
date = strsep(&ptr, ",");
GPS->Speed = (int)atof(speedstring);
GPS->Direction = (int)atof(course);
if ((atof(timestring) > 0) && !SystemTimeHasBeenSet)
{
struct tm tm;
char timedatestring[32];
time_t t;
// Now create a tm structure from our date and time
memset(&tm, 0, sizeof(struct tm));
sprintf(timedatestring, "%c%c-%c%c-20%c%c %c%c:%c%c:%c%c",
date[0], date[1], date[2], date[3], date[4], date[5],
timestring[0], timestring[1], timestring[2], timestring[3], timestring[4], timestring[5]);
strptime(timedatestring, "%d-%m-%Y %H:%M:%S", &tm);
t = mktime(&tm);
if (stime(&t) == -1)
{
printf("Failed to set system time\n");
}
else
{
SystemTimeHasBeenSet = 1;
}
}
}
if (Config.EnableGPSLogging)
{
WriteLog("gps.txt", Buffer);
}
}
else if (strncmp(Buffer+3, "GSV", 3) == 0)
{
// Disable GSV
printf("Disabling GSV\r\n");
unsigned char setGSV[] = { 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x39 };
SendUBX(bb, setGSV, sizeof(setGSV));
}
else if (strncmp(Buffer+3, "GLL", 3) == 0)
{
// Disable GLL
printf("Disabling GLL\r\n");
unsigned char setGLL[] = { 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x2B };
SendUBX(bb, setGLL, sizeof(setGLL));
}
else if (strncmp(Buffer+3, "GSA", 3) == 0)
{
// Disable GSA
printf("Disabling GSA\r\n");
unsigned char setGSA[] = { 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x32 };
SendUBX(bb, setGSA, sizeof(setGSA));
}
else if (strncmp(Buffer+3, "VTG", 3) == 0)
{
// Disable VTG
printf("Disabling VTG\r\n");
unsigned char setVTG[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x05, 0x47};
SendUBX(bb, setVTG, sizeof(setVTG));
}
else
{
printf("Unknown NMEA sentence: %s\n", Buffer);
}
}
else
{
printf("Bad checksum\r\n");
}
}
