private void ProcessSendQueue(object state)
{
byte seq = 0;
using (FileStream s = new FileStream(_recordFileName, FileMode.Create))
{
using (BinaryWriter writer = new BinaryWriter(s))
{
while (true)
{
UasMessage msg;
if (mSendQueue.TryDequeue(out msg))
{
//SendMavlinkMessage(msg);
writer.Write(MavLinkPacket.GetBytesForMessage(
msg, MavlinkSystemId, MavlinkComponentId, seq++, MavLinkGenericPacketWalker.PacketSignalByte));
}
else
{
// Queue is empty, sleep until signalled
mSendSignal.WaitOne();
if (!mIsActive)
{
break;
}
}
}
}
}
isEndSignal.Set();
}
// __ Deserialization _________________________________________________
/*
* 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
*
*/
public static MavLinkPacket Deserialize(BinaryReader s, byte payloadLength)
{
MavLinkPacket result = new MavLinkPacket()
{
PayLoadLength = (payloadLength == 0) ? s.ReadByte() : payloadLength,
InCompatFlags = s.ReadByte(),
CompatFlats = s.ReadByte(),
PacketSequenceNumber = s.ReadByte(),
SystemId = s.ReadByte(),
ComponentId = s.ReadByte(),
//MessageId = s.ReadUInt32(),
};
byte[] ids = new byte[3];
ids[0] = s.ReadByte();
ids[1] = s.ReadByte();
ids[2] = s.ReadByte();
result.MessageId = ids[0] | (uint)(ids[1] << 8) | (uint)(ids[2] << 16);
// Read the payload instead of deserializing so we can validate CRC.
result.Payload = s.ReadBytes(result.PayLoadLength);
result.Checksum1 = s.ReadByte();
result.Checksum2 = s.ReadByte();
if (result.IsValidCrc())
{
result.DeserializeMessage();
}
return(result);
}
// __ Impl ____________________________________________________________
private void Parse(object state)
{
try
{
using (FileStream s = new FileStream(mLogFileName, FileMode.Open))
{
using (BinaryReader reader = new BinaryReader(s))
{
while (true)
{
SyncStream(reader);
MavLinkPacket packet = MavLinkPacket.Deserialize(reader, 0);
if (packet.IsValid)
{
HandlePacketReceived(this, packet);
}
}
}
}
}
catch (EndOfStreamException)
{
}
HandleReceptionEnded(this);
}
public static UInt16 GetPacketCrc(MavLinkPacket p)
{
UInt16 crc = X25CrcSeed;
crc = X25CrcAccumulate(p.PayLoadLength, crc);
crc = X25CrcAccumulate(p.InCompatFlags, crc);
crc = X25CrcAccumulate(p.CompatFlats, crc);
crc = X25CrcAccumulate(p.PacketSequenceNumber, crc);
crc = X25CrcAccumulate(p.SystemId, crc);
crc = X25CrcAccumulate(p.ComponentId, crc);
uint id = p.MessageId;
crc = X25CrcAccumulate((byte)(id & 0xFF), crc);
crc = X25CrcAccumulate((byte)(id >> 8 & 0xFF), crc);
crc = X25CrcAccumulate((byte)(id >> 16 & 0xFF), crc);
//crc = X25CrcAccumulate((byte)(id >> 24 & 0xFF), crc);
for (int i = 0; i < p.Payload.Length; ++i)
{
crc = X25CrcAccumulate(p.Payload[i], crc);
}
crc = X25CrcAccumulate(UasSummary.GetCrcExtraForId(p.MessageId), crc);
return(crc);
}
public static byte[] GetBytesForMessage(
UasMessage msg, byte systemId, byte componentId, byte sequenceNumber, byte signalMark)
{
MavLinkPacket p = MavLinkPacket.GetPacketForMessage(
msg, systemId, componentId, sequenceNumber);
int bufferSize = p.GetPacketSize();
if (signalMark != 0)
{
bufferSize++;
}
byte[] result = new byte[bufferSize];
using (MemoryStream s = new MemoryStream(result))
{
using (BinaryWriter w = new BinaryWriter(s))
{
if (signalMark != 0)
{
w.Write(signalMark);
}
p.Serialize(w);
}
}
return(result);
}
// __ Serialization ___________________________________________________
public static MavLinkPacket GetPacketForMessage(
UasMessage msg, byte systemId, byte componentId, byte sequenceNumber)
{
MavLinkPacket result = new MavLinkPacket()
{
SystemId = systemId,
InCompatFlags = 0,
CompatFlats = 0,
ComponentId = componentId,
PacketSequenceNumber = sequenceNumber,
MessageId = msg.MessageId,
Message = msg
};
using (MemoryStream ms = new MemoryStream())
{
using (BinaryWriter bw = new BinaryWriter(ms))
{
msg.SerializeBody(bw);
}
result.Payload = ms.ToArray();
result.PayLoadLength = (byte)result.Payload.Length;
result.UpdateCrc();
}
return(result);
}
//public abstract void SendRawPacket(MavLinkPacket packet);
// __ MavLink events __________________________________________________
protected void HandlePacketReceived(object sender, MavLinkPacket e)
{
if (OnPacketReceived != null)
{
OnPacketReceived(sender, e);
}
}
protected void NotifyPacketDiscarded(MavLinkPacket packet)
{
if (packet == null || PacketDiscarded == null)
{
return;
}
PacketDiscarded(this, packet);
}
/// <summary>
/// Generates the buffer bytes to be sent on the wire for given message.
/// </summary>
/// <param name="msg"></param>
/// <param name="systemId"></param>
/// <param name="componentId"></param>
/// <param name="includeSignalMark">Whether to include the Packet signal in the buffer or not.</param>
/// <param name="sequenceNumber">A sequence number for the message, if needed.</param>
/// <returns></returns>
public byte[] SerializeMessage(
UasMessage msg, byte systemId, byte componentId,
bool includeSignalMark, byte sequenceNumber = 1)
{
byte mark = includeSignalMark ? PacketSignalByte : (byte)0;
return(MavLinkPacket.GetBytesForMessage(
msg, systemId, componentId, sequenceNumber, mark));
}
// __ Impl ____________________________________________________________
private void PacketProcessingWorker(object state)
{
using (BinaryReader reader = MavLinkPacket.GetBinaryReader(mProcessStream))
{
while (true)
{
SyncStream(reader);
MavLinkPacket packet = MavLinkPacket.Deserialize(reader, 0);
if (packet.IsValid)
{
NotifyPacketReceived(packet);
}
else
{
NotifyPacketDiscarded(packet);
}
}
}
}
