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"); } }