-
Notifications
You must be signed in to change notification settings - Fork 110
/
i2c_data_reader.go
198 lines (172 loc) · 5.61 KB
/
i2c_data_reader.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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
//go:build linux
// Package gpsutils implements a GPS NMEA component. This file contains ways to read data from a
// PMTK device connected over the I2C bus.
package gpsutils
import (
"context"
"errors"
"fmt"
"sync"
"go.viam.com/utils"
"go.viam.com/rdk/components/board/genericlinux/buses"
"go.viam.com/rdk/components/movementsensor"
"go.viam.com/rdk/logging"
)
// PmtkI2cDataReader implements the DataReader interface for a PMTK device by communicating with it
// over an I2C bus.
type PmtkI2cDataReader struct {
data chan string
cancelCtx context.Context
cancelFunc func()
activeBackgroundWorkers sync.WaitGroup
logger logging.Logger
bus buses.I2C
addr byte
baud int
}
// NewI2cDataReader constructs a new DataReader that gets its NMEA messages over an I2C bus.
func NewI2cDataReader(config I2CConfig, bus buses.I2C, logger logging.Logger) (DataReader, error) {
if bus == nil {
var err error
bus, err = buses.NewI2cBus(config.I2CBus)
if err != nil {
return nil, fmt.Errorf("gps init: failed to find i2c bus %s: %w", config.I2CBus, err)
}
}
addr := config.I2CAddr
if addr == -1 {
return nil, errors.New("must specify gps i2c address")
}
baud := config.I2CBaudRate
if baud == 0 {
baud = 38400
logger.Warn("using default baudrate: 38400")
}
data := make(chan string)
cancelCtx, cancelFunc := context.WithCancel(context.Background())
reader := PmtkI2cDataReader{
data: data,
cancelCtx: cancelCtx,
cancelFunc: cancelFunc,
logger: logger,
bus: bus,
addr: byte(addr),
baud: baud,
}
if err := reader.initialize(); err != nil {
return nil, err
}
reader.start()
return &reader, nil
}
// initialize sends commands to the device to put it into a state where we can read data from it.
func (dr *PmtkI2cDataReader) initialize() error {
handle, err := dr.bus.OpenHandle(dr.addr)
if err != nil {
dr.logger.CErrorf(dr.cancelCtx, "can't open gps i2c %s", err)
return err
}
defer utils.UncheckedErrorFunc(handle.Close)
// Set the baud rate
// TODO: does this actually do anything in the current context? The baud rate should be
// governed by the clock line on the I2C bus, not on the device.
baudcmd := fmt.Sprintf("PMTK251,%d", dr.baud)
cmd251 := movementsensor.PMTKAddChk([]byte(baudcmd))
// Output GLL, RMC, VTG, GGA, GSA, and GSV sentences, and nothing else, every position fix
cmd314 := movementsensor.PMTKAddChk([]byte("PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0"))
// Ask for updates every 1000 ms (every second)
cmd220 := movementsensor.PMTKAddChk([]byte("PMTK220,1000"))
err = handle.Write(dr.cancelCtx, cmd251)
if err != nil {
dr.logger.CDebug(dr.cancelCtx, "Failed to set baud rate")
return err
}
err = handle.Write(dr.cancelCtx, cmd314)
if err != nil {
return err
}
err = handle.Write(dr.cancelCtx, cmd220)
if err != nil {
return err
}
return nil
}
func (dr *PmtkI2cDataReader) readData() ([]byte, error) {
handle, err := dr.bus.OpenHandle(dr.addr)
if err != nil {
dr.logger.CErrorf(dr.cancelCtx, "can't open gps i2c %s", err)
return nil, err
}
defer utils.UncheckedErrorFunc(handle.Close)
buffer, err := handle.Read(dr.cancelCtx, 1024)
if err != nil {
dr.logger.CErrorf(dr.cancelCtx, "failed to read handle %s", err)
return nil, err
}
return buffer, nil
}
// start spins up a background coroutine to read data from the I2C bus and put it into the channel
// of complete messages.
func (dr *PmtkI2cDataReader) start() {
dr.activeBackgroundWorkers.Add(1)
utils.PanicCapturingGo(func() {
defer dr.activeBackgroundWorkers.Done()
defer close(dr.data)
strBuf := ""
for {
select {
case <-dr.cancelCtx.Done():
return
default:
}
buffer, err := dr.readData()
if err != nil {
dr.logger.CErrorf(dr.cancelCtx, "failed to read data, retrying: %s", err)
continue
}
for _, b := range buffer {
// PMTK uses CRLF line endings to terminate sentences, but just LF to blank data.
// Since CR should never appear except at the end of our sentence, we use that to
// determine sentence end. LF is merely ignored.
if b == 0x0D { // 0x0D is the ASCII value for a carriage return
if strBuf != "" {
// Sometimes we miss "$" on the first message of the buffer. If the first
// character we read is a "G", it's likely that this has occurred, and we
// should add a "$" at the beginning.
if strBuf[0] == 0x47 { // 0x47 is the ASCII value for "G"
strBuf = "$" + strBuf
}
dr.data <- strBuf
strBuf = ""
// Check if we're supposed to shut down again. Perhaps we waited a long
// time to put data into the channel.
select {
case <-dr.cancelCtx.Done():
return
default:
}
}
} else if b != 0x0A && b < 0x7F { // only append valid (printable) bytes
strBuf += string(b)
}
}
}
})
}
// Messages returns the channel of complete NMEA sentences we have read off of the device. It's part
// of the DataReader interface.
func (dr *PmtkI2cDataReader) Messages() chan string {
return dr.data
}
// Close is part of the DataReader interface. It shuts everything down.
func (dr *PmtkI2cDataReader) Close() error {
dr.cancelFunc()
// If the background coroutine is trying to put a new line of data into the channel, it won't
// notice that we've canceled it until something tries taking the line out of the channel. So,
// let's try to read that out so the coroutine isn't stuck. If the background coroutine shut
// itself down already, the channel will be closed and reading something out of it will just
// return the empty string.
<-dr.data
dr.activeBackgroundWorkers.Wait()
return nil
}