public UavDataparser(UAVObjectManager dataManager)
{
rxTmpBuffer = new ByteBuffer(4);
rxBuffer = new ByteBuffer(uavConsts.MAX_PAYLOAD_LENGTH);
_stats = new ComStats();
rxState = commState.SYNC;
rxPacketLength = 0;
_objMgr = dataManager;
}
public void processInputByte(byte data)
{
_stats.Bytes++;
rxPacketLength++; // update packet byte count
switch (rxState)
{
case commState.SYNC:
if (data != uavConsts.SYNC_VAL)
{
if (preBufferIdx < prebuffer.Length)
{
prebuffer[preBufferIdx++] = data;
}
break;
}
// Initialize and update CRC
rxCS = CRC.updateCRC(0, data);
rxPacketLength = 1;
rxState = commState.TYPE;
break;
case commState.TYPE:
// Update CRC
rxCS = CRC.updateCRC(rxCS, data);
if ((data & uavConsts.TYPE_MASK) != uavConsts.TYPE_VER)
{
if (preBufferIdx + 1 < prebuffer.Length)
{
prebuffer[preBufferIdx++] = uavConsts.SYNC_VAL;
prebuffer[preBufferIdx++] = data;
}
//Debug.WriteLine( "Unknown UAVTalk type: {0:X}", data);
rxState = commState.SYNC;
break;
}
if (preBufferIdx == prebuffer.Length)
{
last_timestamp = BitConverter.ToUInt32(prebuffer, 0);
UInt64 packetSize_pre = BitConverter.ToUInt64(prebuffer, 4);
}
preBufferIdx = 0;
rxType = data;
//Debug.WriteLine("Received packet type: {0:X}", data);
packetSize = 0;
rxState = commState.SIZE;
rxCount = 0;
break;
case commState.SIZE:
// Update CRC
rxCS = CRC.updateCRC(rxCS, data);
if (rxCount == 0)
{
packetSize += data;
rxCount++;
break;
}
packetSize += (data << 8) & 0xff00;
if (packetSize < uavConsts.MIN_HEADER_LENGTH
|| packetSize > uavConsts.MAX_HEADER_LENGTH + uavConsts.MAX_PAYLOAD_LENGTH)
{ // incorrect
// packet
// size
rxState = commState.SYNC;
break;
}
rxCount = 0;
rxState = commState.OBJID;
rxTmpBuffer.position(0);
break;
case commState.OBJID:
// Update CRC
rxCS = CRC.updateCRC(rxCS, data);
rxTmpBuffer.put(rxCount++, data);
if (rxCount < 4)
break;
// Search for object, if not found reset state machine
rxObjId = rxTmpBuffer.getInt(0);
// Because java treats ints as only signed we need to do this manually
UAVObject rxObj = _objMgr.getObject(rxObjId);
if (rxObj == null)
{
Debug.WriteLine("Unknown ID: " + rxObjId);
_stats.Errors++;
rxState = commState.SYNC;
break;
}
// Determine data length
if (rxType == uavConsts.TYPE_OBJ_REQ || rxType == uavConsts.TYPE_ACK || rxType == uavConsts.TYPE_NACK)
rxLength = 0;
else
rxLength = rxObj.getNumBytes();
// Check length and determine next state
if (rxLength >= uavConsts.MAX_PAYLOAD_LENGTH)
{
Debug.WriteLine("Greater than max payload length");
_stats.Errors++;
rxState = commState.SYNC;
break;
}
// Check if this is a single instance object (i.e. if the
// instance ID field is coming next)
if (rxObj.isSingleInstance())
{
// If there is a payload get it, otherwise receive checksum
if (rxLength > 0)
rxState = commState.DATA;
else
rxState = commState.CS;
rxInstId = 0;
rxCount = 0;
}
else
{
rxState = commState.INSTID;
rxCount = 0;
}
break;
case commState.INSTID:
// Update CRC
rxCS = CRC.updateCRC(rxCS, data);
rxTmpBuffer.put(rxCount++, data);
if (rxCount < 2)
break;
rxInstId = rxTmpBuffer.getShort(0);
rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (rxLength > 0)
rxState = commState.DATA;
else
rxState = commState.CS;
break;
case commState.DATA:
// Update CRC
rxCS = CRC.updateCRC(rxCS, data);
rxBuffer.put(rxCount++, data);
if (rxCount < rxLength)
break;
rxState = commState.CS;
rxCount = 0;
break;
case commState.CS:
// The CRC byte
rxCSPacket = data;
if (rxCS != rxCSPacket)
{ // packet error - faulty CRC
Debug.WriteLine("Bad crc");
_stats.Errors++;
rxState = commState.SYNC;
break;
}
if (rxPacketLength != (packetSize + 1))
{ // packet error -
// mismatched packet
// size
Debug.WriteLine("Bad size");
_stats.Errors++;
rxState = commState.SYNC;
break;
}
rxBuffer.position(0);
if(onObjectReceived != null)
onObjectReceived(rxType, rxObjId, rxInstId, last_timestamp, rxBuffer);
_stats.ObjectBytes += rxLength;
_stats.Objects++;
rxState = commState.SYNC;
break;
default:
Debug.WriteLine("Bad state");
rxState = commState.SYNC;
_stats.Errors++;
break;
}
}