reimplement mock messages

2 files changed, 95 insertions, 31 deletions

diff --git a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
index 6c2dbc8..1051607 100644
--- a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
@@ -1,45 +1,51 @@
 package vfd.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 vfd.uav.mock.RandomFlightPlan
 
 class MockConnection(localSystemId: Byte, localComponentId: Byte, remoteSystemId: Byte) extends Actor with ActorLogging with Connection with MavlinkUtil {
   import Connection._
-  import context._
+  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)))
+  }
 
-  val MessageInterval = FiniteDuration(1000, MILLISECONDS)
-
-  def randomData: ByteString =
-    assemble(
-      Heartbeat(
-        MavType.MavTypeGeneric.toByte,
-        MavAutopilot.MavAutopilotGeneric.toByte,
-        (MavModeFlag.MavModeFlagSafetyArmed | MavModeFlag.MavModeFlagManualInputEnabled).toByte,
-        0, //no custom mode
-        MavState.MavStateActive.toByte,
-        0 //TODO properly implement read-only fields
-        ))
-
-  override def preStart() = context.system.scheduler.schedule(MessageInterval, MessageInterval) {
-    sendAll(Received(randomData))
+  override def preStart() = {
+    //increment state
+    system.scheduler.schedule(0.01.seconds, 0.01.seconds){plan.tick(0.01)}
+    
+    //send messages
+    scheduleMessage(0.1.seconds)(plan.position)
+    scheduleMessage(0.1.seconds)(plan.attitude)
+    scheduleMessage(2.seconds)(plan.heartbeat)
+    
+    //simulate noisy line
+    scheduleBytes(0.3.seconds)(MockPackets.invalidCrc)
+    scheduleBytes(1.5.seconds)(MockPackets.invalidOverflow)
   }
 
   def receive = registration
@@ -51,23 +57,12 @@ object MockConnection {
 }
 
 object MockPackets {
-  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
-  }
-
   val invalidCrc = Array(254, 1, 123, 13, 13).map(_.toByte)
   val invalidOverflow = {
-    val data = Array.fill[Byte](Packet.MaxPayloadLength + 10)(42)
+    val data = Array.fill[Byte](Packet.MaxPayloadLength + 100)(42)
     data(0) = -2
     data(1) = 2
     data(1) = -1
     data
   }
-
-  def invalid = r.nextInt(2) match {
-    case 0 => invalidCrc
-    case 1 => invalidOverflow
-  }
 }
\ No newline at end of file
diff --git a/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala b/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala
new file mode 100644
index 0000000..e2c5708
--- /dev/null
+++ b/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala
@@ -0,0 +1,69 @@
+package vfd.uav.mock
+
+import scala.util.Random
+
+import org.mavlink.Mavlink
+import org.mavlink.enums.MavAutopilot
+import org.mavlink.enums.MavModeFlag
+import org.mavlink.enums.MavState
+import org.mavlink.enums.MavType
+import org.mavlink.messages.Attitude
+import org.mavlink.messages.GlobalPositionInt
+import org.mavlink.messages.Heartbeat
+
+class RandomFlightPlan {
+  
+  private var time: Double = 0
+  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 val EarthRadius = 6000000
+  private val StartLat = 46.518513 //N
+  private val StartLon = 6.566923 //E
+
+  def position = GlobalPositionInt(
+    (time * 1000).toInt,
+    (StartLat + x / EarthRadius).toInt,
+    (StartLon + y / EarthRadius).toInt,
+    0,
+    0,
+    (vX * 100).toShort,
+    (vY * 100).toShort,
+    0,
+    0
+  )
+
+  def attitude = Attitude(
+    (time * 1000).toInt,
+    (time / 5 * math.Pi * 2).toFloat,
+    (time / 4 * math.Pi * 2).toFloat,
+    (time / 3 * math.Pi * 2).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
+  )
+
+}
\ No newline at end of file