aboutsummaryrefslogtreecommitdiff
path: root/protocol.go
blob: 9c03fa95cdffbf0cd804701cae51b98ba0548033 (plain)
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
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
package anki

import (
	"bytes"
	"encoding/binary"
	"log"
)

// VehicleMsgMaxSize        = 20
// VehicleMsgPayloadMaxSize = 18
// VehicleMsgBaseSize       = 1

// Identifier for a vehicle message
const (
	// BLE Connection
	VehicleMsgC2VDisconnect ID = 0x0d

	// Ping request / response
	VehicleMsgC2VPingRequest  ID = 0x16
	VehicleMsgV2CPingResponse ID = 0x17

	// Messages for checking vehicle version info
	VehicleMsgC2VVersionRequest  ID = 0x18
	VehicleMsgV2CVersionResponse ID = 0x19

	// Baterry level
	VehicleMsgC2VBatteryLevelRequest  ID = 0x1a
	VehicleMsgV2CBatteryLevelResponse ID = 0x1b

	// Lights
	VehicleMsgC2VSetLights ID = 0x1d

	// Driving Commands
	VehicleMsgC2VSetSpeed         ID = 0x24
	VehicleMsgC2VChangeLane       ID = 0x25
	VehicleMsgC2VCancelLaneChange ID = 0x26
	VehicleMsgC2VTurn             ID = 0x32

	// Vehicle position updates
	VehicleMsgV2CLocalizationPositionUpdate     ID = 0x27
	VehicleMsgV2CLocalizationTransitionUpdate   ID = 0x29
	VehicleMsgV2CLocalizationIntersectionUpdate ID = 0x2a
	VehicleMsgV2cVehicleDelocalized             ID = 0x2b
	VehicleMsgC2VSetOffsetFromRoadCenter        ID = 0x2c
	VehicleMsgV2COffsetFromRoadCenterUpdate     ID = 0x2d

	// Light Patterns
	VehicleMsgC2VLightsPattern ID = 0x33

	// Vehicle Configuration Prameters
	VehicleMsgC2VSetConfigParams ID = 0x45

	// SDK Mode
	VehicleMsgC2VSDKMode ID = 0x90
)

type VehicleMsg struct {
	Size    uint8
	MsgID   ID
	Payload []uint8 // MaxSize 18
}

func SplitMsg(b []byte) (id ID, payload []byte) {
	if len(b) < 2 {
		return 0, nil
	}
	return ID(b[1]), b[2:]
}

func Encode(v interface{}) []byte {
	buf := new(bytes.Buffer)
	err := binary.Write(buf, binary.LittleEndian, v)
	if err != nil {
		log.Println("Encode", err)
	}
	b := buf.Bytes()[:]
	z := make([]byte, len(b))
	copy(z, b) // FIXME workaround "cgo argument has Go pointer to Go pointer"
	return z
}

func EncodeMsg(id ID, v interface{}) ([]byte, error) {
	buf := new(bytes.Buffer)
	sz := binary.Size(v) + 1
	if err := buf.WriteByte(byte(sz)); err != nil {
		return nil, err
	}
	if err := buf.WriteByte(byte(id)); err != nil {
		return nil, err
	}
	if err := binary.Write(buf, binary.LittleEndian, v); err != nil {
		return nil, err
	}
	b := make([]byte, buf.Len())
	copy(b, buf.Bytes())
	return b, nil
}

func Decode(b []byte, v interface{}) {
	buf := bytes.NewBuffer(b)
	err := binary.Read(buf, binary.LittleEndian, v)
	if err != nil {
		log.Println("Decode", err)
	}
}

func DecodeMsg(b []byte, v interface{}) error {
	buf := bytes.NewReader(b)
	if err := binary.Read(buf, binary.LittleEndian, v); err != nil {
		return err
	}
	return nil
}

type VehicleMsgVersionResponse struct {
	Size    uint8
	MsgID   ID
	Version uint32
}

type VehicleMsgBatteryLevelResponse struct {
	Size         uint8
	MsgID        ID
	BatteryLevel MilliVolt // mV
}

type VehicleMsgSDKMode struct {
	Size  uint8
	MsgID ID
	On    uint8
	Flags uint8
}

func (v *VehicleMsgSDKMode) Set(on uint8, flags uint8) {
	v.Size = uint8(binary.Size(v) - 1)
	v.MsgID = VehicleMsgC2VSDKMode
	v.On = on
	v.Flags = flags
}

type VehicleMsgSetSpeed struct {
	Size       uint8
	MsgID      ID
	Speed      MMperSec   // mm/sec
	Accel      MMperSecSQ // mm/secĀ²
	SpeedLimit Bool       // respect road piece speed limit
}

func (v *VehicleMsgSetSpeed) Set(speed MMperSec, accel MMperSecSQ) {
	v.Size = uint8(binary.Size(v) - 1)
	v.MsgID = VehicleMsgC2VSetSpeed
	v.Speed = speed
	v.Accel = accel
}

const (
	VehicleTurnNone = iota
	VehicleTurnLeft
	VehicleTurnRight
	VehicleTurnUTurn
	VehicleTurnUTurnJump
)

const (
	VehicleTurnTriggerImmediate    = iota // Run immediately
	VehicleTurnTriggerIntersection        // Run at the next intersection
)

type VehicleMsgTurn struct {
	Size    uint8
	MsgID   ID
	Type    uint8
	Trigger uint8
}

const (
	Lane1  float32 = 68.0
	Lane2  float32 = 23.0
	Center float32 = 0.0
	Lane3  float32 = -23.0
	Lane4  float32 = -68.0
)

/* Lanes
direction
^         -23.0  23.0
|   -68.0      0.0     68.0     offset
| |____._____.__+__._____.____|
       4     3     2     1      track

see also: https://github.com/IBM-Bluemix/node-mqtt-for-anki-overdrive
*/

type VehicleMsgSetOffsetFromRoadCenter struct {
	Size   uint8
	MsgID  ID
	Offset OffsetMM // mm
}

func (v *VehicleMsgSetOffsetFromRoadCenter) Set(offset OffsetMM) {
	v.Size = uint8(binary.Size(v) - 1)
	v.MsgID = VehicleMsgC2VSetOffsetFromRoadCenter
	v.Offset = offset
}

type VehicleMsgChangeLane struct {
	Size            uint8
	MsgID           ID
	HorizontalSpeed MMperSec   // mm/sec
	HorizontalAccel MMperSecSQ // mm/secĀ²
	Offset          OffsetMM   // from road center mm
	HopIntent       uint8
	Tag             uint8
}

func (v *VehicleMsgChangeLane) Set(hspeed MMperSec, haccel MMperSecSQ, offset OffsetMM) {
	v.Size = uint8(binary.Size(v) - 1)
	v.MsgID = VehicleMsgC2VChangeLane
	v.HorizontalSpeed = hspeed
	v.HorizontalAccel = haccel
	v.Offset = offset
}

const (
	ParseflagsMaskNumBits        Flags = 0x0f
	ParseflagsMaskInvertedColor  Flags = 0x80
	ParseflagsMaskReverseParsing Flags = 0x40
	ParseflagsMaskReverseDriving Flags = 0x20
)

type VehicleLocalizationPositionUpdate struct {
	Size           uint8
	MsgID          ID
	LocalizationID uint8
	RoadPieceID    uint8
	Offset         OffsetMM // from road center mm
	Speed          MMperSec // mm/sec
	ParsingFlags   Flags

	// ACK commands received
	LastRecvLaneChangeCmdID    uint8
	LastExecLaneChangeCmdID    uint8
	LastDesiredHorizontalSpeed MMperSec // mm/sec
	LastDesiredSpeed           MMperSec // mm/sec
}

// VehicleDrivingDirection
const (
	Forward Direction = iota
	Reverse
)

type VehicleMsgLocalizationTransitionUpdate struct {
	Size             uint8
	MsgID            ID
	RoadPieceIDX     uint8
	RoadPieceIDXPrev uint8
	Offset           float32 // from road center mm

	Direction Direction // driving direction

	// ACK commands received
	LastRecvLaneChangeCmdID    uint8
	LastExecLaneChangeCmdID    uint8
	LastDesiredHorizontalSpeed MMperSec // mm/sec
	LastDesiredSpeed           MMperSec // mm/sec

	// track grade detection
	UphillCounter   uint8
	DownhillCounter uint8

	// wheel displacement (cm) since last transition bar
	LeftWheelDist  uint8 // cm
	RightWheelDist uint8 // cm
}

const (
	IntersectionCodeEntryFirst = iota
	IntersectionCodeExitFirst
	IntersectionCodeEntrySecond
	IntersectionCodeExitSecond
)

type VehicleMsgLocalizationIntersectionUpdate struct {
	Size         uint8
	MsgID        ID
	RoadPieceIDX uint8
	Offset       float32 // from road center mm

	Direction        uint8 // driving direction
	IntersectionCode uint8
	IntersectionTurn uint8
	IsExiting        uint8
}

type VehicleMsgOffsetFromRoadCenterUpdate struct {
	Size         uint8
	MsgID        ID
	Offset       float32 // from road center mm
	LaneChangeID uint8
}

// Lights
// The bits in the simple light message corresponding to each type of light
const (
	LightHeadlights = 1 << iota
	LightBrakelights
	LightFrontlights
	LightEngine
)

type VehicleMsgSetLights struct {
	Size      uint8
	MsgID     ID
	LightMask uint8 // valid and value bits for lights
}

func (v *VehicleMsgSetLights) Set(mask uint8) {
	v.Size = uint8(binary.Size(v) - 1)
	v.MsgID = VehicleMsgC2VSetLights
	v.LightMask = mask
}

const (
	VehicleMaxLightIntensity = 14
	VehicleMaxLightTime      = 11
)

// LED channel definitions - for RGB engine, front, and tail lights
const (
	LightRed = iota
	LightTail
	LightBlue
	LightGreen
	LightFrontL
	LightFrontR
)

const (
	EffectSteady = iota // Simply set the light intensity to 'start' value
	EffectFade          // Fade intensity from 'start' to 'end'
	EffectThrob         // Fade intensity from 'start' to 'end' and back to 'start'
	EffectFlash         // Tun on LED between time 'start' and time 'end' inclusive
	EffectRandom        // Flash the LED erratically - ignoring start/end
)

type VehicleLightConfig struct {
	Channel uint8
	Effect  uint8
	Start   uint8
	End     uint8
	Cycles  uint8 // per 10 sec
}

func (v *VehicleLightConfig) Set(channel, effect uint8, start, end uint8, cycles uint16) {
	v.Channel = channel
	v.Effect = effect
	v.Start = start
	if v.Start > VehicleMaxLightIntensity {
		v.Start = VehicleMaxLightIntensity
	}
	v.End = end
	if v.End > VehicleMaxLightIntensity {
		v.End = VehicleMaxLightIntensity
	}
	cpm := cycles
	if cpm > VehicleMaxLightTime {
		cpm = VehicleMaxLightTime
	}
	v.Cycles = uint8(cpm / 6) // BUG integer division
}

type VehicleMsgLightsPattern struct {
	Size          uint8
	MsgID         ID
	ChannelCount  uint8
	ChannelConfig [3]VehicleLightConfig
}

func (v *VehicleMsgLightsPattern) Set(config VehicleLightConfig) {
	v.Size = uint8(binary.Size(v) - 1)
	v.MsgID = VehicleMsgC2VLightsPattern
	if int(v.ChannelCount) < len(v.ChannelConfig) {
		v.ChannelConfig[v.ChannelCount] = config
		v.ChannelCount++
	}
}

const (
	TrackMaterialPlastic = iota
	TrackMaterialVinyl
)

const (
	SupercodeNone = iota
	SupercodeBoostJump
)

type VehicleMsgSetConfigParams struct {
	Size               uint8
	MsgID              ID
	SuperCodeParseMask uint8
	TrackMaterial      uint8
}