Drive tracking view from OBS message if no tracking or measurement state received

(cherry picked from commit 7a2e445)

Author: denniszollo

piksi_tools/console/console.py

@@ -668,7 +668,8 @@ def __init__(self,
                 self.link,
                 name='Local',
                 relay=False,
-                dirname=self.directory_name)
+                dirname=self.directory_name,
+                tracking_view=self.tracking_view)
             self.observation_view_base = ObservationView(
                 self.link,
                 name='Remote',

piksi_tools/console/observation_view.py

@@ -155,6 +155,7 @@ def reset():
             self.old_cp = dict(self.new_cp)
             self.new_cp.clear()
             self.incoming_obs.clear()
+            self.incoming_obs_cn0.clear()
 
         # Confirm this packet is good.
         # Assumes no out-of-order packets
@@ -186,6 +187,8 @@ def reset():
 
             prn = (prn, o.sid.code)
 
+            self.incoming_obs_cn0[(prn[1], prn[0])] = float(o.cn0) / 4
+
             # DEP_B and DEP_A obs had different pseudorange scaling
             if sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B]:
                 divisor = 1e2
@@ -285,16 +288,21 @@ def reset():
                 self.update_scheduler.schedule_update('update_obs', self.update_obs, self.incoming_obs.copy())
                 self.last_table_update_tow = self.gps_tow
                 self.last_table_update_time = monotonic()
+                if self.tracking_view is not None:
+                    self.tracking_view.update_from_obs(self.incoming_obs_cn0)
+
         return
 
-    def __init__(self, link, name='Local', relay=False, dirname=None):
+    def __init__(self, link, name='Local', relay=False, dirname=None, tracking_view=None):
         super(ObservationView, self).__init__()
+        self.tracking_view = tracking_view
         self.dirname = dirname
         self.last_table_update_tow = 0
         self.last_table_update_time = 0
         self.old_cp = {}
         self.new_cp = {}
         self.incoming_obs = {}
+        self.incoming_obs_cn0 = {}
         self.gps_tow = 0.0
         self.old_tow = 0.0
         self.gps_week = 0

piksi_tools/console/tracking_view.py

@@ -117,6 +117,7 @@ def clean_cn0(self, t):
                 del self.CN0_age[k]
 
     def measurement_state_callback(self, sbp_msg, **metadata):
+        self._at_least_one_track_received = True
         with self.CN0_lock:
             codes_that_came = []
             t = monotonic() - self.t_init
@@ -148,7 +149,31 @@ def measurement_state_callback(self, sbp_msg, **metadata):
             self.clean_cn0(t)
         self.update_scheduler.schedule_update('update_plot', self.update_plot)
 
+    def update_from_obs(self, obs_dict):
+        if not self._at_least_one_track_received:
+            with self.CN0_lock:
+                codes_that_came = []
+                t = monotonic() - self.t_init
+                self.time.append(t)
+                # first we loop over all the SIDs / channel keys we have stored and set 0 in for CN0
+                for i, (key, cn0) in enumerate(obs_dict.items()):
+                    code = key[0]
+                    codes_that_came.append(key)
+                    if cn0 != 0:
+                        self.CN0_dict[key].append(cn0)
+                        self.CN0_age[key] = t
+                    received_code_list = getattr(self, "received_codes", [])
+                    if code not in received_code_list:
+                        received_code_list.append(code)
+                        self.received_codes = received_code_list
+                for key, cno_array in list(self.CN0_dict.items()):
+                    if key not in codes_that_came:
+                        cno_array.append(0)
+                self.clean_cn0(t)
+            self.update_scheduler.schedule_update('update_plot', self.update_plot)
+
     def tracking_state_callback(self, sbp_msg, **metadata):
+        self._at_least_one_track_received = True
         with self.CN0_lock:
             codes_that_came = []
             t = monotonic() - self.t_init
@@ -244,6 +269,7 @@ def get_tracked_sv_labels(self):
 
     def __init__(self, link):
         super(TrackingView, self).__init__()
+        self._at_least_one_track_received = False
         self.t_init = monotonic()
         self.time = deque([x * 1 / TRK_RATE for x in range(-NUM_POINTS, 0, 1)], maxlen=NUM_POINTS)
         self.CN0_lock = threading.Lock()