-
Notifications
You must be signed in to change notification settings - Fork 1
/
payload.go
64 lines (49 loc) · 1.15 KB
/
payload.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
package sphero
import "fmt"
type payload struct {
Flags uint8
DeviceID uint8
Command uint8
Sequence uint8
Error uint8
Payload []byte
}
func (p *payload) encode() []byte {
sendBytes := []byte{
DataPacketStart, // first byte is always 0x08
p.Flags, // set the flags
p.DeviceID, // send is for the given device id
p.Command, // with the command
p.Sequence, // set the sequence id to ensure that packets are orderable
}
// add the payload
sendBytes = append(sendBytes, p.Payload...)
// calculateChecksum
cs := calculateChecksum(sendBytes)
sendBytes = append(sendBytes, cs, DataPacketEnd)
return sendBytes
}
func (p *payload) decode(d []byte) error {
p.Flags = d[1]
p.DeviceID = d[2]
p.Command = d[3]
p.Sequence = d[4]
p.Error = d[5]
checksum := d[6]
// compare checksum
cc := calculateChecksum(d[1 : len(d)-1])
if checksum != cc {
return fmt.Errorf("checksum invalid")
}
if len(d) > 7 {
p.Payload = d[7 : len(d)-2]
}
return nil
}
func calculateChecksum(b []byte) uint8 {
checksum := uint16(0)
for _, b := range b[1:] {
checksum = checksum + uint16(b)
}
return uint8(^(checksum % 256))
}