summaryrefslogtreecommitdiff
path: root/src/GPSService.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/GPSService.cpp')
-rw-r--r--src/GPSService.cpp509
1 files changed, 509 insertions, 0 deletions
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;
+ }
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2025-12-06 05:50:27 +0000