blob: f0e30dc145ca84944c0f5967cf6c60f3b8ee5802 (
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
|
@()
package org.mavlink
import scala.collection.mutable.ArrayBuffer
object Parser {
object ParseStates {
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
}
object ParseErrors {
sealed trait ParseError
case object CrcError extends ParseError
case object OverflowError extends ParseError
}
}
class Parser(receiver: Packet => Unit, error: Parser.ParseErrors.ParseError => Unit = _ => ()) {
import Parser._
private var state: ParseStates.State = ParseStates.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 payload = new ArrayBuffer[Byte]
var crc: Crc = new Crc()
}
def push(c: Byte): Unit = {
import ParseStates._
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()
}
case GotMsgId =>
inbound.payload += c
inbound.crc = inbound.crc.accumulate(c)
if(inbound.payload.length >= Packet.MaxPayloadLength) {
state = Idle
error(ParseErrors.OverflowError)
}
if (inbound.payload.length >= 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(ParseErrors.CrcError)
} else {
state = GotCrc1
}
case GotCrc1 =>
if (c != inbound.crc.msb) {
state = Idle
if (c == Packet.Stx) {
state = GotStx
}
error(ParseErrors.CrcError)
} else {
val packet = Packet(
inbound.seq,
inbound.systemId,
inbound.componentId,
inbound.messageId,
inbound.payload)
state = Idle
inbound.payload = new ArrayBuffer[Byte]()
receiver(packet)
}
}
}
}
|
