-
Notifications
You must be signed in to change notification settings - Fork 9
/
mavlink.go
121 lines (100 loc) · 2.92 KB
/
mavlink.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
package mavlink
import (
"bytes"
"encoding/binary"
"fmt"
"io"
)
type parserState uint8
type ParseCharFunc func(byte) (packet *MavPacket, err error)
const (
mavlink_crc_extra_enabled = true
frameStart = uint8(0xFE)
mavlink_parse_state_uninit = 0
mavlink_parse_state_idle = 1
mavlink_parse_state_got_stx = 2
mavlink_parse_state_got_length = 3
mavlink_parse_state_got_seq = 4
mavlink_parse_state_got_sysid = 5
mavlink_parse_state_got_compid = 6
mavlink_parse_state_got_msgid = 7
mavlink_parse_state_got_payload = 8
mavlink_parse_state_got_crc1 = 9
mavlink_parse_state_got_packet = 10
)
var (
packetSeqGenerator = getPacketSeqGenerator()
)
type ErrUnknownMessageID uint8
func (self ErrUnknownMessageID) Error() string {
return fmt.Sprintf("Unknow message ID %d", self)
}
type ErrInvalidChecksum uint16
func (self ErrInvalidChecksum) Error() string {
return fmt.Sprintf("Invalid checksum %d", self)
}
type ErrInvalidStartframe uint8
func (self ErrInvalidStartframe) Error() string {
return fmt.Sprintf("Invalid start of frame %d", self)
}
type ErrInvalidPayloadLength uint8
func (self ErrInvalidPayloadLength) Error() string {
return fmt.Sprintf("Message ID and PayloadLength (%d) don't match", self)
}
// Create a MavPacket, and write it on the writer
func Send(writer io.Writer, systemID, componentID uint8, message Message) error {
packet, err := CreatePacket(systemID, componentID, message)
if err != nil {
return err
}
return binary.Write(writer, binary.LittleEndian, packet.Bytes())
}
// Create a MavPacket then compute the checksum.
// TODO : compute the checksum on the go
func CreatePacket(systemID, componentID uint8, message Message) (*MavPacket, error) {
packet := &MavPacket{
Header: MavHeader{
FrameStart: frameStart,
PayloadLength: message.Size(),
PacketSequence: packetSeqGenerator(),
SystemID: systemID,
ComponentID: componentID,
MessageID: message.ID(),
},
Msg: message,
}
packet.computeChecksum()
return packet, nil // todo : check errors...
}
func checkChecksum(buf *bytes.Buffer, packet *MavPacket) error {
var streamChecksum uint16
err := binary.Read(buf, binary.LittleEndian, &streamChecksum)
if err != nil {
return err
}
if streamChecksum != packet.computeChecksum() {
return ErrInvalidChecksum(streamChecksum)
}
return nil
}
func GetMavParser() ParseCharFunc {
var step parserState
var pInternal *MavPacketInternal = new(MavPacketInternal)
pInternal.packet = new(MavPacket)
step = mavlink_parse_state_idle
return func(c byte) (_ *MavPacket, err error) {
if charParser, ok := charParserFactory[step]; ok {
step, err = charParser(c, pInternal)
if err != nil {
pInternal.rawBuffer.Reset()
return nil, err
}
}
if step == mavlink_parse_state_got_packet {
pInternal.rawBuffer.Reset()
step = mavlink_parse_state_idle
return pInternal.packet, nil
}
return nil, nil
}
}