blob: ca04ae1b541f507ac39be72467b4a38bdc5938eb (
plain) (
blame)
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
|
@(__context: Context)@_header(__context)
package org.mavlink
import java.nio.ByteBuffer
object Parser {
/** Internal parser states. */
object States {
sealed trait State
case object Idle extends State
case object GotStx extends State
case object GotLength extends State
case object GotSeq extends State
case object GotSysId extends State
case object GotCompId extends State
case object GotMsgId extends State
case object GotCrc1 extends State
case object GotPayload extends State
}
/** Errors that may occur while receiving data. */
object Errors {
sealed trait Error
case object CrcError extends Error
case object OverflowError extends Error
}
}
/**
* A parser to divide byte streams into mavlink packets.
* A parser is created with receiver and error functions and bytes are then fed into it. Once
* a valid packet has been received (or an error encountered), the receiver (or error) functions
* are called.
* Note that due to memory and performance issues, a received packet and payload is
* only guaranteed to be valid within the receiver function. After exiting the function, the
* underlying packet's data may be overwritten by a new packet.
*
* @@param buffer a buffer into which the received payload is stored
* @@param receiver called when a valid packet has been received
* @@param error called when invalid data was received
*/
class Parser(payload: ByteBuffer, receiver: Packet => Unit, error: Parser.Errors.Error => Unit = _ => ()) {
import Parser._
private var state: States.State = States.Idle
private object inbound {
var length: Int = 0
var seq: Byte = 0
var systemId: Byte = 0
var componentId: Byte = 0
var messageId: Byte = 0
var crc: Crc = new Crc()
}
/**
* Parses a byte as part of an incoming MAVLink message. May result
* in calling receiver or error function.
*/
def push(c: Byte): Unit = {
import States._
state match {
case Idle =>
if (c == Packet.Stx) {
state = GotStx
}
case GotStx =>
inbound.crc = new Crc()
inbound.length = (c & 0xff)
inbound.crc = inbound.crc.accumulate(c)
state = GotLength
case GotLength =>
inbound.seq = c;
inbound.crc = inbound.crc.accumulate(c)
state = GotSeq
case GotSeq =>
inbound.systemId = c
inbound.crc = inbound.crc.accumulate(c)
state = GotSysId
case GotSysId =>
inbound.componentId = c
inbound.crc = inbound.crc.accumulate(c)
state = GotCompId
case GotCompId =>
inbound.messageId = c
inbound.crc = inbound.crc.accumulate(c)
if (inbound.length == 0) {
state = GotPayload
} else {
state = GotMsgId
payload.clear()
}
case GotMsgId =>
if (!payload.hasRemaining) {
state = Idle
error(Errors.OverflowError)
} else {
payload.put(c)
inbound.crc = inbound.crc.accumulate(c)
if (payload.position >= inbound.length) {
state = GotPayload
}
}
case GotPayload =>
inbound.crc = inbound.crc.accumulate(Packet.extraCrc(inbound.messageId))
if (c != inbound.crc.lsb) {
state = Idle
if (c == Packet.Stx) {
state = GotStx
}
error(Errors.CrcError)
} else {
state = GotCrc1
}
case GotCrc1 =>
if (c != inbound.crc.msb) {
state = Idle
if (c == Packet.Stx) {
state = GotStx
}
error(Errors.CrcError)
} else {
val packet = Packet(
inbound.seq,
inbound.systemId,
inbound.componentId,
inbound.messageId,
payload)
state = Idle
receiver(packet)
}
}
}
/**
* Parses a sequence of bytes.
*/
def push(bytes: Traversable[Byte]): Unit = for (b <- bytes) push(b)
/**
* Parses all bytes of contained in a byte buffer.
*/
def push(bytes: ByteBuffer): Unit = while(bytes.hasRemaining) {
push(bytes.get())
}
}
|
