1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
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
)
}
|
