Diff: GroveGPS.h

Revision:

4:0ee729b4a211

Parent:

3:037ec2b52d31

Child:

5:8a5414710483

--- a/GroveGPS.h	Thu Jun 06 20:17:47 2019 -0500
+++ b/GroveGPS.h	Thu Jun 06 21:26:39 2019 -0500
@@ -9,20 +9,13 @@
 
 public:
 
-	GroveGPS(PinName tx=D1, PinName rx=D0) : _last_line(""), gps_serial(tx, rx, 9600) {}
-
-	void readCharacter(char newCharacter) {
-		if (newCharacter == '\n') {
-			parseLine();
-			_last_line = "";
-		} else {
-			_last_line += newCharacter;
-		}
-
-		if (_last_line.length() > max_line_length) {
-		    error("GPS error - line too long");
-		    _last_line = "";
-		}
+	GroveGPS(PinName tx=D1, PinName rx=D0) : gps_serial(tx, rx, 9600) {
+	    memset(_isr_line_bufs, 0, sizeof(_isr_line_bufs));
+	    _first_line_in_use = true;
+	    _isr_line_buf_pos = 0;
+	    memset(_last_line, 0, sizeof(_last_line));
+	    _last_line_updated = false;
+	    gps_serial.attach(callback(this, &GroveGPS::read_serial), SerialBase::RxIrq);
 	}
 
 	struct GGA {
@@ -40,17 +33,20 @@
 		char    geoid_separation_units;
 		long    age_of_diff;
 		long    diff_ref_station_id;
-		std::string  checksum;
 	} gps_gga;
 
 	void getTimestamp(char* buffer) {
         m.lock();
+        parseLine();
+
 		sprintf(buffer, "%f", gps_gga.utc_time);
         m.unlock();
 	}
 
 	void getLatitude(char* buffer) {
         m.lock();
+        parseLine();
+
 		double coordinate = convertGPSToDecimal(gps_gga.latitude);
 		if (gps_gga.position_fix==0)
 			sprintf(buffer, "N/A");
@@ -61,6 +57,8 @@
 
 	void getLongitude(char* buffer) {
         m.lock();
+        parseLine();
+
 		double coordinate = convertGPSToDecimal(gps_gga.longitude);
 		if (gps_gga.position_fix==0)
 			sprintf(buffer, "N/A");
@@ -68,22 +66,43 @@
 		sprintf(buffer, "%c%f", (gps_gga.ew_indicator == 'E') ? '0' : '-', coordinate);
         m.unlock();
 	}
-    
-    void start() {
-        serial_thread.start(callback(this, &GroveGPS::read_serial));
-    }
-        
 
 private:
-    static const size_t max_line_length = 1024;
-	std::string _last_line;
-    Thread serial_thread;
-    Serial gps_serial;
+    static const size_t max_line_length = 256;
+    char _isr_line_bufs[2][max_line_length];
+    bool _first_line_in_use;
+    size_t _isr_line_buf_pos;
+    char _last_line[max_line_length];
+    bool _last_line_updated;
+
+    RawSerial gps_serial;
     Mutex m;
+
     void read_serial() {
-        while (true) {
-            if (gps_serial.readable()) {
-                 readCharacter(gps_serial.getc());
+        while (gps_serial.readable()) {
+
+            // Check for overflow
+            if (_isr_line_buf_pos > max_line_length -1 ) {
+                error("GPS error - line too long");
+                _isr_line_buf_pos = 0;
+            }
+
+            // Add a character to the active buffer
+            char *buf = _isr_line_bufs[_first_line_in_use ? 0 : 1];
+            char value = gps_serial.getc();
+            buf[_isr_line_buf_pos] = value;
+            _isr_line_buf_pos++;
+
+            // Check for end of line
+            if (value == '\n') {
+                buf[_isr_line_buf_pos] = 0;
+                _isr_line_buf_pos = 0;
+
+                // Save off this line if it is valid
+                if (memcmp("$GPGGA", buf, 6) == 0) {
+                    _first_line_in_use = !_first_line_in_use;
+                    _last_line_updated = true;
+                }
             }
         }
     }
@@ -97,32 +116,45 @@
 	}
 
 	void parseLine() {
-		if (_last_line.find("GPGGA") != std::string::npos) {
-            m.lock();
-			parseGGA();
-            m.unlock();
-		}
+	    bool parse_gga = false;
+
+	    // Atomically copy the line buffer since the ISR can change it at any time
+	    core_util_critical_section_enter();
+	    if (_last_line_updated) {
+	        char *buf_saved = _isr_line_bufs[_first_line_in_use ? 1 : 0];
+	        strcpy(_last_line, buf_saved);
+	        parse_gga = true;
+            _last_line_updated = false;
+	    }
+	    core_util_critical_section_exit();
+
+	    if (parse_gga) {
+	        parseGGA();
+	    }
 	}
 
 	void parseGGA() {
-  		char* pEnd;
+  		char *line_pos = _last_line;
 		for (int i=0; i<14; i++) {
-			std::string current_item = _last_line.substr(0,_last_line.find(","));
-			_last_line = _last_line.substr(_last_line.find(",")+1);
 			if (i==0) { 		// NMEA Tag
 			} else if (i==1) { 	// UTC time
-				gps_gga.utc_time = strtod(current_item.c_str(), &pEnd);
+				gps_gga.utc_time = strtod(line_pos, 0);
 			} else if (i==2) { 	// Latitude
-				gps_gga.latitude = strtod(current_item.c_str(), &pEnd);
+				gps_gga.latitude = strtod(line_pos, 0);
 			} else if (i==3) { 	// Latitude North/South indicator
-				gps_gga.ns_indicator = current_item[0];
+				gps_gga.ns_indicator = line_pos[0];
 			} else if (i==4) { 	// Longitude
-				gps_gga.longitude = strtod(current_item.c_str(), &pEnd);
+				gps_gga.longitude = strtod(line_pos, 0);
 			} else if (i==5) { 	// Longitude indicator
-				gps_gga.ew_indicator = current_item[0];
+				gps_gga.ew_indicator = line_pos[0];
 			} else if (i==6) {
-				gps_gga.position_fix = strtod(current_item.c_str(), &pEnd);
+				gps_gga.position_fix = strtod(line_pos, 0);
 			}
+			line_pos = strchr(line_pos, ',');
+			if (line_pos == NULL) {
+			    break;
+			}
+			line_pos += 1;
 		}
 	}
 };