diff options
Diffstat (limited to 'mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala')
-rw-r--r-- | mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala | 103 |
1 files changed, 103 insertions, 0 deletions
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala b/mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala new file mode 100644 index 0000000..1e5431e --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/mock/RandomFlightPlan.scala @@ -0,0 +1,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 + 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 + ) + +} |