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
|
@(messages: Seq[parsing.Message])
@extract(field: parsing.Field, offset: Int) = {
@field.tpe.width match {
case 1 => {payload(@offset)}
case 2 => {((payload(@offset) & 0xff) | ((payload(@{offset+1}) & 0xff) << 8)).toShort}
case 4 => {(payload(@offset) & 0xff) | ((payload(@{offset+1}) & 0xff) << 8) | ((payload(@{offset+2}) & 0xff) << 16) | ((payload(@{offset+3}) & 0xff) << 24)}
case 8 => {(payload(@offset) & 0xffl) |
((payload(@{offset+2}) & 0xffl) << 8) |
((payload(@{offset+3}) & 0xffl) << 16) |
((payload(@{offset+4}) & 0xffl) << 24) |
((payload(@{offset+5}) & 0xffl) << 32) |
((payload(@{offset+6}) & 0xffl) << 40) |
((payload(@{offset+7}) & 0xffl) << 48) |
((payload(@{offset+8}) & 0xffl) << 56)}
}
}
@insert(field: parsing.Field, offset: Int) = {
@defining("m." + field.scalaName) { attr =>
@if(field.tpe.width >= 1) { arr(@offset) = (@attr & 0xff).toByte }
@if(field.tpe.width >= 2) { arr(@{offset+1}) = ((@attr >> 8) & 0xff).toByte }
@if(field.tpe.width >= 3) { arr(@{offset+2}) = ((@attr >> 16) & 0xff).toByte }
@if(field.tpe.width >= 4) { arr(@{offset+3}) = ((@attr >> 24) & 0xff).toByte }
@if(field.tpe.width >= 5) { arr(@{offset+4}) = ((@attr >> 32) & 0xff).toByte }
@if(field.tpe.width >= 6) { arr(@{offset+5}) = ((@attr >> 40) & 0xff).toByte }
@if(field.tpe.width >= 7) { arr(@{offset+6}) = ((@attr >> 48) & 0xff).toByte }
@if(field.tpe.width >= 8) { arr(@{offset+7}) = ((@attr >> 56) & 0xff).toByte }
}
}
@org.mavlink.txt._header()
package org.mavlink.messages
sealed trait Message
@for(message <- messages) {
@_message_class(message)
}
case class Unknown(id: Byte, payload: Seq[Byte]) extends Message
object Message {
def unpack(id: Byte, payload: Seq[Byte]) = id match {
@for(message <- messages) {
case @message.id =>
@defining(message.orderedFields){ ordered =>
@defining(ordered.map(_.tpe.width).scanLeft(0)(_ + _)){ offsets =>
@for((field, offset) <- ordered zip offsets) {
val @field.scalaName: @field.tpe.scalaType = @extract(field, offset)
}}}
@{message.scalaName}@message.fields.map(_.scalaName).mkString("(", ", ", ")")
}
case id => Unknown(id, payload)
}
def pack(message: Message): (Byte, Seq[Byte]) = message match {
@for(message <- messages) {
case m: @message.scalaName =>
val arr = new Array[Byte](@message.fields.map(_.tpe.width).sum)
@defining(message.orderedFields) { ordered =>
@defining(ordered.map(_.tpe.width).scanLeft(0)(_ + _)){ offsets =>
@for((field, offset) <- ordered zip offsets) {
@insert(field, offset)
}
(@message.id, arr)
}}}
case u: Unknown => (u.id, u.payload)
}
}
|