From c1eed835318e4be95a59eae644aaf2d52e9f974e Mon Sep 17 00:00:00 2001
From: Jakob Odersky <jodersky@gmail.com>
Date: Fri, 3 Apr 2015 17:41:54 +0200
Subject: add motor load and distance to dummy flight plan

---
 vfd-main/conf/application.conf                     |  5 +-
 .../src/main/scala/vfd/uav/MockConnection.scala    |  2 +
 .../main/scala/vfd/uav/mock/RandomFlightPlan.scala | 59 +++++++++++++++++-----
 3 files changed, 52 insertions(+), 14 deletions(-)

diff --git a/vfd-main/conf/application.conf b/vfd-main/conf/application.conf
index c2026a3..182b39d 100644
--- a/vfd-main/conf/application.conf
+++ b/vfd-main/conf/application.conf
@@ -84,4 +84,7 @@ uav.connection.heartbeat=2000
 uav.serial.port="/dev/ttyUSB0"
 uav.serial.baud=115200
 uav.serial.two_stop_bits=false
-uav.serial.parity=0
\ No newline at end of file
+uav.serial.parity=0
+
+# Mock connectopn specific
+uav.mock.remote_system_id=42
\ 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 d05d824..8543fab 100644
--- a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala
@@ -41,6 +41,8 @@ class MockConnection(localSystemId: Byte, localComponentId: Byte, remoteSystemId
     //send messages
     scheduleMessage(0.05.seconds)(plan.position)
     scheduleMessage(0.05.seconds)(plan.attitude)
+    scheduleMessage(0.05.seconds)(plan.motors)
+    scheduleMessage(0.1.seconds)(plan.distance)
     scheduleMessage(2.seconds)(plan.heartbeat)
     
     //simulate noisy line
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
-- 
cgit v1.2.3