gps: improvements and bugfixes (#186)

* gps: buxfixes and refactoring of API to separate device from parser

Signed-off-by: deadprogram <ron@hybridgroup.com>

* gps: simplify time parser

Signed-off-by: deadprogram <ron@hybridgroup.com>

* gps: add support for RMC sentences

Signed-off-by: deadprogram <ron@hybridgroup.com>

* gps: small renaming to remove reduntant use of word GPS

Signed-off-by: deadprogram <ron@hybridgroup.com>

examples/gps/i2c/main.go

@@ -1,8 +1,8 @@
 package main
 
 import (
-	"fmt"
 	"machine"
+	"time"
 
 	"tinygo.org/x/drivers/gps"
 )
@@ -11,19 +11,32 @@ func main() {
 	println("GPS I2C Example")
 	machine.I2C0.Configure(machine.I2CConfig{})
 	ublox := gps.NewI2C(&machine.I2C0)
-	parser := gps.Parser(ublox)
+	parser := gps.NewParser()
 	var fix gps.Fix
 	for {
-		fix = parser.NextFix()
+		s, err := ublox.NextSentence()
+		if err != nil {
+			println(err)
+			continue
+		}
+
+		fix, err = parser.Parse(s)
+		if err != nil {
+			println(err)
+			continue
+		}
 		if fix.Valid {
 			print(fix.Time.Format("15:04:05"))
-			print(", lat=", fmt.Sprintf("%f", fix.Latitude))
-			print(", long=", fmt.Sprintf("%f", fix.Longitude))
-			print(", altitude:=", fix.Altitude)
+			print(", lat=")
+			print(fix.Latitude)
+			print(", long=")
+			print(fix.Longitude)
+			print(", altitude=", fix.Altitude)
 			print(", satellites=", fix.Satellites)
 			println()
 		} else {
 			println("No fix")
 		}
+		time.Sleep(200 * time.Millisecond)
 	}
 }

examples/gps/uart/main.go

@@ -1,8 +1,8 @@
 package main
 
 import (
-	"fmt"
 	"machine"
+	"time"
 
 	"tinygo.org/x/drivers/gps"
 )
@@ -11,19 +11,32 @@ func main() {
 	println("GPS UART Example")
 	machine.UART1.Configure(machine.UARTConfig{BaudRate: 9600})
 	ublox := gps.NewUART(&machine.UART1)
-	parser := gps.Parser(ublox)
+	parser := gps.NewParser()
 	var fix gps.Fix
 	for {
-		fix = parser.NextFix()
+		s, err := ublox.NextSentence()
+		if err != nil {
+			println(err)
+			continue
+		}
+
+		fix, err = parser.Parse(s)
+		if err != nil {
+			println(err)
+			continue
+		}
 		if fix.Valid {
 			print(fix.Time.Format("15:04:05"))
-			print(", lat=", fmt.Sprintf("%f", fix.Latitude))
-			print(", long=", fmt.Sprintf("%f", fix.Longitude))
-			print(", altitude:=", fix.Altitude)
+			print(", lat=")
+			print(fix.Latitude)
+			print(", long=")
+			print(fix.Longitude)
+			print(", altitude=", fix.Altitude)
 			print(", satellites=", fix.Satellites)
 			println()
 		} else {
 			println("No fix")
 		}
+		time.Sleep(200 * time.Millisecond)
 	}
 }

gps/gps.go

@@ -3,13 +3,19 @@ package gps // import "tinygo.org/x/drivers/gps"
 
 import (
 	"encoding/hex"
+	"errors"
 	"machine"
 	"strings"
 	"time"
 )
 
+var (
+	errInvalidNMEASentenceLength = errors.New("invalid NMEA sentence length")
+	errInvalidNMEAChecksum       = errors.New("invalid NMEA sentence checksum")
+)
+
 // Device wraps a connection to a GPS device.
-type GPSDevice struct {
+type Device struct {
 	buffer   []byte
 	bufIdx   int
 	sentence strings.Builder
@@ -19,8 +25,8 @@ type GPSDevice struct {
 }
 
 // NewUART creates a new UART GPS connection. The UART must already be configured.
-func NewUART(uart *machine.UART) GPSDevice {
-	return GPSDevice{
+func NewUART(uart *machine.UART) Device {
+	return Device{
 		uart:     uart,
 		buffer:   make([]byte, bufferSize),
 		bufIdx:   bufferSize,
@@ -29,8 +35,8 @@ func NewUART(uart *machine.UART) GPSDevice {
 }
 
 // NewI2C creates a new I2C GPS connection.
-func NewI2C(bus *machine.I2C) GPSDevice {
-	return GPSDevice{
+func NewI2C(bus *machine.I2C) Device {
+	return Device{
 		bus:      bus,
 		address:  I2C_ADDRESS,
 		buffer:   make([]byte, bufferSize),
@@ -39,17 +45,17 @@ func NewI2C(bus *machine.I2C) GPSDevice {
 	}
 }
 
-// ReadNextSentence returns the next valid NMEA sentence from the GPS device.
-func (gps *GPSDevice) NextSentence() (sentence string) {
+// NextSentence returns the next valid NMEA sentence from the GPS device.
+func (gps *Device) NextSentence() (sentence string, err error) {
 	sentence = gps.readNextSentence()
-	for !validSentence(sentence) {
-		sentence = gps.readNextSentence()
+	if err = validSentence(sentence); err != nil {
+		return "", err
 	}
-	return sentence
+	return sentence, nil
 }
 
 // readNextSentence returns the next sentence from the GPS device.
-func (gps *GPSDevice) readNextSentence() (sentence string) {
+func (gps *Device) readNextSentence() (sentence string) {
 	gps.sentence.Reset()
 	var b byte = ' '
 
@@ -69,7 +75,7 @@ func (gps *GPSDevice) readNextSentence() (sentence string) {
 	return sentence
 }
 
-func (gps *GPSDevice) readNextByte() (b byte) {
+func (gps *Device) readNextByte() (b byte) {
 	gps.bufIdx += 1
 	if gps.bufIdx >= bufferSize {
 		gps.fillBuffer()
@@ -77,7 +83,7 @@ func (gps *GPSDevice) readNextByte() (b byte) {
 	return gps.buffer[gps.bufIdx]
 }
 
-func (gps *GPSDevice) fillBuffer() {
+func (gps *Device) fillBuffer() {
 	if gps.uart != nil {
 		gps.uartFillBuffer()
 	} else {
@@ -85,7 +91,7 @@ func (gps *GPSDevice) fillBuffer() {
 	}
 }
 
-func (gps *GPSDevice) uartFillBuffer() {
+func (gps *Device) uartFillBuffer() {
 	for gps.uart.Buffered() < bufferSize {
 		time.Sleep(100 * time.Millisecond)
 	}
@@ -93,7 +99,7 @@ func (gps *GPSDevice) uartFillBuffer() {
 	gps.bufIdx = 0
 }
 
-func (gps *GPSDevice) i2cFillBuffer() {
+func (gps *Device) i2cFillBuffer() {
 	for gps.available() < bufferSize {
 		time.Sleep(100 * time.Millisecond)
 	}
@@ -102,7 +108,7 @@ func (gps *GPSDevice) i2cFillBuffer() {
 }
 
 // Available returns how many bytes of GPS data are currently available.
-func (gps *GPSDevice) available() (available int) {
+func (gps *Device) available() (available int) {
 	var lengthBytes [2]byte
 	gps.bus.Tx(gps.address, []byte{BYTES_AVAIL_REG}, lengthBytes[0:2])
 	available = int(lengthBytes[0])*256 + int(lengthBytes[1])
@@ -110,7 +116,7 @@ func (gps *GPSDevice) available() (available int) {
 }
 
 // WriteBytes sends data/commands to the GPS device
-func (gps *GPSDevice) WriteBytes(bytes []byte) {
+func (gps *Device) WriteBytes(bytes []byte) {
 	if gps.uart != nil {
 		gps.uart.Write(bytes)
 	} else {
@@ -119,14 +125,18 @@ func (gps *GPSDevice) WriteBytes(bytes []byte) {
 }
 
 // validSentence checks if a sentence has been received uncorrupted
-func validSentence(sentence string) bool {
+func validSentence(sentence string) error {
 	if len(sentence) < 4 || sentence[0] != '$' || sentence[len(sentence)-3] != '*' {
-		return false
+		return errInvalidNMEASentenceLength
 	}
 	var cs byte = 0
 	for i := 1; i < len(sentence)-3; i++ {
 		cs ^= sentence[i]
 	}
 	checksum := hex.EncodeToString([]byte{cs})
-	return (checksum[0] == sentence[len(sentence)-2]) && (checksum[1] == sentence[len(sentence)-1])
+	if (checksum[0] != sentence[len(sentence)-2]) || (checksum[1] != sentence[len(sentence)-1]) {
+		return errInvalidNMEAChecksum
+	}
+
+	return nil
 }

gps/gpsparser.go

@@ -1,92 +1,123 @@
 package gps
 
 import (
+	"errors"
 	"strconv"
 	"strings"
 	"time"
 )
 
-type GPSParser struct {
-	gpsDevice GPSDevice
+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 {
 }
 
-// fix is a GPS location fix
+// Fix is a GPS location fix
 type Fix struct {
-	Valid      bool
-	Time       time.Time
-	Latitude   float32
-	Longitude  float32
-	Altitude   int32
+	// Valid if the fix was valid.
+	Valid bool
+
+	// Time that the fix was taken, in UTC time.
+	Time time.Time
+
+	// Latitude is the decimal latitude. Negative numbers indicate S.
+	Latitude float32
+
+	// Longitude is the decimal longitude. Negative numbers indicate E.
+	Longitude float32
+
+	// Altitude is only returned for GGA sentences.
+	Altitude int32
+
+	// Satellites is the number of visible satellites, but is only returned for GGA sentences.
 	Satellites int16
 }
 
-func Parser(gpsDevice GPSDevice) GPSParser {
-	return GPSParser{
-		gpsDevice: gpsDevice,
+// NewParser returns a GPS NMEA Parser.
+func NewParser() Parser {
+	return Parser{}
+}
+
+// Parse parses a NMEA sentence looking for fix info.
+func (parser *Parser) Parse(sentence string) (fix Fix, err error) {
+	if sentence == "" {
+		err = errEmptyNMEASentence
+		return
 	}
-}
-
-// NextFix returns the next GPS location Fix from the GPS device
-func (parser *GPSParser) NextFix() (fix Fix) {
-	var ggaSentence = nextGGA(parser.gpsDevice)
-	var ggaFields = strings.Split(ggaSentence, ",")
-	fix.Altitude = findAltitude(ggaFields)
-	fix.Satellites = findSatellites(ggaFields)
-	fix.Longitude = findLongitude(ggaFields)
-	fix.Latitude = findLatitude(ggaFields)
-	fix.Time = findTime(ggaFields)
-	fix.Valid = (fix.Altitude != -99999) && (fix.Satellites > 0)
-	return fix
-}
-
-// nextGGA returns the next GGA type sentence from the GPS device
-// $--GGA,,,,,,,,,,,,,,*hh
-func nextGGA(gpsDevice GPSDevice) (sentence string) {
-	for {
-		sentence = gpsDevice.NextSentence()
-		if sentence[3:6] == "GGA" {
-			return sentence
+	typ := sentence[3:6]
+	switch typ {
+	case "GGA":
+		fields := strings.Split(sentence, ",")
+		if len(fields) != 15 {
+			err = errInvalidGGASentence
+			return
 		}
+
+		fix.Altitude = findAltitude(fields[9])
+		fix.Satellites = findSatellites(fields[7])
+		fix.Longitude = findLongitude(fields[4], fields[5])
+		fix.Latitude = findLatitude(fields[2], fields[3])
+		fix.Time = findTime(fields[1])
+		fix.Valid = (fix.Altitude != -99999) && (fix.Satellites > 0)
+	case "RMC":
+		fields := strings.Split(sentence, ",")
+		if len(fields) != 13 {
+			err = errInvalidRMCSentence
+			return
+		}
+
+		fix.Longitude = findLongitude(fields[5], fields[6])
+		fix.Latitude = findLatitude(fields[3], fields[4])
+		fix.Time = findTime(fields[1])
+		fix.Valid = (len(fields[2]) > 0 && fields[2][0:1] == "A")
+	default:
+		err = errUnknownNMEASentence
 	}
+	return
 }
 
-// findTime returns the time from a GGA sentence:
+// findTime returns the time from an NMEA sentence:
 // $--GGA,hhmmss.ss,,,,,,,,,,,,,*xx
-func findTime(ggaFields []string) time.Time {
-	if len(ggaFields) < 1 || len(ggaFields[1]) < 6 {
+func findTime(val string) time.Time {
+	if len(val) < 6 {
 		return time.Time{}
 	}
-	ts := strings.Builder{}
-	ts.WriteString(ggaFields[1][0:2])
-	ts.WriteString(":")
-	ts.WriteString(ggaFields[1][2:4])
-	ts.WriteString(":")
-	ts.WriteString(ggaFields[1][4:6])
-	var t, _ = time.Parse("15:04:05", ts.String())
+
+	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)
+	t := time.Date(0, 0, 0, int(h), int(m), int(s), int(ms), time.UTC)
 
 	return t
 }
 
-// findAltitude returns the altitude from a GGA sentence:
+// findAltitude returns the altitude from an NMEA sentence:
 // $--GGA,,,,,,,,,25.8,,,,,*63
-func findAltitude(ggaFields []string) int32 {
-	if len(ggaFields) > 8 && len(ggaFields[9]) > 0 {
-		var v, _ = strconv.ParseFloat(ggaFields[9], 32)
+func findAltitude(val string) int32 {
+	if len(val) > 0 {
+		var v, _ = strconv.ParseFloat(val, 32)
 		return int32(v)
 	}
 	return -99999
 }
 
-// findLatitude returns the Latitude from a GGA sentence:
+// findLatitude returns the Latitude from an NMEA sentence:
 // $--GGA,,ddmm.mmmmm,x,,,,,,,,,,,*hh
-func findLatitude(ggaFields []string) float32 {
-	if len(ggaFields) > 2 && len(ggaFields[2]) > 8 {
-		var dd = ggaFields[2][0:2]
-		var mm = ggaFields[2][2:]
+func findLatitude(val, hemi string) float32 {
+	if len(val) > 8 {
+		var dd = val[0:2]
+		var mm = val[2:]
 		var d, _ = strconv.ParseFloat(dd, 32)
 		var m, _ = strconv.ParseFloat(mm, 32)
 		var v = float32(d + (m / 60))
-		if ggaFields[3] == "S" {
+		if hemi == "S" {
 			v *= -1
 		}
 		return v
@@ -94,16 +125,16 @@ func findLatitude(ggaFields []string) float32 {
 	return 0.0
 }
 
-// findLatitude returns the longitude from a GGA sentence:
+// findLatitude returns the longitude from an NMEA sentence:
 // $--GGA,,,,dddmm.mmmmm,x,,,,,,,,,*hh
-func findLongitude(ggaFields []string) float32 {
-	if len(ggaFields) > 4 && len(ggaFields[4]) > 8 {
-		var ddd = ggaFields[4][0:3]
-		var mm = ggaFields[4][3:]
+func findLongitude(val, hemi string) float32 {
+	if len(val) > 8 {
+		var ddd = val[0:3]
+		var mm = val[3:]
 		var d, _ = strconv.ParseFloat(ddd, 32)
 		var m, _ = strconv.ParseFloat(mm, 32)
 		var v = float32(d + (m / 60))
-		if ggaFields[5] == "W" {
+		if hemi == "W" {
 			v *= -1
 		}
 		return v
@@ -111,11 +142,11 @@ func findLongitude(ggaFields []string) float32 {
 	return 0.0
 }
 
-// findSatellites returns the satellites from a GGA sentence:
+// findSatellites returns the satellites from an NMEA sentence:
 // $--GGA,,,,,,,nn,,,,,,,*hh
-func findSatellites(ggaFields []string) (n int16) {
-	if len(ggaFields) > 6 && len(ggaFields[7]) > 0 {
-		var nn = ggaFields[7]
+func findSatellites(val string) (n int16) {
+	if len(val) > 0 {
+		var nn = val
 		var v, _ = strconv.ParseInt(nn, 10, 32)
 		n = int16(v)
 		return n

gps/registers.go

@@ -13,5 +13,5 @@ const (
 )
 
 const (
-	bufferSize = 32
+	bufferSize = 100
 )

gps/ublox.go

@@ -24,25 +24,25 @@ var cfg_gnss_cmd = [...]byte{
 	0x01, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00,
 	0x01, 0x01, 0xFC, 0x11}
 
-func FlightMode(gpsDevice GPSDevice) (err error) {
-	err = sendCommand(gpsDevice, flight_mode_cmd[:])
+func FlightMode(d Device) (err error) {
+	err = sendCommand(d, flight_mode_cmd[:])
 	return err
 }
 
-func SetCfgGNSS(gpsDevice GPSDevice) (err error) {
-	err = sendCommand(gpsDevice, cfg_gnss_cmd[:])
+func SetCfgGNSS(d Device) (err error) {
+	err = sendCommand(d, cfg_gnss_cmd[:])
 	return err
 }
 
-func sendCommand(gpsDevice GPSDevice, command []byte) (err error) {
-	gpsDevice.WriteBytes(command)
+func sendCommand(d Device, command []byte) (err error) {
+	d.WriteBytes(command)
 	start := time.Now()
 	for time.Now().Sub(start) < 1000 {
-		if gpsDevice.readNextByte() == '\n' {
-			if gpsDevice.readNextByte() == 0xB5 {
-				gpsDevice.readNextByte()
-				if gpsDevice.readNextByte() == 0x05 {
-					if gpsDevice.readNextByte() == 0x01 {
+		if d.readNextByte() == '\n' {
+			if d.readNextByte() == 0xB5 {
+				d.readNextByte()
+				if d.readNextByte() == 0x05 {
+					if d.readNextByte() == 0x01 {
 						return
 					}
 				}