GPS driver (#55)

* u-blox GPS driver for both I2C and UART
2026-08-04 15:07:46 +00:00 · 2019-05-23 14:29:29 +01:00
parent c259da1f17
commit a24657d1eb
7 changed files with 386 additions and 0 deletions
@@ -19,6 +19,8 @@ smoke-test:
 	tinygo build -size short -o ./build/test.elf -target=itsybitsy-m0 ./examples/espat/espconsole/main.go
 	tinygo build -size short -o ./build/test.elf -target=itsybitsy-m0 ./examples/espat/esphub/main.go
 	tinygo build -size short -o ./build/test.elf -target=itsybitsy-m0 ./examples/espat/espstation/main.go
+	tinygo build -size short -o ./build/test.elf -target=feather-m0 ./examples/gps/i2c/main.go
+	tinygo build -size short -o ./build/test.elf -target=feather-m0 ./examples/gps/uart/main.go
 	tinygo build -size short -o ./build/test.elf -target=microbit ./examples/hd44780/customchar/main.go
 	tinygo build -size short -o ./build/test.elf -target=microbit ./examples/hd44780/text/main.go
 	tinygo build -size short -o ./build/test.elf -target=microbit ./examples/hub75/main.go
@@ -0,0 +1,29 @@
+package main
+
+import (
+	"fmt"
+	"machine"
+
+	"github.com/tinygo-org/drivers/gps"
+)
+
+func main() {
+	println("GPS I2C Example")
+	machine.I2C0.Configure(machine.I2CConfig{})
+	ublox := gps.NewI2C(&machine.I2C0)
+	parser := gps.Parser(ublox)
+	var fix gps.Fix
+	for {
+		fix = parser.NextFix()
+		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(", satellites=", fix.Satellites)
+			println()
+		} else {
+			println("No fix")
+		}
+	}
+}
@@ -0,0 +1,29 @@
+package main
+
+import (
+	"fmt"
+	"machine"
+
+	"github.com/tinygo-org/drivers/gps"
+)
+
+func main() {
+	println("GPS UART Example")
+	machine.UART1.Configure(machine.UARTConfig{BaudRate: 9600})
+	ublox := gps.NewUART(&machine.UART1)
+	parser := gps.Parser(ublox)
+	var fix gps.Fix
+	for {
+		fix = parser.NextFix()
+		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(", satellites=", fix.Satellites)
+			println()
+		} else {
+			println("No fix")
+		}
+	}
+}
@@ -0,0 +1,132 @@
+// Package gps provides a driver for GPS receivers over UART and I2C
+package gps
+
+import (
+	"encoding/hex"
+	"machine"
+	"strings"
+	"time"
+)
+
+// Device wraps a connection to a GPS device.
+type GPSDevice struct {
+	buffer   []byte
+	bufIdx   int
+	sentence strings.Builder
+	uart     *machine.UART
+	bus      *machine.I2C
+	address  uint16
+}
+
+// NewUART creates a new UART GPS connection. The UART must already be configured.
+func NewUART(uart *machine.UART) GPSDevice {
+	return GPSDevice{
+		uart:     uart,
+		buffer:   make([]byte, bufferSize),
+		bufIdx:   bufferSize,
+		sentence: strings.Builder{},
+	}
+}
+
+// NewI2C creates a new I2C GPS connection.
+func NewI2C(bus *machine.I2C) GPSDevice {
+	return GPSDevice{
+		bus:      bus,
+		address:  I2C_ADDRESS,
+		buffer:   make([]byte, bufferSize),
+		bufIdx:   bufferSize,
+		sentence: strings.Builder{},
+	}
+}
+
+// ReadNextSentence returns the next valid NMEA sentence from the GPS device.
+func (gps *GPSDevice) NextSentence() (sentence string) {
+	sentence = gps.readNextSentence()
+	for !validSentence(sentence) {
+		sentence = gps.readNextSentence()
+	}
+	return sentence
+}
+
+// readNextSentence returns the next sentence from the GPS device.
+func (gps *GPSDevice) readNextSentence() (sentence string) {
+	gps.sentence.Reset()
+	var b byte = ' '
+
+	for b != '$' {
+		b = gps.readNextByte()
+	}
+
+	for b != '*' {
+		gps.sentence.WriteByte(b)
+		b = gps.readNextByte()
+	}
+	gps.sentence.WriteByte(b)
+	gps.sentence.WriteByte(gps.readNextByte())
+	gps.sentence.WriteByte(gps.readNextByte())
+
+	sentence = gps.sentence.String()
+	return sentence
+}
+
+func (gps *GPSDevice) readNextByte() (b byte) {
+	gps.bufIdx += 1
+	if gps.bufIdx >= bufferSize {
+		gps.fillBuffer()
+	}
+	return gps.buffer[gps.bufIdx]
+}
+
+func (gps *GPSDevice) fillBuffer() {
+	if gps.uart != nil {
+		gps.uartFillBuffer()
+	} else {
+		gps.i2cFillBuffer()
+	}
+}
+
+func (gps *GPSDevice) uartFillBuffer() {
+	for gps.uart.Buffered() < bufferSize {
+		time.Sleep(100 * time.Millisecond)
+	}
+	gps.uart.Read(gps.buffer[0:bufferSize])
+	gps.bufIdx = 0
+}
+
+func (gps *GPSDevice) i2cFillBuffer() {
+	for gps.available() < bufferSize {
+		time.Sleep(100 * time.Millisecond)
+	}
+	gps.bus.Tx(gps.address, []byte{DATA_STREAM_REG}, gps.buffer[0:bufferSize])
+	gps.bufIdx = 0
+}
+
+// Available returns how many bytes of GPS data are currently available.
+func (gps *GPSDevice) 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])
+	return available
+}
+
+// WriteBytes sends data/commands to the GPS device
+func (gps *GPSDevice) WriteBytes(bytes []byte) {
+	if gps.uart != nil {
+		gps.uart.Write(bytes)
+	} else {
+		gps.bus.Tx(gps.address, []byte{}, bytes)
+	}
+}
+
+// validSentence checks if a sentence has been received uncorrupted
+func validSentence(sentence string) bool {
+	if len(sentence) < 4 || sentence[0] != '$' || sentence[len(sentence)-3] != '*' {
+		return false
+	}
+	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])
+}
@@ -0,0 +1,124 @@
+package gps
+
+import (
+	"strconv"
+	"strings"
+	"time"
+)
+
+type GPSParser struct {
+	gpsDevice GPSDevice
+}
+
+// fix is a GPS location fix
+type Fix struct {
+	Valid      bool
+	Time       time.Time
+	Latitude   float32
+	Longitude  float32
+	Altitude   int32
+	Satellites int16
+}
+
+func Parser(gpsDevice GPSDevice) GPSParser {
+	return GPSParser{
+		gpsDevice: gpsDevice,
+	}
+}
+
+// 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
+		}
+	}
+}
+
+// findTime returns the time from a GGA sentence:
+// $--GGA,hhmmss.ss,,,,,,,,,,,,,*xx
+func findTime(ggaFields []string) time.Time {
+	if len(ggaFields) < 1 || len(ggaFields[1]) < 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())
+
+	return t
+}
+
+// findAltitude returns the altitude from a GGA 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)
+		return int32(v)
+	}
+	return -99999
+}
+
+// findLatitude returns the Latitude from a GGA 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:]
+		var d, _ = strconv.ParseFloat(dd, 32)
+		var m, _ = strconv.ParseFloat(mm, 32)
+		var v = float32(d + (m / 60))
+		if ggaFields[3] == "S" {
+			v *= -1
+		}
+		return v
+	}
+	return 0.0
+}
+
+// findLatitude returns the longitude from a GGA 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:]
+		var d, _ = strconv.ParseFloat(ddd, 32)
+		var m, _ = strconv.ParseFloat(mm, 32)
+		var v = float32(d + (m / 60))
+		if ggaFields[5] == "W" {
+			v *= -1
+		}
+		return v
+	}
+	return 0.0
+}
+
+// findSatellites returns the satellites from a GGA sentence:
+// $--GGA,,,,,,,nn,,,,,,,*hh
+func findSatellites(ggaFields []string) (n int16) {
+	if len(ggaFields) > 6 && len(ggaFields[7]) > 0 {
+		var nn = ggaFields[7]
+		var v, _ = strconv.ParseInt(nn, 10, 32)
+		n = int16(v)
+		return n
+	}
+	return 0
+}
@@ -0,0 +1,17 @@
+package gps
+
+// Constants/addresses used for u-blox I2C.
+
+// The I2C address which this device listens to.
+const (
+	I2C_ADDRESS = 0x42
+)
+
+const (
+	BYTES_AVAIL_REG = 0xfd
+	DATA_STREAM_REG = 0xff
+)
+
+const (
+	bufferSize = 32
+)
@@ -0,0 +1,53 @@
+package gps
+
+import (
+	"errors"
+	"time"
+)
+
+// flight mode disables the GPS COCOM limits
+var flight_mode_cmd = [...]byte{
+	0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00,
+	0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00,
+	0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x16, 0xDC}
+
+// Sets CFG-GNSS to disable everything other than GPS GNSS
+// solution. Failure to do this means GPS power saving
+// doesn't work. Not needed for MAX7, needed for MAX8's
+var cfg_gnss_cmd = [...]byte{
+	0xB5, 0x62, 0x06, 0x3E, 0x2C, 0x00, 0x00, 0x00,
+	0x20, 0x05, 0x00, 0x08, 0x10, 0x00, 0x01, 0x00,
+	0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x00, 0x00,
+	0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00,
+	0x01, 0x01, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00,
+	0x01, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00,
+	0x01, 0x01, 0xFC, 0x11}
+
+func FlightMode(gpsDevice GPSDevice) (err error) {
+	err = sendCommand(gpsDevice, flight_mode_cmd[:])
+	return err
+}
+
+func SetCfgGNSS(gpsDevice GPSDevice) (err error) {
+	err = sendCommand(gpsDevice, cfg_gnss_cmd[:])
+	return err
+}
+
+func sendCommand(gpsDevice GPSDevice, command []byte) (err error) {
+	gpsDevice.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 {
+						return
+					}
+				}
+			}
+		}
+	}
+	return errors.New("No ACK to GPS command")
+}