diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/GPSFix.cpp | 412 | ||||
| -rw-r--r-- | src/GPSService.cpp | 509 | ||||
| -rw-r--r-- | src/NMEACommand.cpp | 108 | ||||
| -rw-r--r-- | src/NMEAParser.cpp | 466 | ||||
| -rw-r--r-- | src/NumberConversion.cpp | 144 |
5 files changed, 1639 insertions, 0 deletions
diff --git a/src/GPSFix.cpp b/src/GPSFix.cpp new file mode 100644 index 0000000..818b232 --- /dev/null +++ b/src/GPSFix.cpp @@ -0,0 +1,412 @@ +/* + * GPSFix.cpp + * + * Created on: Jul 23, 2014 + * Author: Cameron Karlsson + */ + +#include <nmeaparse/GPSFix.h> +#include <cmath> +#include <string> +#include <sstream> +#include <iomanip> + +using namespace std; +using namespace std::chrono; + +using namespace nmea; + + + +// =========================================================== +// ======================== GPS SATELLITE ==================== +// =========================================================== + +string GPSSatellite::toString(){ + stringstream ss; + + ss << "[PRN: " << setw(3) << setfill(' ') << prn << " " + << " SNR: " << setw(3) << setfill(' ') << snr << " dB " + << " Azimuth: " << setw(3) << setfill(' ') << azimuth << " deg " + << " Elevation: " << setw(3) << setfill(' ') << elevation << " deg " + << "]"; + + return ss.str(); +} +GPSSatellite::operator std::string(){ + return toString(); +} + + + +// ========================================================= +// ======================== GPS ALMANAC ==================== +// ========================================================= + +void GPSAlmanac::clear(){ + lastPage = 0; + totalPages = 0; + processedPages = 0; + visibleSize = 0; + satellites.clear(); +} +void GPSAlmanac::updateSatellite(GPSSatellite sat){ + if (satellites.size() > visibleSize) + { //we missed the new almanac start page, start over. + clear(); + } + + satellites.push_back(sat); +} +double GPSAlmanac::percentComplete(){ + if (totalPages == 0){ + return 0.0; + } + + return ((double)processedPages) / ((double)totalPages) * 100.0; +} +double GPSAlmanac::averageSNR(){ + + double avg = 0; + double relevant = 0; + for (size_t i = 0; i < satellites.size(); i++){ + if (satellites[i].snr > 0){ + relevant += 1.0; + } + } + + for (size_t i = 0; i < satellites.size(); i++){ + if (satellites[i].snr > 0){ + avg += satellites[i].snr / relevant; + } + } + + return avg; +} +double GPSAlmanac::minSNR(){ + double min = 9999999; + if (satellites.size() == 0){ + return 0; + } + int32_t num_over_zero = 0; + for (size_t i = 0; i < satellites.size(); i++){ + if (satellites[i].snr > 0){ + num_over_zero++; + if (satellites[i].snr < min){ + min = satellites[i].snr; + } + } + } + if (num_over_zero == 0){ + return 0; + } + return min; +} + +double GPSAlmanac::maxSNR(){ + double max = 0; + for (size_t i = 0; i < satellites.size(); i++){ + if (satellites[i].snr > 0){ + if (satellites[i].snr > max){ + max = satellites[i].snr; + } + } + } + return max; +} + + + + +// =========================================================== +// ======================== GPS TIMESTAMP ==================== +// =========================================================== + + +GPSTimestamp::GPSTimestamp(){ + hour = 0; + min = 0; + sec = 0; + + month = 1; + day = 1; + year = 1970; + + rawTime = 0; + rawDate = 0; +}; + +// indexed from 1! +std::string GPSTimestamp::monthName(uint32_t index){ + if (index < 1 || index > 12){ + std::stringstream ss; + ss << "[month:" << index << "]"; + return ss.str(); + } + + std::string names[] = { + "January", + "February", + "March", + "April", + "May", + "June", + "July", + "August", + "September", + "October", + "November", + "December" + }; + return names[index - 1]; +}; + +// Returns seconds since Jan 1, 1970. Classic Epoch time. +time_t GPSTimestamp::getTime() { + struct tm t = { 0 }; + t.tm_year = year - 1900; // This is year-1900, so 112 = 2012 + t.tm_mon = month; // month from 0:Jan + t.tm_mday = day; + t.tm_hour = hour; + t.tm_min = min; + t.tm_sec = (int)sec; + return mktime(&t); +} + +void GPSTimestamp::setTime(double raw_ts){ + rawTime = raw_ts; + + hour = (int32_t)trunc(raw_ts / 10000.0); + min = (int32_t)trunc((raw_ts - hour * 10000) / 100.0); + sec = raw_ts - min * 100 - hour * 10000; +} + +//ddmmyy +void GPSTimestamp::setDate(int32_t raw_date){ + rawDate = raw_date; + // If uninitialized, use posix time. + if(rawDate == 0) { + month = 1; + day = 1; + year = 1970; + } + else { + day = (int32_t)trunc(raw_date / 10000.0); + month = (int32_t)trunc((raw_date - 10000 * day) / 100.0); + year = raw_date - 10000 * day - 100 * month + 2000; + } +} + +std::string GPSTimestamp::toString(){ + std::stringstream ss; + ss << hour << "h " << min << "m " << sec << "s" << " " << monthName(month) << " " << day << " " << year; + return ss.str(); +}; + + + + + + + +// ===================================================== +// ======================== GPS FIX ==================== +// ===================================================== + +GPSFix::GPSFix() { + + quality = 0; // Searching... + status = 'V'; // Void + type = 1; // 1=none, 2=2d, 3=3d + + haslock = 0; + + dilution = 0; + horizontalDilution = 0; // Horizontal - Best is 1, >20 is terrible, so 0 means uninitialized + verticalDilution = 0; + latitude = 0; + longitude = 0; + speed = 0; + travelAngle = 0; + altitude = 0; + trackingSatellites = 0; + visibleSatellites = 0; + + +} + +GPSFix::~GPSFix() { + // TODO Auto-generated destructor stub +} + +// Returns the duration since the Host has received information +seconds GPSFix::timeSinceLastUpdate(){ + time_t now = time(NULL); + struct tm stamp = { 0 }; + + stamp.tm_hour = timestamp.hour; + stamp.tm_min = timestamp.min; + stamp.tm_sec = (int)timestamp.sec; + stamp.tm_year = timestamp.year-1900; + stamp.tm_mon = timestamp.month-1; + stamp.tm_mday = timestamp.day; + + time_t then = mktime(&stamp); + uint64_t secs = (uint64_t)difftime(now,then); + return seconds((uint64_t)secs); +} + +bool GPSFix::hasEstimate(){ + return (latitude != 0 && longitude != 0) || (quality == 6); +} + +bool GPSFix::setlock(bool locked){ + if (haslock != locked){ + haslock = locked; + return true; + } + return false; +} + +bool GPSFix::locked(){ + return haslock; +} + +// Returns meters +double GPSFix::horizontalAccuracy(){ + // horizontal 2drms 95% = 4.0 -- from GPS CHIP datasheets + return 4.0 * horizontalDilution; +} + +// Returns meters +double GPSFix::verticalAccuracy(){ + // Vertical 2drms 95% = 6.0 -- from GPS CHIP datasheets + return 6.0 * verticalDilution; +} + +// Takes a degree travel heading (0-360') and returns the name +std::string GPSFix::travelAngleToCompassDirection(double deg, bool abbrev){ + + //normalize, just in case + int32_t c = (int32_t)round(deg / 360.0 * 8.0); + int32_t r = c % 8; + if (r < 0){ + r = 8 + r; + } + + if (abbrev){ + std::string dirs[] = { + "N", + "NE", + "E", + "SE", + "S", + "SW", + "W", + "NW", + "N" + }; + return dirs[r]; + } + else { + std::string dirs[] = { + "North", + "North East", + "East", + "South East", + "South", + "South West", + "West", + "North West", + "North" + }; + return dirs[r]; + } + +}; + + +std::string fixStatusToString(char status){ + switch (status){ + case 'A': + return "Active"; + case 'V': + return "Void"; + default: + return "Unknown"; + } +} +std::string fixTypeToString(uint8_t type){ + switch (type){ + case 1: + return "None"; + case 2: + return "2D"; + case 3: + return "3D"; + default: + return "Unknown"; + } +} +std::string fixQualityToString(uint8_t quality){ + switch (quality){ + case 0: + return "Invalid"; + case 1: + return "Standard"; + case 2: + return "DGPS"; + case 3: + return "PPS fix"; + case 4: + return "Real Time Kinetic"; + case 5: + return "Real Time Kinetic (float)"; + case 6: + return "Estimate"; + default: + return "Unknown"; + } +} + +std::string GPSFix::toString(){ + stringstream ss; + ios_base::fmtflags oldflags = ss.flags(); + + ss << "========================== GPS FIX ================================" << endl + << " Status: \t\t" << ((haslock) ? "LOCK!" : "SEARCHING...") << endl + << " Satellites: \t\t" << trackingSatellites << " (tracking) of " << visibleSatellites << " (visible)" << endl + << " < Fix Details >" << endl + << " Age: " << timeSinceLastUpdate().count() << " s" << endl + << " Timestamp: " << timestamp.toString() << " UTC \n\t\t\t(raw: " << timestamp.rawTime << " time, " << timestamp.rawDate << " date)" << endl + << " Raw Status: " << status << " (" << fixStatusToString(status) << ")" << endl + << " Type: " << (int)type << " (" << fixTypeToString(type) << ")" << endl + << " Quality: " << (int)quality << " (" << fixQualityToString(quality) << ")" << endl + << " Lat/Lon (N,E): " << setprecision(6) << fixed << latitude << "' N, " << longitude << "' E" << endl; + + ss.flags(oldflags); //reset + + ss << " DOP (P,H,V): " << dilution << ", " << horizontalDilution << ", " << verticalDilution << endl + << " Accuracy(H,V): " << horizontalAccuracy() << " m, " << verticalAccuracy() << " m" << endl; + + ss << " Altitude: " << altitude << " m" << endl + << " Speed: " << speed << " km/h" << endl + << " Travel Dir: " << travelAngle << " deg [" << travelAngleToCompassDirection(travelAngle) << "]" << endl + << " SNR: avg: " << almanac.averageSNR() << " dB [min: " << almanac.minSNR() << " dB, max:" << almanac.maxSNR() << " dB]" << endl; + + ss << " < Almanac (" << almanac.percentComplete() << "%) >" << endl; + if (almanac.satellites.size() == 0){ + ss << " > No satellite info in almanac." << endl; + } + for (size_t i = 0; i < almanac.satellites.size(); i++){ + ss << " [" << setw(2) << setfill(' ') << (i + 1) << "] " << almanac.satellites[i].toString() << endl; + } + + return ss.str(); +} +GPSFix::operator std::string(){ + return toString(); +} + + + diff --git a/src/GPSService.cpp b/src/GPSService.cpp new file mode 100644 index 0000000..4f90a0f --- /dev/null +++ b/src/GPSService.cpp @@ -0,0 +1,509 @@ +/* + * GPSService.cpp + * + * Created on: Aug 14, 2014 + * Author: Cameron Karlsson + */ + + +#include <nmeaparse/GPSService.h> +#include <nmeaparse/NumberConversion.h> + +#include <iostream> +#include <cmath> + +using namespace std; +using namespace std::chrono; + +using namespace nmea; + + +// ------ Some helpers ---------- +// Takes the NMEA lat/long format (dddmm.mmmm, [N/S,E/W]) and converts to degrees N,E only +double convertLatLongToDeg(string llstr, string dir){ + + double pd = parseDouble(llstr); + double deg = trunc(pd / 100); //get ddd from dddmm.mmmm + double mins = pd - deg * 100; + + deg = deg + mins / 60.0; + + char hdg = 'x'; + if (dir.size() > 0){ + hdg = dir[0]; + } + + //everything should be N/E, so flip S,W + if (hdg == 'S' || hdg == 'W'){ + deg *= -1.0; + } + + return deg; +} +double convertKnotsToKilometersPerHour(double knots){ + return knots * 1.852; +} + + + +// ------------- GPSSERVICE CLASS ------------- + + + + +GPSService::GPSService(NMEAParser& parser) { + attachToParser(parser); // attach to parser in the GPS object +} + +GPSService::~GPSService() { + // TODO Auto-generated destructor stub +} + +void GPSService::attachToParser(NMEAParser& _parser){ + + // http://www.gpsinformation.org/dale/nmea.htm + /* used sentences... + $GPGGA - time,position,fix data + $GPGSA - gps receiver operating mode, satellites used in position and DOP values + $GPGSV - number of gps satellites in view, satellite ID, elevation,azimuth, and SNR + $GPRMC - time,date, position,course, and speed data + $GPVTG - course and speed information relative to the ground + $GPZDA - 1pps timing message + $PSRF150 - gps module "ok to send" + */ + _parser.setSentenceHandler("PSRF150", [this](const NMEASentence& nmea){ + this->read_PSRF150(nmea); + }); + _parser.setSentenceHandler("GPGGA", [this](const NMEASentence& nmea){ + this->read_GPGGA(nmea); + }); + _parser.setSentenceHandler("GPGSA", [this](const NMEASentence& nmea){ + this->read_GPGSA(nmea); + }); + _parser.setSentenceHandler("GPGSV", [this](const NMEASentence& nmea){ + this->read_GPGSV(nmea); + }); + _parser.setSentenceHandler("GPRMC", [this](const NMEASentence& nmea){ + this->read_GPRMC(nmea); + }); + _parser.setSentenceHandler("GPVTG", [this](const NMEASentence& nmea){ + this->read_GPVTG(nmea); + }); + +} + + + + +void GPSService::read_PSRF150(const NMEASentence& nmea){ + // nothing right now... + // Called with checksum 3E (valid) for GPS turning ON + // Called with checksum 3F (invalid) for GPS turning OFF +} + +void GPSService::read_GPGGA(const NMEASentence& nmea){ + /* -- EXAMPLE -- + $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47 + + $GPGGA,205630.945,3346.1070,N,08423.6687,W,0,03,,30.8,M,-30.8,M,,0000*73 // ATLANTA!!!! + + Where: + GGA Global Positioning System Fix Data + index: + [0] 123519 Fix taken at 12:35:19 UTC + [1-2] 4807.038,N Latitude 48 deg 07.038' N + [3-4] 1131.000,E Longitude 11 deg 31.000' E + [5] 1 Fix quality: 0 = invalid + 1 = GPS fix (SPS) + 2 = DGPS fix + 3 = PPS fix + 4 = Real Time Kinematic + 5 = Float RTK + 6 = estimated (dead reckoning) (2.3 feature) + 7 = Manual input mode + 8 = Simulation mode + [6] 08 Number of satellites being tracked + [7] 0.9 Horizontal dilution of position + [8-9] 545.4,M Altitude, Meters, above mean sea level + [10-11] 46.9,M Height of geoid (mean sea level) above WGS84 + ellipsoid + [12] (empty field) time in seconds since last DGPS update + [13] (empty field) DGPS station ID number + [13] *47 the checksum data, always begins with * + */ + try + { + if (!nmea.checksumOK()){ + throw NMEAParseError("Checksum is invalid!"); + } + + if (nmea.parameters.size() < 14){ + throw NMEAParseError("GPS data is missing parameters."); + } + + + // TIMESTAMP + this->fix.timestamp.setTime(parseDouble(nmea.parameters[0])); + + string sll; + string dir; + // LAT + sll = nmea.parameters[1]; + dir = nmea.parameters[2]; + if (sll.size() > 0){ + this->fix.latitude = convertLatLongToDeg(sll, dir); + } + + // LONG + sll = nmea.parameters[3]; + dir = nmea.parameters[4]; + if (sll.size() > 0){ + this->fix.longitude = convertLatLongToDeg(sll, dir); + } + + + // FIX QUALITY + bool lockupdate = false; + this->fix.quality = (uint8_t)parseInt(nmea.parameters[5]); + if (this->fix.quality == 0){ + lockupdate = this->fix.setlock(false); + } + else if (this->fix.quality == 1){ + lockupdate = this->fix.setlock(true); + } + else {} + + + // TRACKING SATELLITES + this->fix.trackingSatellites = (int32_t)parseInt(nmea.parameters[6]); + if (this->fix.visibleSatellites < this->fix.trackingSatellites){ + this->fix.visibleSatellites = this->fix.trackingSatellites; // the visible count is in another sentence. + } + + // ALTITUDE + if (nmea.parameters[8].size() > 0){ + this->fix.altitude = parseDouble(nmea.parameters[8]); + } + else { + // leave old value + } + + //calling handlers + if (lockupdate){ + this->onLockStateChanged(this->fix.haslock); + } + this->onUpdate(); + } + catch (NumberConversionError& ex) + { + NMEAParseError pe("GPS Number Bad Format [$GPGGA] :: " + ex.message, nmea); + throw pe; + } + catch (NMEAParseError& ex) + { + NMEAParseError pe("GPS Data Bad Format [$GPGGA] :: " + ex.message, nmea); + throw pe; + } +} + +void GPSService::read_GPGSA(const NMEASentence& nmea){ + /* -- EXAMPLE -- + $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39 + + $GPGSA,A,3,18,21,22,14,27,19,,,,,,,4.4,2.7,3.4*32 + + Where: + GSA Satellite status + [0] A Auto selection of 2D or 3D fix (M = manual) + [1] 3 3D fix - values include: 1 = no fix + 2 = 2D fix + 3 = 3D fix + [2-13] 04,05... PRNs of satellites used for fix (space for 12) + [14] 2.5 PDOP (dilution of precision) + [15] 1.3 Horizontal dilution of precision (HDOP) + [16] 2.1 Vertical dilution of precision (VDOP) + [16] *39 the checksum data, always begins with * + */ + + + try + { + if (!nmea.checksumOK()){ + throw NMEAParseError("Checksum is invalid!"); + } + + if (nmea.parameters.size() < 17){ + throw NMEAParseError("GPS data is missing parameters."); + } + + + // FIX TYPE + bool lockupdate = false; + uint64_t fixtype = parseInt(nmea.parameters[1]); + this->fix.type = (int8_t)fixtype; + if (fixtype == 1){ + lockupdate = this->fix.setlock(false); + } + else if (fixtype == 3) { + lockupdate = this->fix.setlock(true); + } + else {} + + + // DILUTION OF PRECISION -- PDOP + double dop = parseDouble(nmea.parameters[14]); + this->fix.dilution = dop; + + // HORIZONTAL DILUTION OF PRECISION -- HDOP + double hdop = parseDouble(nmea.parameters[15]); + this->fix.horizontalDilution = hdop; + + // VERTICAL DILUTION OF PRECISION -- VDOP + double vdop = parseDouble(nmea.parameters[16]); + this->fix.verticalDilution = vdop; + + //calling handlers + if (lockupdate){ + this->onLockStateChanged(this->fix.haslock); + } + this->onUpdate(); + } + catch (NumberConversionError& ex) + { + NMEAParseError pe("GPS Number Bad Format [$GPGSA] :: " + ex.message, nmea); + throw pe; + } + catch (NMEAParseError& ex) + { + NMEAParseError pe("GPS Data Bad Format [$GPGSA] :: " + ex.message, nmea); + throw pe; + } +} + +void GPSService::read_GPGSV(const NMEASentence& nmea){ + /* -- EXAMPLE -- + $GPGSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75 + + + $GPGSV,3,1,12,01,00,000,,02,00,000,,03,00,000,,04,00,000,*7C + $GPGSV,3,2,12,05,00,000,,06,00,000,,07,00,000,,08,00,000,*77 + $GPGSV,3,3,12,09,00,000,,10,00,000,,11,00,000,,12,00,000,*71 + + Where: + GSV Satellites in view + [0] 2 Number of sentences for full data + [1] 1 sentence 1 of 2 + [2] 08 Number of satellites in view + + [3] 01 Satellite PRN number + [4] 40 Elevation, degrees + [5] 083 Azimuth, degrees + [6] 46 SNR - higher is better + [...] for up to 4 satellites per sentence + [17] *75 the checksum data, always begins with * + */ + + try + { + if (!nmea.checksumOK()){ + throw NMEAParseError("Checksum is invalid!"); + } + + // can't do this check because the length varies depending on satallites... + //if(nmea.parameters.size() < 18){ + // throw NMEAParseError("GPS data is missing parameters."); + //} + + // VISIBLE SATELLITES + this->fix.visibleSatellites = (int32_t)parseInt(nmea.parameters[2]); + if (this->fix.trackingSatellites == 0){ + this->fix.visibleSatellites = 0; // if no satellites are tracking, then none are visible! + } // Also NMEA defaults to 12 visible when chip powers on. Obviously not right. + + uint32_t totalPages = (uint32_t)parseInt(nmea.parameters[0]); + uint32_t currentPage = (uint32_t)parseInt(nmea.parameters[1]); + + + //if this is the first page, then reset the almanac + if (currentPage == 1){ + this->fix.almanac.clear(); + //cout << "CLEARING ALMANAC" << endl; + } + + this->fix.almanac.lastPage = currentPage; + this->fix.almanac.totalPages = totalPages; + this->fix.almanac.visibleSize = this->fix.visibleSatellites; + + int entriesInPage = (nmea.parameters.size() - 3) >> 2; //first 3 are not satellite info + //- entries come in 4-ples, and truncate, so used shift + GPSSatellite sat; + for (int i = 0; i < entriesInPage; i++){ + int prop = 3 + i * 4; + + // PRN, ELEVATION, AZIMUTH, SNR + sat.prn = (uint32_t)parseInt(nmea.parameters[prop]); + sat.elevation = (uint32_t)parseInt(nmea.parameters[prop + 1]); + sat.azimuth = (uint32_t)parseInt(nmea.parameters[prop + 2]); + sat.snr = (uint32_t)parseInt(nmea.parameters[prop + 3]); + + //cout << "ADDING SATELLITE ::" << sat.toString() << endl; + this->fix.almanac.updateSatellite(sat); + } + + this->fix.almanac.processedPages++; + + // + if (this->fix.visibleSatellites == 0){ + this->fix.almanac.clear(); + } + + + //cout << "ALMANAC FINISHED page " << this->fix.almanac.processedPages << " of " << this->fix.almanac.totalPages << endl; + this->onUpdate(); + + } + catch (NumberConversionError& ex) + { + NMEAParseError pe("GPS Number Bad Format [$GPGSV] :: " + ex.message, nmea); + throw pe; + } + catch (NMEAParseError& ex) + { + NMEAParseError pe("GPS Data Bad Format [$GPGSV] :: " + ex.message, nmea); + throw pe; + } +} + +void GPSService::read_GPRMC(const NMEASentence& nmea){ + /* -- EXAMPLE --- + $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A + $GPRMC,235957.025,V,,,,,,,070810,,,N*4B + $GPRMC,061425.000,A,3346.2243,N,08423.4706,W,0.45,18.77,060914,,,A*47 + + Where: + RMC Recommended Minimum sentence C + [0] 123519 Fix taken at 12:35:19 UTC + [1] A Status A=active or V=Void. + [2-3] 4807.038,N Latitude 48 deg 07.038' N + [4-5] 01131.000,E Longitude 11 deg 31.000' E + [6] 022.4 Speed over the ground in knots + [7] 084.4 Track angle in degrees True + [8] 230394 Date - 23rd of March 1994 + [9-10] 003.1,W Magnetic Variation + [10] *6A The checksum data, always begins with * + // NMEA 2.3 includes another field after + */ + + try + { + if (!nmea.checksumOK()){ + throw NMEAParseError("Checksum is invalid!"); + } + + if (nmea.parameters.size() < 11){ + throw NMEAParseError("GPS data is missing parameters."); + } + + // TIMESTAMP + this->fix.timestamp.setTime(parseDouble(nmea.parameters[0])); + + string sll; + string dir; + // LAT + sll = nmea.parameters[2]; + dir = nmea.parameters[3]; + if (sll.size() > 0){ + this->fix.latitude = convertLatLongToDeg(sll, dir); + } + + // LONG + sll = nmea.parameters[4]; + dir = nmea.parameters[5]; + if (sll.size() > 0){ + this->fix.longitude = convertLatLongToDeg(sll, dir); + } + + + // ACTIVE + bool lockupdate = false; + char status = 'V'; + if (nmea.parameters[1].size() > 0){ + status = nmea.parameters[1][0]; + } + this->fix.status = status; + if (status == 'V'){ + lockupdate = this->fix.setlock(false); + } + else if (status == 'A') { + lockupdate = this->fix.setlock(true); + } + else { + lockupdate = this->fix.setlock(false); //not A or V, so must be wrong... no lock + } + + + this->fix.speed = convertKnotsToKilometersPerHour(parseDouble(nmea.parameters[6])); // received as knots, convert to km/h + this->fix.travelAngle = parseDouble(nmea.parameters[7]); + this->fix.timestamp.setDate((int32_t)parseInt(nmea.parameters[8])); + + + //calling handlers + if (lockupdate){ + this->onLockStateChanged(this->fix.haslock); + } + this->onUpdate(); + } + catch (NumberConversionError& ex) + { + NMEAParseError pe("GPS Number Bad Format [$GPRMC] :: " + ex.message, nmea); + throw pe; + } + catch (NMEAParseError& ex) + { + NMEAParseError pe("GPS Data Bad Format [$GPRMC] :: " + ex.message, nmea); + throw pe; + } +} + +void GPSService::read_GPVTG(const NMEASentence& nmea){ + /* + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + where: + VTG Track made good and ground speed + [0-1] 054.7,T True track made good (degrees) + [2-3] 034.4,M Magnetic track made good + [4-5] 005.5,N Ground speed, knots + [6-7] 010.2,K Ground speed, Kilometers per hour + [7] *48 Checksum + */ + + try + { + if (!nmea.checksumOK()){ + throw NMEAParseError("Checksum is invalid!"); + } + + if (nmea.parameters.size() < 8){ + throw NMEAParseError("GPS data is missing parameters."); + } + + // SPEED + // if empty, is converted to 0 + this->fix.speed = parseDouble(nmea.parameters[6]); //km/h + + + this->onUpdate(); + } + catch (NumberConversionError& ex) + { + NMEAParseError pe("GPS Number Bad Format [$GPVTG] :: " + ex.message, nmea); + throw pe; + } + catch (NMEAParseError& ex) + { + NMEAParseError pe("GPS Data Bad Format [$GPVTG] :: " + ex.message, nmea); + throw pe; + } +} + diff --git a/src/NMEACommand.cpp b/src/NMEACommand.cpp new file mode 100644 index 0000000..a432767 --- /dev/null +++ b/src/NMEACommand.cpp @@ -0,0 +1,108 @@ +/* + * NMEACommand.cpp + * + * Created on: Sep 8, 2014 + * Author: Cameron Karlsson + */ + +#include <nmeaparse/NMEACommand.h> +#include <iomanip> +#include <sstream> + +using namespace std; +using namespace nmea; + + +NMEACommand::NMEACommand(){}; + +NMEACommand::~NMEACommand(){}; + +string NMEACommand::toString(){ + return addChecksum(message); +} + +string NMEACommand::addChecksum(std::string s){ + stringstream zz; + zz << name << "," << s; + checksum = NMEAParser::calculateChecksum(zz.str()); + + stringstream ss; + ios_base::fmtflags oldflags = ss.flags(); + ss << "$" << zz.str() << "*" << hex << uppercase << internal << setfill('0') << setw(2) << (int)checksum << "\r\n"; + ss.flags(oldflags); //reset + + return ss.str(); +}; + + + + /* + // $PSRF100,0,9600,8,1,0*0C + + Table 2-4 Set Serial Port Data Format + Name Example Unit Description + Message ID $PSRF100 PSRF100 protocol header + Protocol 0 0=SiRF binary, 1=NMEA + Baud 9600 1200, 2400, 4800, 9600, 19200, 38400, 57600, and 115200 + DataBits 8 8,71 + + StopBits 1 0,1 1. SiRF protocol is only valid for 8 data bits, 1stop bit, and no parity. + Parity 0 0=None, 1=Odd, 2=Even + Checksum *0C + <CR> <LF> End of message termination + */ +std::string NMEACommandSerialConfiguration::toString(){ + stringstream ss; + + ss << "1," << baud << "," << databits << "," << stopbits << "," << parity; + message = ss.str(); + + return NMEACommand::addChecksum(message); +} + + + + // $PSRF103,00,01,00,01*25 + /* + * Table 2-9 Query/Rate Control Data Format + Name Example Unit Description + Message ID $PSRF103 PSRF103 protocol header + Msg 00 See Table 2-10 + Mode 01 0=SetRate, 1=Query + Rate 00 sec Output—off=0, max=255 + CksumEnable 01 0=Disable Checksum, 1=Enable Checksum + Checksum *25 + <CR> <LF> End of message termination + */ + /* + * Table 2-10 Messages + Value Description + 0 GGA + 1 GLL + 2 GSA + 3 GSV + 4 RMC + 5 VTG + 6 MSS (If internal beacon is supported) + 7 Not defined + 8 ZDA (if 1PPS output is supported) + 9 Not defined + */ +// Data Members: +// int messageID; +// int mode; +// int rate; +// int checksumEnable; +// Creates a valid NMEA $PSRF103 command sentence. +std::string NMEACommandQueryRate::toString(){ + stringstream ss; + + ss << setfill('0') << setw(2) << messageID << "," + << setfill('0') << setw(2) << mode << "," + << setfill('0') << setw(2) << rate << "," + << setfill('0') << setw(2) << checksumEnable; + message = ss.str(); + + return NMEACommand::addChecksum(message); +} + diff --git a/src/NMEAParser.cpp b/src/NMEAParser.cpp new file mode 100644 index 0000000..f5d9653 --- /dev/null +++ b/src/NMEAParser.cpp @@ -0,0 +1,466 @@ +/* + * NMEAParser.cpp + * + * Created on: Aug 12, 2014 + * Author: Cameron Karlsson + */ + +#include <nmeaparse/NMEAParser.h> +#include <nmeaparse/NumberConversion.h> +#include <sstream> +#include <iostream> +#include <algorithm> +#include <cctype> + +using namespace std; +using namespace nmea; + + + +// --------- NMEA PARSE ERROR-------------- + +NMEAParseError::NMEAParseError(std::string msg) + : message(msg) +{} +NMEAParseError::NMEAParseError(std::string msg, NMEASentence n) + : message(msg), nmea(n) +{} + +NMEAParseError::~NMEAParseError() +{} + +std::string NMEAParseError::what(){ + return message; +} + + + + + // --------- NMEA SENTENCE -------------- + +NMEASentence::NMEASentence() : isvalid(false), calculatedChecksum(0), parsedChecksum(0) +{} +NMEASentence::~NMEASentence() +{} +bool NMEASentence::valid() const { + return isvalid; +} +bool NMEASentence::checksumOK() const { + return (parsedChecksum != 0 && calculatedChecksum != 0) + && + (parsedChecksum == calculatedChecksum); +} + + + +// true if the text contains a non-alpha numeric value +bool hasNonAlphaNum(string txt){ + for (size_t i = 0; i < txt.size(); i++){ + if ( !isalnum(txt[i]) ){ + return true; + } + } + return false; +} + +// true if alphanumeric or '-' +bool validParamChars(string txt){ + for (size_t i = 0; i < txt.size(); i++){ + if (!isalnum(txt[i])){ + if (txt[i] != '-'){ + return false; + } + } + } + return true; +} + +// remove all whitespace +void squish(string& str){ + + char chars[] = {'\t',' '}; + for (size_t i = 0; i < sizeof(chars); ++i) + { + // needs include <algorithm> + str.erase(std::remove(str.begin(), str.end(), chars[i]), str.end()); + } +} + +// remove side whitespace +void trim(string& str){ + stringstream trimmer; + trimmer << str; + str.clear(); + trimmer >> str; +} + + + // --------- NMEA PARSER -------------- + + + +NMEAParser::NMEAParser() + : log(false), maxbuffersize(NMEA_PARSER_MAX_BUFFER_SIZE), fillingbuffer(false) +{ + + +} + +NMEAParser::~NMEAParser() { + // TODO Auto-generated destructor stub +} + + +void NMEAParser::setSentenceHandler(std::string cmdKey, std::function<void(const NMEASentence&)> handler){ + eventTable.erase(cmdKey); + + //std::pair<string, function<void(NMEASentence)>> entry(cmdKey, handler); + //eventTable.insert(entry); + eventTable.insert({ cmdKey, handler }); +} + +void NMEAParser::readByte(uint8_t b){ + uint8_t startbyte = '$'; + + if (fillingbuffer){ + if (b == '\n'){ + buffer.push_back(b); + try { + readSentence(buffer); + buffer.clear(); + fillingbuffer = false; + } + catch (exception&){ + // If anything happens, let it pass through, but reset the buffer first. + buffer.clear(); + fillingbuffer = false; + throw; + } + } + else{ + if (buffer.size() < maxbuffersize){ + buffer.push_back(b); + } + else { + buffer.clear(); //clear the host buffer so it won't overflow. + fillingbuffer = false; + } + } + } + else { + if (b == startbyte){ // only start filling when we see the start byte. + fillingbuffer = true; + buffer.push_back(b); + } + } +} + +void NMEAParser::readBuffer(uint8_t* b, uint32_t size){ + for (uint32_t i = 0; i < size; ++i){ + readByte(b[i]); + } +} + +void NMEAParser::readLine(string cmd){ + cmd += "\r\n"; + for (size_t i = 0; i < cmd.size(); ++i){ + readByte(cmd[i]); + } +} + +// Loggers +void NMEAParser::onInfo(NMEASentence& nmea, string txt){ + if (log){ + cout << "[Info] " << txt << endl; + } +} +void NMEAParser::onWarning(NMEASentence& nmea, string txt){ + if (log){ + cout << "[Warning] " << txt << endl; + } +} +void NMEAParser::onError(NMEASentence& nmea, string txt){ + throw NMEAParseError("[ERROR] " + txt); +} + +// takes a complete NMEA string and gets the data bits from it, +// calls the corresponding handler in eventTable, based on the 5 letter sentence code +void NMEAParser::readSentence(std::string cmd){ + + NMEASentence nmea; + + onInfo(nmea, "Processing NEW string..."); + + if (cmd.size() == 0){ + onWarning(nmea, "Blank string -- Skipped processing."); + return; + } + + // If there is a newline at the end (we are coming from the byte reader + if ( *(cmd.end()-1) == '\n'){ + if (*(cmd.end() - 2) == '\r'){ // if there is a \r before the newline, remove it. + cmd = cmd.substr(0, cmd.size() - 2); + } + else + { + onWarning(nmea, "Malformed newline, missing carriage return (\\r) "); + cmd = cmd.substr(0, cmd.size()-1); + } + } + + ios_base::fmtflags oldflags = cout.flags(); + + // Remove all whitespace characters. + size_t beginsize = cmd.size(); + squish(cmd); + if (cmd.size() != beginsize){ + stringstream ss; + ss << "New NMEA string was full of " << (beginsize - cmd.size()) << " whitespaces!"; + onWarning(nmea, ss.str()); + } + + + onInfo(nmea, string("NMEA string: (\"") + cmd + "\")"); + + + // Seperates the data now that everything is formatted + try{ + parseText(nmea, cmd); + } + catch (NMEAParseError&){ + throw; + } + catch (std::exception& e){ + string s = " >> NMEA Parser Internal Error: Indexing error?... "; + throw std::runtime_error(s + e.what()); + } + cout.flags(oldflags); //reset + + // Handle/Throw parse errors + if (!nmea.valid()){ + + size_t linewidth = 35; + stringstream ss; + if (nmea.text.size() > linewidth){ + ss << "Invalid text. (\"" << nmea.text.substr(0, linewidth) << "...\")"; + } + else{ + ss << "Invalid text. (\"" << nmea.text << "\")"; + } + + onError(nmea, ss.str()); + return; + } + + + // Call the "any sentence" event handler, even if invalid checksum, for possible logging elsewhere. + onInfo(nmea, "Calling generic onSentence()."); + onSentence(nmea); + + + // Call event handlers based on map entries + function<void(const NMEASentence&)> handler = eventTable[nmea.name]; + if (handler){ + onInfo(nmea, string("Calling specific handler for sentence named \"") + nmea.name + "\""); + handler(nmea); + } + else + { + onWarning(nmea, string("Null event handler for type (name: \"") + nmea.name + "\")"); + } + + + + + + cout.flags(oldflags); //reset + +} + +// takes the string *between* the '$' and '*' in nmea sentence, +// then calculates a rolling XOR on the bytes +uint8_t NMEAParser::calculateChecksum(string s){ + uint8_t checksum = 0; + for (size_t i = 0; i < s.size(); i++){ + checksum = checksum ^ s[i]; + } + + // will display the calculated checksum in hex + //if(log) + //{ + // ios_base::fmtflags oldflags = cout.flags(); + // cout << "NMEA parser Info: calculated CHECKSUM for \"" << s << "\": 0x" << std::hex << (int)checksum << endl; + // cout.flags(oldflags); //reset + //} + return checksum; +} + + +void NMEAParser::parseText(NMEASentence& nmea, string txt){ + + if (txt.size() == 0){ + nmea.isvalid = false; + return; + } + + nmea.isvalid = false; // assume it's invalid first + nmea.text = txt; // save the received text of the sentence + + // Looking for index of last '$' + size_t startbyte = 0; + size_t dollar = txt.find_last_of('$'); + if (dollar == string::npos){ + // No dollar sign... INVALID! + return; + } + else + { + startbyte = dollar; + } + + + // Get rid of data up to last'$' + txt = txt.substr(startbyte + 1); + + + // Look for checksum + size_t checkstri = txt.find_last_of('*'); + bool haschecksum = checkstri != string::npos; + if (haschecksum){ + // A checksum was passed in the message, so calculate what we expect to see + nmea.calculatedChecksum = calculateChecksum(txt.substr(0, checkstri)); + } + else + { + // No checksum is only a warning because some devices allow sending data with no checksum. + onWarning(nmea, "No checksum information provided. Could not find '*'."); + } + + // Handle comma edge cases + size_t comma = txt.find(','); + if (comma == string::npos){ //comma not found, but there is a name... + if (txt.size() > 0) + { // the received data must just be the name + if ( hasNonAlphaNum(txt) ){ + nmea.isvalid = false; + return; + } + nmea.name = txt; + nmea.isvalid = true; + return; + } + else + { //it is a '$' with no information + nmea.isvalid = false; + return; + } + } + + //"$," case - no name + if (comma == 0){ + nmea.isvalid = false; + return; + } + + + //name should not include first comma + nmea.name = txt.substr(0, comma); + if ( hasNonAlphaNum(nmea.name) ){ + nmea.isvalid = false; + return; + } + + + //comma is the last character/only comma + if (comma + 1 == txt.size()){ + nmea.parameters.push_back(""); + nmea.isvalid = true; + return; + } + + + //move to data after first comma + txt = txt.substr(comma + 1, txt.size() - (comma + 1)); + + //parse parameters according to csv + istringstream f(txt); + string s; + while (getline(f, s, ',')) { + //cout << s << endl; + nmea.parameters.push_back(s); + } + + + //above line parsing does not add a blank parameter if there is a comma at the end... + // so do it here. + if (*(txt.end() - 1) == ','){ + + // supposed to have checksum but there is a comma at the end... invalid + if (haschecksum){ + nmea.isvalid = false; + return; + } + + //cout << "NMEA parser Warning: extra comma at end of sentence, but no information...?" << endl; // it's actually standard, if checksum is disabled + nmea.parameters.push_back(""); + + stringstream sz; + sz << "Found " << nmea.parameters.size() << " parameters."; + onInfo(nmea, sz.str()); + + } + else + { + stringstream sz; + sz << "Found " << nmea.parameters.size() << " parameters."; + onInfo(nmea, sz.str()); + + //possible checksum at end... + size_t endi = nmea.parameters.size() - 1; + size_t checki = nmea.parameters[endi].find_last_of('*'); + if (checki != string::npos){ + string last = nmea.parameters[endi]; + nmea.parameters[endi] = last.substr(0, checki); + if (checki == last.size() - 1){ + onError(nmea, "Checksum '*' character at end, but no data."); + } + else{ + nmea.checksum = last.substr(checki + 1, last.size() - checki); //extract checksum without '*' + + onInfo(nmea, string("Found checksum. (\"*") + nmea.checksum + "\")"); + + try + { + nmea.parsedChecksum = (uint8_t)parseInt(nmea.checksum, 16); + } + catch( NumberConversionError& ) + { + onError(nmea, string("parseInt() error. Parsed checksum string was not readable as hex. (\"") + nmea.checksum + "\")"); + } + + onInfo(nmea, string("Checksum ok? ") + (nmea.checksumOK() ? "YES" : "NO") + "!"); + + + } + } + } + + + for (size_t i = 0; i < nmea.parameters.size(); i++){ + if (!validParamChars(nmea.parameters[i])){ + nmea.isvalid = false; + stringstream ss; + ss << "Invalid character in parameter (from 0) " << i << "."; + onError(nmea, ss.str() ); + break; + } + } + + + nmea.isvalid = true; + + return; + +} + + diff --git a/src/NumberConversion.cpp b/src/NumberConversion.cpp new file mode 100644 index 0000000..a5d429d --- /dev/null +++ b/src/NumberConversion.cpp @@ -0,0 +1,144 @@ +/* + * NumberConversion.cpp + * + * Created on: Sep 2, 2014 + * Author: Cameron Karlsson + */ + +#include <nmeaparse/NumberConversion.h> + +using namespace std; + +namespace nmea { +// Note: both parseDouble and parseInt return 0 with "" input. + + double parseDouble(std::string s){ + + char* p; + double d = ::strtod(s.c_str(), &p); + if (*p != 0){ + std::stringstream ss; + ss << "NumberConversionError: parseDouble() error in argument \"" << s << "\", '" + << *p << "' is not a number."; + throw NumberConversionError(ss.str()); + } + return d; + } + int64_t parseInt(std::string s, int radix){ + char* p; + + int64_t d = ::strtoll(s.c_str(), &p, radix); + + if (*p != 0) { + std::stringstream ss; + ss << "NumberConversionError: parseInt() error in argument \"" << s << "\", '" + << *p << "' is not a number."; + throw NumberConversionError(ss.str()); + } + return d; + } + +} + +/* +#include <iostream> +void NumberConversion_test(){ + string s; + float f; + long long k; + + try{ + s = "-1.345"; + f = parseDouble(s); + cout << s << ": " << f << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + + try{ + s = "-1.23e-2"; + f = parseDouble(s); + cout << s << ": " << f << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + + try{ + s = ""; + f = parseDouble(s); + cout << s << ": " << f << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + + + try{ + // -- fails, ok + s = "asd"; + f = parseDouble(s); + cout << s << ": " << f << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + try{ + s = "-1234.123"; + k = parseInt(s); + cout << s << ": " << k << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + + + try{ + s = "01234"; + k = parseInt(s); + cout << s << ": " << k << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + + try{ + // -- converts to 0 + s = ""; + k = parseInt(s); + cout << s << ": " << k << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + + try{ + // -- fails, ok + s = "asd"; + k = parseInt(s); + cout << s << ": " << k << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + try{ + // -- fails, ok + s = "-16"; + k = parseInt(s); + cout << s << ": " << k << endl; + } + catch(NumberConversionError& ex){ + cout << ex.message << endl; + } + + } + */
\ No newline at end of file |