From 3319ddb90058d29569e23847f338910cf90fed0a Mon Sep 17 00:00:00 2001
From: Mehmet Killioglu <mehmet.killioglu@unikie.com>
Date: Wed, 28 Feb 2024 13:38:44 +0200
Subject: [PATCH] Fix the message type inspection

---
 scripts/ntrip_ros.py | 38 ++++++++++++++++++--------------------
 1 file changed, 18 insertions(+), 20 deletions(-)

diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py
index 9ea2702..620dfc9 100755
--- a/scripts/ntrip_ros.py
+++ b/scripts/ntrip_ros.py
@@ -33,11 +33,16 @@
 
 rtcm_message_types = {
   1005: '1005 - Stationary RTK reference station ARP',
+  1074: '1074 - GPS MSM4',
   1077: '1077 - GPS MSM7',
+  1084: '1084 - GLONASS MSM4',
   1087: '1087 - GLONASS MSM7',
+  1094: '1094 - Galileo MSM4',
   1097: '1097 - Galileo MSM7',
+  1124: '1124 - BeiDou MSM4',
   1127: '1127 - BeiDou MSM7',
-  1230: '1230 - GLONASS code-phase biases'
+  1230: '1230 - GLONASS code-phase biases',
+  4072: '4072 - Reference station PVT (u-blox proprietary RTCM message)',
 }
 
 class NTRIPRos(Node):
@@ -180,17 +185,17 @@ def publish_rtcm(self):
       rtcm = np.frombuffer(raw_rtcm, dtype=np.uint8)
       len_rtcm = len(rtcm)
 
-      # Extract the message type from the first 12 bits of the message
-      msg_type = int.from_bytes(rtcm[:2], byteorder='big') >> 4
-      msg_type_description = rtcm_message_types.get(msg_type, f'Unknown RTCM message type: {msg_type}')
+      rtcm_msg_len = 256 * rtcm[1] + rtcm[2]
+      rtcm_msg_no = int((256 * rtcm[3] + rtcm[4]) / 16)
+      msg_type_description = rtcm_message_types.get(rtcm_msg_no, f'Unknown RTCM message type: {rtcm_msg_no}')
 
-      self.get_logger().info(' package length {:3}, RTCM message: {}'.format(len_rtcm, msg_type_description))
+      self.get_logger().info(' package length {:3} ({:3}), Type: {}'.format(len_rtcm, rtcm_msg_len, msg_type_description))
 
-      # if (len_rtcm > 255):  # Even though the message can have size of 300, len must be between [0,255]
-      #   self.get_logger().info('  Dropping....')
-      #   continue
+      if (len_rtcm > MAX_LEN):  # Even though the message can have size of 300, len must be between [0,255]
+        self.get_logger().info('  Dropping....')
+        continue
 
-      if (len_rtcm < MAX_LEN):
+      if (len_rtcm <= MAX_LEN):
         extend_array = np.zeros(MAX_LEN)
         rtcm = np.append(rtcm,extend_array)    
 
@@ -203,17 +208,8 @@ def publish_rtcm(self):
 
         self._rtcm_pub.publish(msg)
 
-      elif (len_rtcm == MAX_LEN):
-        msg=GpsInjectData(
-        timestamp=self.get_clock().now().nanoseconds,
-        flags = (self._sequenceId & 0x1F) << 3,
-        len=len_rtcm,
-        data=rtcm
-        )
-
-        self._rtcm_pub.publish(msg)
-
       else:
+        # not used as dropping the messages with length > 300
         fragmentId = 0
         start = 0
         while (start < len_rtcm):
@@ -242,7 +238,9 @@ def publish_rtcm(self):
 
           start += length
 
-      self._sequenceId += 1 
+      self._sequenceId += 1
+      if self._sequenceId > 31:
+        self._sequenceId = 0
     
 
 if __name__ == '__main__':