migrate to using mavlink

5 files changed, 98 insertions, 142 deletions

diff --git a/vfd-uav/src/main/scala/vfd/uav/Connection.scala b/vfd-uav/src/main/scala/vfd/uav/Connection.scala
index b4e6493..5d9ed54 100644
--- a/vfd-uav/src/main/scala/vfd/uav/Connection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/Connection.scala
@@ -1,22 +1,19 @@
 package vfd.uav
 
+import scala.collection.mutable.ArrayBuffer
+
 import akka.actor.Actor
 import akka.actor.ActorRef
-import akka.actor.Props
-import scala.collection.mutable.ArrayBuffer
 
 object Connection {
-  def dummy = Props(classOf[DummyConnection])
-  def fcu(port: String, baud: Int) = Props(classOf[FcuConnection], port, baud)
-
   trait Event
   trait Command
   case object Register extends Command
-  case class NewDataFrame(df: DataFrame) extends Event
+  case class Received(bytes: Array[Byte]) extends Event
 
 }
 
-trait Connection {that: Actor =>
+trait Connection { that: Actor =>
   private val _clients = new ArrayBuffer[ActorRef]
   def clients = _clients.toSeq
   def register(client: ActorRef) = {
diff --git a/vfd-uav/src/main/scala/vfd/uav/DummyConnection.scala b/vfd-uav/src/main/scala/vfd/uav/DummyConnection.scala
index 991a303..7cf7bb1 100644
--- a/vfd-uav/src/main/scala/vfd/uav/DummyConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/DummyConnection.scala
@@ -1,9 +1,13 @@
 package vfd.uav
 
+import java.util.concurrent.TimeUnit.MILLISECONDS
+
+import scala.concurrent.duration.FiniteDuration
+
 import akka.actor.Actor
+import akka.actor.Props
 import akka.actor.Terminated
-import scala.concurrent.duration.FiniteDuration
-import java.util.concurrent.TimeUnit._
+import akka.actor.actorRef2Scala
 
 class DummyConnection extends Actor with Connection {
   import context._
@@ -12,18 +16,11 @@ class DummyConnection extends Actor with Connection {
   val messageInterval = FiniteDuration(50, MILLISECONDS)
 
   def flightData(time: Double) = {
-    Connection.NewDataFrame(DataFrame(
-      math.sin(time/6000) * math.Pi,
-      math.sin(time/5050) * math.Pi/4,
-      time/5000 * 2 * math.Pi,
-      time/1000,
-      22
-    ))
+    new Array[Byte](10)
   }
 
-
   override def preStart() = {
-    context.system.scheduler.schedule(messageInterval, messageInterval){
+    context.system.scheduler.schedule(messageInterval, messageInterval) {
       time += messageInterval.toMillis
       clients foreach (_ ! flightData(time))
     }
@@ -36,5 +33,7 @@ class DummyConnection extends Actor with Connection {
 
 }
 
-
+object DummyConnection {
+  def apply = Props(classOf[DummyConnection])
+}
 
diff --git a/vfd-uav/src/main/scala/vfd/uav/FcuConnection.scala b/vfd-uav/src/main/scala/vfd/uav/FcuConnection.scala
deleted file mode 100644
index f9f267d..0000000
--- a/vfd-uav/src/main/scala/vfd/uav/FcuConnection.scala
+++ /dev/null
@@ -1,53 +0,0 @@
-package vfd.uav
-
-import akka.actor.Actor
-import akka.actor.Props
-import akka.actor.Terminated
-import akka.io.IO
-import com.github.jodersky.flow._
-import com.github.jodersky.flow.Serial
-
-
-class FcuConnection(port: String, baud: Int) extends Actor with Connection {
-  import context._
-
-  val settings = SerialSettings(
-    baud = this.baud,
-    characterSize = 8,
-    twoStopBits = false,
-    parity = Parity.None
-  )
-
-  override def preStart() = {
-    IO(Serial) ! Serial.Open(port, settings)
-  }
-
-  def receive = {
-    case Connection.Register => register(sender)
-    case Terminated(client) => unregister(client)
-    case Serial.CommandFailed(cmd: Serial.Open, reason: AccessDeniedException) => println("you're not allowed to open that port!")
-    case Serial.CommandFailed(cmd: Serial.Open, reason) => println("could not open port for some other reason: " + reason)
-    case Serial.Opened(settings) => {
-      val operator = sender
-      
-    }
-    case Serial.Received(bstr) => 
-      val str = (new String(bstr.toArray, "UTF-8")).trim
-      
-
-      val LinePattern = ".*[(](.+)[)].*".r
-      val Number = "([-]?\\d+[.]\\d+?)".r
-      val Components = "(.+),(.+),(.+),(.+),(.+)".r
-
-      str match {
-        case LinePattern(Components(Number(r),Number(p),Number(h),Number(a),Number(t))) => 
-          val data = Connection.NewDataFrame(DataFrame(r.toDouble, p.toDouble, h.toDouble, a.toDouble, t.toDouble))
-           println(data)
-          for (client <- clients) {
-            client ! data
-          }
-        case _ => println("unknown message: " + str)
-      }
-  }
-
-}
diff --git a/vfd-uav/src/main/scala/vfd/uav/Framer.scala b/vfd-uav/src/main/scala/vfd/uav/Framer.scala
deleted file mode 100644
index f587c9e..0000000
--- a/vfd-uav/src/main/scala/vfd/uav/Framer.scala
+++ /dev/null
@@ -1,70 +0,0 @@
-package vfd.uav
-
-import scala.collection.mutable.ArrayBuffer
-
-class Framer {
-  final val MTU: Int = 1024
-
-  final val START: Byte = 0xfd.toByte
-  final val STOP: Byte = 0xfe.toByte
-  final val ESCAPE: Byte = 0xff.toByte
-
-  final val WAITING = 0
-  final val RECEIVING = 1
-  final val ESCAPING = 2
-
-  private val data = new Array[Byte](MTU)
-  private var index = 0
-  private var state = WAITING
-
-  private def add(byte: Byte): Unit = {
-    data(index) = byte
-    index += 1
-    if (index >= MTU) index = 0
-  }
-
-  private def clear(): Unit = index = 0
-
-
-  def push(byte: Byte): Option[Array[Byte]] = state match {
-    case WAITING =>
-      if (byte == START) {
-        clear()
-        state = RECEIVING
-      }
-      None
-
-    case RECEIVING => byte match {
-      case START =>
-        clear()
-        state = RECEIVING  
-        None
-      case ESCAPE =>
-        state = ESCAPING
-        None
-      case STOP =>
-        state = WAITING
-        Some(java.util.Arrays.copyOfRange(data, 0, index))
-
-      case _ =>
-        add(byte)
-        None
-    }
-    case ESCAPING =>
-      add(byte)
-      state = RECEIVING
-      None
-  }
-
-  def push(bytes: Array[Byte]): Seq[Array[Byte]] = {
-    val messages = new ArrayBuffer[Array[Byte]]
-
-    for (byte <- bytes) push(byte) match {
-      case None => ()
-      case Some(message) => messages += message
-    }
-
-    messages
-  }
-
-}
\ 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
new file mode 100644
index 0000000..f1d3186
--- /dev/null
+++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
@@ -0,0 +1,83 @@
+package vfd.uav
+
+import com.github.jodersky.flow.Parity
+import com.github.jodersky.flow.SerialSettings
+
+import akka.actor.Actor
+import akka.actor.Props
+
+class SerialConnection(id: Byte, heartbeat: Int, port: String, settings: SerialSettings) extends Actor with Connection {
+  import context._
+  
+  //TODO implement actor logic
+
+  def receive = {
+    case _ => ()
+  }
+/*
+  override def preStart() = {
+    context.system.scheduler.schedule(messageInterval, messageInterval){
+      self ! Connection.Write(Array(-2, 9, -121, 20, -56, 0, 0, 0, 0, 0, 2, 0, 0, 3, 3, -112, 76).map(_.toByte))
+    }
+  }
+
+
+  def receive = closed
+
+  def closed: Receive = {
+    case Connection.Register(client) => 
+      register(client)
+      IO(Serial) ! Open(port, settings)
+      context become opening
+
+    case Terminated(client) if (clients contains client) => unregister(client)
+
+    case Connection.Write(data) =>
+      IO(Serial) ! Open(port, settings)
+      context become opening
+
+  }
+
+  def opening: Receive = {
+    case Connection.Register(client) => register(client)
+    case Terminated(client) if (clients contains client) => unregister(client)
+
+    case Connection.Write(data) =>
+
+    case Serial.CommandFailed(cmd: Serial.Open, reason) =>
+      Log(reason)
+      //for (c <- clients) client ! Error //TODO send proper error code
+      context become closed
+
+    case Serial.Opened(settings) =>
+      val operator = sender
+      context watch operator
+      context become open(operator)
+
+  }
+
+  def open(operator: ActorRef): Receive = {
+    case Terminated(`operator`) => 
+      //for (client <- clients) ! Error //TODO send error code
+      context become closed
+
+    case Connection.Write(data) => operator ! ByteString(data)
+  }
+*/
+}
+
+object SerialConnection {
+  def apply(id: Byte, hearbeat: Int, port: String, baud: Int, tsb: Boolean, parity: Int) = {
+    val settings = SerialSettings(
+      baud,
+      8,
+      tsb,
+      parity match {
+        case 0 => Parity.None
+        case 1 => Parity.Odd
+        case 2 => Parity.Even
+      }
+    )
+    Props(classOf[SerialConnection], id, hearbeat, port, settings)
+  }
+}
\ No newline at end of file