rework to use RTCM3 Library

Author: jmachuca77

MAVProxy/modules/mavproxy_DGPS.py

@@ -13,8 +13,9 @@
 Set the RTCM Messages to 10Hz and enable any that are needed.
 '''
 
-import socket, errno
+import socket, errno, threading
 import time
+from MAVProxy.modules.lib import rtcm3
 from pymavlink import mavutil
 from MAVProxy.modules.lib import mp_module
 from MAVProxy.modules.lib import mp_settings
@@ -37,9 +38,19 @@ def __init__(self, mpstate):
         self.add_completion_function('(DGPSSETTING)',
                                      self.dgps_settings.completion)
 
-        self.port = None      
+        self.port = None    
+        self.waiting = False  
         self.last_pkt = None 
         self.inject_seq_nr = 0
+        self.pkt_count = 0
+        self.last_rate = None
+        self.rate_total = 0
+        self.rate = 0
+        # RTCM3 parser
+        self.rtcm3 = rtcm3.RTCM3()
+        self.last_id = None
+        self.id_counts = {}
+        self.last_by_id = {}
 
     def log_rtcm(self, data):
         '''optionally log rtcm data'''
@@ -74,7 +85,10 @@ def cmd_start(self):
             self.connect_tcp_rtcm_base(self.dgps_settings.ip, self.dgps_settings.port)
         else:
             print("Undefined connection type!")
-            return           
+            return    
+
+        self.last_rate = time.time()       
+        self.rate_total = 0
 
     def dgps_status(self):
         '''show dgps status'''
@@ -91,12 +105,12 @@ def dgps_status(self):
             print("DGPS: no data")
             return
 
-        print ("Last packet recieved %.3fs ago" % (now - self.last_pkt))
-        # frame_size = 0
-        # for id in sorted(self.id_counts.keys()):
-        #     print(" %4u: %u (len %u)" % (id, self.id_counts[id], len(self.last_by_id[id])))
-        #     frame_size += len(self.last_by_id[id])
-        # print("dgps: %u packets, %.1f bytes/sec last %.1fs ago framesize %u" % (self.pkt_count, self.rate, now - self.last_pkt, frame_size))
+        # print ("Last packet recieved %.3fs ago" % (now - self.last_pkt))
+        frame_size = 0
+        for id in sorted(self.id_counts.keys()):
+            print(" %4u: %u (len %u)" % (id, self.id_counts[id], len(self.last_by_id[id])))
+            frame_size += len(self.last_by_id[id])
+        print("dgps: %u packets, %.2f bytes/sec last %.3fs ago framesize %u" % (self.pkt_count, self.rate, now - self.last_pkt, frame_size))
 
     def connect_udp_rtcm_base(self, ip, port):
         print ("UDP Connection")
@@ -116,76 +130,79 @@ def connect_tcp_rtcm_base(self, ip, port):
             self.port.settimeout(1)
             self.port.setblocking(0)
         except:
-            print ("ERROR: Failed to connect to RTCM base over TCP")
+            print ("ERROR: Failed to connect to RTCM base over TCP, retrying in 2.5s")
+            self.waiting = True
+            threading.Timer(2.5,self.connect_tcp_rtcm_base(ip, port)).start()
         else:
             print ("Connected to base using TCP")
-
-    def send_rtcm_msg(self, data):
-        msglen = 180;
-        
-        if (len(data) > msglen * 4):
-            print("DGPS: Message too large", len(data))
-            return
-        
-        # How many messages will we send?
-        msgs = 0
-        if (len(data) % msglen == 0):
-            msgs = len(data) / msglen
-        else:
-            msgs = (len(data) / msglen) + 1
-        
-        for a in range(0, msgs):
-            
-            flags = 0
-            
-            # Set the fragment flag if we're sending more than 1 packet.
-            if (msgs) > 1:
-                flags = 1
-            
-            # Set the ID of this fragment
-            flags |= (a & 0x3) << 1
-            
-            # Set an overall sequence number
-            flags |= (self.inject_seq_nr & 0x1f) << 3
-            
-            
-            amount = min(len(data) - a * msglen, msglen)
-            datachunk = data[a*msglen : a*msglen + amount]
-            
-            self.master.mav.gps_rtcm_data_send(
-                flags,
-                len(datachunk),
-                bytearray(datachunk.ljust(180, '\0')))
-        
-        # Send a terminal 0-length message if we sent 2 or 3 exactly-full messages.     
-        if (msgs < 4) and (len(data) % msglen == 0) and (len(data) > msglen):
-            flags = 1 | (msgs & 0x3)  << 1 | (self.inject_seq_nr & 0x1f) << 3
-            self.master.mav.gps_rtcm_data_send(
-                flags,
-                0,
-                bytearray("".ljust(180, '\0')))
-            
-        self.inject_seq_nr += 1
+            self.waiting = False
 
     def idle_task(self):
         '''called in idle time'''
         if self.port is None:
             return
 
+        if self.waiting is True:
+            return
+
         try:
-            data = self.port.recv(1024) # Attempt to read up to 1024 bytes.
+            data = self.port.recv(1) # Attempt to read up to 1024 bytes.
         except socket.error as e:
             if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
                 return
             raise
-        try:
-            now = time.time()
-            self.send_rtcm_msg(data)
+        
+        now = time.time()
+        if self.rtcm3.read(data):
+            self.last_id = self.rtcm3.get_packet_ID()
+            # print(self.last_id)
+            rtcm3Data = self.rtcm3.get_packet()
+            if self.rtcm3.get_packet() is None:
+                print ("No packet")
+                return
+
             self.log_rtcm(data)
+            
+            if not self.last_id in self.id_counts:
+                self.id_counts[self.last_id] = 0
+                self.last_by_id[self.last_id] = data[:]
+            self.id_counts[self.last_id] += 1     
+
+            blen = len(data)
+            if blen > 4*180:
+                # can't send this with GPS_RTCM_DATA
+                print("WTF")
+                return
+            self.rate_total += blen
+
+            if blen > 180:
+                flags = 1 # fragmented
+            else:
+                flags = 0
+
+            # add in the sequence number
+            flags |= (self.pkt_count & 0x1F) << 3
+
+            fragment = 0
+            while blen > 0:
+                send_data = bytearray(data[:180])
+                frag_len = len(send_data)
+                data = data[frag_len:]
+                if frag_len < 180:
+                    send_data.extend(bytearray([0]*(180-frag_len)))
+                self.master.mav.gps_rtcm_data_send(flags | (fragment<<1), frag_len, send_data)
+                fragment += 1
+                blen -= frag_len
+            self.pkt_count += 1
+
             self.last_pkt = now
 
-        except Exception as e:
-            print("DGPS: GPS Inject Failed:", e)
+        if now - self.last_rate > 1:
+            dt = now - self.last_rate
+            rate_now = self.rate_total / float(dt)
+            self.rate = 0.9 * self.rate + 0.1 * rate_now
+            self.last_rate = now
+            self.rate_total = 0
 
 def init(mpstate):
     '''initialise module'''