blob: 97068e64ddddc347df09d8a279d46c54c155c348 (
plain) (
tree)
|
|
@(__context: Context)@_header(__context)
package org.mavlink
import scala.collection.mutable.ArrayBuilder
object Parser {
/** Internal parser states. */
object States {
sealed trait State
case object Idle extends State
case object GotStx extends State
case object GotLength extends State
case object GotSeq extends State
case object GotSysId extends State
case object GotCompId extends State
case object GotMsgId extends State
case object GotCrc1 extends State
case object GotPayload extends State
}
/** Errors that may occur while receiving data. */
object Errors {
sealed trait Error
case object CrcError extends Error
case object OverflowError extends Error
}
}
/**
* A parser to divide byte streams into mavlink packets.
* A parser is created with receiver and error functions and bytes are then fed into it. Once
* a valid packet has been received (or an error encountered), the receiver (or error) functions
* are called.
*
* @@param receiver called when a valid packet has been received
* @@param error called when invalid data was received
*/
class Parser(receiver: Packet => Unit, error: Parser.Errors.Error => Unit = _ => ()) {
import Parser._
private var state: States.State = States.Idle
private object inbound {
var length: Int = 0
var seq: Byte = 0
var systemId: Byte = 0
var componentId: Byte = 0
var messageId: Byte = 0
var currentLength: Int = 0
val payload = ArrayBuilder.make[Byte]
var crc: Crc = new Crc()
}
/**
* Parses a byte as part of an incoming MAVLink message. May result
* in calling receiver or error function.
*/
def push(c: Byte): Unit = {
import States._
state match {
case Idle =>
if (c == Packet.Stx) {
state = GotStx
}
case GotStx =>
inbound.crc = new Crc()
inbound.length = (c & 0xff)
inbound.crc = inbound.crc.accumulate(c)
state = GotLength
case GotLength =>
inbound.seq = c;
inbound.crc = inbound.crc.accumulate(c)
state = GotSeq
case GotSeq =>
inbound.systemId = c
inbound.crc = inbound.crc.accumulate(c)
state = GotSysId
case GotSysId =>
inbound.componentId = c
inbound.crc = inbound.crc.accumulate(c)
state = GotCompId
case GotCompId =>
inbound.messageId = c
inbound.crc = inbound.crc.accumulate(c)
if (inbound.length == 0) {
state = GotPayload
} else {
state = GotMsgId
inbound.payload.clear()
inbound.currentLength = 0
}
case GotMsgId =>
inbound.payload += c
inbound.currentLength += 1
inbound.crc = inbound.crc.accumulate(c)
if(inbound.currentLength > Packet.MaxPayloadLength) {
state = Idle
error(Errors.OverflowError)
} else if (inbound.currentLength >= inbound.length) {
state = GotPayload
}
case GotPayload =>
inbound.crc = inbound.crc.accumulate(Packet.extraCrc(inbound.messageId))
if (c != inbound.crc.lsb) {
state = Idle
if (c == Packet.Stx) {
state = GotStx
}
error(Errors.CrcError)
} else {
state = GotCrc1
}
case GotCrc1 =>
if (c != inbound.crc.msb) {
state = Idle
if (c == Packet.Stx) {
state = GotStx
}
error(Errors.CrcError)
} else {
val packet = Packet(
inbound.seq,
inbound.systemId,
inbound.componentId,
inbound.messageId,
inbound.payload.result)
state = Idle
receiver(packet)
}
}
}
/**
* Parses a sequence of bytes.
*/
def push(bytes: Traversable[Byte]): Unit = for (b <- bytes) push(b)
}
|