From 0d664478218ba29a83216088eb3b327bfe61c239 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 20 Mar 2019 18:47:27 -0700 Subject: [PATCH 1/3] merge --- MavLinkCom/src/impl/MavLinkConnectionImpl.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index f51827606f..a7f2b75f88 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -259,6 +259,16 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg) if (msg.msgid == MavLinkTelemetry::kMessageId) { msglen = 28; // mavlink doesn't know about our custom telemetry message. } + + if (len != msglen) { + if (mavlink1) { + throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen)); + } + else { + // mavlink2 supports trimming the payload of trailing zeros so the messages + // are variable length as a result. + } + } len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen); msg.len = len; @@ -336,6 +346,11 @@ void MavLinkConnectionImpl::joinLeftSubscriber(std::shared_ptrpImpl->supports_mavlink2_ = true; + } remote->sendMessage(msg); } @@ -406,7 +421,7 @@ void MavLinkConnectionImpl::readPackets() if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { // then this is a mavlink 1 message - } else { + } else if (!supports_mavlink2_) { // then this mavlink sender supports mavlink 2 supports_mavlink2_ = true; } From 1e21620d1de55f0e3afa6b8e1e1fbbbfb4641150 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Thu, 21 Mar 2019 13:42:07 -0700 Subject: [PATCH 2/3] Fix log viewer so it can handle mavlink2 messages. --- .../Networking/Mavlink/MavlinkChannel.cs | 157 +++++++++++++++--- 1 file changed, 135 insertions(+), 22 deletions(-) diff --git a/LogViewer/Networking/Mavlink/MavlinkChannel.cs b/LogViewer/Networking/Mavlink/MavlinkChannel.cs index 25459a78f9..ddb8a66d4d 100644 --- a/LogViewer/Networking/Mavlink/MavlinkChannel.cs +++ b/LogViewer/Networking/Mavlink/MavlinkChannel.cs @@ -10,15 +10,22 @@ namespace Microsoft.Networking.Mavlink { public class MavLinkMessage { + public const byte Mavlink1Stx = 254; + public const byte Mavlink2Stx = 253; + public byte Magic { get; set; } - public MAVLink.MAVLINK_MSG_ID MsgId { get; set; } public byte Length { get; set; } - public byte ComponentId { get; set; } - public byte SystemId { get; set; } + public byte IncompatFlags { get; set; } // flags that must be understood + public byte CompatFlags { get; set; } // flags that can be ignored if not understood public byte SequenceNumber { get; set; } + public byte SystemId { get; set; } + public byte ComponentId { get; set; } + public MAVLink.MAVLINK_MSG_ID MsgId { get; set; } public ushort Crc { get; set; } public UInt64 Time { get; set; } public byte[] Payload { get; set; } + public byte[] Signature { get; set; } + public byte ProtocolVersion { get; set; } public object TypedPayload { get; set; } public override string ToString() @@ -111,10 +118,23 @@ private ushort crc_calculate(byte[] buffer, int len) ushort crcTmp = 0xffff; // X25_INIT_CRC; // Start with the MAVLINK_CORE_HEADER_LEN bytes. crcTmp = crc_accumulate((byte)this.Length, crcTmp); + if (this.Magic == MavLinkMessage.Mavlink2Stx) + { + crcTmp = crc_accumulate((byte)this.IncompatFlags, crcTmp); + crcTmp = crc_accumulate((byte)this.CompatFlags, crcTmp); + } crcTmp = crc_accumulate((byte)this.SequenceNumber, crcTmp); crcTmp = crc_accumulate((byte)this.SystemId, crcTmp); crcTmp = crc_accumulate((byte)this.ComponentId, crcTmp); + crcTmp = crc_accumulate((byte)this.MsgId, crcTmp); + if (this.Magic == MavLinkMessage.Mavlink2Stx) + { + byte b = (byte)((uint)this.MsgId >> 8); + crcTmp = crc_accumulate(b, crcTmp); + b = (byte)((uint)this.MsgId >> 16); + crcTmp = crc_accumulate(b, crcTmp); + } crcTmp = crc_accumulate(buffer, len, crcTmp); return crc_accumulate(crc_extra, crcTmp); } @@ -172,6 +192,8 @@ internal void Serialize() this.Length = (byte)block.Length; } } + + } public class MavlinkChannel @@ -228,11 +250,17 @@ void ReceiveThread() { byte[] buffer = new byte[1]; + const byte MAVLINK_IFLAG_SIGNED = 0x01; + const int MAVLINK_SIGNATURE_BLOCK_LEN = 13; MavLinkMessage msg = null; ReadState state = ReadState.Init; int payloadPos = 0; ushort crc = 0; - + int signaturePos = 0; + uint msgid = 0; + int msgIdPos = 0; + int MaxPayloadLength = 255; + bool messageComplete = false; System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch(); watch.Start(); @@ -248,22 +276,61 @@ void ReceiveThread() switch (state) { case ReadState.Init: - if (b == MagicMarker) + if (b == MavLinkMessage.Mavlink1Stx) + { + state = ReadState.GotMagic; + msg = new MavLinkMessage(); + msg.Time = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds + msg.Magic = MavLinkMessage.Mavlink1Stx; + payloadPos = 0; + msgIdPos = 0; + msgid = 0; + crc = 0; + messageComplete = false; + } + else if (b == MavLinkMessage.Mavlink2Stx) { state = ReadState.GotMagic; msg = new MavLinkMessage(); msg.Time = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds - msg.Magic = MagicMarker; + msg.Magic = MavLinkMessage.Mavlink2Stx; payloadPos = 0; + msgIdPos = 0; + msgid = 0; crc = 0; + messageComplete = false; } break; case ReadState.GotMagic: - msg.Length = b; - msg.Payload = new byte[msg.Length]; - state = ReadState.GotLength; + if (b > MaxPayloadLength) + { + state = ReadState.Init; + } + else + { + if (msg.Magic == MavLinkMessage.Mavlink1Stx) + { + msg.IncompatFlags = 0; + msg.CompatFlags = 0; + state = ReadState.GotCompatFlags; + } + else + { + msg.Length = b; + msg.Payload = new byte[msg.Length]; + state = ReadState.GotLength; + } + } break; case ReadState.GotLength: + msg.IncompatFlags = b; + state = ReadState.GotIncompatFlags; + break; + case ReadState.GotIncompatFlags: + msg.CompatFlags = b; + state = ReadState.GotCompatFlags; + break; + case ReadState.GotCompatFlags: msg.SequenceNumber = b; state = ReadState.GotSequenceNumber; break; @@ -276,15 +343,37 @@ void ReceiveThread() state = ReadState.GotComponentId; break; case ReadState.GotComponentId: - msg.MsgId = (MAVLink.MAVLINK_MSG_ID)b; - if (msg.Length == 0) + if (msg.Magic == MavLinkMessage.Mavlink1Stx) { - // done! - state = ReadState.GotPayload; + msg.MsgId = (MAVLink.MAVLINK_MSG_ID)b; + if (msg.Length == 0) + { + // done! + state = ReadState.GotPayload; + } + else + { + state = ReadState.GotMessageId; + } } else { - state = ReadState.GotMessageId; + // msgid is 24 bits + switch(msgIdPos) + { + case 0: + msgid = b; + break; + case 1: + msgid |= ((uint)b << 8); + break; + case 2: + msgid |= ((uint)b << 16); + msg.MsgId = (MAVLink.MAVLINK_MSG_ID)msgid; + state = ReadState.GotMessageId; + break; + } + msgIdPos++; } break; @@ -311,16 +400,39 @@ void ReceiveThread() } else { - // try and deserialize the payload. - msg.Deserialize(); - if (MessageReceived != null) + if ((msg.IncompatFlags & MAVLINK_IFLAG_SIGNED) == MAVLINK_IFLAG_SIGNED) { - MessageReceived(this, msg); + signaturePos = 0; + msg.Signature = new byte[MAVLINK_SIGNATURE_BLOCK_LEN]; + state = ReadState.GetSignature; } + else + { + messageComplete = true; + } + } + break; + case ReadState.GetSignature: + msg.Signature[signaturePos++] = b; + if (signaturePos == MAVLINK_SIGNATURE_BLOCK_LEN) + { + // todo: check the signature. + messageComplete = true; } - state = ReadState.Init; break; } + + if (messageComplete) + { + // try and deserialize the payload. + msg.Deserialize(); + if (MessageReceived != null) + { + MessageReceived(this, msg); + } + // reset for next message. + state = ReadState.Init; + } } } @@ -336,17 +448,18 @@ enum ReadState { Init, GotMagic, + GotIncompatFlags, + GotCompatFlags, GotLength, GotSequenceNumber, GotSystemId, GotComponentId, GotMessageId, GotPayload, - GotCrc1 + GotCrc1, + GetSignature } - const int MagicMarker = 254; - } From fadb35440c4506bd7cb389cf8080f89354754f7a Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Thu, 21 Mar 2019 13:46:17 -0700 Subject: [PATCH 3/3] publish new version. --- LogViewer/LogViewer/LogViewer.csproj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 5622abfc25..12cc477b24 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -33,7 +33,7 @@ PX4 Log Viewer true publish.htm - 46 + 48 1.0.0.%2a false true