From 75dd37c70962c71f3096125f9da6fc658284961c Mon Sep 17 00:00:00 2001 From: Jakob Odersky Date: Sat, 15 Nov 2014 12:40:46 +0100 Subject: add further support for mavlink notes: * the protocol seems bloated for our needs * I'm not sure about the handling of primitive types in the protocol, especially the scalajs handling of numeric types of different widths seems cumbersome --- vfd-frontend/src/main/scala/org/mavlink/js.scala | 69 ++++++++++++++++++++++ .../src/main/scala/vfd/frontend/Main.scala | 10 +++- .../scala/vfd/frontend/ui/panels/Primary.scala | 16 +++-- .../scala/org/mavlink/enums/SystemStatus.scala | 30 ++++++++++ .../main/scala/org/mavlink/messages/Message.scala | 30 ---------- .../main/scala/org/mavlink/messages/message.scala | 60 +++++++++++++++++++ .../main/scala/org/mavlink/messages/messages.scala | 47 --------------- .../main/scala/org/mavlink/messages/payload.scala | 19 ++++++ .../src/main/scala/org/mavlink/payload.scala | 21 ------- .../scala/org/mavlink/BufferPayloadReader.scala | 36 ----------- vfd-uav/src/main/scala/org/mavlink/jvm.scala | 48 +++++++++++++++ vfd-uav/src/main/scala/org/mavlink/package.scala | 9 --- .../src/main/scala/vfd/uav/SerialConnection.scala | 13 +--- 13 files changed, 247 insertions(+), 161 deletions(-) create mode 100644 vfd-frontend/src/main/scala/org/mavlink/js.scala create mode 100644 vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala delete mode 100644 vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala create mode 100644 vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala delete mode 100644 vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala create mode 100644 vfd-mavlink/src/main/scala/org/mavlink/messages/payload.scala delete mode 100644 vfd-mavlink/src/main/scala/org/mavlink/payload.scala delete mode 100644 vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala create mode 100644 vfd-uav/src/main/scala/org/mavlink/jvm.scala delete mode 100644 vfd-uav/src/main/scala/org/mavlink/package.scala diff --git a/vfd-frontend/src/main/scala/org/mavlink/js.scala b/vfd-frontend/src/main/scala/org/mavlink/js.scala new file mode 100644 index 0000000..b9bcd2b --- /dev/null +++ b/vfd-frontend/src/main/scala/org/mavlink/js.scala @@ -0,0 +1,69 @@ +package org.mavlink + +import scala.language.implicitConversions +import scala.scalajs.js.JSConverters.array2JSRichGenTrav +import scala.scalajs.js.typedarray.DataView +import scala.scalajs.js.typedarray.Int8Array + +import org.mavlink.messages.PayloadReader +import org.mavlink.messages.PayloadWriter + +package object messages { + import org.mavlink.messages.PayloadReader + import org.mavlink.messages.PayloadWriter + + implicit def mkReader(s: Seq[Byte]) = new BufferedPayloadReader(s.toArray) + implicit def mkWriter(a: Array[Byte]) = new BufferedPayloadWriter(a) + +} + +package messages { + + class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader { + private val buffer = new Int8Array(payload.toJSArray) + private val view = new DataView(buffer.buffer) + private var pos = 0 + + def int8 = { + val r = view.getInt8(pos) + pos += 1 + r + } + def int16 = { + val r = view.getInt16(pos, true) + pos += 2 + r + } + def int32 = { + val r = view.getInt32(pos, true) + pos += 4 + r + } + def int64 = { + val l = int32 + val m = int32 + (m.asInstanceOf[Long] << 32) | l.asInstanceOf[Long] + } + def float = { + val r = view.getFloat32(pos, true) + pos += 4 + r + } + def double = { + val r = view.getFloat64(pos, true) + pos += 8 + r + } + + } + + class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter { + def int8(x: Byte) = ??? + def int16(x: Short) = ??? + def int32(x: Int) = ??? + def int64(x: Long) = ??? + def float(x: Float) = ??? + def double(x: Double) = ??? + } +} + diff --git a/vfd-frontend/src/main/scala/vfd/frontend/Main.scala b/vfd-frontend/src/main/scala/vfd/frontend/Main.scala index aa6d8d8..a6511d5 100644 --- a/vfd-frontend/src/main/scala/vfd/frontend/Main.scala +++ b/vfd-frontend/src/main/scala/vfd/frontend/Main.scala @@ -10,9 +10,9 @@ import rx.Rx import rx.Var import rx.Var import scalatags.JsDom.all._ - import vfd.frontend.ui.panels import vfd.frontend.util.Environment +import org.mavlink.messages.Message object Main { @@ -21,6 +21,12 @@ object Main { val remoteSystemId = args("remotesystemid").toInt val socket = new MavlinkSocket(socketUrl, remoteSystemId) + + val message = Rx{ + val p = socket.packet() + Message.unpack(p.messageId, p.payload) + } + val element = div(`class` := "container-fluid")( div(`class` := "row")( @@ -44,7 +50,7 @@ object Main { div(`class` := "col-xs-5")( div(`class` := "panel panel-default")( div(`class` := "panel-body")( - panels.Primary()))), + panels.Primary(message)))), div(`class` := "col-xs-3")( div(`class` := "panel panel-default")( div(`class` := "panel-body")( diff --git a/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala b/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala index 3b48375..71e0fbb 100644 --- a/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala +++ b/vfd-frontend/src/main/scala/vfd/frontend/ui/panels/Primary.scala @@ -2,15 +2,23 @@ package vfd.frontend.ui.panels import rx.core.Var import scalatags.JsDom.all.div +import vfd.frontend.util._ import vfd.frontend.ui.Components import vfd.frontend.util.Environment +import org.mavlink.messages.Message +import org.mavlink.messages.Attitude +import rx.core.Rx +import org.mavlink.messages.Pressure object Primary { - def apply()(implicit env: Environment) = { - val pitchRoll = Var((0.0, 0.0)) - val heading = Var(0.0) - val altitude = Var(0.0) + def apply(packet: Rx[Message])(implicit env: Environment) = { + val attitude = packet.only[Attitude] + val pressure = packet.only[Pressure] + + val pitchRoll = Rx{(attitude().pitch.toDouble, attitude().roll.toDouble)} + val heading = Rx{attitude().heading.toDouble} + val altitude = Rx{pressure().pressure.toDouble} div( Components.heading(heading, "33%"), diff --git a/vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala b/vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala new file mode 100644 index 0000000..69d0889 --- /dev/null +++ b/vfd-mavlink/src/main/scala/org/mavlink/enums/SystemStatus.scala @@ -0,0 +1,30 @@ +package org.mavlink.enums + +object SystemStatus extends Enumeration { + type SystemStatus = Value + + /** Uninitialized system, state is unknown. */ + val Uninitialized = Value(0) + + /** System is booting up. */ + val Booting = Value(1) + + /** System is calibrating and not flight-ready. */ + val Calibrating = Value(2) + + /** System is grounded and ready to fly. */ + val Standby = Value(3) + + /** System is active an might already be airborne. */ + val Active = Value(4) + + /** System is in a non-normal flight mode. It can however still navigate. */ + val Critical = Value(5) + + /** System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. */ + val Emergency = Value(6) + + /** System just initialized its power-down sequence, will shut down now. */ + val Poweroff = Value(7) + +} \ No newline at end of file diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala deleted file mode 100644 index 7e215c6..0000000 --- a/vfd-mavlink/src/main/scala/org/mavlink/messages/Message.scala +++ /dev/null @@ -1,30 +0,0 @@ -package org.mavlink.messages - -import org.mavlink.PayloadReader -import org.mavlink.PayloadWriter -import org.mavlink.Packet - -trait Message { - def pack(implicit mkWriter: Array[Byte] => PayloadWriter): Array[Byte] -} - -trait MessageCompanion[M <: Message] { - def unpack(bytes: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader): M -} - -object Message { - - def unpack(packet: Packet)(implicit mkReader: Seq[Byte] => PayloadReader) = packet.messageId match { - case 0 => Heartbeat.unpack(packet.payload) - case 109 => RadioStatus.unpack(packet.payload) - } - - /* - private val msg = Heartbeat(23,1,2,4,5,6) - def foo = { - val spec = msg.pickle.value - (spec.id, spec.payload.mkString("(", ",", ")")) - }*/ - -} - \ No newline at end of file diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala new file mode 100644 index 0000000..86691b9 --- /dev/null +++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala @@ -0,0 +1,60 @@ +package org.mavlink.messages + +import org.mavlink.Packet + +import org.mavlink.enums.SystemStatus +import org.mavlink.enums.SystemStatus._ + +sealed trait Message +case class Heartbeat(`type`: Byte, autopilot: Byte, baseMode: Byte, customMode: Int, systemStatus: SystemStatus, mavlinkVersion: Byte) extends Message +case class RadioStatus(rssi: Byte, remoteRssi: Byte, txBuf: Byte, noise: Byte, remoteNoise: Byte, rxErrors: Short, fixed: Short) extends Message +case class Attitude(time: Int, roll: Float, pitch: Float, heading: Float) extends Message +case class Pressure(time: Int, pressure: Float, diffPressure: Float, temperature: Short) extends Message +case class Unknown(id: Int, payload: Seq[Byte]) extends Message + +object Message { + + def unpack(id: Byte, payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader): Message = { + val r = mkReader(payload) + + id.toInt match { + case 0 => + val cm = r.int32 + Heartbeat(r.int8, r.int8, r.int8, cm, SystemStatus(r.int8), r.int8) + case 28 => + Pressure(r.int32, r.float, r.float, r.int16) + case 30 => + Attitude(r.int32, r.float, r.float, r.float) + case 109 => + val re = r.int16 + val fi = r.int16 + RadioStatus(r.int8, r.int8, r.int8, r.int8, r.int8, re, fi) + + case u => Unknown(u, payload) + } + } + + def pack(m: Message)(implicit mkWriter: Array[Byte] => PayloadWriter): (Byte, Seq[Byte]) = { + val (id, size) = m match { + case _: Heartbeat => (0, 9) + case _: RadioStatus => (109, 9) + case u: Unknown => (u.id, u.payload.length) + } + + val arr = new Array[Byte](size) + val w = mkWriter(arr) + + m match { + case Heartbeat(tp, a, b, cm, s, mv) => w.int32(cm); w.int8(tp); w.int8(b); w.int8(s.id.toByte); w.int8(mv); + case RadioStatus(r, rr, tx, n, rn, rx, fi) => w.int16(rx); w.int16(fi); w.int8(r); w.int8(rr); w.int8(tx); w.int8(n); w.int8(rn); + case Unknown(_, payload) => for (p <- payload) w.int8(p) + } + + (id.toByte, arr) + + } + + + +} + \ No newline at end of file diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala deleted file mode 100644 index 3f519dc..0000000 --- a/vfd-mavlink/src/main/scala/org/mavlink/messages/messages.scala +++ /dev/null @@ -1,47 +0,0 @@ -package org.mavlink.messages - -import org.mavlink.PayloadWriter -import org.mavlink.PayloadReader - -case class Heartbeat(customMode: Int, `type`: Byte, autopilot: Byte, baseMode: Byte, systemStatus: Byte, mavlinkVersion: Byte) extends Message { - def pack(implicit mkWriter: Array[Byte] => PayloadWriter): Array[Byte] = { - val arr = new Array[Byte](9) - val writer = mkWriter(arr) - writer.writeInt32(customMode) - writer.writeInt8(`type`) - writer.writeInt8(autopilot) - writer.writeInt8(baseMode) - writer.writeInt8(systemStatus) - writer.writeInt8(mavlinkVersion) - arr - } -} - -object Heartbeat extends MessageCompanion[Heartbeat]{ - def unpack(payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader) = { - val reader = mkReader(payload) - Heartbeat(reader.nextInt32, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8) - } -} - -case class RadioStatus(rxErrors: Short, fixed: Short, rssi: Byte, remoteRssi: Byte, txBuf: Byte, noise: Byte, remoteNoise: Byte) extends Message { - def pack(implicit mkWriter: Array[Byte] => PayloadWriter): Array[Byte] = { - val arr = new Array[Byte](9) - val writer = mkWriter(arr) - writer.writeInt16(rxErrors) - writer.writeInt16(fixed) - writer.writeInt8(rssi) - writer.writeInt8(remoteRssi) - writer.writeInt8(txBuf) - writer.writeInt8(noise) - writer.writeInt8(remoteNoise) - arr - } -} - -object RadioStatus extends MessageCompanion[RadioStatus]{ - def unpack(payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader) = { - val reader = mkReader(payload) - RadioStatus(reader.nextInt16, reader.nextInt16, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8, reader.nextInt8) - } -} \ No newline at end of file diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/payload.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/payload.scala new file mode 100644 index 0000000..76076b2 --- /dev/null +++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/payload.scala @@ -0,0 +1,19 @@ +package org.mavlink.messages + +trait PayloadReader { + def int8: Byte + def int16: Short + def int32: Int + def int64: Long + def float: Float + def double: Double +} + +trait PayloadWriter { + def int8(x: Byte) + def int16(x: Short) + def int32(x: Int) + def int64(x: Long) + def float(x: Float) + def double(x: Double) +} \ No newline at end of file diff --git a/vfd-mavlink/src/main/scala/org/mavlink/payload.scala b/vfd-mavlink/src/main/scala/org/mavlink/payload.scala deleted file mode 100644 index ff31e9d..0000000 --- a/vfd-mavlink/src/main/scala/org/mavlink/payload.scala +++ /dev/null @@ -1,21 +0,0 @@ -package org.mavlink - -trait PayloadReader { - def nextInt8: Byte - def nextInt16: Short - def nextInt32: Int - def nextInt64: Long - def nextFloat: Float - def nextDouble: Double - def nextChar: Char -} - -trait PayloadWriter { - def writeInt8(x: Byte) - def writeInt16(x: Short) - def writeInt32(x: Int) - def writeInt64(x: Long) - def writeFloat(x: Float) - def writeDouble(x: Double) - def writeChar(x: Char) -} \ No newline at end of file diff --git a/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala b/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala deleted file mode 100644 index 9f13bff..0000000 --- a/vfd-uav/src/main/scala/org/mavlink/BufferPayloadReader.scala +++ /dev/null @@ -1,36 +0,0 @@ -package org.mavlink - -import java.nio.ByteBuffer -import java.nio.ByteOrder - -class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader { - private val buffer = ByteBuffer.wrap(payload) - - //mavlink uses little endian - buffer.order(ByteOrder.LITTLE_ENDIAN) - - def nextInt8 = buffer.get() - def nextInt16 = buffer.getShort() - def nextInt32 = buffer.getInt() - def nextInt64 = buffer.getLong() - def nextFloat = buffer.getFloat() - def nextDouble = buffer.getDouble() - def nextChar = buffer.getChar() - -} - -class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter { - private val buffer = ByteBuffer.wrap(payload) - - //mavlink uses little endian - buffer.order(ByteOrder.LITTLE_ENDIAN) - - def writeInt8(x: Byte) = buffer.put(x) - def writeInt16(x: Short) = buffer.putShort(x) - def writeInt32(x: Int) = buffer.putInt(x) - def writeInt64(x: Long) = buffer.putLong(x) - def writeFloat(x: Float) = buffer.putFloat(x) - def writeDouble(x: Double) = buffer.putDouble(x) - def writeChar(x: Char) = buffer.putChar(x) - -} \ No newline at end of file diff --git a/vfd-uav/src/main/scala/org/mavlink/jvm.scala b/vfd-uav/src/main/scala/org/mavlink/jvm.scala new file mode 100644 index 0000000..ac711be --- /dev/null +++ b/vfd-uav/src/main/scala/org/mavlink/jvm.scala @@ -0,0 +1,48 @@ +package org.mavlink + +import scala.language.implicitConversions + +import java.nio.ByteBuffer +import java.nio.ByteOrder + +package object messages { + import org.mavlink.messages.PayloadReader + import org.mavlink.messages.PayloadWriter + + implicit def mkReader(s: Seq[Byte]) = new BufferedPayloadReader(s.toArray) + implicit def mkWriter(a: Array[Byte]) = new BufferedPayloadWriter(a) + +} + +package messages { + + class BufferedPayloadReader(payload: Array[Byte]) extends PayloadReader { + private val buffer = ByteBuffer.wrap(payload) + + //mavlink uses little endian + buffer.order(ByteOrder.LITTLE_ENDIAN) + + def int8 = buffer.get() + def int16 = buffer.getShort() + def int32 = buffer.getInt() + def int64 = buffer.getLong() + def float = buffer.getFloat() + def double = buffer.getDouble() + + } + + class BufferedPayloadWriter(payload: Array[Byte]) extends PayloadWriter { + private val buffer = ByteBuffer.wrap(payload) + + //mavlink uses little endian + buffer.order(ByteOrder.LITTLE_ENDIAN) + + def int8(x: Byte) = buffer.put(x) + def int16(x: Short) = buffer.putShort(x) + def int32(x: Int) = buffer.putInt(x) + def int64(x: Long) = buffer.putLong(x) + def float(x: Float) = buffer.putFloat(x) + def double(x: Double) = buffer.putDouble(x) + } +} + diff --git a/vfd-uav/src/main/scala/org/mavlink/package.scala b/vfd-uav/src/main/scala/org/mavlink/package.scala deleted file mode 100644 index 87a8add..0000000 --- a/vfd-uav/src/main/scala/org/mavlink/package.scala +++ /dev/null @@ -1,9 +0,0 @@ -package org - -import scala.language.implicitConversions - -package object mavlink { - - implicit def mkReader(bytes: Seq[Byte]): PayloadReader = new BufferedPayloadReader(bytes.toArray) - -} \ 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 306d3dc..cfdf60f 100644 --- a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala +++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala @@ -63,15 +63,7 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String } - val parser = new Parser( - pckt => try { - log.info(Message.unpack(pckt).toString()) - } catch { - case err: MatchError => - log.info("unknown message: " + pckt.payload.map(_.formatted("%02x").mkString(" "))) - } - ) - + def _opened(operator: ActorRef): Receive = { case Terminated(`operator`) => @@ -83,9 +75,6 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String context become closed case Serial.Received(bstr) => - for (b <- bstr) { - parser.push(b) - } sendAll(Connection.Received(bstr)) case Connection.Send(bstr) => -- cgit v1.2.3