public static bool TryReadDatagram(byte[] receivedBytes, out UdpTelemetryDatagram datagram)
{
datagram = null;
using (var stream = new MemoryStream(receivedBytes))
{
using (var reader = new BinaryReader(stream))
{
if (!reader.TryReadStruct(out DatagramHeader header))
{
Debug.WriteLine("Could not read DatagramHeader", "Serialization Error");
return(false);
}
if (header.Version != 1)
{
Debug.WriteLine($"Unknown datagram version {header.Version}", "Serialization Error");
return(false);
}
if (reader.BytesRemaining() != header.PayloadLength)
{
Debug.WriteLine($"Invalid datagram payload size. Expected {header.PayloadLength} bytes, got {reader.BytesRemaining()}", "Serialization Error");
return(false);
}
datagram = new UdpTelemetryDatagram
{
Header = header,
Payload = reader.ReadBytes(header.PayloadLength)
};
return(true);
}
}
}
public byte[] WriteTelemetry(ITelemetryEvent telemetryEvent, byte[] encryptionKey = null)
{
var t = (TelemetryEvent)telemetryEvent;
if (t == null)
{
throw new SerializationException("Unknown telemetry event type");
}
byte[] payload;
switch (t.Message)
{
case UavPositionReport r:
payload = UavPositionReportSerializer.WriteReport(r);
break;
default:
throw new SerializationException($"Unknown message type: {t.MessageType}");
}
var wrapper = new TelemetryWrapper(AutpMessageTypes.UavPosition, payload);
byte[] wrapperBytes = TelemetryWrapperSerializer.WriteWrapper(wrapper);
wrapperBytes = encryptionKey != null
? EncryptBytes(encryptionKey, t.SequenceNumber, wrapperBytes)
: EncryptBytes(t.TelemetryId, t.SequenceNumber, wrapperBytes);
var datagram = new UdpTelemetryDatagram(t.TelemetryId, t.SequenceNumber, wrapperBytes, true);
return(TelemetryDatagramSerializer.WriteDatagram(datagram));
}
public static byte[] WriteDatagram(UdpTelemetryDatagram datagram)
{
using (var stream = new MemoryStream())
{
using (var writer = new BinaryWriter(stream))
{
try
{
writer.WriteStruct(datagram.Header);
writer.Write(datagram.Payload);
}
catch (Exception e)
{
throw new SerializationException(e.ToString());
}
}
return(stream.ToArray());
}
}
