From 12ee4250ab270cfd7c48ffa7488a2245ac914f06 Mon Sep 17 00:00:00 2001 From: Jakob Odersky Date: Fri, 26 Dec 2014 14:57:10 +0100 Subject: update mavlink communication --- project/Dependencies.scala | 8 +-- .../scala/com/github/jodersky/mavlink/Main.scala | 1 + .../src/main/twirl/org/mavlink/Assembler.scala.txt | 15 ++++++ .../src/main/twirl/org/mavlink/Packet.scala.txt | 1 + .../src/main/twirl/org/mavlink/Parser.scala.txt | 3 ++ .../src/main/twirl/org/mavlink/_header.scala.txt | 2 +- .../twirl/org/mavlink/messages/Message.scala.txt | 2 +- vfd-uav/src/main/scala/vfd/uav/Connection.scala | 41 +++++++++----- vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala | 45 ++++++++++++++++ .../src/main/scala/vfd/uav/MockConnection.scala | 62 +++++++++++----------- .../src/main/scala/vfd/uav/SerialConnection.scala | 53 +++++++++--------- 11 files changed, 160 insertions(+), 73 deletions(-) create mode 100644 project/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt create mode 100644 vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala diff --git a/project/Dependencies.scala b/project/Dependencies.scala index 6c705eb..b6a2094 100644 --- a/project/Dependencies.scala +++ b/project/Dependencies.scala @@ -5,12 +5,12 @@ object Dependencies { val akkaActor = "com.typesafe.akka" %% "akka-actor" % "2.3.6" - val flow = "com.github.jodersky" %% "flow" % "2.0.6" - val flowNative = "com.github.jodersky" % "flow-native" % "2.0.6" + val flow = "com.github.jodersky" %% "flow" % "2.0.8" + val flowNative = "com.github.jodersky" % "flow-native" % "2.0.8" - val bootstrap = "org.webjars" % "bootstrap" % "3.2.0" + val bootstrap = "org.webjars" % "bootstrap" % "3.3.1" val fontawesome = "org.webjars" % "font-awesome" % "4.2.0" - val jquery = "org.webjars" % "jquery" % "2.1.1" + val jquery = "org.webjars" % "jquery" % "2.1.3" val dom = "org.scala-lang.modules.scalajs" %%%! "scalajs-dom" % "0.6" val tag = "com.scalatags" %%%! "scalatags" % "0.4.1" diff --git a/project/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Main.scala b/project/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Main.scala index 5f77c1a..57d1fcc 100644 --- a/project/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Main.scala +++ b/project/mavlink-library/src/main/scala/com/github/jodersky/mavlink/Main.scala @@ -17,6 +17,7 @@ object Main { org.mavlink.txt.Crc().body -> Path("Crc.scala"), org.mavlink.txt.Packet(protocol.messages).body -> Path("Packet.scala"), org.mavlink.txt.Parser().body -> Path("Parser.scala"), + org.mavlink.txt.Assembler().body -> Path("Assembler.scala"), org.mavlink.messages.txt.Message(protocol.messages).body -> Path.fromString("messages/messages.scala") ) diff --git a/project/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt b/project/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt new file mode 100644 index 0000000..8b26b69 --- /dev/null +++ b/project/mavlink-library/src/main/twirl/org/mavlink/Assembler.scala.txt @@ -0,0 +1,15 @@ +@() + +@_header() +package org.mavlink + +class Assembler(systemId: Byte, componentId: Byte) { + private var seq = 0; + + def assemble(messageId: Byte, payload: Seq[Byte]): Packet = { + val p = Packet(seq.toByte, systemId, componentId, messageId, payload) + seq += 1 + p + } + +} \ No newline at end of file diff --git a/project/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt b/project/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt index 6f96b40..9a3ad7f 100644 --- a/project/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt +++ b/project/mavlink-library/src/main/twirl/org/mavlink/Packet.scala.txt @@ -7,6 +7,7 @@ } } +@_header() package org.mavlink case class Packet( diff --git a/project/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt b/project/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt index f0e30dc..f37fc8c 100644 --- a/project/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt +++ b/project/mavlink-library/src/main/twirl/org/mavlink/Parser.scala.txt @@ -1,5 +1,6 @@ @() +@_header() package org.mavlink import scala.collection.mutable.ArrayBuffer @@ -125,4 +126,6 @@ class Parser(receiver: Packet => Unit, error: Parser.ParseErrors.ParseError => U } } + def push(bytes: Traversable[Byte]): Unit = for (b <- bytes) push(b) + } diff --git a/project/mavlink-library/src/main/twirl/org/mavlink/_header.scala.txt b/project/mavlink-library/src/main/twirl/org/mavlink/_header.scala.txt index 77ad6d2..6d27c2c 100644 --- a/project/mavlink-library/src/main/twirl/org/mavlink/_header.scala.txt +++ b/project/mavlink-library/src/main/twirl/org/mavlink/_header.scala.txt @@ -1,5 +1,5 @@ @() /** - * This file has been machine generated. + * This file is machine-generated. */ \ No newline at end of file diff --git a/project/mavlink-library/src/main/twirl/org/mavlink/messages/Message.scala.txt b/project/mavlink-library/src/main/twirl/org/mavlink/messages/Message.scala.txt index 4bba2a1..f1d6334 100644 --- a/project/mavlink-library/src/main/twirl/org/mavlink/messages/Message.scala.txt +++ b/project/mavlink-library/src/main/twirl/org/mavlink/messages/Message.scala.txt @@ -40,7 +40,7 @@ sealed trait Message case class Unknown(id: Byte, payload: Seq[Byte]) extends Message object Message { - def unpack(id: Byte, payload: Seq[Byte]) = id match { + def unpack(id: Byte, payload: Seq[Byte]): Message = id match { @for(message <- messages) { case @message.id => @defining(message.orderedFields){ ordered => 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 -- cgit v1.2.3