Skip to content

Commit

Permalink
Merge pull request microsoft#1851 from lovettchris/review/logview_mav…
Browse files Browse the repository at this point in the history 
…link2

Fix LogViewer so it can handle mavlink2 messages.
  • Loading branch information
@sytelus
sytelus committed Mar 27, 2019
2 parents 73c1ab2 + fadb354 commit fa1f04d
Show file tree
Hide file tree
Showing 3 changed files with 152 additions and 24 deletions.
2 changes: 1 addition & 1 deletion LogViewer/LogViewer/LogViewer.csproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
<SuiteName>PX4 Log Viewer</SuiteName>
<CreateWebPageOnPublish>true</CreateWebPageOnPublish>
<WebPage>publish.htm</WebPage>
<ApplicationRevision>46</ApplicationRevision>
<ApplicationRevision>48</ApplicationRevision>
<ApplicationVersion>1.0.0.%2a</ApplicationVersion>
<UseApplicationTrust>false</UseApplicationTrust>
<PublishWizardCompleted>true</PublishWizardCompleted>
Expand Down
157 changes: 135 additions & 22 deletions LogViewer/Networking/Mavlink/MavlinkChannel.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -172,6 +192,8 @@ internal void Serialize()
this.Length = (byte)block.Length;
}
}


}

public class MavlinkChannel
Expand Down Expand Up @@ -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();

Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -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;
}
}

}
Expand All @@ -336,17 +448,18 @@ enum ReadState
{
Init,
GotMagic,
GotIncompatFlags,
GotCompatFlags,
GotLength,
GotSequenceNumber,
GotSystemId,
GotComponentId,
GotMessageId,
GotPayload,
GotCrc1
GotCrc1,
GetSignature
}

const int MagicMarker = 254;

}


Expand Down
17 changes: 16 additions & 1 deletion MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -336,6 +346,11 @@ void MavLinkConnectionImpl::joinLeftSubscriber(std::shared_ptr<MavLinkConnection
{
unused(connection);
// forward messages from our connected node to the remote proxy.
if (supports_mavlink2_)
{
// tell the remote connection to expect mavlink2 messages.
remote->pImpl->supports_mavlink2_ = true;
}
remote->sendMessage(msg);
}

Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit fa1f04d

Please sign in to comment.