From d7f5411e2c3be1d58128cb110b5ecae382c9f52c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 23 Dec 2017 13:50:17 +0100
Subject: [PATCH] RTCMMavlink: fix sequence numbers

sequence numbers are only increased after a new RTCM message, while only
the fragment id is incremented within a fragmented message.
---
 src/GPS/RTCM/RTCMMavlink.cc | 9 +++------
 src/GPS/RTCM/RTCMMavlink.h  | 1 +
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc
index d8f6c92..e81dd2b 100644
--- a/src/GPS/RTCM/RTCMMavlink.cc
+++ b/src/GPS/RTCM/RTCMMavlink.cc
@@ -38,29 +38,26 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message)
 
     if (message.size() < maxMessageLength) {
         mavlinkRtcmData.len = message.size();
+        mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3;
         memcpy(&mavlinkRtcmData.data, message.data(), message.size());
         sendMessageToVehicle(mavlinkRtcmData);
     } else {
         // We need to fragment
 
-        static uint8_t sequenceId = 0;  // Sequence id is used to indicate that the individual fragements belong to the same set
         uint8_t fragmentId = 0;         // Fragment id indicates the fragment within a set
-
         int start = 0;
         while (start < message.size()) {
             int length = std::min(message.size() - start, maxMessageLength);
             mavlinkRtcmData.flags = 1;                      // LSB set indicates message is fragmented
             mavlinkRtcmData.flags |= fragmentId++ << 1;     // Next 2 bits are fragment id
-            mavlinkRtcmData.flags |= sequenceId++ << 3;     // Next 5 bits are sequence id
+            mavlinkRtcmData.flags |= (_sequenceId & 0x1F) << 3;     // Next 5 bits are sequence id
             mavlinkRtcmData.len = length;
             memcpy(&mavlinkRtcmData.data, message.data() + start, length);
             sendMessageToVehicle(mavlinkRtcmData);
             start += length;
         }
-        if (sequenceId == 0x1F) {
-            sequenceId = 0;
-        }
     }
+    ++_sequenceId;
 }
 
 void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h
index 6c59974..a568a2d 100644
--- a/src/GPS/RTCM/RTCMMavlink.h
+++ b/src/GPS/RTCM/RTCMMavlink.h
@@ -36,4 +36,5 @@ private:
     QGCToolbox& _toolbox;
     QElapsedTimer _bandwidthTimer;
     int _bandwidthByteCounter = 0;
+    uint8_t _sequenceId = 0;
 };