diff options
Diffstat (limited to 'mavigator-uav')
7 files changed, 338 insertions, 0 deletions
diff --git a/mavigator-uav/build.sbt b/mavigator-uav/build.sbt new file mode 100644 index 0000000..abe935b --- /dev/null +++ b/mavigator-uav/build.sbt @@ -0,0 +1,11 @@ +import mavigator.{Dependencies, MavigatorBuild} + +MavigatorBuild.defaultSettings + +libraryDependencies ++= Seq( + Dependencies.akkaActor, + Dependencies.akkaStream, + Dependencies.flow, + Dependencies.flowNative, + Dependencies.reactiveStreams +) diff --git a/mavigator-uav/src/main/resources/reference.conf b/mavigator-uav/src/main/resources/reference.conf new file mode 100644 index 0000000..3b43d37 --- /dev/null +++ b/mavigator-uav/src/main/resources/reference.conf @@ -0,0 +1,39 @@ +# Settings related to the connection with a UAV +mavigator.uav { + # The type of connection to use + # 'mock' or 'serial' + type = mock + + # Mavlink component ID used by this connection, + # in case it needs to inject messages. I.e. heartbeats + # will originate from this ID. + component_id = 1 + + # Interval in milliseconds between heartbeat messages injected by + # the connection + # 0 = no heartbeats injected + heartbeat = 2000 + + # Settings related to serial connections + serial { + # Serial port + port = "/dev/ttyUSB0" + # Baud rate (b/s) + baud = 115200 + # Use two stop bits + two_stop_bits = false + # Parity check + # 0 = None, 1 = Odd, 2 = Even + parity = 0 + } + + # Settings related to mock connections + mock { + # Mavlink system ID of the simulated UAV + remote_system_id = 42 + # Mavlink component ID of the simulated UAV + remote_system_id = 0 + # Divide simulated message frequency + prescaler = 1 + } +}
\ No newline at end of file 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..0fb56b0 --- /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/Multiplexer.scala b/mavigator-uav/src/main/scala/mavigator/uav/Multiplexer.scala new file mode 100644 index 0000000..5e48ea1 --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/Multiplexer.scala @@ -0,0 +1,20 @@ +package mavigator +package uav + +import akka.stream.scaladsl._ +import akka.stream._ +import org.reactivestreams._ + +private[uav] class Multiplexer[In, Out](service: Flow[In, Out, _])(implicit materializer: Materializer) { + + private val endpoint: Flow[Out, In, (Publisher[Out], Subscriber[In])] = Flow.fromSinkAndSourceMat( + Sink.asPublisher[Out](fanout = true), + Source.asSubscriber[In])((pub, sub) => (pub, sub)) + + private lazy val (publisher, subscriber) = (service.joinMat(endpoint)(Keep.right)).run() + + def connect(client: Flow[Out, In, _]) = { + Source.fromPublisher(publisher).via(client).to(Sink.ignore).run() + } + +} diff --git a/mavigator-uav/src/main/scala/mavigator/uav/Uav.scala b/mavigator-uav/src/main/scala/mavigator/uav/Uav.scala new file mode 100644 index 0000000..06a8f00 --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/Uav.scala @@ -0,0 +1,49 @@ +package mavigator +package uav + +import java.lang.IllegalArgumentException +import mock._ +import akka._ +import akka.actor._ +import akka.util._ +import akka.stream.scaladsl._ + +class Uav(system: ExtendedActorSystem) extends Extension { + + private lazy val config = system.settings.config.getConfig("mavigator.uav") + private lazy val tpe = config.getString("type") + private lazy val componentId = config.getInt("componentId").toByte + private lazy val heartbeat = config.getInt("heartbeat") + private lazy val connection = config.getConfig(tpe) + + lazy val source = tpe match { + case "mock" => + new MockConnection( + connection.getInt("remote_system_id").toByte, + componentId, + connection.getDouble("prescaler") + ) + + case "serial" => ??? + + case _ => throw new IllegalArgumentException(s"Unsupported connection type: $tpe") + } + + def connect(): Flow[ByteString, ByteString, NotUsed] = { + Flow.fromSinkAndSource( + Sink.ignore, + (new MockConnection(0,0,1)).data + ) + } + +} + +object Uav extends ExtensionId[Uav] with ExtensionIdProvider { + + override def lookup = Uav + + override def createExtension(system: ExtendedActorSystem) = new Uav(system) + + def apply()(implicit system: ActorSystem) = super.apply(system) + +} diff --git a/mavigator-uav/src/main/scala/mavigator/uav/mock/MockConnection.scala b/mavigator-uav/src/main/scala/mavigator/uav/mock/MockConnection.scala new file mode 100644 index 0000000..58b4977 --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/mock/MockConnection.scala @@ -0,0 +1,70 @@ +package mavigator +package uav +package mock + +import scala.concurrent.duration._ + +import akka.NotUsed +import akka.stream._ +import akka.stream.Attributes._ +import akka.stream.scaladsl._ +import akka.util._ +import org.mavlink._ +import org.mavlink.messages.{Heartbeat, Message} + + +class MockConnection( + remoteSystemId: Byte, + remoteComponentId: Byte, + prescaler: Double +) { + import MockConnection._ + + private lazy val assembler = new Assembler(remoteSystemId, remoteComponentId) + + private def delayed(delaySeconds: Double)(message: RandomFlightPlan => Message): Flow[RandomFlightPlan, Message, NotUsed] = { + val dt = delaySeconds / prescaler + Flow[RandomFlightPlan].withAttributes(inputBuffer(1,1)).delay(dt.seconds).map(message) + } + + private val messages: Source[Message, NotUsed] = fromPlan(new RandomFlightPlan)( + delayed(2)(_.heartbeat), + delayed(0.2)(_.position), + delayed(0.05)(_.attitude), + delayed(0.05)(_.motors), + delayed(0.1)(_.distance) + ) + + val data: Source[ByteString, NotUsed] = messages.map{ message => + val (messageId, payload) = Message.pack(message) + val packet = assembler.assemble(messageId, payload) + ByteString(packet.toArray) + } + +} + +object MockConnection { + + final val ClockTick: FiniteDuration = 0.02.seconds + + private def fromPlan(plan: RandomFlightPlan)(messages: Flow[RandomFlightPlan, Message, _]*): Source[Message, NotUsed] = { + import GraphDSL.Implicits._ + Source.fromGraph(GraphDSL.create() { implicit b => + + val clock = Source.tick(ClockTick, ClockTick, plan) map { plan => + plan.tick(0.01) + plan + } + val bcast = b.add(Broadcast[RandomFlightPlan](messages.length)) + val merge = b.add(Merge[Message](messages.length)) + + clock ~> bcast + for (message <- messages) { + bcast ~> message ~> merge + } + + SourceShape(merge.out) + }) + } + +} 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..89e5a75 --- /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 //current time in seconds + 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 + ) + +} |