Rename project and update dependencies

6 files changed, 444 insertions, 0 deletions

diff --git a/mavigator-uav/build.sbt b/mavigator-uav/build.sbt
new file mode 100644
index 0000000..13a8b5a
--- /dev/null
+++ b/mavigator-uav/build.sbt
@@ -0,0 +1,9 @@
+import mavigator.{Dependencies, MavigatorBuild}
+
+MavigatorBuild.defaultSettings
+
+libraryDependencies ++= Seq(
+  Dependencies.akkaActor,
+  Dependencies.flow,
+  Dependencies.flowNative
+)
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/Connection.scala b/mavigator-uav/src/main/scala/mavigator/uav/Connection.scala
new file mode 100644
index 0000000..7399657
--- /dev/null
+++ b/mavigator-uav/src/main/scala/mavigator/uav/Connection.scala
@@ -0,0 +1,73 @@
+package mavigator.uav
+
+import akka.actor.ActorLogging
+import scala.collection.mutable.ArrayBuffer
+
+import akka.actor.Actor
+import akka.actor.ActorRef
+import akka.actor.Terminated
+import akka.actor.actorRef2Scala
+import akka.util.ByteString
+
+/** Protocol definition. */
+object Connection {
+
+  /** Any messages received and transmitted by actors implementing this protocol. */
+  sealed trait Message
+
+  /** Messages emitted by actors implementing this protocol. */
+  trait Event extends Message
+
+  /** Received data from the uav (or any other systems on the link) */
+  case class Received(bstr: ByteString) extends Event
+
+  /** The connection closed or could not be opened */
+  case class Closed(message: String) extends Event
+
+  /** Messages that can be received by actors implementing this protocol. */
+  trait Command extends Message
+
+  /** Register the sender to be notified on events */
+  case object Register extends Command
+
+  case class Unregister(client: ActorRef) extends Command
+
+  /** Send given bytes out to the uav (or any other systems on the link) */
+  case class Send(bstr: ByteString) extends Command
+
+}
+
+/** Common behavior of connection actors. */
+trait Connection { myself: Actor with ActorLogging =>
+
+  private val _clients = new ArrayBuffer[ActorRef]
+
+  /** Current clients that should be notified on incoming messages. */
+  def clients = _clients.toSeq
+
+  /** Adds a client to the client list and acquires a deathwatch. */
+  protected def register(client: ActorRef): Unit = {
+    _clients += client
+    myself.context.watch(client)
+    myself.log.info("Client registered {}", client)
+  }
+
+  /** Remove client and release deathwatch. */
+  protected def unregister(client: ActorRef): Unit = if (clients contains client) {
+    _clients -= client
+    myself.context.unwatch(client)
+    myself.log.info("Client unregistered {}", client)
+  }
+
+  /** Sends a message to all registered clients. */
+  protected def sendAll(msg: Any): Unit = clients foreach (_ ! msg)
+
+  /** Common registration behavior. Manages the messages `Register` and `Terminated` by
+    * registering and unregistering clients. */
+  protected def handleRegistration: Receive = {
+    case Connection.Register => register(sender)
+    case Terminated(client) => unregister(client)
+    case other => myself.log.warning("Unknown message: {}", other)
+  }
+
+}
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/MavlinkUtil.scala b/mavigator-uav/src/main/scala/mavigator/uav/MavlinkUtil.scala
new file mode 100644
index 0000000..3756dd6
--- /dev/null
+++ b/mavigator-uav/src/main/scala/mavigator/uav/MavlinkUtil.scala
@@ -0,0 +1,46 @@
+package mavigator.uav
+
+import org.mavlink.Assembler
+import org.mavlink.Packet
+import org.mavlink.Parser
+import org.mavlink.messages.Message
+
+import akka.actor.Actor
+import akka.actor.ActorLogging
+import akka.util.ByteString
+
+/** Provides utilities for actors representing a mavlink connection. */
+trait MavlinkUtil { myself: Actor with ActorLogging =>
+  
+  /** Mavlink system ID of this connection. */
+  val systemId: Byte
+
+  /** Mavlink component ID of this connection. */
+  val componentId: Byte
+  
+  /** Assembler for creating packets originating from this connection. */
+  private lazy val assembler = new Assembler(systemId, componentId)
+  
+  /** Assembles a message into a bytestring representing a packet sent from this connection. */
+  protected def assemble(message: Message): ByteString = {
+    val (messageId, payload) = Message.pack(message)
+    val packet: Packet = assembler.assemble(messageId, payload)
+    ByteString(packet.toArray)
+  }
+
+  /** Parser for messages being sent to the uav. */
+  protected val outgoing: Parser = new Parser(packet => Message.unpack(packet.messageId, packet.payload) match {
+    //TODO handle ping
+    /*
+    case Ping(`systemId`, `componentId`) =>
+      val message = Ack(packet.systemId, packet.componentId)
+      val data = assemble(message)
+      self ! Connection.Received(data)*/
+    case _ => ()
+  })
+
+  /** Parser for messages coming from the uav. */
+  protected val incoming: Parser = new Parser(pckt =>
+    log.debug("incoming message: " + Message.unpack(pckt.messageId, pckt.payload)))
+
+}
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/MockConnection.scala b/mavigator-uav/src/main/scala/mavigator/uav/MockConnection.scala
new file mode 100644
index 0000000..5949a3a
--- /dev/null
+++ b/mavigator-uav/src/main/scala/mavigator/uav/MockConnection.scala
@@ -0,0 +1,77 @@
+package mavigator.uav
+
+import java.util.concurrent.TimeUnit.MILLISECONDS
+import scala.concurrent.duration.FiniteDuration
+import scala.util.Random
+import org.mavlink.Packet
+import org.mavlink.enums.MavAutopilot
+import org.mavlink.enums.MavModeFlag
+import org.mavlink.enums.MavState
+import org.mavlink.enums.MavType
+import org.mavlink.messages.Heartbeat
+import Connection.Received
+import akka.actor.Actor
+import akka.actor.ActorLogging
+import akka.actor.Props
+import akka.util.ByteString
+import scala.concurrent.duration._
+import org.mavlink.messages.Message
+import mock.RandomFlightPlan
+
+class MockConnection(
+  localSystemId: Byte,
+  localComponentId: Byte,
+  remoteSystemId: Byte,
+  prescaler: Int
+)
+  extends Actor with ActorLogging with Connection with MavlinkUtil {
+
+  import context._
+
+  override val systemId = remoteSystemId
+  override val componentId = remoteSystemId
+
+  val plan = new RandomFlightPlan
+
+  def scheduleMessage(delay: FiniteDuration)(fct: => Message) = system.scheduler.schedule(delay, delay) {
+    sendAll(Received(assemble(fct)))
+  }
+  def scheduleBytes(delay: FiniteDuration)(fct: => Array[Byte]) = system.scheduler.schedule(delay, delay) {
+    sendAll(Received(ByteString(fct)))
+  }
+
+  override def preStart() = {
+    //increment state
+    system.scheduler.schedule(0.01.seconds * prescaler, 0.01.seconds * prescaler) { plan.tick(0.01) }
+
+    //send messages
+    scheduleMessage(0.1.seconds * prescaler)(plan.position)
+    scheduleMessage(0.05.seconds * prescaler)(plan.attitude)
+    scheduleMessage(0.05.seconds * prescaler)(plan.motors)
+    scheduleMessage(0.1.seconds * prescaler)(plan.distance)
+    scheduleMessage(1.seconds)(plan.heartbeat)
+
+    //simulate noisy line
+    scheduleBytes(0.3.seconds * prescaler)(MockPackets.invalidCrc)
+    scheduleBytes(1.5.seconds * prescaler)(MockPackets.invalidOverflow)
+  }
+
+  override def receive = handleRegistration
+
+}
+
+object MockConnection {
+  def apply(systemId: Byte, componentId: Byte, remoteSystemId: Byte, prescaler: Int = 1) =
+    Props(classOf[MockConnection], systemId, componentId, remoteSystemId, prescaler)
+}
+
+object MockPackets {
+  val invalidCrc = Array(254, 1, 123, 13, 13).map(_.toByte)
+  val invalidOverflow = {
+    val data = Array.fill[Byte](Packet.MaxPayloadLength + 100)(42)
+    data(0) = -2
+    data(1) = 2
+    data(1) = -1
+    data
+  }
+}
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/SerialConnection.scala b/mavigator-uav/src/main/scala/mavigator/uav/SerialConnection.scala
new file mode 100644
index 0000000..24c70a1
--- /dev/null
+++ b/mavigator-uav/src/main/scala/mavigator/uav/SerialConnection.scala
@@ -0,0 +1,136 @@
+package mavigator.uav
+
+import java.util.concurrent.TimeUnit.MILLISECONDS
+
+import scala.concurrent.duration.FiniteDuration
+
+import org.mavlink.enums.MavAutopilot
+import org.mavlink.enums.MavModeFlag
+import org.mavlink.enums.MavState
+import org.mavlink.enums.MavType
+import org.mavlink.messages.Heartbeat
+
+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.ActorLogging
+import akka.actor.ActorRef
+import akka.actor.Props
+import akka.actor.Terminated
+import akka.actor.actorRef2Scala
+import akka.io.IO
+
+class SerialConnection(
+  val systemId: Byte,
+  val componentId: Byte,
+  heartbeatInterval: Option[FiniteDuration],
+  port: String,
+  settings: SerialSettings) extends Actor with ActorLogging with Connection with MavlinkUtil {
+
+  import context._
+
+  override def preStart() = heartbeatInterval foreach { interval =>
+    context.system.scheduler.schedule(interval, interval) {
+      self ! Connection.Send(
+        assemble(
+          Heartbeat(
+            MavType.MavTypeGeneric.toByte,
+            MavAutopilot.MavAutopilotGeneric.toByte,
+            0, //no base mode
+            0, //no custom mode
+            MavState.MavStateActive.toByte,
+            0 //TODO properly implement read-only fields
+          )
+        )
+      )
+    }
+  }
+
+  def _closed: Receive = {
+
+    case Connection.Register =>
+      register(sender)
+      IO(Serial) ! Serial.Open(port, settings)
+      context become opening
+
+    case Connection.Send(_) =>
+      IO(Serial) ! Serial.Open(port, settings)
+      context become opening
+
+  }
+
+  def _opening: Receive = {
+
+    case Serial.CommandFailed(cmd: Serial.Open, reason) =>
+      sendAll(Connection.Closed(reason.toString))
+      context become closed
+
+    case Serial.Opened(_) =>
+      context watch (sender)
+      context become opened(sender)
+
+    case Connection.Send(_) => () // ignore
+      /*
+       * During opening, any outgoing messages are discarded.
+       * By using some kind of message stashing, maybe messages could be treated
+       * once the port has been opened. However, in such a case failure also needs
+       * to be considered, thus complicating the protocol. Since opening is typically
+       * quite fast and since mavlink uses heartbeats and acknowledgements (in certain
+       * circumstances) anyway, keeping messages is not really required.  
+       */
+
+  }
+
+  def _opened(operator: ActorRef): Receive = {
+
+    case Terminated(`operator`) =>
+      sendAll(Connection.Closed("Serial connection crashed."))
+      context become closed
+
+    case Serial.Closed =>
+      sendAll(Connection.Closed("Serial connection was closed."))
+      context become closed
+
+    case Serial.Received(bstr) =>
+      sendAll(Connection.Received(bstr))
+      incoming.push(bstr)
+
+    case Connection.Send(bstr) =>
+      outgoing.push(bstr)
+      //no sending is currently enabled
+
+  }
+
+  def closed = _closed orElse handleRegistration
+  def opening = _opening orElse handleRegistration
+  def opened(op: ActorRef) = _opened(op) orElse handleRegistration
+  override def receive = closed
+
+}
+
+object SerialConnection {
+  def apply(
+    systemId: Byte,
+    componentId: Byte,
+    heartbeatInterval: Int,
+    port: String,
+    baud: Int,
+    tsb: Boolean,
+    parity: Int): Props = {
+
+    val settings = SerialSettings(
+      baud,
+      8,
+      tsb,
+      parity match {
+        case 0 => Parity.None
+        case 1 => Parity.Odd
+        case 2 => Parity.Even
+      })
+    val hb = if (heartbeatInterval == 0) None else Some(FiniteDuration(heartbeatInterval, MILLISECONDS))
+
+    Props(classOf[SerialConnection], systemId, componentId, hb, port, settings)
+  }
+}
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala b/mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala
new file mode 100644
index 0000000..1e5431e
--- /dev/null
+++ b/mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala
@@ -0,0 +1,103 @@
+package mavigator.uav
+package mock
+
+import scala.util.Random
+
+import org.mavlink.Mavlink
+import org.mavlink.enums._
+import org.mavlink.messages._
+
+class RandomFlightPlan {
+  
+  private var time: Double = 0
+  private def millis = (time * 1000).toInt
+  private def micros = (time * 1E6).toInt
+
+  // an oscilliating function
+  private def osc[A](min: A, max: A, period: Double, offset: Double = 0)(implicit n: Numeric[A]): A = {
+    val amplitude = (n.toDouble(max) - n.toDouble(min)) / 2
+    val base = (n.toDouble(max) + n.toDouble(min)) / 2
+    val factor = math.sin(2 * math.Pi * (time + offset) / period)
+    n.fromInt((base + amplitude * factor).toInt)
+  }
+
+  private var x: Double = 0.0
+  private var y: Double = 0.0
+  private var vX: Double = 0.0
+  private var vY: Double = 0.0
+
+  def tick(delta: Double) {
+    val aX = Random.nextDouble() * 5
+    val aY = Random.nextDouble() * 5
+
+    x += vX * delta
+    y += vY * delta
+    vX += aX * delta
+    vY += aY * delta
+
+    time += delta
+  }
+
+  private final val EarthRadius = 6000000 //m
+  private final val StartLat = 46.518513 //deg N
+  private final val StartLon = 6.566923 //deg E
+
+  def position = GlobalPositionInt(
+    millis,
+    (StartLat + x / EarthRadius).toInt,
+    (StartLon + y / EarthRadius).toInt,
+    0,
+    0,
+    (vX * 100).toShort,
+    (vY * 100).toShort,
+    0,
+    0
+  )
+
+  def attitude = Attitude(
+    millis,
+    (2 * math.Pi * time / 6).toFloat,
+    (math.sin(2 * math.Pi * time / 5) * math.Pi / 6).toFloat,
+    (2 * math.Pi * time / 4).toFloat,
+    0,
+    0,
+    0
+  )
+
+  def heartbeat = Heartbeat(  
+    MavType.MavTypeGeneric.toByte,
+    MavAutopilot.MavAutopilotGeneric.toByte,
+    (MavModeFlag.MavModeFlagSafetyArmed | MavModeFlag.MavModeFlagManualInputEnabled).toByte,
+    0, //no custom mode
+    MavState.MavStateActive.toByte,
+    Mavlink.MavlinkVersion
+  )
+
+  private final val DistanceMin: Short = 10
+  private final val DistanceMax: Short = 500
+  def distance = DistanceSensor(
+    timeBootMs = millis,
+    minDistance = DistanceMin,
+    maxDistance = DistanceMax,
+    currentDistance = osc(DistanceMin, DistanceMax, 6),
+    `type` = MavDistanceSensor.MavDistanceSensorUltrasound.toByte,
+    id = 0: Byte,
+    orientation = -1: Byte,
+    covariance = 3: Byte)
+
+  private final val MotorsMax: Short = 2000 //usec, ppm signal => 100%
+  private final val MotorsMin: Short = 1000 //usec, ppm signal => 0%
+  def motors = ServoOutputRaw(
+    timeUsec = micros,
+    port = 0: Byte,
+    servo1Raw =  osc(MotorsMin, MotorsMax, 6, 0),
+    servo2Raw = osc(MotorsMin, MotorsMax, 6, 5),
+    servo3Raw = osc(MotorsMin, MotorsMax, 6, 2),
+    servo4Raw = osc(MotorsMin, MotorsMax, 6, 4),
+    servo5Raw = 0,
+    servo6Raw = 0,
+    servo7Raw = 0,
+    servo8Raw = 0
+  )
+
+}