1 files changed, 46 insertions, 13 deletions

diff --git a/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala b/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala
index df05d98..12e3d5c 100644
--- a/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/mock/RandomFlightPlan.scala
@@ -3,17 +3,23 @@ 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
+import org.mavlink.enums._
+import org.mavlink.messages._
 
 class RandomFlightPlan {
   
   private var time: Double = 0
+  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
@@ -31,12 +37,12 @@ class RandomFlightPlan {
     time += delta
   }
 
-  private val EarthRadius = 6000000
-  private val StartLat = 46.518513 //N
-  private val StartLon = 6.566923 //E
+  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(
-    (time * 1000).toInt,
+    millis,
     (StartLat + x / EarthRadius).toInt,
     (StartLon + y / EarthRadius).toInt,
     0,
@@ -48,7 +54,7 @@ class RandomFlightPlan {
   )
 
   def attitude = Attitude(
-    (time * 1000).toInt,
+    millis,
     (2 * math.Pi * time / 6).toFloat,
     (math.sin(2 * math.Pi * time / 5) * math.Pi / 6).toFloat,
     (2 * math.Pi * time / 4).toFloat,
@@ -57,7 +63,7 @@ class RandomFlightPlan {
     0
   )
 
-  def heartbeat = Heartbeat(
+  def heartbeat = Heartbeat(  
     MavType.MavTypeGeneric.toByte,
     MavAutopilot.MavAutopilotGeneric.toByte,
     (MavModeFlag.MavModeFlagSafetyArmed | MavModeFlag.MavModeFlagManualInputEnabled).toByte,
@@ -66,4 +72,31 @@ class RandomFlightPlan {
     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
+  )
+
 }
\ No newline at end of file