diff --git a/examples/gps/i2c/main.go b/examples/gps/i2c/main.go index 769884a..b767f0c 100644 --- a/examples/gps/i2c/main.go +++ b/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) } } diff --git a/examples/gps/uart/main.go b/examples/gps/uart/main.go index 795af05..82328e8 100644 --- a/examples/gps/uart/main.go +++ b/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) } } diff --git a/gps/gps.go b/gps/gps.go index 471589f..54baba2 100644 --- a/gps/gps.go +++ b/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 } diff --git a/gps/gpsparser.go b/gps/gpsparser.go index d47dfa5..54587d7 100644 --- a/gps/gpsparser.go +++ b/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 diff --git a/gps/registers.go b/gps/registers.go index 665cd93..5ef9918 100644 --- a/gps/registers.go +++ b/gps/registers.go @@ -13,5 +13,5 @@ const ( ) const ( - bufferSize = 32 + bufferSize = 100 ) diff --git a/gps/ublox.go b/gps/ublox.go index b10f572..7d2e00a 100644 --- a/gps/ublox.go +++ b/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 } }