forked from tinygo-org/drivers
/
gps.go
143 lines (124 loc) · 3.34 KB
/
gps.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
// Package gps provides a driver for GPS receivers over UART and I2C
package gps // import "tinygo.org/x/drivers/gps"
import (
"encoding/hex"
"errors"
"strings"
"time"
"tinygo.org/x/drivers"
)
var (
errInvalidNMEASentenceLength = errors.New("invalid NMEA sentence length")
errInvalidNMEAChecksum = errors.New("invalid NMEA sentence checksum")
)
// Device wraps a connection to a GPS device.
type Device struct {
buffer []byte
bufIdx int
sentence strings.Builder
uart drivers.UART
bus drivers.I2C
address uint16
}
// NewUART creates a new UART GPS connection. The UART must already be configured.
func NewUART(uart drivers.UART) Device {
return Device{
uart: uart,
buffer: make([]byte, bufferSize),
bufIdx: bufferSize,
sentence: strings.Builder{},
}
}
// NewI2C creates a new I2C GPS connection.
func NewI2C(bus drivers.I2C) Device {
return Device{
bus: bus,
address: I2C_ADDRESS,
buffer: make([]byte, bufferSize),
bufIdx: bufferSize,
sentence: strings.Builder{},
}
}
// NextSentence returns the next valid NMEA sentence from the GPS device.
func (gps *Device) NextSentence() (sentence string, err error) {
sentence = gps.readNextSentence()
if err = validSentence(sentence); err != nil {
return "", err
}
return sentence, nil
}
// readNextSentence returns the next sentence from the GPS device.
func (gps *Device) 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 *Device) readNextByte() (b byte) {
gps.bufIdx += 1
if gps.bufIdx >= bufferSize {
gps.fillBuffer()
}
return gps.buffer[gps.bufIdx]
}
func (gps *Device) fillBuffer() {
if gps.uart != nil {
gps.uartFillBuffer()
} else {
gps.i2cFillBuffer()
}
}
func (gps *Device) uartFillBuffer() {
for gps.uart.Buffered() < bufferSize {
time.Sleep(100 * time.Millisecond)
}
gps.uart.Read(gps.buffer[0:bufferSize])
gps.bufIdx = 0
}
func (gps *Device) 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 *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])
return available
}
// WriteBytes sends data/commands to the GPS device
func (gps *Device) 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) error {
if len(sentence) < 4 || 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
}
return nil
}