Merge pull request #244 from AvSquirrel/gps_memleak

Additional GPS compatibility improvements

main/ry835ai.go

@@ -408,6 +408,17 @@ func calculateNACp(accuracy float32) uint8 {
 	return ret
 }
 
+/*
+processNMEALine parses NMEA-0183 formatted strings against several message types.
+
+Standard messages supported: RMC GGA VTG GSA
+U-blox proprietary messages: PUBX,00 PUBX,03 PUBX,04
+
+return is false if errors occur during parse, or if GPS position is invalid
+return is true if parse occurs correctly and position is valid.
+
+*/
+
 func processNMEALine(l string) bool {
 	replayLog(l, MSGCLASS_GPS)
 	l_valid, validNMEAcs := validateNMEAChecksum(l)
@@ -428,6 +439,48 @@ func processNMEALine(l string) bool {
 			mySituation.mu_GPS.Lock()
 			defer mySituation.mu_GPS.Unlock()
 
+			// Do the accuracy / quality fields first to prevent invalid position etc. from being sent downstream
+
+			// field 8 = nav status
+			// DR = dead reckoning, G2= 2D GPS, G3 = 3D GPS, D2= 2D diff, D3 = 3D diff, RK = GPS+DR, TT = time only
+
+			okReturn := true
+
+			if x[8] == "D2" || x[8] == "D3" {
+				mySituation.quality = 2
+			} else if x[8] == "G2" || x[8] == "G3" {
+				mySituation.quality = 1
+			} else if x[8] == "DR" || x[8] == "RK" {
+				mySituation.quality = 6
+			} else if x[8] == "NF" {
+				mySituation.quality = 0
+				okReturn = false //  better to have no data than wrong data
+			} else {
+				mySituation.quality = 0
+				okReturn = false //  better to have no data than wrong data
+			}
+
+			// field 9 = horizontal accuracy, m
+			hAcc, err := strconv.ParseFloat(x[9], 32)
+			if err != nil {
+				okReturn = false
+			}
+			mySituation.Accuracy = float32(hAcc * 2) // UBX reports 1-sigma variation; NACp is 95% confidence (2-sigma)
+
+			// NACp estimate.
+			mySituation.NACp = calculateNACp(mySituation.Accuracy)
+
+			// field 10 = vertical accuracy, m
+			vAcc, err := strconv.ParseFloat(x[10], 32)
+			if err != nil {
+				okReturn = false
+			}
+			mySituation.AccuracyVert = float32(vAcc * 2) // UBX reports 1-sigma variation; we want 95% confidence
+
+			if !okReturn {
+				return false
+			}
+
 			// field 2 = time
 			if len(x[2]) < 9 {
 				return false
@@ -482,38 +535,7 @@ func processNMEALine(l string) bool {
 			alt := float32(hae*3.28084) - mySituation.GeoidSep // convert to feet and offset by geoid separation
 			mySituation.Alt = alt
 
-			// field 8 = nav status
+			mySituation.LastFixLocalTime = stratuxClock.Time
-			// DR = dead reckoning, G2= 2D GPS, G3 = 3D GPS, D2= 2D diff, D3 = 3D diff, RK = GPS+DR, TT = time only
-
-			if x[8] == "D2" || x[8] == "D3" {
-				mySituation.quality = 2
-			} else if x[8] == "G2" || x[8] == "G3" {
-				mySituation.quality = 1
-			} else if x[8] == "DR" || x[8] == "RK" {
-				mySituation.quality = 6
-			} else if x[8] == "NF" {
-				mySituation.quality = 0
-				return false // return false if no valid fix.
-			} else {
-				mySituation.quality = 0
-			}
-
-			// field 9 = horizontal accuracy, m
-			hAcc, err := strconv.ParseFloat(x[9], 32)
-			if err != nil {
-				return false
-			}
-			mySituation.Accuracy = float32(hAcc * 2) // UBX reports 1-sigma variation; NACp is 95% confidence (2-sigma)
-
-			// NACp estimate.
-			mySituation.NACp = calculateNACp(mySituation.Accuracy)
-
-			// field 10 = vertical accuracy, m
-			vAcc, err := strconv.ParseFloat(x[10], 32)
-			if err != nil {
-				return false
-			}
-			mySituation.AccuracyVert = float32(vAcc * 2) // UBX reports 1-sigma variation; we want 95% confidence
 
 			// field 11 = groundspeed, km/h
 			groundspeed, err := strconv.ParseFloat(x[11], 32)
@@ -521,26 +543,22 @@ func processNMEALine(l string) bool {
 				return false
 			}
 			groundspeed = groundspeed * 0.540003 // convert to knots
+			mySituation.GroundSpeed = uint16(groundspeed)
 
 			// field 12 = track, deg
 			trueCourse := uint16(0)
-			if len(x[12]) > 0 && groundspeed > 3 {
 			tc, err := strconv.ParseFloat(x[12], 32)
 			if err != nil {
 				return false
 			}
+			if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
 				trueCourse = uint16(tc)
-			} else {
-				// No movement.
-				mySituation.TrueCourse = 0
-				mySituation.GroundSpeed = 0
-				mySituation.LastGroundTrackTime = time.Time{}
-			}
-
 				setTrueCourse(uint16(groundspeed), trueCourse)
-
 				mySituation.TrueCourse = uint16(trueCourse)
-			mySituation.GroundSpeed = uint16(groundspeed)
+			} else {
+				// Negligible movement. Don't update course, but do use the slow speed.
+				// TO-DO: use average course over last n seconds?
+			}
 			mySituation.LastGroundTrackTime = stratuxClock.Time
 
 			// field 13 = vertical velocity, m/s
@@ -559,8 +577,6 @@ func processNMEALine(l string) bool {
 			}
 			mySituation.Satellites = uint16(sat)
 
-			mySituation.LastFixLocalTime = stratuxClock.Time
-
 		} else if x[1] == "03" { // satellite status message
 
 			// field 2 = number of satellites tracked
@@ -572,11 +588,15 @@ func processNMEALine(l string) bool {
 			mySituation.SatellitesTracked = uint16(satTracked)
 
 			// fields 3-8 are repeated block
+
 			for i := 0; i < satTracked; i++ {
-				if x[7+6*i] != "" {
+				j := 7 + 6*i
+				if j < len(x) {
+					if x[j] != "" {
 						satSeen++
 					}
 				}
+			}
 
 			mySituation.SatellitesSeen = uint16(satSeen)
 			// log.Printf("Satellites with signal: %v\n",mySituation.SatellitesSeen)
@@ -638,36 +658,33 @@ func processNMEALine(l string) bool {
 			}
 		}
 
-		// otherwise parse the NMEA standard messages as a fall-back / compatibility option
+		// otherwise parse the NMEA standard messages as a compatibility option for SIRF, generic NMEA, etc.
 	} else if (x[0] == "GNVTG") || (x[0] == "GPVTG") { // Ground track information.
-		mySituation.mu_GPS.Lock()
-		defer mySituation.mu_GPS.Unlock()
 		if len(x) < 9 { // Reduce from 10 to 9 to allow parsing by devices pre-NMEA v2.3
 			return false
 		}
+		mySituation.mu_GPS.Lock()
+		defer mySituation.mu_GPS.Unlock()
+
+		groundspeed, err := strconv.ParseFloat(x[5], 32) // Knots.
+		if err != nil {
+			return false
+		}
+		mySituation.GroundSpeed = uint16(groundspeed)
+
 		trueCourse := uint16(0)
-		if len(x[1]) > 0 {
 		tc, err := strconv.ParseFloat(x[1], 32)
 		if err != nil {
 			return false
 		}
+		if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
 			trueCourse = uint16(tc)
-		} else {
+			setTrueCourse(uint16(groundspeed), trueCourse)
-			// No movement.
-			mySituation.TrueCourse = 0
-			mySituation.GroundSpeed = 0
-			mySituation.LastGroundTrackTime = time.Time{}
-			return true
-		}
-		groundSpeed, err := strconv.ParseFloat(x[5], 32) // Knots.
-		if err != nil {
-			return false
-		}
-
-		setTrueCourse(uint16(groundSpeed), trueCourse)
-
 			mySituation.TrueCourse = uint16(trueCourse)
-		mySituation.GroundSpeed = uint16(groundSpeed)
+		} else {
+			// Negligible movement. Don't update course, but do use the slow speed.
+			// TO-DO: use average course over last n seconds?
+		}
 		mySituation.LastGroundTrackTime = stratuxClock.Time
 
 	} else if (x[0] == "GNGGA") || (x[0] == "GPGGA") { // GPS fix.
@@ -676,6 +693,14 @@ func processNMEALine(l string) bool {
 		}
 		mySituation.mu_GPS.Lock()
 		defer mySituation.mu_GPS.Unlock()
+
+		// Quality indicator.
+		q, err1 := strconv.Atoi(x[6])
+		if err1 != nil {
+			return false
+		}
+		mySituation.quality = uint8(q) // 1 = 3D GPS; 2 = DGPS (SBAS /WAAS)
+
 		// Timestamp.
 		if len(x[1]) < 9 {
 			return false
@@ -720,13 +745,6 @@ func processNMEALine(l string) bool {
 			mySituation.Lng = -mySituation.Lng
 		}
 
-		// Quality indicator.
-		q, err1 := strconv.Atoi(x[6])
-		if err1 != nil {
-			return false
-		}
-		mySituation.quality = uint8(q) // 1 = 3D GPS; 2 = DGPS (SBAS /WAAS)
-
 		/* Satellite count and horizontal accuracy deprecated. Using PUBX,00 with fallback to GSA.
 		// Satellites.
 		sat, err1 := strconv.Atoi(x[7])
@@ -791,6 +809,13 @@ func processNMEALine(l string) bool {
 		mySituation.mu_GPS.Lock()
 		defer mySituation.mu_GPS.Unlock()
 
+		if x[2] != "A" { // invalid fix
+			mySituation.quality = 0
+			return false
+		} else if mySituation.quality == 0 {
+			mySituation.quality = 1 // fallback option; indicate if the position fix is valid even if GGA or PUBX,00 aren't received
+		}
+
 		// Timestamp.
 		if len(x[1]) < 9 {
 			return false
@@ -820,10 +845,6 @@ func processNMEALine(l string) bool {
 			}
 		}
 
-		if x[2] != "A" { // invalid fix
-			return false
-		}
-
 		// Latitude.
 		if len(x[3]) < 4 {
 			return false
@@ -850,18 +871,32 @@ func processNMEALine(l string) bool {
 		if x[6] == "W" { // West = negative.
 			mySituation.Lng = -mySituation.Lng
 		}
+
+		mySituation.LastFixLocalTime = stratuxClock.Time
+
 		// ground speed in kts (field 7)
 		groundspeed, err := strconv.ParseFloat(x[7], 32)
 		if err != nil {
 			return false
 		}
 		mySituation.GroundSpeed = uint16(groundspeed)
-		// ground track "True" field 8
+
+		// ground track "True" (field 8)
+		trueCourse := uint16(0)
 		tc, err := strconv.ParseFloat(x[8], 32)
 		if err != nil {
 			return false
 		}
-		mySituation.TrueCourse = uint16(tc)
+		if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
+			trueCourse = uint16(tc)
+			setTrueCourse(uint16(groundspeed), trueCourse)
+			mySituation.TrueCourse = uint16(trueCourse)
+		} else {
+			// Negligible movement. Don't update course, but do use the slow speed.
+			// TO-DO: use average course over last n seconds?
+		}
+
+		mySituation.LastGroundTrackTime = stratuxClock.Time
 
 	} else if (x[0] == "GNGSA") || (x[0] == "GPGSA") {
 		if len(x) < 18 {
@@ -872,6 +907,7 @@ func processNMEALine(l string) bool {
 		// M: manual forced to 2D or 3D mode
 		// A: automatic switching between 2D and 3D modes
 		if (x[1] != "A") && (x[1] != "M") { // invalid fix
+			mySituation.quality = 0
 			return false
 		}
 
@@ -931,13 +967,15 @@ func gpsSerialReader() {
 	scanner := bufio.NewScanner(serialPort)
 	for scanner.Scan() && globalStatus.GPS_connected && globalSettings.GPS_Enabled {
 		i++
-		if i%50 == 0 {
+		if i%100 == 0 {
 			fmt.Printf("gpsSerialReader() scanner loop iteration i=%d\n", i) // debug monitor
 		}
 
 		s := scanner.Text()
 		//fmt.Printf("Output: %s\n", s)
-		processNMEALine(s)
+		if !(processNMEALine(s)) {
+			//	fmt.Printf("processNMEALine() exited early -- %s\n",s) //debug code. comment out before pushing to github
+		}
 	}
 	if err := scanner.Err(); err != nil {
 		log.Printf("reading standard input: %s\n", err.Error())