initial commit

7 files changed, 412 insertions, 0 deletions

diff --git a/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt
new file mode 100644
index 0000000..e3571e6
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt
@@ -0,0 +1,19 @@
+@(__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.
+ */
+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 = {
+    val p = Packet(seq.toByte, systemId, componentId, messageId, payload)
+    seq += 1
+    p
+  }	
+}
\ No newline at end of file
diff --git a/mavlink-library/src/main/twirl/org/mavlink/Crc.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/Crc.scala.txt
new file mode 100644
index 0000000..f6e3cce
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/Crc.scala.txt
@@ -0,0 +1,28 @@
+@(__context: Context)@_header(__context)
+package org.mavlink
+
+/**
+ * X.25 CRC calculation for MAVlink messages. The checksum must be initialized,
+ * updated with each field of a message, and then finished with the message
+ * id.
+ */
+case class Crc(val crc: Int = 0xffff) extends AnyVal {
+
+  /**
+   * Accumulates data into a new checksum.
+   */
+  def accumulate(datum: Byte): Crc = {
+    val d = datum & 0xff
+    var tmp = d ^ (crc & 0xff)
+    tmp ^= (tmp << 4) & 0xff;
+    Crc(
+      ((crc >> 8) & 0xff) ^ (tmp << 8) ^ (tmp << 3) ^ ((tmp >> 4) & 0xff))
+  }
+
+  /** Least significant byte of checksum. */
+  def lsb: Byte = crc.toByte
+
+  /** Most significant byte of checksum. */
+  def msb: Byte = (crc >> 8).toByte
+
+}
\ No newline at end of file
diff --git a/mavlink-library/src/main/twirl/org/mavlink/MavlinkBuffer.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/MavlinkBuffer.scala.txt
new file mode 100644
index 0000000..9a01dbe
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/MavlinkBuffer.scala.txt
@@ -0,0 +1,24 @@
+@(__context: Context)@_header(__context)
+package org.mavlink
+
+import java.nio.ByteBuffer
+import java.nio.ByteOrder
+
+/** Utility functions for using ByteBuffers. */
+class MavlinkBuffer {
+
+  /**
+   * Allocates a ByteBuffer for using in MAVLink message processing.
+   * @@param direct specifies if a direct buffer should be used
+   */
+  def allocate(direct: Boolean = true) = {
+    val buffer = if (direct) {
+      ByteBuffer.allocateDirect(Packet.MaxPayloadLength)
+    } else {
+      val underlying = new Array[Byte](Packet.MaxPayloadLength)
+      ByteBuffer.wrap(underlying)
+    }
+    buffer.order(ByteOrder.LITTLE_ENDIAN) // MAVLink uses little endian
+  }
+
+}
\ No newline at end of file
diff --git a/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt
new file mode 100644
index 0000000..a0d11a5
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt
@@ -0,0 +1,59 @@
+@(__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
+) {
+
+  /*
+  def 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)
+    while (payload.)
+    for (p <- payload) {
+      c = c.accumulate(p)
+    }
+    c = c.accumulate(Packet.extraCrc(messageId))
+    c
+  }*/
+}
+
+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
+
+  /** 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)))
+
+}
\ 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
new file mode 100644
index 0000000..ca04ae1
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt
@@ -0,0 +1,157 @@
+@(__context: Context)@_header(__context)
+package org.mavlink
+
+import java.nio.ByteBuffer
+
+object Parser {
+
+  /** Internal parser states. */
+  object States {
+    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
+  }
+  
+  /** Errors that may occur while receiving data. */
+  object Errors {
+    sealed trait Error
+    case object CrcError extends Error
+    case object OverflowError extends Error
+  }
+}
+
+/**
+ * A parser to divide byte streams into mavlink packets.
+ * 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 = _ => ()) {
+  import Parser._
+
+  private var state: States.State = States.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 crc: Crc = new Crc()
+  }
+
+  /**
+   * Parses a byte as part of an incoming MAVLink message. May result
+   * in calling receiver or error function.
+   */
+  def push(c: Byte): Unit = {
+    import States._
+
+    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
+          payload.clear()
+        }
+
+      case GotMsgId =>
+        if (!payload.hasRemaining) {
+          state = Idle
+          error(Errors.OverflowError)
+        } else {
+          payload.put(c)
+          inbound.crc = inbound.crc.accumulate(c)
+          if (payload.position >= 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(Errors.CrcError)
+        } else {
+          state = GotCrc1
+        }
+
+      case GotCrc1 =>
+        if (c != inbound.crc.msb) {
+          state = Idle
+          if (c == Packet.Stx) {
+            state = GotStx
+          }
+          error(Errors.CrcError)
+        } else {
+          val packet = Packet(
+            inbound.seq,
+            inbound.systemId,
+            inbound.componentId,
+            inbound.messageId,
+            payload)
+          state = Idle
+          receiver(packet)
+        }
+    }
+  }
+
+  /**
+   * Parses a sequence of bytes.
+   */
+  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/_header.scala.txt b/mavlink-library/src/main/twirl/org/mavlink/_header.scala.txt
new file mode 100644
index 0000000..3fd5da9
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/_header.scala.txt
@@ -0,0 +1,5 @@
+@(__context: Context)/*
+ * MAVLink Abstraction Layer for Scala.
+ *
+ * This file was automatically generated.
+ */
\ 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
new file mode 100644
index 0000000..6f444dc
--- /dev/null
+++ b/mavlink-library/src/main/twirl/org/mavlink/messages/messages.scala.txt
@@ -0,0 +1,120 @@
+@(__context: Context, __messages: Set[Message])@org.mavlink.txt._header(__context)
+package org.mavlink.messages
+
+import java.nio.ByteBuffer
+import java.nio.charset.Charset
+
+@__scalaMessageType(message: Message) = {@StringUtils.Camelify(message.name)}
+
+@__scalaFieldName(field: Field) = {@StringUtils.camelify(field.name)}
+
+@__scalaFieldType(tpe: Type) = {@tpe match {
+  case IntType(1, _) => {Byte}
+  case IntType(2, _) => {Short}
+  case IntType(4, _) => {Int}
+  case IntType(8, _) => {Long}
+  case FloatType(4) => {Float}
+  case FloatType(8) => {Double}
+  case StringType(_) => {String}
+  case ArrayType(underlying, _) => {Seq[@__scalaFieldType(underlying)]}
+}}
+@__scalaFieldFormal(field: Field) = {@__scalaFieldName(field): @__scalaFieldType(field.tpe)}
+
+@__bufferReadMethod(buffer: String, tpe: Type) = {@tpe match {
+  case IntType(1, _) => {@{buffer}.get()}
+  case IntType(2, _) => {@{buffer}.getShort()}
+  case IntType(4, _) => {@{buffer}.getInt()}
+  case IntType(8, _) => {@{buffer}.getLong()}
+  case FloatType(4) => {@{buffer}.getFloat()}
+  case FloatType(8) => {@{buffer}.getDouble()}
+  case StringType(maxLength) =>{{
+    val bytes = Array[Byte](@maxLength)
+    @{buffer}.get(bytes, 0, @maxLength)
+    val length = bytes.indexOf(0) match {
+      case -1 => @maxLength
+      case i => i
+    }
+    new String(bytes, 0, length, Charset.forName("UTF-8"))
+  }}  
+  case ArrayType(underlying, length) => {for (i <- 0 until @length) yield {@__bufferReadMethod(buffer, underlying)}}
+}}
+@__bufferWriteMethod(buffer: String, data: String, tpe: Type) = {@tpe match {
+  case IntType(1, _) => {@{buffer}.put(@data)}
+  case IntType(2, _) => {@{buffer}.putShort(@data)}
+  case IntType(4, _) => {@{buffer}.putInt(@data)}
+  case IntType(8, _) => {@{buffer}.putLong(@data)}
+  case FloatType(4) => {@{buffer}.putFloat(@data)}
+  case FloatType(8) => {@{buffer}.putDouble(@data)}
+  case StringType(maxLength) => {
+  {
+    val bytes = @{data}.getBytes(Charset.forName("UTF-8"))
+    val endPosition = @{buffer}.position + @maxLength
+    @{buffer}.put(bytes, 0, math.max(bytes.length, @maxLength))
+    while (@{buffer}.position < endPosition) {
+      @{buffer}.put(0: Byte)
+    }
+  }}  
+  case ArrayType(underlying, length) => {for (i <- 0 until @length) {@__bufferWriteMethod(buffer, data + "(i)", underlying)}}
+}}
+
+@__commentParagraphs(paragraphs:  Seq[String]) = {@paragraphs.mkString("/**\n * ", "\n * ", "\n */")}
+@__comment(message: Message) = {@__commentParagraphs(message.description.grouped(100).toList ++ message.fields.map(field => "@param " + field.name + " " + field.description))}
+
+/** Marker trait for MAVLink messages. All supported messages extend this trait. */
+sealed trait Message
+
+@for(__msg <- __messages) {
+@__comment(__msg)
+case class @{__scalaMessageType(__msg)}(@__msg.fields.map(__scalaFieldFormal).mkString(", ")) extends Message
+}
+
+/**
+ * Wraps an unknown message.
+ * @@param id the ID of the message
+ * @@param payload the message's contents
+ */
+case class Unknown(id: Byte, payload: ByteBuffer) extends Message
+
+/**
+ * Provides utility methods for converting data to and from MAVLink messages.
+ */
+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 {
+    @for(__msg <- __messages) {
+      case @__msg.id =>
+        @for(__field <- __msg.orderedFields) {val @__scalaFieldFormal(__field) = @__bufferReadMethod("payload", __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.
+   * @@param message the message to write
+   * @@param payload buffer that will hold the encoded message
+   * @@return id of the encoded message
+   */
+  def pack(message: Message, payload: ByteBuffer): Byte = message match {
+    @for(__msg <- __messages) {
+      case m: @__scalaMessageType(__msg) =>
+        @for(__field <- __msg.orderedFields) {@__bufferWriteMethod("payload", "m." + __scalaFieldName(__field), __field.tpe)
+        }
+        @__msg.id
+    }
+    case u: Unknown =>
+      payload.put(u.payload)
+      u.id
+  }
+}
\ No newline at end of file