#include #include #include "GpsUtils.h" #include "MathP.h" #include "Syslog.h" char GpsUtils::hemisphere(GpsIF::Hemisphere h) { char errorBuf[100]; switch (h) { case GpsIF::Northern: return 'N'; break; case GpsIF::Southern: return 'S'; break; case GpsIF::Eastern: return 'E'; break; case GpsIF::Western: return 'W'; break; default: sprintf(errorBuf, "GpsUtils::hemisphere() - bad GpsIF::Hemisphere value: %d", h); throw Exception(errorBuf); } return 0; } GpsIF::Hemisphere GpsUtils::hemisphere(char hem) { static GpsIF::Hemisphere h; char errorBuf[100]; switch (hem) { case 'N': h = GpsIF::Northern; break; case 'S': h = GpsIF::Southern; break; case 'E': h = GpsIF::Eastern; break; case 'W': h = GpsIF::Western; break; default: sprintf(errorBuf, "GpsUtils::hemisphere() - bad hemisphere value: %c", hem); throw Exception(errorBuf); } return h; } /* NOTE: This method implementation is not very smart yet. The following should probably be added: Check for "GGA" in the label Check for valid values in each field Also, the NMEA GGA standard apparently just contains UTC time-of-day, and does NOT include the date! T.O'R. */ void GpsUtils::parseFix(const char *ggaSentence, GpsIF::Fix *fix) { char errorBuf[100]; enum { InLabel, InTime, InLatitude, InNsHemisphere, InLongitude, InEwHemisphere, InQuality, InNSatellites, InHdop, InAltitude, InAltitudeUnits, InGeoidHeight, InGeoidHeightUnits, InDgpsAge, InDgpsStationId, Finished }; Boolean debug = False; // True; dprintf("GpsUtils::parseFix() - %s\n", ggaSentence); int state = InLabel; // Checksum is at the end char *endPtr = strchr(ggaSentence, '*'); char *checkSumPtr; if (endPtr) { checkSumPtr = endPtr; } else { // // No checksum on this sentence. // // Note: The Ashtech 630852DG16 receiver (the one in the Kearfott) // apparently doesn't generate a checksum or an asterisk. // checkSumPtr = 0; endPtr = (char *)ggaSentence + strlen(ggaSentence); } char buf[256]; int nBytes = min(endPtr - ggaSentence, sizeof(buf)); strncpy(buf, ggaSentence, nBytes); buf[nBytes] = '\0'; char *token = (char *)buf; while (state != Finished && token != 0) { switch (state) { case InLabel: break; case InTime: char timeStr[16]; sscanf( token, "%9s", timeStr ); //Adds a terminating null sscanf( timeStr, "%2d%2d%2d.%2d", &(fix->ggatime.hours), &(fix->ggatime.minutes), &(fix->ggatime.seconds), &(fix->ggatime.centiSeconds)); break; case InLatitude: fix->latitude = GpsUtils::latitude(token); break; case InNsHemisphere: fix->nsHemisphere = GpsUtils::hemisphere(token[0]); break; case InLongitude: fix->longitude = GpsUtils::longitude(token); break; case InEwHemisphere: fix->ewHemisphere = GpsUtils::hemisphere(token[0]); break; case InQuality: fix->quality = (GpsIF::Quality )atoi(token); break; case InNSatellites: fix->nSatellites = atoi(token); break; case InHdop: fix->hdop = atof(token); break; case InAltitude: fix->altitude = atof(token); break; case InAltitudeUnits: break; case InGeoidHeight: fix->geoidHeight = atof(token); break; case InGeoidHeightUnits: break; case InDgpsAge: fix->dgpsDataAge = atoi(token); break; case InDgpsStationId: fix->dgpsStationId = atoi(token); break; } // Advance to next state state++; // Point to next token if ((token = strchr(token, ',')) == 0) // No more tokens break; token++; } if (state != Finished) { sprintf(errorBuf, "GpsUtils::parseFix() - incomplete GGA sentence?\n" "%s", ggaSentence); throw Exception(errorBuf); } if (token) { sprintf(errorBuf, "GpsUtils::parseFix() - Extra stuff in GGA sentence?\n" "%s", ggaSentence); throw Exception(errorBuf); } dprintf("successfully parse sentence at %d:%d:%d\n", fix->ggatime.hours, fix->ggatime.minutes, fix->ggatime.seconds); return; } // // Parse Gps fix from NMEA "GSA sentence". // static void GpsUtils::parseGSA(const char *gsaSentence, GpsIF::GsaStruct *gsa) { char errorBuf[100]; enum { Header, Mode, FixMode, SatID1, SatID2, SatID3, SatID4, SatID5, SatID6, SatID7, SatID8, SatID9, SatID10, SatID11, SatID12, SatID13, SatID14, Pdop, HorizDop, VertiDop, Finished }; Boolean debug = False; dprintf("GpsUtils::parseGSA() - %s\n", gsaSentence); int state = Header; // Checksum is at the end char *endPtr = strchr(gsaSentence, '*'); char *checkSumPtr; if (endPtr) { checkSumPtr = endPtr; } else { // // No checksum on this sentence. // // Note: The Ashtech 630852DG16 receiver (the one in the Kearfott) // apparently doesn't generate a checksum or an asterisk. // checkSumPtr = 0; endPtr = (char *)gsaSentence + strlen(gsaSentence); } char buf[256]; int nBytes = min(endPtr - gsaSentence, sizeof(buf)); strncpy(buf, gsaSentence, nBytes); buf[nBytes] = '\0'; char *token = (char *)buf; while (state != Finished && token != 0) { switch (state) { case Header: break; case Mode: gsa->mode = atoi(token); break; case FixMode: gsa->fixMode = atoi(token); break; // // The Ashtech DG16 that is in the Kearfott outputs 14 satellite IDs. // case SatID1: gsa->satID1 = atoi(token); break; case SatID2: gsa->satID2 = atoi(token); break; case SatID3: gsa->satID3 = atoi(token); break; case SatID4: gsa->satID4 = atoi(token); break; case SatID5: gsa->satID5 = atoi(token); break; case SatID6: gsa->satID6 = atoi(token); break; case SatID7: gsa->satID7 = atoi(token); break; case SatID8: gsa->satID8 = atoi(token); break; case SatID9: gsa->satID9 = atoi(token); break; case SatID10: gsa->satID10 = atoi(token); break; case SatID11: gsa->satID11 = atoi(token); break; case SatID12: gsa->satID12 = atoi(token); break; case SatID13: gsa->satID13 = atoi(token); break; case SatID14: gsa->satID14 = atoi(token); break; case Pdop: gsa->pdop = atof(token); break; case HorizDop: gsa->horizDop = atof(token); break; case VertiDop: gsa->vertiDop = atof(token); break; } // Advance to next state state++; // Point to next token if ((token = strchr(token, ',')) == 0) // No more tokens break; token++; //So that it points after the ','. } if (state != Finished) { sprintf(errorBuf, "GpsUtils::parseGSA() - incomplete GSA sentence?\n" "%s", gsaSentence); throw Exception(errorBuf); } if (token) { sprintf(errorBuf, "GpsUtils::parseGSA() - Extra stuff in GSA sentence?\n" "%s", gsaSentence); throw Exception(errorBuf); } return; } // // Parse Gps fix from NMEA VGT sentence. // static void GpsUtils::parseVGT(const char *vgtSentence, GpsIF::VgtStruct *vgt) { char errorBuf[100]; enum { Header, TrueCourse, TrueCourseIndicator, MagCourse, MagCourseIndicator, SpeedKnots, SpeedKnotsUnits, SpeedKmHr, SpeedKmHrUnits, PositionMode, Finished }; Boolean debug = False; dprintf("GpsUtils::parseVGT() - %s\n", vgtSentence); int state = Header; // Checksum is at the end char *endPtr = strchr(vgtSentence, '*'); char *checkSumPtr; if (endPtr) { checkSumPtr = endPtr; } else { // // No checksum on this sentence. // // Note: The Ashtech 630852DG16 receiver (the one in the Kearfott) // apparently doesn't generate a checksum or an asterisk. // checkSumPtr = 0; endPtr = (char *)vgtSentence + strlen(vgtSentence); } char buf[256]; int nBytes = min(endPtr - vgtSentence, sizeof(buf)); strncpy(buf, vgtSentence, nBytes); buf[nBytes] = '\0'; char *token = (char *)buf; while (state != Finished && token != 0) { switch (state) { case Header: break; case TrueCourse: vgt->trueCourse = atof(token); break; case TrueCourseIndicator: vgt->trueCourseIndicator = *token; break; case MagCourse: vgt->magCourse = atof(token); break; case MagCourseIndicator: vgt->magCourseIndicator = *token; break; case SpeedKnots: vgt->speedKnots = atof(token); break; case SpeedKnotsUnits: vgt->speedKnotsUnits = *token; break; case SpeedKmHr: vgt->speedKmHr = atof(token); break; case SpeedKmHrUnits: vgt->speedKmHrUnits = *token; break; case PositionMode: vgt->positionMode = *token; break; } // end switch (state) // Advance to next state state++; // Point to next token if ((token = strchr(token, ',')) == 0) // No more tokens break; token++; //So that it points after the ','. } //end while if (state != Finished) { sprintf(errorBuf, "GpsUtils::parseVGT() - incomplete VGT sentence?\n" "%s", vgtSentence); throw Exception(errorBuf); } if (token) { sprintf(errorBuf, "GpsUtils::parseVGT() - Extra stuff in VGT sentence?\n" "%s", vgtSentence); throw Exception(errorBuf); } return; } double GpsUtils::latitude(const char *token) { char *decimalPtr; char errorBuf[100]; char buf[16]; double value; // First two characters are degrees strncpy(buf, token, 2); value = atof(buf); token += 2; value += (atof(token) / 60.); // Return value in radians return (value * Math::RadsPerDeg); } double GpsUtils::longitude(const char *token) { char *decimalPtr; char errorBuf[100]; char buf[16]; double value; // First three characters are degrees strncpy(buf, token, 3); value = atof(buf); token += 3; value += (atof(token) / 60.); // Return value in radians return (value * Math::RadsPerDeg); } char *GpsUtils::latitude(double value) { static char string[16]; // Convert from radians to degrees value /= Math::RadsPerDeg; double minutes = 60. * (value - int(value)); sprintf(string, "%02d", (int )value); if (minutes < 10.) strcat(string, "0"); sprintf(string + strlen(string), "%2.4f", minutes); return string; } char *GpsUtils::longitude(double value) { // Convert from radians to degrees value /= Math::RadsPerDeg; static char string[16]; double minutes = 60. * (value - int(value)); sprintf(string, "%03d", (int )value); if (minutes < 10.) strcat(string, "0"); sprintf(string + strlen(string), "%2.4f", minutes); return string; }