public override object ReadJson(JsonReader reader, Type objectType, object existingValue, JsonSerializer serializer)
{
JObject jObj = JObject.Load(reader);
// Validate the required fields
var system_id = jObj.GetValue("system_id");
var component_id = jObj.GetValue("component_id");
var message_name = jObj.GetValue("message_name");
var payload = jObj.GetValue("payload");
if (system_id == null)
{
throw new JsonSerializationException("system_id is a required message field");
}
if (component_id == null)
{
throw new JsonSerializationException("component_id is a required message field");
}
if (message_name == null)
{
throw new JsonSerializationException("message_name is a required message field");
}
if (payload == null)
{
throw new JsonSerializationException("payload is a required message field");
}
MavnetMessage msg = new MavnetMessage();
msg.system_id = system_id.Value <byte>();
msg.component_id = component_id.Value <byte>();
msg.message_name = message_name.Value <string>();
// Lookup the Mavlink info object by name
var messageInfo = MAVLink.MAVLINK_MESSAGE_INFOS.FirstOrDefault(m => m.name.Equals(msg.message_name));
if (messageInfo.type == null)
{
throw new JsonSerializationException("unrecognized message_name: " + msg.message_name);
}
// Deserialize the payload
msg.message_id = messageInfo.msgid;
msg.payload = Activator.CreateInstance(messageInfo.type);
serializer.Populate(payload.CreateReader(), msg.payload);
return(msg);
}
static void Main(string[] args)
{
const string DATA_FILE = "/Users/thadthompson/dev/uvdl/thirdparty/genwork/test_packets_v2.bin";
const string TEST_OUT_FILE = "/Users/thadthompson/dev/uvdl/thirdparty/genwork/test_output_packets_v2.bin";
Console.WriteLine("Starting MAVLink test");
var data = System.IO.File.ReadAllBytes(DATA_FILE);
// Create a parser with logging output to Console.WriteLine
MavnetParser parser = new MavnetParser(Console.WriteLine);
parser.PacketReceived += (sender, e) =>
{
Console.WriteLine("Received Packet: " + e);
};
var messages = parser.ParseBytes(data);
using (var fout = System.IO.File.OpenWrite(TEST_OUT_FILE))
{
foreach (var msg in messages)
{
// Round trip through JSON
string js = msg.toJson();
MavnetMessage jsmsg = Newtonsoft.Json.JsonConvert.DeserializeObject <MavnetMessage>(js);
fout.Write(jsmsg.toBytes());
}
}
Console.WriteLine("Testing incremental message parsing");
for (int i = 0; i < data.Length; i++)
{
var messages2 = parser.ParseBytes(new byte[] { data[i] });
if (messages2.Count > 0)
{
Console.WriteLine("Parsed message at {0} bytes: {1}", i, messages2[0].toJson());
}
}
// string TEST_MESSAGE = "{\"system_id\":123,\"component_id\":212,\"message_name\":\"HEARTBEAT\",\"payload\":{\"custom_mode\":4,\"type\":1,\"autopilot\":2,\"base_mode\":3,\"system_status\":5,\"mavlink_version\":1}}";
// //MavnetMessage jsmsg = Newtonsoft.Json.JsonConvert.DeserializeObject<MavnetMessage>(TEST_MESSAGE);
// Console.WriteLine(jsmsg.toJson());
}
/// <summary>
/// Add an array of bytes to the parser. Any newly parsed packets are returned.
/// </summary>
public IList <MavnetMessage> ParseBytes(byte [] buf)
{
var newMessages = new List <MavnetMessage>();
// Combine any previous data we were holding with the new buffer
byte[] parseBuf;
if (_residual == null)
{
parseBuf = buf;
}
else
{
parseBuf = new byte[_residual.Length + buf.Length];
Buffer.BlockCopy(_residual, 0, parseBuf, 0, _residual.Length);
Buffer.BlockCopy(buf, 0, parseBuf, _residual.Length, buf.Length);
}
_residual = null;
// Start parsing
int idx = 0;
while (idx < parseBuf.Length)
{
// Search for a packet header
for (; idx < parseBuf.Length && parseBuf[idx] != MAVLink.MAVLINK_STX && parseBuf[idx] != MAVLink.MAVLINK_STX_MAVLINK1; idx++)
{
trashBytes++;
}
// Check for data left
if (idx == parseBuf.Length)
{
break;
}
// Create a slice in the parse buffer for our potential packet
Span <byte> pkt = new Span <byte>(parseBuf, idx, parseBuf.Length - idx);
bool isMavlinkV2 = pkt[0] == MAVLink.MAVLINK_STX;
// Check to see if we have the header
int headerlength = isMavlinkV2 ? MAVLink.MAVLINK_CORE_HEADER_LEN : MAVLink.MAVLINK_CORE_HEADER_MAVLINK1_LEN;
if (pkt.Length < headerlength)
{
break;
}
// packet length
int packetLength = pkt[1] + headerlength + 1 + 2; // payload + protocol marker + header + checksum
// MAVLink v2 may to add a signature block
if (isMavlinkV2 && (pkt[2] & MAVLink.MAVLINK_IFLAG_SIGNED) > 0)
{
packetLength += MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN;
}
// Check to see if we have a full packet
if (pkt.Length < packetLength)
{
break;
}
// Slice out the packet bytes and try to parse it into a MavLink packet
idx += packetLength;
Span <byte> fullPacket = pkt.Slice(0, packetLength);
MavnetMessage msg = null;
try
{
msg = ParseSinglePacket(fullPacket);
}
catch (Exception ex)
{
log("Unable to parse packet: " + ex.Message);
parseErrors++;
}
if (msg != null)
{
newMessages.Add(msg);
// Raise event when packet is received.
PacketReceived?.Invoke(this, msg);
}
}
// Anything leftover is residual that we'll keep to try to reassmble with more data when it arrives.
if (idx < parseBuf.Length)
{
_residual = new byte[parseBuf.Length - idx];
Buffer.BlockCopy(parseBuf, idx, _residual, 0, parseBuf.Length - idx);
}
return(newMessages);
}
// Parse a single Mavnet message from a slice of a byte array
private MavnetMessage ParseSinglePacket(Span <byte> pkt)
{
MavnetMessage msg = new MavnetMessage();
// Parse out the header
if (pkt[0] == MAVLink.MAVLINK_STX)
{
msg.is_mavlink_v2 = true;
msg.payload_length = pkt[1];
msg.incompat_flags = pkt[2];
msg.compat_flags = pkt[3];
msg.sequence = pkt[4];
msg.system_id = pkt[5];
msg.component_id = pkt[6];
msg.message_id = (uint)((pkt[9] << 16) + (pkt[8] << 8) + pkt[7]);
if ((msg.incompat_flags & MAVLink.MAVLINK_IFLAG_SIGNED) > 0)
{
Span <byte> sig_block = pkt.Slice(pkt.Length - MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN);
msg.signature_link_id = sig_block[0];
msg.signature_timestamp = BitConverter.ToUInt64(sig_block.Slice(1, 6).ToArray(), 0);
msg.signature = sig_block.Slice(7).ToArray();
}
}
else
{
msg.is_mavlink_v2 = false;
msg.payload_length = pkt[1];
msg.sequence = pkt[2];
msg.system_id = pkt[3];
msg.component_id = pkt[4];
msg.message_id = pkt[5];
}
// Lookup the message type info.
MAVLink.message_info messageInfo;
if (!_messageInfoTable.TryGetValue(msg.message_id, out messageInfo))
{
log("Unknown MAVLink message ID: " + msg.message_id);
unknownMessageTypes++;
return(null);
}
msg.message_name = messageInfo.name;
// Check the CRC
var headerLen = msg.is_mavlink_v2 ? MAVLink.MAVLINK_CORE_HEADER_LEN : MAVLink.MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var crc1 = headerLen + msg.payload_length + 1;
var crc2 = crc1 + 1;
ushort crc16 = (ushort)((pkt[crc2] << 8) + pkt[crc1]);
ushort calcCrc = MAVLink.MavlinkCRC.crc_calculate(pkt.ToArray(), pkt.Length - 2);
calcCrc = MAVLink.MavlinkCRC.crc_accumulate(messageInfo.crc, calcCrc);
if (crc16 != calcCrc)
{
log(string.Format("Bad message CRC for {0} message: expected: {1}, found: {2} ", msg.message_name, calcCrc, crc16));
crcErrors++;
return(null);
}
if (msg.is_mavlink_v2)
{
// Mavlink V2
if (msg.payload_length == messageInfo.length)
{
msg.payload = messageInfo.deserializer(pkt.Slice(MAVLink.MAVLINK_NUM_HEADER_BYTES, msg.payload_length));
}
else
{
// Handle zero byte payload truncation
// see: https://mavlink.io/en/guide/serialization.html#payload_truncation
Span <byte> paddedPayload = stackalloc byte[messageInfo.length];
pkt.Slice(MAVLink.MAVLINK_NUM_HEADER_BYTES, msg.payload_length).CopyTo(paddedPayload);
msg.payload = messageInfo.deserializer(paddedPayload);
}
}
else
{
// Mavlink V1
msg.payload = messageInfo.deserializer(pkt.Slice(MAVLink.MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1, msg.payload_length));
}
return(msg);
}
// Parse a single Mavnet message from a slice of a byte array
private MavnetMessage ParseSinglePacket(Span <byte> pkt)
{
MavnetMessage msg = new MavnetMessage();
// Parse out the header
if (pkt[0] == MAVLink.MAVLINK_STX)
{
msg.is_mavlink_v2 = true;
msg.payload_length = pkt[1];
msg.incompat_flags = pkt[2];
msg.compat_flags = pkt[3];
msg.sequence = pkt[4];
msg.system_id = pkt[5];
msg.component_id = pkt[6];
msg.message_id = (uint)((pkt[9] << 16) + (pkt[8] << 8) + pkt[7]);
if ((msg.incompat_flags & MAVLink.MAVLINK_IFLAG_SIGNED) > 0)
{
Span <byte> sig_block = pkt.Slice(pkt.Length - MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN);
msg.signature_link_id = sig_block[0];
msg.signature_timestamp = BitConverter.ToUInt64(sig_block.Slice(1, 6));
msg.signature = sig_block.Slice(7).ToArray();
}
}
else
{
msg.is_mavlink_v2 = false;
msg.payload_length = pkt[1];
msg.sequence = pkt[2];
msg.system_id = pkt[3];
msg.component_id = pkt[4];
msg.message_id = pkt[5];
}
// Lookup the message type info.
MAVLink.message_info messageInfo;
if (!_messageInfoTable.TryGetValue(msg.message_id, out messageInfo))
{
log("Unknown MAVLink message ID: " + msg.message_id);
unknownMessageTypes++;
return(null);
}
msg.message_name = messageInfo.name;
// Check the CRC
var headerLen = msg.is_mavlink_v2 ? MAVLink.MAVLINK_CORE_HEADER_LEN : MAVLink.MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var crc1 = headerLen + msg.payload_length + 1;
var crc2 = crc1 + 1;
ushort crc16 = (ushort)((pkt[crc2] << 8) + pkt[crc1]);
ushort calcCrc = MAVLink.MavlinkCRC.crc_calculate(pkt.ToArray(), pkt.Length - 2);
calcCrc = MAVLink.MavlinkCRC.crc_accumulate(messageInfo.crc, calcCrc);
if (crc16 != calcCrc)
{
log(string.Format("Bad message CRC for {0} message: expected: {1}, found: {2} ", msg.message_name, calcCrc, crc16));
crcErrors++;
return(null);
}
// Deserialize the payload
try
{
object payload = Activator.CreateInstance(messageInfo.type);
if (msg.is_mavlink_v2)
{
MavlinkUtil.ByteArrayToStructure(pkt.ToArray(), ref payload, MAVLink.MAVLINK_NUM_HEADER_BYTES, msg.payload_length);
}
else
{
MavlinkUtil.ByteArrayToStructure(pkt.ToArray(), ref payload, 6, msg.payload_length);
}
msg.payload = payload;
}
catch (Exception ex)
{
log(ex.ToString());
return(null);
}
return(msg);
}
