diff options
author | Jakob Odersky <jodersky@gmail.com> | 2015-03-20 13:11:02 +0100 |
---|---|---|
committer | Jakob Odersky <jodersky@gmail.com> | 2015-03-20 13:11:02 +0100 |
commit | 5f97b8128adf7d0bdcc9da0d7ead2d7faedc0abc (patch) | |
tree | 318a8de9e8e0e586749b912620557343d17b3809 | |
parent | 3147aba7dc79e64d2da50d7d1c705bfd0e99eeca (diff) | |
download | sbt-mavlink-5f97b8128adf7d0bdcc9da0d7ead2d7faedc0abc.tar.gz sbt-mavlink-5f97b8128adf7d0bdcc9da0d7ead2d7faedc0abc.tar.bz2 sbt-mavlink-5f97b8128adf7d0bdcc9da0d7ead2d7faedc0abc.zip |
move back to arrays
7 files changed, 58 insertions, 83 deletions
diff --git a/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Generator.scala b/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Generator.scala index 06a4909..3ee3492 100644 --- a/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Generator.scala +++ b/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Generator.scala @@ -14,12 +14,7 @@ import trees._ */ class Generator(dialect: Dialect) { - lazy val maxPayloadLength = { - val widths = dialect.messages map { msg => - msg.fields.map(_.tpe.sizeof).sum - } - widths.max - } + lazy val maxPayloadLength = dialect.messages.map(_.length).max lazy val extraCrcs = Array.tabulate[Byte](255){i => val message = dialect.messages.find(_.id == i) diff --git a/mavlink-library/src/main/scala/com/github/jodersky/mavlink/trees/package.scala b/mavlink-library/src/main/scala/com/github/jodersky/mavlink/trees/package.scala index 49b697b..fc4b5f3 100644 --- a/mavlink-library/src/main/scala/com/github/jodersky/mavlink/trees/package.scala +++ b/mavlink-library/src/main/scala/com/github/jodersky/mavlink/trees/package.scala @@ -10,6 +10,7 @@ package trees { case class Field(tpe: Type, nativeType: String, name: String, enum: Option[String], description: String) extends Tree case class Message(id: Byte, name: String, description: String, fields: Seq[Field]) extends Tree { def orderedFields = fields.toSeq.sortBy(_.tpe.width)(Ordering[Int].reverse) + def length = fields.map(_.tpe.sizeof).sum lazy val checksum = { var c = new Crc() diff --git a/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt index fc5becf..72f9ae1 100644 --- a/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt +++ b/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt @@ -1,8 +1,6 @@ @(__context: Context)@_header(__context) package org.mavlink -import java.nio.ByteBuffer - /** * Utility class for assembling packets with increasing sequence number * originating from given system and component IDs. @@ -11,7 +9,7 @@ class Assembler(systemId: Byte, componentId: Byte) { private var seq = 0; /** Assemble a given message ID and payload into a packet. */ - def assemble(messageId: Byte, payload: ByteBuffer): Packet = { + def assemble(messageId: Byte, payload: Array[Byte]): Packet = { val p = Packet(seq.toByte, systemId, componentId, messageId, payload) seq += 1 p diff --git a/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt index 6802a0a..b521746 100644 --- a/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt +++ b/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt @@ -1,7 +1,7 @@ @(__context: Context, __maxPayloadLength: Int, __extraCrcs: Seq[Byte])@_header(__context) package org.mavlink -import java.nio.ByteBuffer +import scala.collection.mutable.ArrayBuilder /** * Represents a MAVLink packet. @@ -17,34 +17,33 @@ case class Packet( systemId: Byte, componentId: Byte, messageId: Byte, - payload: ByteBuffer + payload: Array[Byte] ) { lazy val crc = { var c = new Crc() - c = c.accumulate(payload.remaining.toByte) + c = c.accumulate(payload.length.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()) - } + for (b <- payload) c = c.accumulate(b) 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 + val toArray = { + val builder = ArrayBuilder.make[Byte] + builder += Packet.Stx + builder += payload.length.toByte + builder += seq + builder += systemId + builder += componentId + builder += messageId + builder ++= payload + builder += crc.lsb + builder += crc.msb + builder.result } } @@ -57,9 +56,6 @@ object Packet { /** 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 ") @@ -69,6 +65,6 @@ object Packet { 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))) + def empty = Packet(0, 0, 0, -128, Array(0: Byte)) }
\ No newline at end of file diff --git a/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt index 81affc5..48af210 100644 --- a/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt +++ b/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt @@ -1,7 +1,7 @@ @(__context: Context)@_header(__context) package org.mavlink -import java.nio.ByteBuffer +import scala.collection.mutable.ArrayBuilder object Parser { @@ -32,15 +32,11 @@ object Parser { * 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. - * Note that due to memory and performance issues, a received packet and payload is - * only guaranteed to be valid within the receiver function. After exiting the function, the - * underlying packet's data may be overwritten by a new packet. * - * @@param buffer a buffer into which the received payload is stored * @@param receiver called when a valid packet has been received * @@param error called when invalid data was received */ -class Parser(payload: ByteBuffer)(receiver: Packet => Unit, error: Parser.Errors.Error => Unit = _ => ()) { +class Parser(receiver: Packet => Unit, error: Parser.Errors.Error => Unit = _ => ()) { import Parser._ private var state: States.State = States.Idle @@ -51,6 +47,8 @@ class Parser(payload: ByteBuffer)(receiver: Packet => Unit, error: Parser.Errors 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() } @@ -95,19 +93,19 @@ class Parser(payload: ByteBuffer)(receiver: Packet => Unit, error: Parser.Errors state = GotPayload } else { state = GotMsgId - payload.clear() + inbound.payload.clear() + inbound.currentLength = 0 } case GotMsgId => - if (!payload.hasRemaining) { + inbound.payload += c + inbound.currentLength += 1 + inbound.crc = inbound.crc.accumulate(c) + if(inbound.currentLength >= Packet.MaxPayloadLength) { state = Idle error(Errors.OverflowError) - } else { - payload.put(c) - inbound.crc = inbound.crc.accumulate(c) - if (payload.position >= inbound.length) { - state = GotPayload - } + } else if (inbound.currentLength>= inbound.length) { + state = GotPayload } case GotPayload => @@ -135,7 +133,7 @@ class Parser(payload: ByteBuffer)(receiver: Packet => Unit, error: Parser.Errors inbound.systemId, inbound.componentId, inbound.messageId, - payload) + inbound.payload.result) state = Idle receiver(packet) } @@ -147,11 +145,4 @@ class Parser(payload: ByteBuffer)(receiver: Packet => Unit, error: Parser.Errors */ def push(bytes: Traversable[Byte]): Unit = for (b <- bytes) push(b) - /** - * Parses all bytes of contained in a byte buffer. - */ - def push(bytes: ByteBuffer): Unit = while(bytes.hasRemaining) { - push(bytes.get()) - } - }
\ No newline at end of file diff --git a/mavlink-library/src/main/twirl/org/mavlink/messages/messages.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/messages/messages.scala.txt index 6f444dc..e973547 100644 --- a/mavlink-library/src/main/twirl/org/mavlink/messages/messages.scala.txt +++ b/mavlink-library/src/main/twirl/org/mavlink/messages/messages.scala.txt @@ -73,7 +73,7 @@ case class @{__scalaMessageType(__msg)}(@__msg.fields.map(__scalaFieldFormal).mk * @@param id the ID of the message * @@param payload the message's contents */ -case class Unknown(id: Byte, payload: ByteBuffer) extends Message +case class Unknown(id: Byte, payload: Array[Byte]) extends Message /** * Provides utility methods for converting data to and from MAVLink messages. @@ -83,38 +83,39 @@ object Message { /** * Interprets an ID and payload as a message. The contents must be ordered * according to the MAVLink specification. - * Note: this method reads from the provided ByteBuffer and thereby modifies its - * internal state. * @@param id ID of the message * @@param payload contents of the message * @@return payload interpreted as a message or 'Unknown' in case of an unknown ID */ - def unpack(id: Byte, payload: ByteBuffer): Message = id match { + def unpack(id: Byte, payload: Array[Byte]): Message = { + val buffer = ByteBuffer.wrap(payload) + id match { @for(__msg <- __messages) { case @__msg.id => - @for(__field <- __msg.orderedFields) {val @__scalaFieldFormal(__field) = @__bufferReadMethod("payload", __field.tpe) + @for(__field <- __msg.orderedFields) {val @__scalaFieldFormal(__field) = @__bufferReadMethod("buffer", __field.tpe) } @{__scalaMessageType(__msg)}(@__msg.fields.map(__scalaFieldName(_)).mkString(", ")) } case u => Unknown(u, payload) + } } /** - * Writes the contents of a message to a ByteBuffer. The message is encoded according - * to the MAVLink specification. + * Encodes a message according to the MAVLink specification. * @@param message the message to write - * @@param payload buffer that will hold the encoded message - * @@return id of the encoded message + * @@return (id, payload) id and payload of the encoded message */ - def pack(message: Message, payload: ByteBuffer): Byte = message match { + def pack(message: Message): (Byte, Array[Byte]) = message match { @for(__msg <- __messages) { case m: @__scalaMessageType(__msg) => - @for(__field <- __msg.orderedFields) {@__bufferWriteMethod("payload", "m." + __scalaFieldName(__field), __field.tpe) + val arr = new Array[Byte](@__msg.length) + val buffer = ByteBuffer.wrap(arr) + + @for(__field <- __msg.orderedFields) {@__bufferWriteMethod("buffer", "m." + __scalaFieldName(__field), __field.tpe) } - @__msg.id + (@__msg.id, arr) } case u: Unknown => - payload.put(u.payload) - u.id + (u.id, u.payload) } }
\ No newline at end of file diff --git a/mavlink-plugin/src/sbt-test/sbt-mavlink/codec/src/main/scala/Main.scala b/mavlink-plugin/src/sbt-test/sbt-mavlink/codec/src/main/scala/Main.scala index 6c84968..b7772a7 100644 --- a/mavlink-plugin/src/sbt-test/sbt-mavlink/codec/src/main/scala/Main.scala +++ b/mavlink-plugin/src/sbt-test/sbt-mavlink/codec/src/main/scala/Main.scala @@ -9,45 +9,38 @@ object Main { val ReceiverComponentId = 0: Byte def main(args: Array[String]): Unit = { + println("Echo test") echoTest() } def echoTest() = { - //represents the line buffer, i.e. all data going in and out - val line = new Array[Byte](Packet.MaxPacketLength) - - //payload of incoming messages - val in = MavlinkBuffer.allocate() //parser to transform an incoming byte stream into packets - val parser = new Parser(in)( - pckt => { - val msg = Message.unpack(pckt.messageId, pckt.payload) + val parser = new Parser( + (pckt: Packet) => { + val msg: Message = Message.unpack(pckt.messageId, pckt.payload) println("received message: " + msg) }, - err => { + (err: Parser.Errors.Error) => { sys.error("parse error: " + err) } ) - //payload buffer of outgoing messages - val out = MavlinkBuffer.allocate() - - //assembles messages into pakets from a specific sender + //assembles messages into packets from a specific sender val assembler = new Assembler(SenderSystemId, SenderComponentId) //create an explicit message val message = Heartbeat(0) //pack the message into a payload - val id = Message.pack(message, out) + val (id: Byte, payload: Array[Byte]) = Message.pack(message) //assemble into packet - val packet = assembler.assemble(id, out) + val packet = assembler.assemble(id, payload) //simulate wire transfer - packet.writeTo(line) - parser.push(line) + val data = packet.toArray + parser.push(data) } }
\ No newline at end of file |