|
|
|
@ -38,29 +38,26 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message)
@@ -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) |
|
|
|
|