hand-implement some messages

12 files changed, 125 insertions, 46 deletions

diff --git a/project/Build.scala b/project/Build.scala
index fe49146..2728c48 100644
--- a/project/Build.scala
+++ b/project/Build.scala
@@ -29,6 +29,7 @@ object ApplicationBuild extends Build {
 
   lazy val mavlink = (
     Project("vfd-mavlink", file("vfd-mavlink"))
+    settings(common: _*)
   )
 
   lazy val uav = (
diff --git a/project/Dependencies.scala b/project/Dependencies.scala
index 8aa8e03..6c705eb 100644
--- a/project/Dependencies.scala
+++ b/project/Dependencies.scala
@@ -15,5 +15,5 @@ object Dependencies {
   val dom = "org.scala-lang.modules.scalajs" %%%! "scalajs-dom" % "0.6"
   val tag = "com.scalatags" %%%! "scalatags" % "0.4.1"
   val rx = "com.scalarx" %%%! "scalarx" % "0.2.6"
-
+  
 }
\ No newline at end of file
diff --git a/vfd-backend/app/plugins/UavClientConnection.scala b/vfd-backend/app/plugins/UavClientConnection.scala
new file mode 100644
index 0000000..b479aa5
--- /dev/null
+++ b/vfd-backend/app/plugins/UavClientConnection.scala
@@ -0,0 +1,29 @@
+package plugins
+
+import akka.actor.Actor
+import akka.actor.ActorLogging
+import akka.actor.ActorRef
+import akka.actor.actorRef2Scala
+import vfd.uav.Connection
+
+/**
+ * Interfaces traffic from a websocket with a connection to a UAV.
+ */
+class UavClientConnection(websocket: ActorRef, uav: ActorRef) extends Actor with ActorLogging {
+
+  override def preStart = {
+    uav ! Connection.Register
+  }
+
+  def receive = {
+
+    case Connection.Received(bstr) =>
+      websocket ! bstr.toArray
+
+    case Connection.Closed(msg) =>
+      log.warning(msg)
+      context stop self
+
+  }
+
+}
\ No newline at end of file
diff --git a/vfd-backend/app/plugins/UavPlugin.scala b/vfd-backend/app/plugins/UavPlugin.scala
index 2e5af65..43e015c 100644
--- a/vfd-backend/app/plugins/UavPlugin.scala
+++ b/vfd-backend/app/plugins/UavPlugin.scala
@@ -1,14 +1,10 @@
 package plugins
 
-import akka.actor.Actor
-import akka.actor.ActorLogging
 import akka.actor.ActorRef
 import akka.actor.Props
-import akka.actor.actorRef2Scala
 import play.api.Application
 import play.api.Plugin
 import play.api.libs.concurrent.Akka
-import vfd.uav.Connection
 import vfd.uav.MockConnection
 import vfd.uav.SerialConnection
 
@@ -44,27 +40,6 @@ class UavPlugin(app: Application) extends Plugin {
     Akka.system(app).actorOf(props, name = "uav-connection")
   }
 
-  def register(websocket: ActorRef): Props = Props(classOf[ClientConnection], websocket, connection)
-
-}
-
-class ClientConnection(websocket: ActorRef, uav: ActorRef) extends Actor with ActorLogging {
-
-  override def preStart = {
-    uav ! Connection.Register
-  }
-
-  def receive = {
-
-    case Connection.Received(bstr) =>
-      log.info(bstr.toArray.mkString(","))
-      websocket ! bstr.toArray
-
-
-    case Connection.Closed(msg) =>
-      log.warning(msg)
-      context stop self
-
-  }
+  def register(websocket: ActorRef): Props = Props(classOf[UavClientConnection], websocket, connection)
 
 }
\ No newline at end of file
diff --git a/vfd-backend/conf/application.conf b/vfd-backend/conf/application.conf
index 1559b89..e069b23 100644
--- a/vfd-backend/conf/application.conf
+++ b/vfd-backend/conf/application.conf
@@ -71,8 +71,8 @@ uav.system_id=1
 # 'mock' for a sample connection 
 uav.connection.type=serial
 
-# Mavlink component id used by this connection
-# (not the web frontend), in case it needs to inject messages
+# Mavlink component id used by this connection (not the web frontend),
+# in case it needs to inject messages
 uav.connection.component_id=99
 
 # Delay in milliseconds between heartbeat messages injected by
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)
diff --git a/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala b/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala
index c552a96..9f13bff 100644
--- a/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala
+++ b/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala
@@ -19,7 +19,7 @@ class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader {
 
 }
 
-class BufferedPayloadBuilder(payload: Array[Byte]) extends PayloadBuilder {
+class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter {
   private val buffer = ByteBuffer.wrap(payload)
 
   //mavlink uses little endian
diff --git a/vfd-uav/src/main/scala/org/mavlink/package.scala b/vfd-uav/src/main/scala/org/mavlink/package.scala
index 080648d..87a8add 100644
--- a/vfd-uav/src/main/scala/org/mavlink/package.scala
+++ b/vfd-uav/src/main/scala/org/mavlink/package.scala
@@ -1,7 +1,9 @@
 package org
 
+import scala.language.implicitConversions
+
 package object mavlink {
   
-  
+  implicit def mkReader(bytes: Seq[Byte]): PayloadReader = new BufferedPayloadReader(bytes.toArray)
 
 }
\ No newline at end of file
diff --git a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
index 3b08215..306d3dc 100644
--- a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
@@ -1,13 +1,12 @@
 package vfd.uav
 
 import java.util.concurrent.TimeUnit.MILLISECONDS
-
 import scala.concurrent.duration.FiniteDuration
-
+import org.mavlink.Parser
+import org.mavlink.messages.Message
 import com.github.jodersky.flow.Parity
 import com.github.jodersky.flow.Serial
 import com.github.jodersky.flow.SerialSettings
-
 import akka.actor.Actor
 import akka.actor.ActorRef
 import akka.actor.Props
@@ -15,8 +14,9 @@ import akka.actor.Terminated
 import akka.actor.actorRef2Scala
 import akka.io.IO
 import akka.util.ByteString
+import akka.actor.ActorLogging
 
-class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String, settings: SerialSettings) extends Actor with Connection {
+class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String, settings: SerialSettings) extends Actor with ActorLogging with Connection {
   import context._
 
   val Heartbeat = ByteString(
@@ -62,6 +62,15 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String
        */
 
   }
+  
+  val parser = new Parser(
+      pckt => try {
+         log.info(Message.unpack(pckt).toString()) 
+      } catch {
+        case err: MatchError =>
+          log.info("unknown message: " + pckt.payload.map(_.formatted("%02x").mkString(" ")))
+      }
+  )
 
   def _opened(operator: ActorRef): Receive = {
 
@@ -74,6 +83,9 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String
       context become closed
 
     case Serial.Received(bstr) =>
+      for (b <- bstr) {
+        parser.push(b)
+      }
       sendAll(Connection.Received(bstr))
 
     case Connection.Send(bstr) =>