From a24657d1ebe18ac4b2b767ab3f702e676235f31f Mon Sep 17 00:00:00 2001 From: Anthony Elder Date: Thu, 23 May 2019 14:29:29 +0100 Subject: [PATCH] GPS driver (#55) * u-blox GPS driver for both I2C and UART --- Makefile | 2 + examples/gps/i2c/main.go | 29 +++++++++ examples/gps/uart/main.go | 29 +++++++++ gps/gps.go | 132 ++++++++++++++++++++++++++++++++++++++ gps/gpsparser.go | 124 +++++++++++++++++++++++++++++++++++ gps/registers.go | 17 +++++ gps/ublox.go | 53 +++++++++++++++ 7 files changed, 386 insertions(+) create mode 100644 examples/gps/i2c/main.go create mode 100644 examples/gps/uart/main.go create mode 100644 gps/gps.go create mode 100644 gps/gpsparser.go create mode 100644 gps/registers.go create mode 100644 gps/ublox.go diff --git a/Makefile b/Makefile index 9c7ffb6..cdf13f2 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/examples/gps/i2c/main.go b/examples/gps/i2c/main.go new file mode 100644 index 0000000..6774758 --- /dev/null +++ b/examples/gps/i2c/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") + } + } +} diff --git a/examples/gps/uart/main.go b/examples/gps/uart/main.go new file mode 100644 index 0000000..53d27e5 --- /dev/null +++ b/examples/gps/uart/main.go @@ -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") + } + } +} diff --git a/gps/gps.go b/gps/gps.go new file mode 100644 index 0000000..93339d2 --- /dev/null +++ b/gps/gps.go @@ -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]) +} diff --git a/gps/gpsparser.go b/gps/gpsparser.go new file mode 100644 index 0000000..d47dfa5 --- /dev/null +++ b/gps/gpsparser.go @@ -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 +} diff --git a/gps/registers.go b/gps/registers.go new file mode 100644 index 0000000..665cd93 --- /dev/null +++ b/gps/registers.go @@ -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 +) diff --git a/gps/ublox.go b/gps/ublox.go new file mode 100644 index 0000000..1b0a34b --- /dev/null +++ b/gps/ublox.go @@ -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") +}