From 5b461ec2d133f40f5d45d89010b3f3672a48692c Mon Sep 17 00:00:00 2001
From: deadprogram <ron@hybridgroup.com>
Date: Sat, 14 Jan 2023 11:06:25 +0100
Subject: [PATCH] gps: improve error handling

Signed-off-by: deadprogram <ron@hybridgroup.com>
---
 gps/gps.go       | 29 +++++++++++++++++++++++++----
 gps/gpsparser.go | 39 +++++++++++++++++++--------------------
 2 files changed, 44 insertions(+), 24 deletions(-)

diff --git a/gps/gps.go b/gps/gps.go
index 09c5af8..80cdb03 100644
--- a/gps/gps.go
+++ b/gps/gps.go
@@ -13,8 +13,28 @@ import (
 var (
 	errInvalidNMEASentenceLength = errors.New("invalid NMEA sentence length")
 	errInvalidNMEAChecksum       = errors.New("invalid NMEA sentence checksum")
+	errEmptyNMEASentence         = errors.New("cannot parse empty NMEA sentence")
+	errUnknownNMEASentence       = errors.New("unsupported NMEA sentence type")
+	errInvalidGGASentence        = errors.New("invalid GGA NMEA sentence")
+	errInvalidRMCSentence        = errors.New("invalid RMC NMEA sentence")
 )
 
+type GPSError struct {
+	Info string
+	Err  error
+}
+
+func newGPSError(err error, info string) GPSError {
+	return GPSError{
+		Info: info,
+		Err:  err,
+	}
+}
+
+func (ge GPSError) Error() string {
+	return ge.Err.Error() + " " + ge.Info
+}
+
 // Device wraps a connection to a GPS device.
 type Device struct {
 	buffer   []byte
@@ -127,16 +147,17 @@ func (gps *Device) WriteBytes(bytes []byte) {
 
 // validSentence checks if a sentence has been received uncorrupted
 func validSentence(sentence string) error {
-	if len(sentence) < 4 || sentence[0] != '$' || sentence[len(sentence)-3] != '*' {
+	if len(sentence) < 7 || sentence[0] != '$' || sentence[len(sentence)-3] != '*' {
 		return errInvalidNMEASentenceLength
 	}
 	var cs byte = 0
 	for i := 1; i < len(sentence)-3; i++ {
 		cs ^= sentence[i]
 	}
-	checksum := hex.EncodeToString([]byte{cs})
-	if (checksum[0] != sentence[len(sentence)-2]) || (checksum[1] != sentence[len(sentence)-1]) {
-		return errInvalidNMEAChecksum
+	checksum := strings.ToUpper(hex.EncodeToString([]byte{cs}))
+	if checksum != sentence[len(sentence)-2:len(sentence)] {
+		return newGPSError(errInvalidNMEAChecksum, "expected "+sentence[len(sentence)-2:len(sentence)]+
+			" got "+checksum)
 	}
 
 	return nil
diff --git a/gps/gpsparser.go b/gps/gpsparser.go
index befc141..77abacc 100644
--- a/gps/gpsparser.go
+++ b/gps/gpsparser.go
@@ -1,19 +1,11 @@
 package gps
 
 import (
-	"errors"
 	"strconv"
 	"strings"
 	"time"
 )
 
-var (
-	errEmptyNMEASentence   = errors.New("cannot parse empty NMEA sentence")
-	errUnknownNMEASentence = errors.New("unsupported NMEA sentence type")
-	errInvalidGGASentence  = errors.New("invalid GGA NMEA sentence")
-	errInvalidRMCSentence  = errors.New("invalid RMC NMEA sentence")
-)
-
 // Parser for GPS NMEA sentences.
 type Parser struct {
 }
@@ -51,18 +43,20 @@ func NewParser() Parser {
 }
 
 // Parse parses a NMEA sentence looking for fix info.
-func (parser *Parser) Parse(sentence string) (fix Fix, err error) {
+func (parser *Parser) Parse(sentence string) (Fix, error) {
+	var fix Fix
 	if sentence == "" {
-		err = errEmptyNMEASentence
-		return
+		return fix, errEmptyNMEASentence
+	}
+	if len(sentence) < 6 {
+		return fix, errInvalidNMEASentenceLength
 	}
 	typ := sentence[3:6]
 	switch typ {
 	case "GGA":
 		fields := strings.Split(sentence, ",")
 		if len(fields) != 15 {
-			err = errInvalidGGASentence
-			return
+			return fix, errInvalidGGASentence
 		}
 
 		fix.Altitude = findAltitude(fields[9])
@@ -71,11 +65,12 @@ func (parser *Parser) Parse(sentence string) (fix Fix, err error) {
 		fix.Latitude = findLatitude(fields[2], fields[3])
 		fix.Time = findTime(fields[1])
 		fix.Valid = (fix.Altitude != -99999) && (fix.Satellites > 0)
+
+		return fix, nil
 	case "RMC":
 		fields := strings.Split(sentence, ",")
 		if len(fields) != 13 {
-			err = errInvalidRMCSentence
-			return
+			return fix, errInvalidRMCSentence
 		}
 
 		fix.Longitude = findLongitude(fields[5], fields[6])
@@ -83,11 +78,12 @@ func (parser *Parser) Parse(sentence string) (fix Fix, err error) {
 		fix.Time = findTime(fields[1])
 		fix.Speed = findSpeed(fields[7])
 		fix.Heading = findHeading(fields[8])
-		fix.Valid = (len(fields[2]) > 0 && fields[2][0:1] == "A")
-	default:
-		err = errUnknownNMEASentence
+		fix.Valid = (len(fields[2]) > 0 && fields[2] == "A")
+
+		return fix, nil
 	}
-	return
+
+	return fix, newGPSError(errUnknownNMEASentence, typ)
 }
 
 // findTime returns the time from an NMEA sentence:
@@ -100,7 +96,10 @@ func findTime(val string) time.Time {
 	h, _ := strconv.ParseInt(val[0:2], 10, 8)
 	m, _ := strconv.ParseInt(val[2:4], 10, 8)
 	s, _ := strconv.ParseInt(val[4:6], 10, 8)
-	ms, _ := strconv.ParseInt(val[7:10], 10, 16)
+	ms := int64(0)
+	if len(val) == 10 {
+		ms, _ = strconv.ParseInt(val[7:10], 10, 16)
+	}
 	t := time.Date(0, 0, 0, int(h), int(m), int(s), int(ms), time.UTC)
 
 	return t