rtcm_parser.py
Go to the documentation of this file.
1 import logging
2 
3 _RTCM_3_2_PREAMBLE = 0b11010011
4 _RTCM_CRC_LOOKUP = [
5  0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
6  0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
7  0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
8  0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
9  0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
10  0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
11  0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
12  0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
13  0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
14  0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
15  0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
16  0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
17  0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
18  0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
19  0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
20  0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
21  0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
22  0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
23  0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
24  0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
25  0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
26  0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
27  0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
28  0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
29  0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
30  0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
31  0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
32  0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
33  0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
34  0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
35  0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
36  0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
37 ]
38 
39 # If we find the beginning of a packet, but not the end, we will cache up to this number of bytes
40 _MAX_BUFFER_SIZE = 1024 * 10
41 
42 class RTCMParser:
43 
44  def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=logging.info, logdebug=logging.debug):
45  # Bit of a strange pattern here, but save the log functions so we can be agnostic of ROS
46  self._logerr = logerr
47  self._logwarn = logwarn
48  self._loginfo = loginfo
49  self._logdebug = logdebug
50 
51  # Empty buffer, will be filled out
52  self._caching_data = False
53  self._buffer = b''
54 
55  def parse(self, buffer):
56  # Add any data that we have cached
57  if self._caching_data:
58  combined_buffer = self._buffer + buffer
59  else:
60  combined_buffer = buffer
61 
62  # Loop over the passed buffer, and parse all available RTCM packets
63  index = 0
64  rtcm_packets = []
65  while index < len(combined_buffer):
66  # Find the start of the RTCM 3.2 packet
67  if combined_buffer[index] == _RTCM_3_2_PREAMBLE:
68  # Make sure we have enough data to find the length
69  if len(combined_buffer) <= index + 2:
70  self._logdebug('Found beginning of RTCM packet at {}, but there is not enough data in the buffer to find the message length'.format(index))
71  self._caching_data = True
72  buffer = buffer[index:]
73  break
74 
75  # Make sure we have enough data in the packet to validate it
76  message_length = (combined_buffer[index + 1] << 8 | combined_buffer[index + 2]) & 0x03FF
77  if index + message_length + 6 <= len(combined_buffer):
78  # Grab the packet from the buffer, and verify that it is valid by comparing checksums
79  packet = combined_buffer[index:index + message_length + 6]
80  expected_checksum = packet[-3] << 16 | packet[-2] << 8 | packet[-1]
81  actual_checksum = self._checksum(packet[:-3])
82  if expected_checksum == actual_checksum:
83  self._logdebug('Found valid packet at {} with length {}'.format(index, message_length))
84  rtcm_packets.append(packet)
85  index += message_length + 6
86 
87  # Remove the packet we just found from the cached buffer
88  self._caching_data = False
89  self._buffer = self._buffer[message_length + 5:]
90  continue
91  else:
92  self._logwarn('Found packet, but checksums didn\'t match')
93  self._logwarn('Expected Checksum: 0x{:X}'.format(expected_checksum))
94  self._logwarn('Actual Checksum: 0x{:X}'.format(actual_checksum))
95  else:
96  self._logdebug('Found beginning of RTCM packet at {}, but there is not enough data in the buffer to extract it, caching'.format(index))
97  self._caching_data = True
98 
99  # If we didn't find a message, manually move on to the next byte
100  index += 1
101 
102  # If we didn't find a full packet, cache this one for next time
103  if self._caching_data:
104  self._buffer += buffer
105 
106  # Throw away old data if we are at our limit
107  if len(self._buffer) > _MAX_BUFFER_SIZE:
108  self._logwarn("Too much data buffered, trimming to {} bytes.".format(_MAX_BUFFER_SIZE))
109  self._buffer = self._buffer[:_MAX_BUFFER_SIZE]
110 
111  # Return the RTCM packets we found
112  return rtcm_packets
113 
114  def _checksum(self, packet):
115  crc = 0
116  for byte in packet:
117  crc = ((crc << 8) & 0xFFFFFF) ^ _RTCM_CRC_LOOKUP[(crc >> 16) ^ byte]
118  return crc
def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=logging.info, logdebug=logging.debug)
Definition: rtcm_parser.py:44


ntrip_client
Author(s): Parker Hannifin Corp
autogenerated on Thu Aug 18 2022 02:39:24