add battery status message

2 files changed, 26 insertions, 2 deletions

diff --git a/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala b/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala
index bb72f07..cf8694a 100644
--- a/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala
+++ b/vfd-mavlink/src/main/scala/org/mavlink/messages/message.scala
@@ -10,6 +10,8 @@ case class Heartbeat(`type`: Byte, autopilot: Byte, baseMode: Byte, customMode:
 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 Battery(id: Byte, function: Byte, `type`: Byte, temperature: Short,
+    voltages: Seq[Short], current: Short, currentConsumed: Int, energyConsumed: Int, remaining: Byte) extends Message
 case class Unknown(id: Int, payload: Seq[Byte]) extends Message
 
 object Message {
@@ -17,7 +19,7 @@ object Message {
   def unpack(id: Byte,  payload: Seq[Byte])(implicit mkReader: Seq[Byte] => PayloadReader): Message = {
     val r = mkReader(payload)
     
-    id.toInt match {
+    id & 0xff match {
       case 0 => 
         val cm = r.int32
         Heartbeat(r.int8, r.int8, r.int8, cm, SystemStatus(r.int8), r.int8)
@@ -29,6 +31,17 @@ object Message {
         val re = r.int16
         val fi = r.int16 
         RadioStatus(r.int8, r.int8, r.int8, r.int8, r.int8, re, fi)
+      case 147 =>
+        val cc = r.int32
+        val ec = r.int32
+        val t = r.int16
+        val v = for (i <- 0 until 10) yield r.int16
+        val c = r.int16
+        val id = r.int8
+        val fct = r.int8
+        val tpe = r.int8
+        val rm = r.int8
+        Battery(id, fct, tpe, t, v, c, cc, ec, rm)
       
       case u => Unknown(u, payload)
     }
diff --git a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
index 1cb1fbd..7fe9d23 100644
--- a/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
+++ b/vfd-uav/src/main/scala/vfd/uav/SerialConnection.scala
@@ -15,6 +15,7 @@ import akka.actor.actorRef2Scala
 import akka.io.IO
 import akka.util.ByteString
 import akka.actor.ActorLogging
+import org.mavlink.messages.Battery
 
 class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String, settings: SerialSettings) extends Actor with ActorLogging with Connection {
   import context._
@@ -65,7 +66,17 @@ class SerialConnection(id: Byte, heartbeat: Option[FiniteDuration], port: String
 
   }
   
-  val parser = new Parser(pckt => println(Message.unpack(pckt.messageId, pckt.payload)))
+  val last = new collection.mutable.Queue[Int]
+  
+  val parser = new Parser(pckt => 
+    Message.unpack(pckt.messageId, pckt.payload) match {
+      case b: Battery =>
+        last enqueue b.voltages(0)
+        if (last.size > 20) last.dequeue
+        println("batt: " + last.sum / last.size) 
+      case _ => ()//println(pckt.messageId)
+    }
+  )
   
  
   def _opened(operator: ActorRef): Receive = {