From 4b07086fd4b7af44e2370f7ba69ecbb8b81a4f30 Mon Sep 17 00:00:00 2001 From: Bryant Mairs Date: Tue, 10 Jun 2014 13:13:55 -0700 Subject: [PATCH] Fix for #647. In the logplayer, the data stream is read ahead for messages, which is used to determine the size of the output bytearrays. Previously, this wasn't done correctly resulting in non-ideal behavior. Now, the next message is scanned properly, so the only bytearrays that are emitted from QGCMAVLinkLogPlayer are MAVLink messages in their entirety. This also has the benefit of skipping invalid/corrupted messages in the log that's being played back. --- src/ui/QGCMAVLinkLogPlayer.cc | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 3e7aa2f..54ec4ae 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -572,10 +572,18 @@ void QGCMAVLinkLogPlayer::logLoop() if (mavlinkLogFormat) { // Now parse MAVLink messages, grabbing their timestamps as we go. We stop once we - // have at least 3ms until the next one. + // have at least 3ms until the next one. + + // We track what the next execution time should be in milliseconds, which we use to set + // the next timer interrupt. int nextExecutionTime = 0; + + // We use the `findNextMavlinkMessage()` function to scan ahead for MAVLink messages. This + // is necessary because we don't know how big each MAVLink message is until we finish parsing + // one, and since we only output arrays of bytes, we need to know the size of that array. mavlink_message_t msg; - msg.len = 0; // FIXME: Hack, remove once Issue #647 is fixed + findNextMavlinkMessage(&msg); + while (nextExecutionTime < 3) { // Now we're sitting at the start of a MAVLink message, so read it all into a byte array for feeding to our parser.