blob: 97068e64ddddc347df09d8a279d46c54c155c348 (
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
|
@(__context: Context)@_header(__context)
package org.mavlink
import scala.collection.mutable.ArrayBuilder
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.
*
* @@param receiver called when a valid packet has been received
* @@param error called when invalid data was received
*/
class Parser(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 currentLength: Int = 0
val payload = ArrayBuilder.make[Byte]
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
inbound.payload.clear()
inbound.currentLength = 0
}
case GotMsgId =>
inbound.payload += c
inbound.currentLength += 1
inbound.crc = inbound.crc.accumulate(c)
if(inbound.currentLength > Packet.MaxPayloadLength) {
state = Idle
error(Errors.OverflowError)
} else if (inbound.currentLength >= 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,
inbound.payload.result)
state = Idle
receiver(packet)
}
}
}
/**
* Parses a sequence of bytes.
*/
def push(bytes: Traversable[Byte]): Unit = for (b <- bytes) push(b)
}
|
