Merge pull request #5910 from mavlink/rtcm_sequence_number

RTCMMavlink: fix sequence numbers

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)

src/GPS/RTCM/RTCMMavlink.h

@@ -36,4 +36,5 @@ private:
     QGCToolbox& _toolbox;
     QElapsedTimer _bandwidthTimer;
     int _bandwidthByteCounter = 0;
+    uint8_t _sequenceId = 0;
 };