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
|
@(__context: Context, __maxPayloadLength: Int, __extraCrcs: Seq[Byte])@_header(__context)
package org.mavlink
import java.nio.ByteBuffer
/**
* Represents a MAVLink packet.
* Note that due to performance reasons, the packet's payload contents are mutable.
* @@param seq sequence number
* @@param systemId system ID of sender
* @@param componentId component ID of sender
* @@param messageId MAVLink message identification (this depends on the dialect used)
* @@param payload message contents
*/
case class Packet(
seq: Byte,
systemId: Byte,
componentId: Byte,
messageId: Byte,
payload: ByteBuffer
) {
lazy val crc = {
var c = new Crc()
c = c.accumulate(payload.remaining.toByte)
c = c.accumulate(seq)
c = c.accumulate(systemId)
c = c.accumulate(componentId)
c = c.accumulate(messageId)
while (payload.hasRemaining) {
c = c.accumulate(payload.get())
}
c = c.accumulate(Packet.extraCrc(messageId))
c
}
def writeTo(out: Array[Byte], offset: Int = 0): Unit = {
out(offset) = Packet.Stx
out(offset+1) = payload.remaining.toByte
out(offset+2) = seq
out(offset+3) = systemId
out(offset+4) = componentId
out(offset+5) = messageId
val r = payload.remaining()
payload.get(out, 0, out.length)
out(offset + r) = crc.lsb
out(offset + r + 1) = crc.msb
}
}
object Packet {
/** Start byte signaling the beginning of a packet. */
final val Stx: Byte = (0xfe).toByte
/** Maximum length of a payload contained in a packet. */
final val MaxPayloadLength: Int = @__maxPayloadLength
/** Maximum over-the-wire size of a packet, i.e. the maximum payload length plus header and footer data. */
final val MaxPacketLength: Int = MaxPayloadLength + 8
/** Additional CRCs indexed by message ID (see MAVLink specification). */
final val ExtraCrcs: Seq[Byte] = Array[Byte](
@__extraCrcs.map(_ formatted "%3d").grouped(10).map(_.mkString(",")).mkString(",\n ")
)
/** Utility function to index ExtraCrcs with a byte. */
def extraCrc(id: Byte) = ExtraCrcs(id & 0xff)
/** An invalid packet with no payload. */
def emoty = Packet(0, 0, 0, -128, ByteBuffer.wrap(Array(0: Byte)))
}
|
