diff options
Diffstat (limited to 'vfd-uav/src/main/scala/vfd')
5 files changed, 130 insertions, 57 deletions
diff --git a/vfd-uav/src/main/scala/vfd/uav/Connection.scala b/vfd-uav/src/main/scala/vfd/uav/Connection.scala index 38e1836..223a787 100644 --- a/vfd-uav/src/main/scala/vfd/uav/Connection.scala +++ b/vfd-uav/src/main/scala/vfd/uav/Connection.scala @@ -1,16 +1,12 @@ package vfd.uav import scala.collection.mutable.ArrayBuffer + import akka.actor.Actor import akka.actor.ActorRef import akka.actor.Terminated import akka.actor.actorRef2Scala import akka.util.ByteString -import org.mavlink.Assembler -import org.mavlink.messages.Message -import org.mavlink.Parser -import org.mavlink.Packet -import akka.actor.ActorLogging /** Protocol definition. */ object Connection { @@ -39,7 +35,7 @@ trait Connection { myself: Actor => /** Adds a client to the client list and acquires a deathwatch. */ protected def register(client: ActorRef) = { - _clients += client; + _clients += client myself.context.watch(client) } diff --git a/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala b/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala index edc8484..8a8c364 100644 --- a/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala +++ b/vfd-uav/src/main/scala/vfd/uav/MavlinkUtil.scala @@ -1,14 +1,13 @@ package vfd.uav import org.mavlink.Assembler -import akka.util.ByteString import org.mavlink.Packet -import akka.actor.Actor -import org.mavlink.messages.Ping -import org.mavlink.messages.Ack -import org.mavlink.messages.Message import org.mavlink.Parser +import org.mavlink.messages.Message + +import akka.actor.Actor import akka.actor.ActorLogging +import akka.util.ByteString /** Provides utilities for actors representing a mavlink connection. */ trait MavlinkUtil { myself: Actor with ActorLogging => @@ -31,10 +30,12 @@ trait MavlinkUtil { myself: Actor with ActorLogging => /** Parser for messages being sent to the uav. */ protected val outgoing: Parser = new Parser(packet => Message.unpack(packet.messageId, packet.payload) match { + //TODO handle ping + /* case Ping(`systemId`, `componentId`) => val message = Ack(packet.systemId, packet.componentId) val data = assemble(message) - self ! Connection.Received(data) + self ! Connection.Received(data)*/ case _ => () }) diff --git a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala index 801c4ac..1051607 100644 --- a/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala +++ b/vfd-uav/src/main/scala/vfd/uav/MockConnection.scala @@ -3,30 +3,49 @@ 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 org.mavlink.messages._ -import org.mavlink.Packet +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 MessageInterval = FiniteDuration(100, MILLISECONDS) - def randomData: ByteString = Random.nextInt(MockPackets.Messages + 1) match { - case 0 => ByteString(MockPackets.invalid) - case i => assemble(MockPackets.message(i - 1)) - } + val plan = new RandomFlightPlan - override def preStart() = context.system.scheduler.schedule(MessageInterval, MessageInterval) { - sendAll(Received(randomData)) + 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))) + } + + 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 @@ -38,36 +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 - } - - def heartbeat = Heartbeat(0) - def motor = Motor(r.nextByte(101), r.nextByte(101), r.nextByte(101), r.nextByte(101)) - def attitude = Attitude((r.nextInt(160) - 80).toShort, (r.nextInt(160) - 80).toShort, r.nextInt(360).toShort) - def power = Power(Random.nextInt(12000).toShort) - - val Messages = 4 - def message(i: Int) = i match { - case 0 => heartbeat - case 1 => motor - case 2 => attitude - case 3 => power - } - 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/SerialConnection.scala b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala index 4b2e71a..1756cac 100644 --- a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala +++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala @@ -1,16 +1,19 @@ package vfd.uav import java.util.concurrent.TimeUnit.MILLISECONDS + import scala.concurrent.duration.FiniteDuration -import org.mavlink.Packet -import org.mavlink.Parser -import org.mavlink.Assembler + +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 org.mavlink.messages.Ack -import org.mavlink.messages.Message + import com.github.jodersky.flow.Parity import com.github.jodersky.flow.Serial import com.github.jodersky.flow.SerialSettings + import akka.actor.Actor import akka.actor.ActorLogging import akka.actor.ActorRef @@ -18,8 +21,6 @@ import akka.actor.Props import akka.actor.Terminated import akka.actor.actorRef2Scala import akka.io.IO -import akka.util.ByteString -import org.mavlink.messages.Ping class SerialConnection( val systemId: Byte, @@ -32,7 +33,18 @@ class SerialConnection( override def preStart() = heartbeatInterval foreach { interval => context.system.scheduler.schedule(interval, interval) { - self ! Connection.Send(assemble(Heartbeat(0))) + self ! Connection.Send( + assemble( + Heartbeat( + MavType.MavTypeGeneric.toByte, + MavAutopilot.MavAutopilotGeneric.toByte, + 0, //no base mode + 0, //no custom mode + MavState.MavStateActive.toByte, + 0 //TODO properly implement read-only fields + ) + ) + ) } } 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 |