update mavlink communication

4 files changed, 134 insertions, 67 deletions

diff --git a/vfd-uav/src/main/scala/vfd/uav/Connection.scala b/vfd-uav/src/main/scala/vfd/uav/Connection.scala
index afc1c1a..38e1836 100644
--- a/vfd-uav/src/main/scala/vfd/uav/Connection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/Connection.scala
@@ -1,45 +1,62 @@
 package vfd.uav
 
 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
+import org.mavlink.Assembler
+import org.mavlink.messages.Message
+import org.mavlink.Parser
+import org.mavlink.Packet
+import akka.actor.ActorLogging
 
+/** Protocol definition. */
 object Connection {
   trait Event
   trait Command
 
-  //received data from the uav (or any other systems on the link)
+  /** Received data from the uav (or any other systems on the link) */
   case class Received(bstr: ByteString) extends Event
 
-  //the connection closed or could be opened
+  /** The connection closed or could not be opened */
   case class Closed(message: String) extends Event
 
-  //register the sender to be notified on events
+  /** Register the sender to be notified on events */
   case object Register extends Command
 
-  //send given bytes out to the uav (or any other systems on the link)
+  /** Send given bytes out to the uav (or any other systems on the link) */
   case class Send(bstr: ByteString) extends Command
 }
 
-trait Connection { that: Actor =>
-  private val _clients = new ArrayBuffer[ActorRef]
+/** Common behavior of connection actors. */
+trait Connection { myself: Actor =>
 
+  /** Current clients that should be notified on incoming messages. */
+  private val _clients = new ArrayBuffer[ActorRef]
   def clients = _clients.toSeq
 
-  def register(client: ActorRef) = {
+  /** Adds a client to the client list and acquires a deathwatch. */
+  protected def register(client: ActorRef) = {
     _clients += client;
-    that.context.watch(client)
+    myself.context.watch(client)
   }
 
-  def unregister(client: ActorRef) = _clients -= client
+  /** Remove client and release deathwatch. */
+  protected def unregister(client: ActorRef) = {
+    _clients -= client
+    myself.context.unwatch(client)
+  }
 
-  def sendAll(msg: Any) = clients foreach (_ ! msg)
+  /** Sends a message to all registered clients. */
+  protected def sendAll(msg: Any) = clients foreach (_ ! msg)
 
-  def registration: Receive = {
+  /**
+   * Common registration behavior. Manages the events `Register` and `Terminated` by
+   * registering and unregistering clients.
+   */
+  protected def registration: Receive = {
     case Connection.Register => register(sender)
     case Terminated(client) if clients contains client => unregister(client)
   }
diff --git a/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala b/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala
new file mode 100644
index 0000000..1d2ac80
--- /dev/null
+++ b/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala
@@ -0,0 +1,45 @@
+package vfd.uav
+
+import org.mavlink.Assembler
+import akka.util.ByteString
+import org.mavlink.Packet
+import akka.actor.Actor
+import org.mavlink.messages.Ping
+import org.mavlink.messages.Ack
+import org.mavlink.messages.Message
+import org.mavlink.Parser
+import akka.actor.ActorLogging
+
+/** 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: Byte, payload: Seq[Byte]) = Message.pack(message)
+    val packet: Packet = assembler.assemble(messageId, payload)
+    ByteString(packet.toSeq.toArray)
+  }
+
+  /** Parser for messages being sent to the uav. */
+  protected val outgoing: Parser = new Parser(packet => Message.unpack(packet.messageId, packet.payload) match {
+    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)))
+
+}
\ No newline at end of file
diff --git a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
index 238ccdd..801c4ac 100644
--- a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
@@ -11,19 +11,22 @@ import akka.util.ByteString
 import org.mavlink.messages._
 import org.mavlink.Packet
 
-class MockConnection extends Actor with ActorLogging with Connection {
+class MockConnection(localSystemId: Byte, localComponentId: Byte, remoteSystemId: Byte) extends Actor with ActorLogging with Connection with MavlinkUtil {
   import Connection._
   import context._
+  
+  override val systemId = remoteSystemId
+  override val componentId = remoteSystemId
 
-  val messageInterval = FiniteDuration(100, MILLISECONDS)
-
-  override def preStart() = {
-    context.system.scheduler.schedule(messageInterval, messageInterval) {
-      val data = MockPackets.random
-
-      this.log.debug("sending mock flight data: " + data.mkString("(", ",", ")"))
-      sendAll(Received(ByteString(data)))
-    }
+  val MessageInterval = FiniteDuration(100, MILLISECONDS)
+  
+  def randomData: ByteString = Random.nextInt(MockPackets.Messages + 1) match {
+    case 0 => ByteString(MockPackets.invalid)
+    case i => assemble(MockPackets.message(i - 1))
+  }
+  
+  override def preStart() = context.system.scheduler.schedule(MessageInterval, MessageInterval) {
+    sendAll(Received(randomData))
   }
 
   def receive = registration
@@ -31,24 +34,28 @@ class MockConnection extends Actor with ActorLogging with Connection {
 }
 
 object MockConnection {
-  def apply = Props(classOf[MockConnection])
+  def apply(systemId: Byte, componentId: Byte, remoteSystemId: Byte) = Props(classOf[MockConnection], systemId, componentId, remoteSystemId)
 }
 
 object MockPackets {
-
-  private implicit class RichMessage(val message: Message) extends AnyVal {
-    def bytes: Array[Byte] = {
-      val (id, payload) = Message.pack(message)
-      Packet(5, 42, 1, id, payload).toSeq.toArray
-    }
+  private val r = new Random
+  private implicit class RichRandom(val r: Random) extends AnyVal {
+    def nextByte(): Byte = r.nextInt().toByte
+    def nextByte(max: Int): Byte = r.nextInt(max).toByte
+  }
+  
+  def heartbeat = Heartbeat(0)
+  def motor = Motor(r.nextByte(101), r.nextByte(101), r.nextByte(101), r.nextByte(101))
+  def attitude = Attitude((r.nextInt(160) - 80).toShort, (r.nextInt(160) - 80).toShort, r.nextInt(360).toShort)
+  def power = Power(Random.nextInt(12000).toShort)
+  
+  val Messages = 4
+  def message(i: Int) = i match {
+    case 0 => heartbeat
+    case 1 => motor
+    case 2 => attitude
+    case 3 => power
   }
-
-  def messages = Heartbeat(0) ::
-    Motor(Random.nextInt(101).toByte, Random.nextInt(101).toByte, Random.nextInt(101).toByte, Random.nextInt(101).toByte) ::
-    Attitude((Random.nextInt(160) - 80).toShort, (Random.nextInt(160) - 80).toShort.toShort, Random.nextInt(360).toShort) ::
-    Power(Random.nextInt(12000).toShort) :: Nil
-
-  def valid: Array[Byte] = messages.flatMap(_.bytes).toArray
 
   val invalidCrc = Array(254, 1, 123, 13, 13).map(_.toByte)
   val invalidOverflow = {
@@ -59,13 +66,8 @@ object MockPackets {
     data
   }
 
-  def randomInvalid = Random.nextInt(2) match {
+  def invalid = r.nextInt(2) match {
     case 0 => invalidCrc
     case 1 => invalidOverflow
   }
-
-  def random: Array[Byte] = Random.nextInt(messages.length + 1) match {
-    case 0 => randomInvalid
-    case i => messages(i - 1).bytes
-  }
 }
\ 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 4a1e62f..4b2e71a 100644
--- a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
@@ -1,18 +1,16 @@
 package vfd.uav
 
 import java.util.concurrent.TimeUnit.MILLISECONDS
-
 import scala.concurrent.duration.FiniteDuration
-
 import org.mavlink.Packet
 import org.mavlink.Parser
+import org.mavlink.Assembler
 import org.mavlink.messages.Heartbeat
+import org.mavlink.messages.Ack
 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.ActorLogging
 import akka.actor.ActorRef
@@ -21,20 +19,20 @@ import akka.actor.Terminated
 import akka.actor.actorRef2Scala
 import akka.io.IO
 import akka.util.ByteString
+import org.mavlink.messages.Ping
 
-class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String, settings: SerialSettings) extends Actor with ActorLogging with Connection {
-  import context._
+class SerialConnection(
+  val systemId: Byte,
+  val componentId: Byte,
+  heartbeatInterval: Option[FiniteDuration],
+  port: String,
+  settings: SerialSettings) extends Actor with ActorLogging with Connection with MavlinkUtil {
 
-  lazy val hb = {
-    val (id, payload) = Message.pack(Heartbeat(0))
-    Packet(5, 42, 1, id, payload).toSeq.toArray
-  }
+  import context._
 
-  override def preStart() = {
-    heartbeat foreach { interval =>
-      context.system.scheduler.schedule(interval, interval) {
-        self ! Connection.Send(ByteString(hb))
-      }
+  override def preStart() = heartbeatInterval foreach { interval =>
+    context.system.scheduler.schedule(interval, interval) {
+      self ! Connection.Send(assemble(Heartbeat(0)))
     }
   }
 
@@ -66,17 +64,13 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String
     	 * 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 complicating the protocol. Since opening is typically
+       * 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.  
        */
 
   }
 
-  val parser = new Parser(pckt => {
-    println("Received message: " + Message.unpack(pckt.messageId, pckt.payload))
-  })
-
   def _opened(operator: ActorRef): Receive = {
 
     case Terminated(`operator`) =>
@@ -88,11 +82,12 @@ 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))
+      incoming.push(bstr)
 
     case Connection.Send(bstr) =>
-      operator ! Serial.Write(bstr)
+      outgoing.push(bstr)
+    //no sending is enabled
 
   }
 
@@ -104,7 +99,15 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String
 }
 
 object SerialConnection {
-  def apply(id: Byte, heartbeat: Int, port: String, baud: Int, tsb: Boolean, parity: Int) = {
+  def apply(
+    systemId: Byte,
+    componentId: Byte,
+    heartbeatInterval: Int,
+    port: String,
+    baud: Int,
+    tsb: Boolean,
+    parity: Int): Props = {
+
     val settings = SerialSettings(
       baud,
       8,
@@ -114,8 +117,8 @@ object SerialConnection {
         case 1 => Parity.Odd
         case 2 => Parity.Even
       })
-    val hb = if (heartbeat == 0) None else Some(FiniteDuration(heartbeat, MILLISECONDS))
+    val hb = if (heartbeatInterval == 0) None else Some(FiniteDuration(heartbeatInterval, MILLISECONDS))
 
-    Props(classOf[SerialConnection], id, hb, port, settings)
+    Props(classOf[SerialConnection], systemId, componentId, hb, port, settings)
   }
 }
\ No newline at end of file