5 files changed, 50 insertions, 19 deletions
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/Packet.scala b/vfd-mavlink/src/main/scala/org/mavlink/Packet.scala
index 839e5d4..b5b932d 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/Packet.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/Packet.scala
@@ -39,6 +39,7 @@ case class Packet(
 object Packet {
 
   final val Stx: Byte = (0xfe).toByte;
+  final val MaxPayloadLength: Int = 255 
 
   final val MessageIdCrcEnds: Seq[Byte] = Array(
      50, 124, 137,   0, 237, 217, 104, 119,   0,   0,
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala b/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala
index 74562de..9cc5090 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/Parser.scala
@@ -16,9 +16,15 @@ object Parser {
     case object GotCrc1 extends State
     case object GotPayload extends State
   }
+  
+  object ParseErrors {
+    sealed trait ParseError
+    case object CrcError extends ParseError
+    case object OverflowError extends ParseError
+  }
 }
 
-class Parser(receiver: Packet => Unit, crcErrorReceiver: () => Unit) {
+class Parser(receiver: Packet => Unit, error: Parser.ParseErrors.ParseError => Unit) {
   import Parser._
 
   private var state: ParseStates.State = ParseStates.Idle
@@ -43,6 +49,7 @@ class Parser(receiver: Packet => Unit, crcErrorReceiver: () => Unit) {
         }
 
       case GotStx =>
+        inbound.crc = new Crc()
         inbound.length = (c & 0xff)
         inbound.crc = inbound.crc.next(c)
         state = GotLength
@@ -75,6 +82,10 @@ class Parser(receiver: Packet => Unit, crcErrorReceiver: () => Unit) {
       case GotMsgId =>
         inbound.payload += c
         inbound.crc = inbound.crc.next(c)
+        if(inbound.payload.length >= Packet.MaxPayloadLength) {
+          state = Idle
+          error(ParseErrors.OverflowError)
+        }
         if (inbound.payload.length >= inbound.length) {
           state = GotPayload
         }
@@ -86,8 +97,7 @@ class Parser(receiver: Packet => Unit, crcErrorReceiver: () => Unit) {
           if (c == Packet.Stx) {
             state = GotStx
           }
-          inbound.crc = new Crc()
-          crcErrorReceiver()
+          error(ParseErrors.CrcError)
         } else {
           state = GotCrc1
         }
@@ -98,8 +108,7 @@ class Parser(receiver: Packet => Unit, crcErrorReceiver: () => Unit) {
           if (c == Packet.Stx) {
             state = GotStx
           }
-          inbound.crc = new Crc()
-          crcErrorReceiver()
+          error(ParseErrors.CrcError)
         } else {
           val packet = Packet(
             inbound.seq,
@@ -108,7 +117,6 @@ class Parser(receiver: Packet => Unit, crcErrorReceiver: () => Unit) {
             inbound.messageId,
             inbound.payload)
           state = Idle
-          inbound.crc = new Crc()
           inbound.payload = new ArrayBuffer[Byte]()
           receiver(packet)
         }
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 91e446c..f22d0b3 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
@@ -1,16 +1,8 @@
 package org.mavlink.messages
 
-trait Message {}
+import org.mavlink.PayloadReader
+import org.mavlink.Packet
+import org.mavlink.PayloadBuilder
 
-case class Heartbeat(
-  `type`: Int,
-  autopilot: Int,
-  baseMode: Int,
-  customMode: Int,
-  systemStatus: Int,
-  mavlinkVersion: Int
-) extends Message
-
-object Message {
-
-}
-\ No newline at end of file
+trait Message
+  
+\ 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
new file mode 100644
index 0000000..422949f
--- /dev/null
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala
@@ -0,0 +1,9 @@
+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
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/payload.scala b/vfd-mavlink/src/main/scala/org/mavlink/payload.scala
new file mode 100644
index 0000000..63473f0
--- /dev/null
+++ b/vfd-mavlink/src/main/scala/org/mavlink/payload.scala
@@ -0,0 +1,21 @@
+package org.mavlink
+
+trait PayloadReader {
+  def nextInt8: Byte
+  def nextInt16: Short
+  def nextInt32: Int
+  def nextInt64: Long
+  def nextFloat: Float
+  def nextDouble: Double  
+  def nextChar: Char
+}
+
+trait PayloadBuilder {
+  def writeInt8(x: Byte)
+  def writeInt16(x: Short)
+  def writeInt32(x: Int)
+  def writeInt64(x: Long)
+  def writeFloat(x: Float)
+  def writeDouble(x: Double)  
+  def writeChar(x: Char)
+}
+\ No newline at end of file