4 files changed, 71 insertions, 11 deletions
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala b/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala
index 9cc5090..3cd9dc3 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala
@@ -24,7 +24,7 @@ object Parser {
   }
 }
 
-class Parser(receiver: Packet => Unit, error: Parser.ParseErrors.ParseError => Unit) {
+class Parser(receiver: Packet => Unit, error: Parser.ParseErrors.ParseError => Unit = _ => ()) {
   import Parser._
 
   private var state: ParseStates.State = ParseStates.Idle
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
index f22d0b3..7e215c6 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
@@ -1,8 +1,30 @@
 package org.mavlink.messages
 
 import org.mavlink.PayloadReader
+import org.mavlink.PayloadWriter
 import org.mavlink.Packet
-import org.mavlink.PayloadBuilder
 
-trait Message
+trait Message {
+  def pack(implicit mkWriter: Array[Byte] => PayloadWriter): Array[Byte]
+}
+
+trait MessageCompanion[M <: Message] {
+  def unpack(bytes: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader): M
+}
+
+object Message {
+
+  def unpack(packet: Packet)(implicit mkReader: Seq[Byte] => PayloadReader) = packet.messageId match {
+    case 0 => Heartbeat.unpack(packet.payload)
+    case 109 => RadioStatus.unpack(packet.payload)
+  }
+  
+  /*
+  private val msg = Heartbeat(23,1,2,4,5,6)
+  def foo = {
+    val spec = msg.pickle.value
+    (spec.id, spec.payload.mkString("(", ",", ")"))
+  }*/
+
+}
   
 \ No newline at end of file
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala
index 422949f..3f519dc 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala
@@ -1,9 +1,47 @@
 package org.mavlink.messages
 
-case class Heartbeat(
-  customMode: Int,
-  `type`: Byte,
-  autopilot: Byte,
-  baseMode: Byte,
-  systemStatus: Byte,
-  mavlinkVersion: Byte) extends Message
-\ No newline at end of file
+import org.mavlink.PayloadWriter
+import org.mavlink.PayloadReader
+
+case class Heartbeat(customMode: Int, `type`: Byte, autopilot: Byte, baseMode: Byte, systemStatus: Byte, mavlinkVersion: Byte) extends Message {
+  def pack(implicit mkWriter: Array[Byte] => PayloadWriter): Array[Byte] = {
+    val arr = new Array[Byte](9)
+    val writer = mkWriter(arr)
+    writer.writeInt32(customMode)
+    writer.writeInt8(`type`)
+    writer.writeInt8(autopilot)
+    writer.writeInt8(baseMode)
+    writer.writeInt8(systemStatus)
+    writer.writeInt8(mavlinkVersion)
+    arr
+  }
+}
+
+object Heartbeat extends MessageCompanion[Heartbeat]{
+  def unpack(payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader) = {
+    val reader = mkReader(payload)
+    Heartbeat(reader.nextInt32, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8)
+  }
+}
+
+case class RadioStatus(rxErrors: Short, fixed: Short, rssi: Byte, remoteRssi: Byte, txBuf: Byte, noise: Byte, remoteNoise: Byte) extends Message {
+  def pack(implicit mkWriter: Array[Byte] => PayloadWriter): Array[Byte] = {
+    val arr = new Array[Byte](9)
+    val writer = mkWriter(arr)
+    writer.writeInt16(rxErrors)
+    writer.writeInt16(fixed)
+    writer.writeInt8(rssi)
+    writer.writeInt8(remoteRssi)
+    writer.writeInt8(txBuf)
+    writer.writeInt8(noise)
+    writer.writeInt8(remoteNoise)
+    arr
+  }
+}
+
+object RadioStatus extends MessageCompanion[RadioStatus]{
+  def unpack(payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader) = {
+    val reader = mkReader(payload)
+    RadioStatus(reader.nextInt16, reader.nextInt16, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8)
+  }
+}
+\ No newline at end of file
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/payload.scala b/vfd-mavlink/src/main/scala/org/mavlink/payload.scala
index 63473f0..ff31e9d 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/payload.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/payload.scala
@@ -10,7 +10,7 @@ trait PayloadReader {
   def nextChar: Char
 }
 
-trait PayloadBuilder {
+trait PayloadWriter {
   def writeInt8(x: Byte)
   def writeInt16(x: Short)
   def writeInt32(x: Int)