Commit d7f5411e authored by Beat Küng's avatar Beat Küng

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.
parent a88e1c45
......@@ -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)
......
......@@ -36,4 +36,5 @@ private:
QGCToolbox& _toolbox;
QElapsedTimer _bandwidthTimer;
int _bandwidthByteCounter = 0;
uint8_t _sequenceId = 0;
};
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment