From d63600d326d053ef6dfc3bf01758795bd839424a Mon Sep 17 00:00:00 2001
From: Mehmet Killioglu <mehmet.killioglu@unikie.com>
Date: Wed, 24 Jan 2024 15:05:23 +0200
Subject: [PATCH] Add some debug prints and pub until max len

---
 scripts/ntrip_ros.py | 33 +++++++++++++++++++++++++++++----
 1 file changed, 29 insertions(+), 4 deletions(-)

diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py
index c800f10..9ea2702 100755
--- a/scripts/ntrip_ros.py
+++ b/scripts/ntrip_ros.py
@@ -30,6 +30,16 @@
   have_rtcm_msgs = True
   from rtcm_msgs.msg import Message as rtcm_msgs_RTCM
 
+
+rtcm_message_types = {
+  1005: '1005 - Stationary RTK reference station ARP',
+  1077: '1077 - GPS MSM7',
+  1087: '1087 - GLONASS MSM7',
+  1097: '1097 - Galileo MSM7',
+  1127: '1127 - BeiDou MSM7',
+  1230: '1230 - GLONASS code-phase biases'
+}
+
 class NTRIPRos(Node):
   def __init__(self):
     # Read a debug flag from the environment that should have been set by the launch file
@@ -169,11 +179,16 @@ def publish_rtcm(self):
 
       rtcm = np.frombuffer(raw_rtcm, dtype=np.uint8)
       len_rtcm = len(rtcm)
-      self.get_logger().info(' package length {}'.format(len_rtcm))    
 
-      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
+      # 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}')
+
+      self.get_logger().info(' package length {:3}, RTCM message: {}'.format(len_rtcm, 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):
         extend_array = np.zeros(MAX_LEN)
@@ -188,6 +203,16 @@ 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:
         fragmentId = 0
         start = 0