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
|
@(messages: Seq[parsing.Message])
@messageCrcs() = @{
for (i <- 0 to 255) yield {
val crc = messages.find(_.id == i).map(_.checksum).getOrElse(0)
crc.toString
}
}
package org.mavlink
case class Packet(
seq: Byte,
systemId: Byte,
componentId: Byte,
messageId: Byte,
payload: Seq[Byte]
) {
lazy val crc = {
var c = new Crc()
c = c.accumulate(payload.length.toByte)
c = c.accumulate(seq)
c = c.accumulate(systemId)
c = c.accumulate(componentId)
c = c.accumulate(messageId)
for (p <- payload) {
c = c.accumulate(p)
}
c = c.accumulate(Packet.extraCrc(messageId))
c
}
def toSeq: Seq[Byte] = Seq(
Packet.Stx,
payload.length.toByte,
seq,
systemId,
componentId,
messageId
) ++ payload ++ Seq(
crc.lsb,
crc.msb
)
}
object Packet {
final val Stx: Byte = (0xfe).toByte
final val MaxPayloadLength: Int = @messages.map(_.fields.map(_.tpe.width).sum).max
final val ExtraCrcs: Seq[Byte] = @messageCrcs().mkString("Array[Byte](", ", ", ")")
def extraCrc(id: Byte) = ExtraCrcs(id & 0xff)
final val Empty = Packet(0, 0, 0, -1, Array(0: Byte))
}
|
