/// <summary>
/// 组装数据包
/// </summary>
/// <param name="packetData"></param>
/// <returns></returns>
private byte[] SendPacketLinkLayer(byte[] packetData)
{
/*
* Byte order:
*
* 0 Packet start sign
* 1 Payload length 0 - 255
* 2 Packet sequence 0 - 255
* 3 System ID 1 - 255
* 4 Component ID 0 - 255
* 5 Message ID 0 - 255
* 6 to (n+6) Data (0 - 255) bytes
* (n+7) to (n+8) Checksum (high byte, low byte)
*
*/
var outBytes = new byte[packetData.Length + 5];
outBytes[0] = MavlinkSettings.ProtocolMarker;
outBytes[1] = (byte)(packetData.Length - 3); // 3 bytes for sequence, id, msg type which this
// layer does not concern itself with
outBytes[2] = unchecked (txPacketSequence++);
int i;
for (i = 0; i < packetData.Length; i++)
{
outBytes[i + 3] = packetData[i];
}
// Check the CRC. Does not include the starting byte but does include the length
var crc1 = Mavlink_Crc.Calculate(outBytes, 1, (UInt16)(packetData.Length + 2));
if (MavlinkSettings.CrcExtra)
{
var possibleMsgId = outBytes[5];
var extra = MavLinkSerializer.Lookup[possibleMsgId];
crc1 = Mavlink_Crc.CrcAccumulate(extra.CrcExtra, crc1);
}
byte crc_high = (byte)(crc1 & 0xFF);
byte crc_low = (byte)(crc1 >> 8);
outBytes[i + 3] = crc_high;
outBytes[i + 4] = crc_low;
return(outBytes);
}
/// <summary>
/// 解析数据包并触发事件
/// </summary>
public void ParseBytes(byte[] newlyReceived)
{
uint i = 0;
// copy the old and new into a contiguous array
// This is pretty inefficient...
var bytesToProcess = new byte[newlyReceived.Length + leftovers.Length];
int j = 0;
for (i = 0; i < leftovers.Length; i++)
{
bytesToProcess[j++] = leftovers[i];
}
for (i = 0; i < newlyReceived.Length; i++)
{
bytesToProcess[j++] = newlyReceived[i];
}
i = 0;
// we are going to loop and decode packets until we use up the data
// at which point we will return. Hence one call to this method could
// result in multiple packet decode events
while (true)
{
// Hunt for the start char
int huntStartPos = (int)i;
while (i < bytesToProcess.Length && bytesToProcess[i] != MavlinkSettings.ProtocolMarker)
{
i++;
}
if (i == bytesToProcess.Length)
{
// No start byte found in all our bytes. Dump them, Exit.
leftovers = new byte[] { };
return;
}
if (i > huntStartPos)
{
// if we get here then are some bytes which this code thinks are
// not interesting and would be dumped. For diagnostics purposes,
// lets pop these bytes up in an event.
if (BytesUnused != null)
{
var badBytes = new byte[i - huntStartPos];
Array.Copy(bytesToProcess, huntStartPos, badBytes, 0, (int)(i - huntStartPos));
BytesUnused(this, new PacketCRCFailEventArgs(badBytes, bytesToProcess.Length - huntStartPos));
}
}
// We need at least the minimum length of a packet to process it.
// The minimum packet length is 8 bytes for acknowledgement packets without payload
// if we don't have the minimum now, go round again
if (bytesToProcess.Length - i < 8)
{
leftovers = new byte[bytesToProcess.Length - i];
j = 0;
while (i < bytesToProcess.Length)
{
leftovers[j++] = bytesToProcess[i++];
}
return;
}
/*
* Byte order:
*
* 0 Packet start sign
* 1 Payload length 0 - 255
* 2 Packet sequence 0 - 255
* 3 System ID 1 - 255
* 4 Component ID 0 - 255
* 5 Message ID 0 - 255
* 6 to (n+6) Data (0 - 255) bytes
* (n+7) to (n+8) Checksum (high byte, low byte) for v0.9, lowbyte, highbyte for 1.0
*
*/
UInt16 payLoadLength = bytesToProcess[i + 1];
// Now we know the packet length,
// If we don't have enough bytes in this packet to satisfy that packet lenghth,
// then dump the whole lot in the leftovers and do nothing else - go round again
if (payLoadLength > (bytesToProcess.Length - i - 8)) // payload + 'overhead' bytes (crc, system etc)
{
// back up to the start char for next cycle
j = 0;
leftovers = new byte[bytesToProcess.Length - i];
for (; i < bytesToProcess.Length; i++)
{
leftovers[j++] = bytesToProcess[i];
}
return;
}
i++;
// Check the CRC. Does not include the starting 'U' byte but does include the length
var crc1 = Mavlink_Crc.Calculate(bytesToProcess, (UInt16)(i), (UInt16)(payLoadLength + 5));
if (MavlinkSettings.CrcExtra)
{
var possibleMsgId = bytesToProcess[i + 4];
if (!MavLinkSerializer.Lookup.ContainsKey(possibleMsgId))
{
// we have received an unknown message. In this case we don't know the special
// CRC extra, so we have no choice but to fail.
// The way we do this is to just let the procedure continue
// There will be a natural failure of the main packet CRC
}
else
{
var extra = MavLinkSerializer.Lookup[possibleMsgId];
crc1 = Mavlink_Crc.CrcAccumulate(extra.CrcExtra, crc1);
}
}
byte crcHigh = (byte)(crc1 & 0xFF);
byte crcLow = (byte)(crc1 >> 8);
byte messageCrcHigh = bytesToProcess[i + 5 + payLoadLength];
byte messageCrcLow = bytesToProcess[i + 6 + payLoadLength];
if (messageCrcHigh == crcHigh && messageCrcLow == crcLow)
{
// This is used for data drop outs metrics, not packet windows
// so we should consider this here.
// We pass up to subscribers only as an advisory thing
var rxPacketSequence = bytesToProcess[++i];
i++;
var packet = new byte[payLoadLength + 3]; // +3 because we are going to send up the sys and comp id and msg type with the data
for (j = 0; j < packet.Length; j++)
{
packet[j] = bytesToProcess[i + j];
}
var debugArray = new byte[payLoadLength + 7];
Array.Copy(bytesToProcess, (int)(i - 3), debugArray, 0, debugArray.Length);
//OnPacketDecoded(packet, rxPacketSequence, debugArray);
//调用接收消息事件
ProcessPacketBytes(packet, rxPacketSequence);
PacketsReceived++;
// clear leftovers, just incase this is the last packet
leftovers = new byte[] { };
// advance i here by j to avoid unecessary hunting
// todo: could advance by j + 2 I think?
i += (uint)(j + 2);
}
else
{
var badBytes = new byte[i + 7 + payLoadLength];
Array.Copy(bytesToProcess, (int)(i - 1), badBytes, 0, payLoadLength + 7);
PacketFailedCRC?.Invoke(this, new PacketCRCFailEventArgs(badBytes, (int)(bytesToProcess.Length - i - 1)));
BadCrcPacketsReceived++;
}
}
}
