private void ProcessData()
{
// If the parser is already handling a message
if (_currentMessageLength != 0)
{
// If the reader has all the bytes we need
if (DataLength >= _currentMessageLength)
{
var bytes = _reader.ReadBytes(_currentMessageLength);
_currentMessageLength = 0;
MessageParsed?.Invoke(bytes);
ProcessData();
}
}
// Otherwise check if we can read the message's length
else if (DataLength >= 4)
{
_currentMessageLength = _reader.ReadInt32();
ProcessData();
}
// If we can't read anything anymore, might aswell clear the stream
else if (DataLength == 0)
{
_reader.Dispose();
_reader = new BinaryReader(new MemoryStream());
}
}
protected void ReadAllMessages()
{
if (!CanRead)
{
return;
}
Task <HttpMessage> messageTask = null;
//todo: dispose parser
try
{
var parser = new HttpParser(this);
messageTask = parser.ParseMessageAsync();
messageTask.Wait();
}
catch (System.IO.IOException e) { Debug.WriteLine(e); }
catch (System.AggregateException e) { Debug.WriteLine(e); }
if (messageTask?.IsCompletedSuccessfully ?? false)
{
MessageParsed?.Invoke(this, messageTask.Result);
ReadAllMessagesAsync();
}
}
private void ProcessData()
{
while (true)
{
// If the parser is already handling a message
// and if the reader has all the bytes we need
if (_currentMessageLength != 0 && DataLength >= _currentMessageLength)
{
var bytes = _reader.ReadBytes(_currentMessageLength);
_currentMessageLength = 0;
MessageParsed?.Invoke(bytes);
continue;
}
// Otherwise check if we can read the message's length
if (DataLength >= 4)
{
_currentMessageLength = NetworkUtils.BytesToIntBigEndian(_reader.ReadBytes(4));
continue;
}
// If we can't read anything anymore, might aswell clear the stream
if (DataLength == 0)
{
_reader.Dispose();
_reader = new BinaryReader(new MemoryStream());
}
break;
}
}
private void DecodeBuffered(byte[] buffer, long lengthOfFile, ref long lengthRead)
{
byte currentByte;
for (int i = 0; i < READ_BUFFER_SIZE; i++)
{
if (lengthRead++ == lengthOfFile - 1)
{
break;
}
currentByte = buffer[i];
if (currentByte == EQUALS_SIGN)
{
// Tag has been read in.
shouldCaptureTag = false;
}
else if (currentByte == SOH)
{
// Value has been read in.
if (!gotHeader)
{
gotHeader = header.IsHeaderEnd();
}
else
{
ProcessUpdate(value);
}
tag.Length = 0;
value.Length = 0;
shouldCaptureTag = true;
}
else if (currentByte == LINE_FEED)
{
BuildMessage();
MessageParsed.Invoke(message);
ResetDataBlockCounts();
gotHeader = false;
}
else
{
if (shouldCaptureTag)
{
tag.Contents[tag.Length] = currentByte;
tag.Length++;
}
else
{
value.Contents[value.Length] = currentByte;
value.Length++;
}
}
}
}
protected void OnMessageParsed(object sender, HttpMessage message)
{
MessageParsed?.Invoke(sender, message);
if (message is HttpRequest request)
{
RequestParsed?.Invoke(sender, request);
}
else if (message is HttpResponse response)
{
ResponseParsed?.Invoke(sender, response);
}
}
protected virtual void OnMessageParsed(byte[] checkedMessage)
{
MessageParsed?.Invoke(this, checkedMessage);
}
public void HandleBuffer(byte[] buffer, int readCount)
{
for (var idx = 0; idx < readCount; ++idx)
{
var value = (byte)buffer[idx];
if (_currentMessage != null)
{
_currentMessage.add_byte(value);
}
switch (_mavLinkParserState)
{
case MavLinkParserStates.ExpectingStx:
if (value == MAVLINK2_STX)
{
_currentMessage = new MAVLinkMessage();
_currentMessage.add_byte(value);
_mavLinkParserState = MavLinkParserStates.Expecting2PayloadLen;
}
if (value == 254)
{
_currentMessage = new MAVLinkMessage();
_currentMessage.add_byte(value);
_mavLinkParserState = MavLinkParserStates.ExpectingPayloadLen;
}
break;
case MavLinkParserStates.Expecting2PayloadLen:
_currentMessage.payloadlength = value;
_mavLinkParserState = MavLinkParserStates.Expecting2IncompatFlags;
break;
case MavLinkParserStates.Expecting2IncompatFlags:
_currentMessage.incompat_flags = value;
_mavLinkParserState = MavLinkParserStates.Expecting2CompatFlag;
break;
case MavLinkParserStates.Expecting2CompatFlag:
_currentMessage.compat_flags = value;
_mavLinkParserState = MavLinkParserStates.Expecting2Seq;
break;
case MavLinkParserStates.Expecting2Seq:
_currentMessage.seq = value;
_mavLinkParserState = MavLinkParserStates.Expecting2SysId;
break;
case MavLinkParserStates.Expecting2SysId:
_currentMessage.sysid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2ComponentId;
break;
case MavLinkParserStates.Expecting2ComponentId:
_currentMessage.compid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2MsgId07;
_currentMessage.msgid = 0x00;
break;
case MavLinkParserStates.Expecting2MsgId07:
_currentMessage.msgid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2MsgId815;
break;
case MavLinkParserStates.Expecting2MsgId815:
_mavLinkParserState = MavLinkParserStates.Expecting2MsgId1623;
_currentMessage.msgid += (UInt32)(value << 8);
break;
case MavLinkParserStates.Expecting2MsgId1623:
_mavLinkParserState = MavLinkParserStates.Expecting2TargetSysId;
_currentMessage.msgid += (UInt32)(value << 16);
break;
case MavLinkParserStates.Expecting2TargetSysId:
_currentMessage.targetsysid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2TargetComponentId;
break;
case MavLinkParserStates.Expecting2TargetComponentId:
_currentMessage.targetcomponentid = value;
_currentMessage.payload_index = 0;
_mavLinkParserState = MavLinkParserStates.ReadingPayload2;
break;
case MavLinkParserStates.ReadingPayload2:
if (_currentMessage.payload_index < _currentMessage.payloadlength)
{
_currentMessage.payload[_currentMessage.payload_index++] = value;
}
if (_currentMessage.payload_index == _currentMessage.payloadlength)
{
_mavLinkParserState = MavLinkParserStates.Expecting2CheckSum1;
}
break;
case MavLinkParserStates.Expecting2CheckSum1:
_currentMessage.crc16 = value;
_mavLinkParserState = MavLinkParserStates.Expecting2CheckSum2;
break;
case MavLinkParserStates.Expecting2CheckSum2:
_currentMessage.crc16 += (UInt16)(value << 8);
_mavLinkParserState = MavLinkParserStates.Expecting2Signature;
_currentMessage.sig = new byte[13];
_signatureIndex = 0;
break;
case MavLinkParserStates.Expecting2Signature:
_currentMessage.sig[_signatureIndex++] = value;
if (_signatureIndex == SIGNATURE_LEN)
{
Debug.WriteLine($"MAVLINK2 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16}");
_currentMessage.processBuffer(_currentMessage.buffer);
Debug.WriteLine($"MAVLINK2 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16}");
_mavLinkParserState = MavLinkParserStates.ExpectingStx;
_currentMessage = null;
}
break;
case MavLinkParserStates.ExpectingPayloadLen:
_currentMessage.payloadlength = value;
_mavLinkParserState = MavLinkParserStates.ExpectingSeqNumber;
break;
case MavLinkParserStates.ExpectingSeqNumber:
_currentMessage.seq = value;
_mavLinkParserState = MavLinkParserStates.ExpectingSystemId;
break;
case MavLinkParserStates.ExpectingSystemId:
_currentMessage.sysid = value;
_mavLinkParserState = MavLinkParserStates.ExpectingComponentId;
break;
case MavLinkParserStates.ExpectingComponentId:
_currentMessage.compid = value;
_mavLinkParserState = MavLinkParserStates.ExpectingMessageId;
break;
case MavLinkParserStates.ExpectingMessageId:
_currentMessage.msgid = value;
_mavLinkParserState = MavLinkParserStates.ReadingPayloadData;
break;
case MavLinkParserStates.ReadingPayloadData:
if (_currentMessage.payload_index < _currentMessage.payloadlength)
{
_currentMessage.payload[_currentMessage.payload_index++] = value;
}
if (_currentMessage.payload_index == _currentMessage.payloadlength)
{
_currentMessage.MessageInfo = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(_currentMessage.msgid);
_currentMessage.crc16Calc = MavlinkCRC.crc_calculate(_currentMessage.buffer, _currentMessage.buffer_index);
_currentMessage.crc16Calc = MavlinkCRC.crc_accumulate(_currentMessage.MessageInfo.crc, _currentMessage.crc16Calc);
_mavLinkParserState = MavLinkParserStates.ExpectingCRC1;
}
break;
case MavLinkParserStates.ExpectingCRC1:
_currentMessage.crc16 = value;
_mavLinkParserState = MavLinkParserStates.ExpectingCRC2;
break;
case MavLinkParserStates.ExpectingCRC2:
_currentMessage.crc16 += (UInt16)(value << 8);
if ((_currentMessage.crc16 >> 8) != (_currentMessage.crc16Calc >> 8) || (_currentMessage.crc16 & 0xff) != (_currentMessage.crc16Calc & 0xff))
{
//Debug.WriteLine($"INVALID CHECK SUM MAVLINK1 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16:x2} - {_currentMessage.crc16Calc:x2}");
}
else
{
MessageParsed?.Invoke(this, _currentMessage);
///Debug.WriteLine($"VALID CHECK SUM MAVLINK1 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16:x2} - {_currentMessage.crc16Calc:x2}");
}
_currentMessage.processBuffer(_currentMessage.buffer);
_currentMessage = null;
_mavLinkParserState = MavLinkParserStates.ExpectingStx;
break;
}
}
}
protected ApplicationParserBase(DataLinkParserBase <TIn> dataLinkParser)
{
_dataLinkParser = dataLinkParser;
_dataLinkParser.MessageParsed +=
(sender, checkedMessage) => MessageParsed?.Invoke(sender, ApplicationParse(checkedMessage));
}
