diff options
Diffstat (limited to 'mavigator-uav/src')
5 files changed, 435 insertions, 0 deletions
diff --git a/mavigator-uav/src/main/scala/mavigator/uav/Connection.scala b/mavigator-uav/src/main/scala/mavigator/uav/Connection.scala new file mode 100644 index 0000000..7399657 --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/Connection.scala @@ -0,0 +1,73 @@ +package mavigator.uav + +import akka.actor.ActorLogging +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 + +/** Protocol definition. */ +object Connection { + + /** Any messages received and transmitted by actors implementing this protocol. */ + sealed trait Message + + /** Messages emitted by actors implementing this protocol. */ + trait Event extends Message + + /** Received data from the uav (or any other systems on the link) */ + case class Received(bstr: ByteString) extends Event + + /** The connection closed or could not be opened */ + case class Closed(message: String) extends Event + + /** Messages that can be received by actors implementing this protocol. */ + trait Command extends Message + + /** Register the sender to be notified on events */ + case object Register extends Command + + case class Unregister(client: ActorRef) extends Command + + /** Send given bytes out to the uav (or any other systems on the link) */ + case class Send(bstr: ByteString) extends Command + +} + +/** Common behavior of connection actors. */ +trait Connection { myself: Actor with ActorLogging => + + private val _clients = new ArrayBuffer[ActorRef] + + /** Current clients that should be notified on incoming messages. */ + def clients = _clients.toSeq + + /** Adds a client to the client list and acquires a deathwatch. */ + protected def register(client: ActorRef): Unit = { + _clients += client + myself.context.watch(client) + myself.log.info("Client registered {}", client) + } + + /** Remove client and release deathwatch. */ + protected def unregister(client: ActorRef): Unit = if (clients contains client) { + _clients -= client + myself.context.unwatch(client) + myself.log.info("Client unregistered {}", client) + } + + /** Sends a message to all registered clients. */ + protected def sendAll(msg: Any): Unit = clients foreach (_ ! msg) + + /** Common registration behavior. Manages the messages `Register` and `Terminated` by + * registering and unregistering clients. */ + protected def handleRegistration: Receive = { + case Connection.Register => register(sender) + case Terminated(client) => unregister(client) + case other => myself.log.warning("Unknown message: {}", other) + } + +} diff --git a/mavigator-uav/src/main/scala/mavigator/uav/MavlinkUtil.scala b/mavigator-uav/src/main/scala/mavigator/uav/MavlinkUtil.scala new file mode 100644 index 0000000..3756dd6 --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/MavlinkUtil.scala @@ -0,0 +1,46 @@ +package mavigator.uav + +import org.mavlink.Assembler +import org.mavlink.Packet +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 => + + /** Mavlink system ID of this connection. */ + val systemId: Byte + + /** Mavlink component ID of this connection. */ + val componentId: Byte + + /** Assembler for creating packets originating from this connection. */ + private lazy val assembler = new Assembler(systemId, componentId) + + /** Assembles a message into a bytestring representing a packet sent from this connection. */ + protected def assemble(message: Message): ByteString = { + val (messageId, payload) = Message.pack(message) + val packet: Packet = assembler.assemble(messageId, payload) + ByteString(packet.toArray) + } + + /** 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)*/ + case _ => () + }) + + /** Parser for messages coming from the uav. */ + protected val incoming: Parser = new Parser(pckt => + log.debug("incoming message: " + Message.unpack(pckt.messageId, pckt.payload))) + +} diff --git a/mavigator-uav/src/main/scala/mavigator/uav/MockConnection.scala b/mavigator-uav/src/main/scala/mavigator/uav/MockConnection.scala new file mode 100644 index 0000000..5949a3a --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/MockConnection.scala @@ -0,0 +1,77 @@ +package mavigator.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 scala.concurrent.duration._ +import org.mavlink.messages.Message +import mock.RandomFlightPlan + +class MockConnection( + localSystemId: Byte, + localComponentId: Byte, + remoteSystemId: Byte, + prescaler: Int +) + extends Actor with ActorLogging with Connection with MavlinkUtil { + + import context._ + + override val systemId = remoteSystemId + override val componentId = remoteSystemId + + val plan = new RandomFlightPlan + + 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 * prescaler, 0.01.seconds * prescaler) { plan.tick(0.01) } + + //send messages + scheduleMessage(0.1.seconds * prescaler)(plan.position) + scheduleMessage(0.05.seconds * prescaler)(plan.attitude) + scheduleMessage(0.05.seconds * prescaler)(plan.motors) + scheduleMessage(0.1.seconds * prescaler)(plan.distance) + scheduleMessage(1.seconds)(plan.heartbeat) + + //simulate noisy line + scheduleBytes(0.3.seconds * prescaler)(MockPackets.invalidCrc) + scheduleBytes(1.5.seconds * prescaler)(MockPackets.invalidOverflow) + } + + override def receive = handleRegistration + +} + +object MockConnection { + def apply(systemId: Byte, componentId: Byte, remoteSystemId: Byte, prescaler: Int = 1) = + Props(classOf[MockConnection], systemId, componentId, remoteSystemId, prescaler) +} + +object MockPackets { + val invalidCrc = Array(254, 1, 123, 13, 13).map(_.toByte) + val invalidOverflow = { + val data = Array.fill[Byte](Packet.MaxPayloadLength + 100)(42) + data(0) = -2 + data(1) = 2 + data(1) = -1 + data + } +} diff --git a/mavigator-uav/src/main/scala/mavigator/uav/SerialConnection.scala b/mavigator-uav/src/main/scala/mavigator/uav/SerialConnection.scala new file mode 100644 index 0000000..24c70a1 --- /dev/null +++ b/mavigator-uav/src/main/scala/mavigator/uav/SerialConnection.scala @@ -0,0 +1,136 @@ +package mavigator.uav + +import java.util.concurrent.TimeUnit.MILLISECONDS + +import scala.concurrent.duration.FiniteDuration + +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 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 +import akka.actor.Props +import akka.actor.Terminated +import akka.actor.actorRef2Scala +import akka.io.IO + +class SerialConnection( + val systemId: Byte, + val componentId: Byte, + heartbeatInterval: Option[FiniteDuration], + port: String, + settings: SerialSettings) extends Actor with ActorLogging with Connection with MavlinkUtil { + + import context._ + + override def preStart() = heartbeatInterval foreach { interval => + context.system.scheduler.schedule(interval, interval) { + 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 + ) + ) + ) + } + } + + def _closed: Receive = { + + case Connection.Register => + register(sender) + IO(Serial) ! Serial.Open(port, settings) + context become opening + + case Connection.Send(_) => + IO(Serial) ! Serial.Open(port, settings) + context become opening + + } + + def _opening: Receive = { + + case Serial.CommandFailed(cmd: Serial.Open, reason) => + sendAll(Connection.Closed(reason.toString)) + context become closed + + case Serial.Opened(_) => + context watch (sender) + context become opened(sender) + + case Connection.Send(_) => () // ignore + /* + * During opening, any outgoing messages are discarded. + * By using some kind of message stashing, maybe messages could be treated + * once the port has been opened. However, in such a case failure also needs + * to be considered, thus complicating the protocol. Since opening is typically + * quite fast and since mavlink uses heartbeats and acknowledgements (in certain + * circumstances) anyway, keeping messages is not really required. + */ + + } + + def _opened(operator: ActorRef): Receive = { + + case Terminated(`operator`) => + sendAll(Connection.Closed("Serial connection crashed.")) + context become closed + + case Serial.Closed => + sendAll(Connection.Closed("Serial connection was closed.")) + context become closed + + case Serial.Received(bstr) => + sendAll(Connection.Received(bstr)) + incoming.push(bstr) + + case Connection.Send(bstr) => + outgoing.push(bstr) + //no sending is currently enabled + + } + + def closed = _closed orElse handleRegistration + def opening = _opening orElse handleRegistration + def opened(op: ActorRef) = _opened(op) orElse handleRegistration + override def receive = closed + +} + +object SerialConnection { + def apply( + systemId: Byte, + componentId: Byte, + heartbeatInterval: Int, + port: String, + baud: Int, + tsb: Boolean, + parity: Int): Props = { + + val settings = SerialSettings( + baud, + 8, + tsb, + parity match { + case 0 => Parity.None + case 1 => Parity.Odd + case 2 => Parity.Even + }) + val hb = if (heartbeatInterval == 0) None else Some(FiniteDuration(heartbeatInterval, MILLISECONDS)) + + Props(classOf[SerialConnection], systemId, componentId, hb, port, settings) + } +} 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 + ) + +} |