add further support for mavlink

notes:
* the protocol seems bloated for our needs
* I'm not sure about the handling of primitive types in the protocol, especially the scalajs handling of numeric types of different widths seems cumbersome

13 files changed, 247 insertions, 161 deletions

diff --git a/vfd-frontend/src/main/scala/org/mavlink/js.scala b/vfd-frontend/src/main/scala/org/mavlink/js.scala
new file mode 100644
index 0000000..b9bcd2b
--- /dev/null
+++ b/vfd-frontend/src/main/scala/org/mavlink/js.scala
@@ -0,0 +1,69 @@
+package org.mavlink
+
+import scala.language.implicitConversions
+import scala.scalajs.js.JSConverters.array2JSRichGenTrav
+import scala.scalajs.js.typedarray.DataView
+import scala.scalajs.js.typedarray.Int8Array
+
+import org.mavlink.messages.PayloadReader
+import org.mavlink.messages.PayloadWriter
+
+package object messages {
+  import org.mavlink.messages.PayloadReader
+  import org.mavlink.messages.PayloadWriter
+
+  implicit def mkReader(s: Seq[Byte]) = new BufferedPayloadReader(s.toArray)
+  implicit def mkWriter(a: Array[Byte]) = new BufferedPayloadWriter(a)
+
+}
+
+package messages {
+
+  class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader {
+    private val buffer = new Int8Array(payload.toJSArray)
+    private val view = new DataView(buffer.buffer)
+    private var pos = 0
+
+    def int8 = {
+      val r = view.getInt8(pos)
+      pos += 1
+      r
+    }
+    def int16 = {
+      val r = view.getInt16(pos, true)
+      pos += 2
+      r
+    }
+    def int32 = {
+      val r = view.getInt32(pos, true)
+      pos += 4
+      r
+    }
+    def int64 = {
+      val l = int32
+      val m = int32
+      (m.asInstanceOf[Long] << 32) | l.asInstanceOf[Long]
+    }
+    def float = {
+      val r = view.getFloat32(pos, true)
+      pos += 4
+      r
+    }
+    def double = {
+      val r = view.getFloat64(pos, true)
+      pos += 8
+      r
+    }
+
+  }
+
+  class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter {
+    def int8(x: Byte) = ???
+    def int16(x: Short) = ???
+    def int32(x: Int) = ???
+    def int64(x: Long) = ???
+    def float(x: Float) = ???
+    def double(x: Double) = ???
+  }
+}
+
diff --git a/vfd-frontend/src/main/scala/vfd/frontend/Main.scala b/vfd-frontend/src/main/scala/vfd/frontend/Main.scala
index aa6d8d8..a6511d5 100644
--- a/vfd-frontend/src/main/scala/vfd/frontend/Main.scala
+++ b/vfd-frontend/src/main/scala/vfd/frontend/Main.scala
@@ -10,9 +10,9 @@ import rx.Rx
 import rx.Var
 import rx.Var
 import scalatags.JsDom.all._
-
 import vfd.frontend.ui.panels
 import vfd.frontend.util.Environment
+import org.mavlink.messages.Message
 
 object Main {
 
@@ -21,6 +21,12 @@ object Main {
     val remoteSystemId = args("remotesystemid").toInt
 
     val socket = new MavlinkSocket(socketUrl, remoteSystemId)
+    
+    val message = Rx{
+      val p = socket.packet()
+      Message.unpack(p.messageId, p.payload)
+    }
+    
 
     val element = div(`class` := "container-fluid")(
       div(`class` := "row")(
@@ -44,7 +50,7 @@ object Main {
         div(`class` := "col-xs-5")(
           div(`class` := "panel panel-default")(
             div(`class` := "panel-body")(
-              panels.Primary()))),
+              panels.Primary(message)))),
         div(`class` := "col-xs-3")(
           div(`class` := "panel panel-default")(
             div(`class` := "panel-body")(
diff --git a/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala b/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala
index 3b48375..71e0fbb 100644
--- a/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala
+++ b/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala
@@ -2,15 +2,23 @@ package vfd.frontend.ui.panels
 
 import rx.core.Var
 import scalatags.JsDom.all.div
+import vfd.frontend.util._
 import vfd.frontend.ui.Components
 import vfd.frontend.util.Environment
+import org.mavlink.messages.Message
+import org.mavlink.messages.Attitude
+import rx.core.Rx
+import org.mavlink.messages.Pressure
 
 object Primary {
 
-  def apply()(implicit env: Environment) = {
-    val pitchRoll = Var((0.0, 0.0))
-    val heading = Var(0.0)
-    val altitude = Var(0.0)
+  def apply(packet: Rx[Message])(implicit env: Environment) = {
+    val attitude = packet.only[Attitude]
+    val pressure = packet.only[Pressure]
+    
+    val pitchRoll = Rx{(attitude().pitch.toDouble, attitude().roll.toDouble)}
+    val heading = Rx{attitude().heading.toDouble}
+    val altitude = Rx{pressure().pressure.toDouble}
 
     div(
       Components.heading(heading, "33%"),
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala b/vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala
new file mode 100644
index 0000000..69d0889
--- /dev/null
+++ b/vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala
@@ -0,0 +1,30 @@
+package org.mavlink.enums
+
+object SystemStatus extends Enumeration {
+  type SystemStatus = Value
+  
+	/** Uninitialized system, state is unknown. */
+  val Uninitialized = Value(0)
+  
+  /** System is booting up. */
+  val Booting = Value(1)
+  
+  /** System is calibrating and not flight-ready. */
+  val Calibrating = Value(2)
+  
+  /** System is grounded and ready to fly. */
+  val Standby = Value(3)
+  
+  /** System is active an might already be airborne. */
+  val Active = Value(4)
+  
+  /** System is in a non-normal flight mode. It can however still navigate. */
+  val Critical = Value(5)
+  
+  /** System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. */
+  val Emergency = Value(6)
+  
+  /** System just initialized its power-down sequence, will shut down now. */
+  val Poweroff = Value(7)
+
+}
\ No newline at end of file
diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
deleted file mode 100644
index 7e215c6..0000000
--- a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.mavlink.messages
-
-import org.mavlink.PayloadReader
-import org.mavlink.PayloadWriter
-import org.mavlink.Packet
-
-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/message.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala
new file mode 100644
index 0000000..86691b9
--- /dev/null
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala
@@ -0,0 +1,60 @@
+package org.mavlink.messages
+
+import org.mavlink.Packet
+
+import org.mavlink.enums.SystemStatus
+import org.mavlink.enums.SystemStatus._
+
+sealed trait Message
+case class Heartbeat(`type`: Byte, autopilot: Byte, baseMode: Byte, customMode: Int, systemStatus: SystemStatus, mavlinkVersion: Byte) extends Message
+case class RadioStatus(rssi: Byte, remoteRssi: Byte, txBuf: Byte, noise: Byte, remoteNoise: Byte, rxErrors: Short, fixed: Short) extends Message
+case class Attitude(time: Int, roll: Float, pitch: Float, heading: Float) extends Message
+case class Pressure(time: Int, pressure: Float, diffPressure: Float, temperature: Short) extends Message
+case class Unknown(id: Int, payload: Seq[Byte]) extends Message
+
+object Message {
+
+  def unpack(id: Byte,  payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader): Message = {
+    val r = mkReader(payload)
+    
+    id.toInt match {
+      case 0 => 
+        val cm = r.int32
+        Heartbeat(r.int8, r.int8, r.int8, cm, SystemStatus(r.int8), r.int8)
+      case 28 =>
+        Pressure(r.int32, r.float, r.float, r.int16)
+      case 30 =>
+        Attitude(r.int32, r.float, r.float, r.float)
+      case 109 =>
+        val re = r.int16
+        val fi = r.int16 
+        RadioStatus(r.int8, r.int8, r.int8, r.int8, r.int8, re, fi)
+      
+      case u => Unknown(u, payload)
+    }
+  }
+  
+  def pack(m: Message)(implicit mkWriter: Array[Byte] => PayloadWriter): (Byte, Seq[Byte]) = {
+    val (id, size) = m match {
+      case _: Heartbeat => (0, 9) 
+      case _: RadioStatus => (109, 9)
+      case u: Unknown => (u.id, u.payload.length) 
+    }
+    
+    val arr = new Array[Byte](size)
+    val w = mkWriter(arr)
+    
+    m match {
+      case Heartbeat(tp, a, b, cm, s, mv) => w.int32(cm); w.int8(tp); w.int8(b); w.int8(s.id.toByte); w.int8(mv);
+      case RadioStatus(r, rr, tx, n, rn, rx, fi) => w.int16(rx); w.int16(fi); w.int8(r); w.int8(rr); w.int8(tx); w.int8(n); w.int8(rn);
+      case Unknown(_, payload) => for (p <- payload) w.int8(p)
+    }
+    
+    (id.toByte, arr)
+    
+  }
+    
+    
+
+}
+  
\ 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
deleted file mode 100644
index 3f519dc..0000000
--- a/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala
+++ /dev/null
@@ -1,47 +0,0 @@
-package org.mavlink.messages
-
-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/messages/payload.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/payload.scala
new file mode 100644
index 0000000..76076b2
--- /dev/null
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/payload.scala
@@ -0,0 +1,19 @@
+package org.mavlink.messages
+
+trait PayloadReader {
+  def int8: Byte
+  def int16: Short
+  def int32: Int
+  def int64: Long
+  def float: Float
+  def double: Double
+}
+
+trait PayloadWriter {
+  def int8(x: Byte)
+  def int16(x: Short)
+  def int32(x: Int)
+  def int64(x: Long)
+  def float(x: Float)
+  def double(x: Double)
+}
\ 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
deleted file mode 100644
index ff31e9d..0000000
--- a/vfd-mavlink/src/main/scala/org/mavlink/payload.scala
+++ /dev/null
@@ -1,21 +0,0 @@
-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 PayloadWriter {
-  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
diff --git a/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala b/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala
deleted file mode 100644
index 9f13bff..0000000
--- a/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala
+++ /dev/null
@@ -1,36 +0,0 @@
-package org.mavlink
-
-import java.nio.ByteBuffer
-import java.nio.ByteOrder
-
-class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader {
-  private val buffer = ByteBuffer.wrap(payload)
-
-  //mavlink uses little endian
-  buffer.order(ByteOrder.LITTLE_ENDIAN)
-
-  def nextInt8 = buffer.get()
-  def nextInt16 = buffer.getShort()
-  def nextInt32 = buffer.getInt()
-  def nextInt64 = buffer.getLong()
-  def nextFloat = buffer.getFloat()
-  def nextDouble = buffer.getDouble()
-  def nextChar = buffer.getChar()
-
-}
-
-class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter {
-  private val buffer = ByteBuffer.wrap(payload)
-
-  //mavlink uses little endian
-  buffer.order(ByteOrder.LITTLE_ENDIAN)
-
-  def writeInt8(x: Byte) = buffer.put(x)
-  def writeInt16(x: Short) = buffer.putShort(x)
-  def writeInt32(x: Int) = buffer.putInt(x)
-  def writeInt64(x: Long) = buffer.putLong(x)
-  def writeFloat(x: Float) = buffer.putFloat(x)
-  def writeDouble(x: Double) = buffer.putDouble(x)
-  def writeChar(x: Char) = buffer.putChar(x)
-
-}
\ No newline at end of file
diff --git a/vfd-uav/src/main/scala/org/mavlink/jvm.scala b/vfd-uav/src/main/scala/org/mavlink/jvm.scala
new file mode 100644
index 0000000..ac711be
--- /dev/null
+++ b/vfd-uav/src/main/scala/org/mavlink/jvm.scala
@@ -0,0 +1,48 @@
+package org.mavlink
+
+import scala.language.implicitConversions
+
+import java.nio.ByteBuffer
+import java.nio.ByteOrder
+
+package object messages {
+  import org.mavlink.messages.PayloadReader
+  import org.mavlink.messages.PayloadWriter
+
+  implicit def mkReader(s: Seq[Byte]) = new BufferedPayloadReader(s.toArray)
+  implicit def mkWriter(a: Array[Byte]) = new BufferedPayloadWriter(a)
+
+}
+
+package messages {
+
+  class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader {
+    private val buffer = ByteBuffer.wrap(payload)
+
+    //mavlink uses little endian
+    buffer.order(ByteOrder.LITTLE_ENDIAN)
+
+    def int8 = buffer.get()
+    def int16 = buffer.getShort()
+    def int32 = buffer.getInt()
+    def int64 = buffer.getLong()
+    def float = buffer.getFloat()
+    def double = buffer.getDouble()
+
+  }
+
+  class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter {
+    private val buffer = ByteBuffer.wrap(payload)
+
+    //mavlink uses little endian
+    buffer.order(ByteOrder.LITTLE_ENDIAN)
+
+    def int8(x: Byte) = buffer.put(x)
+    def int16(x: Short) = buffer.putShort(x)
+    def int32(x: Int) = buffer.putInt(x)
+    def int64(x: Long) = buffer.putLong(x)
+    def float(x: Float) = buffer.putFloat(x)
+    def double(x: Double) = buffer.putDouble(x)
+  }
+}
+
diff --git a/vfd-uav/src/main/scala/org/mavlink/package.scala b/vfd-uav/src/main/scala/org/mavlink/package.scala
deleted file mode 100644
index 87a8add..0000000
--- a/vfd-uav/src/main/scala/org/mavlink/package.scala
+++ /dev/null
@@ -1,9 +0,0 @@
-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 306d3dc..cfdf60f 100644
--- a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
@@ -63,15 +63,7 @@ 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 = {
 
     case Terminated(`operator`) =>
@@ -83,9 +75,6 @@ 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) =>