/
driver.go
319 lines (273 loc) · 7.4 KB
/
driver.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
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
package computrainer
import (
"context"
"fmt"
"io"
"log"
"sync"
"sync/atomic"
"time"
"github.com/jacobsa/go-serial/serial"
)
const (
// LoadMax is the maximum load that can be set (watts)
LoadMax int32 = 1500
// LoadMin is the maximum load that can be set (watts)
LoadMin int32 = 50
)
// ergoInitCommand puts the computrainer into ergo mode
var ergoInitCommand = []byte{
0x6D, 0x00, 0x00, 0x0A, 0x08, 0x00, 0xE0,
0x65, 0x00, 0x00, 0x0A, 0x10, 0x00, 0xE0,
0x00, 0x00, 0x00, 0x0A, 0x18, 0x5D, 0xC1,
0x33, 0x00, 0x00, 0x0A, 0x24, 0x1E, 0xE0,
0x6A, 0x00, 0x00, 0x0A, 0x2C, 0x5F, 0xE0,
0x41, 0x00, 0x00, 0x0A, 0x34, 0x00, 0xE0,
0x2D, 0x00, 0x00, 0x0A, 0x38, 0x10, 0xC2,
}
// DisconnectError indicates we've lost the connection to the CompuTrainer
// or it's no longer responding. Reconnection will be necessary to continue.
type DisconnectError struct {
Cause error
}
func (d DisconnectError) Error() string {
return fmt.Sprintf("disconnected: %v", d.Cause)
}
// Signals exposes data being published by the CompuTrainer and allows controlling
// CompuTrainer settings
type Signals struct {
Messages <-chan Message
Errors <-chan error
load *int32
cancelChan chan struct{}
connectedWG *sync.WaitGroup
}
// SetLoad sets the load in watts that the CompuTrainer should maintain in erg mode
func (s *Signals) SetLoad(targetLoad int32) {
atomic.StoreInt32(s.load, targetLoad)
}
// Close disconnects from the CompuTrainer and prevents further reading/writing
func (s *Signals) Close() {
log.Printf("Signals: close\n")
close(s.cancelChan)
s.connectedWG.Wait()
log.Printf("Signals: closed\n")
}
type signaler struct {
Messages chan<- Message
Errors chan<- error
}
// Driver handles serial communications with the CompuTrainer
type Driver struct {
portName string
com io.ReadWriteCloser
targetLoad int32
}
// NewDriver returns a Driver using the specified com port
func NewDriver(comPort string) (*Driver, error) {
return &Driver{portName: comPort, targetLoad: LoadMin}, nil
}
// Connect attempts to establish communications with the CompuTrainer. If successful
// then the returned Signals will allow interacting with the CompuTrainer.
// Signals should be closed before closing the Driver
func (d *Driver) Connect(ctx context.Context) (*Signals, error) {
log.Printf("Driver: Connect\n")
if d.com == nil {
port, err := serial.Open(serial.OpenOptions{
PortName: d.portName,
BaudRate: 2400,
DataBits: 8,
StopBits: 1,
ParityMode: serial.PARITY_NONE,
InterCharacterTimeout: 1000,
})
if err != nil {
return nil, fmt.Errorf("failed to open serial: %v", err)
}
d.com = port
}
// Try to clean out any stale data in the read buffers from previous
// connections.
for {
drainBuf := make([]byte, 6)
n, err := d.com.Read(drainBuf)
if err != nil {
if err != io.EOF {
return nil, fmt.Errorf("failed to read: %v", err)
}
}
if n != 6 {
break
}
}
if _, err := io.WriteString(d.com, "RacerMate"); err != nil {
return nil, fmt.Errorf("failed to send initial hello: %v", err)
}
result := make(chan error)
go func() {
buf := make([]byte, 6)
if err := read(ctx, d.com, buf); err != nil {
result <- fmt.Errorf("failed to read hello response: %w", err)
}
bufMsg := string(buf)
if bufMsg != "LinkUp" {
result <- fmt.Errorf("unexpected hello response: %s", bufMsg)
return
}
close(result)
}()
select {
case <-ctx.Done():
// TODO: wait for read goroutine to end or close port?
return nil, fmt.Errorf("failed to connect before timeout: %w", ctx.Err())
case err := <-result:
if err != nil {
return nil, err
}
}
if _, err := d.com.Write(ergoInitCommand); err != nil {
return nil, fmt.Errorf("failed to write ergo init: %v", err)
}
var load int32
msgChan := make(chan Message)
errChan := make(chan error)
cancelChan := make(chan struct{})
connectedWG := &sync.WaitGroup{}
d.startConnectedLoop(&signaler{msgChan, errChan}, &load, cancelChan, connectedWG)
return &Signals{
Messages: msgChan,
Errors: errChan,
load: &load,
cancelChan: cancelChan,
connectedWG: connectedWG,
}, nil
}
func (d *Driver) startConnectedLoop(signaler *signaler, targetLoad *int32, cancel <-chan struct{}, connectedWG *sync.WaitGroup) {
log.Printf("Driver: startConnectedLoop\n")
buf := make([]byte, 7)
loopCtx := context.Background()
readMsgChan := make(chan Message)
readErrChan := make(chan error)
connectedWG.Add(1)
go func() {
defer connectedWG.Done()
for i := 0; ; i++ {
if i%4 == 0 {
if err := writeControlMessage(d.com, atomic.LoadInt32(targetLoad)); err != nil {
signaler.Errors <- DisconnectError{fmt.Errorf("failed to write load init: %v", err)}
return
}
}
ctx, ctxCancel := context.WithCancel(loopCtx)
exit := func() bool {
defer ctxCancel()
go d.readTo(ctx, buf, readMsgChan, readErrChan)
select {
case <-cancel:
ctxCancel()
return true
case msg := <-readMsgChan:
if msg.Type != DataNone {
signaler.Messages <- msg
}
case err := <-readErrChan:
signaler.Errors <- DisconnectError{fmt.Errorf("failed to read message: %v", err)}
return true
}
return false
}()
if exit {
return
}
}
}()
}
func (d *Driver) readTo(ctx context.Context, buf []byte, msgChan chan<- Message, errChan chan<- error) {
if err := readWithTimeout(ctx, d.com, buf, 1*time.Second); err != nil {
if err != context.Canceled {
select {
case <-ctx.Done():
return
case errChan <- err:
return
}
}
return
}
msg := ParseMessage(buf)
select {
case <-ctx.Done():
return
case msgChan <- msg:
return
}
}
func (d *Driver) setTargetLoad(load int32) {
switch {
case load > LoadMax:
d.targetLoad = LoadMax
case load < LoadMin:
d.targetLoad = LoadMin
default:
d.targetLoad = load
}
}
// Close releases the com port opened by the driver
func (d *Driver) Close() error {
err := d.com.Close()
d.com = nil
return err
}
func readWithTimeout(ctx context.Context, r io.Reader, buf []byte, timeout time.Duration) error {
ctx, cancel := context.WithTimeout(ctx, timeout)
defer cancel()
return read(ctx, r, buf)
}
func read(ctx context.Context, r io.Reader, buf []byte) error {
for read := 0; read < len(buf); {
n, err := r.Read(buf[read:])
if err != nil {
return fmt.Errorf("read error: %v", err)
}
select {
case <-ctx.Done():
return ctx.Err()
default:
// keep going
}
read += n
}
return nil
}
func writeControlMessage(w io.Writer, targetLoad int32) error {
msg := make([]byte, 7)
crc := calcCRC(targetLoad)
// BYTE 0 - 49 is b0, 53 is b4, 54 is b5, 55 is b6
msg[0] = byte(crc >> 1) // set byte 0
msg[3] = 0x0A
// BYTE 4 - command and highbyte
msg[4] = 0x40 // set command
msg[4] |= byte((targetLoad & (2048 + 1024 + 512)) >> 9)
// BYTE 5 - low 7
msg[5] = 0
msg[5] |= byte((targetLoad & (128 + 64 + 32 + 16 + 8 + 4 + 2)) >> 1)
// BYTE 6 - sync + z set
msg[6] = byte(128 + 64)
// low bit of supplement in bit 6 (32)
if (crc & 1) > 0 {
msg[6] |= 32
}
// Bit 2 (0x02) is low bit of high byte in load (bit 9 0x256)
if (targetLoad & 256) > 0 {
msg[6] |= 2
}
// Bit 1 (0x01) is low bit of low byte in load (but 1 0x01)
msg[6] |= byte(targetLoad & 1)
if _, err := w.Write(msg); err != nil {
return fmt.Errorf("failed to write control msg: %v", err)
}
return nil
}
func calcCRC(value int32) int32 {
return (0xff & (107 - (value & 0xff) - (value >> 8)))
}