1 files changed, 95 insertions, 0 deletions
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/mock/MockBackend.scala b/mavigator-uav/src/main/scala/mavigator/uav/mock/MockBackend.scala
new file mode 100644
index 0000000..0b89fbd
--- /dev/null
+++ b/mavigator-uav/src/main/scala/mavigator/uav/mock/MockBackend.scala
@@ -0,0 +1,95 @@
+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.Message
+
+
+/** A test connection that produces random MAVLink messages. */
+class MockBackend(
+  remoteSystemId: Byte,
+  remoteComponentId: Byte,
+  prescaler: Double
+) {
+  import MockBackend._
+
+  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)
+  )
+
+  private val data: Source[ByteString, NotUsed] = messages.map{ message =>
+    val (messageId, payload) = Message.pack(message)
+    val packet = assembler.assemble(messageId, payload)
+    ByteString(packet.toArray)
+  }
+
+}
+
+object MockBackend extends Backend {
+
+  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(ClockTick.toMillis / 1000.0)
+        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)
+    })
+  }
+
+
+  override def init(core: Core) = {
+    import core.materializer
+    import core.system
+
+    system.log.info("Initializing mock backend...")
+
+    val config = system.settings.config.getConfig("mavigator.uav.mock")
+
+    val mock = new MockBackend(
+      config.getInt("remote_system_id").toByte,
+      config.getInt("remote_component_id").toByte,
+      config.getDouble("prescaler")
+    )
+
+    val mockFlow = Flow.fromSinkAndSource(
+      Sink.ignore,
+      mock.data
+    )
+
+    (mockFlow join core.setBackend()).run()
+
+  }
+
+}