def tracking_state_callback(self, sbp_msg, **metadata): t = time.time() - self.t_init self.time[0:-1] = self.time[1:] self.time[-1] = t # first we loop over all the SIDs / channel keys we have stored and set 0 in for CN0 for key, cno_array in self.CN0_dict.items(): # p if (cno_array == 0).all(): self.CN0_dict.pop(key) else: new_arr = np.roll(cno_array, -1) new_arr[-1] = 0 self.CN0_dict[key] = new_arr # If the whole array is 0 we remove it # for each satellite, we have a (code, prn, channel) keyed dict # for each SID, an array of size MAX PLOT with the history of CN0's stored # If there is no CN0 or not tracking for an epoch, 0 will be used # each array can be plotted against host_time, t for i, s in enumerate(sbp_msg.states): if code_is_gps(s.sid.code): sat = s.sid.sat elif code_is_glo(s.sid.code): sat = s.fcn - GLO_FCN_OFFSET self.glo_slot_dict[sat] = s.sid.sat key = (s.sid.code, sat, i) if s.cn0 != 0: self.CN0_dict[key][-1] = s.cn0 / 4.0 GUI.invoke_later(self.update_plot)
def tracking_state_callback(self, sbp_msg, **metadata): t = time.time() - self.t_init self.time[0:-1] = self.time[1:] self.time[-1] = t # first we loop over all the SIDs / channel keys we have stored and set 0 in for CN0 for key, cno_array in self.CN0_dict.items(): # p if (cno_array==0).all(): self.CN0_dict.pop(key) else: self.CN0_dict[key][0:-1] = cno_array[1:] self.CN0_dict[key][-1] = 0 # If the whole array is 0 we remove it # for each satellite, we have a (code, prn, channel) keyed dict # for each SID, an array of size MAX PLOT with the history of CN0's stored # If there is no CN0 or not tracking for an epoch, 0 will be used # each array can be plotted against host_time, t for i,s in enumerate(sbp_msg.states): prn = s.sid.sat if code_is_gps(s.sid.code): prn += 1 key = (s.sid.code, prn, i) if s.state != 0: if len(self.CN0_dict.get(key, [])) == 0: self.CN0_dict[key] = np.zeros(NUM_POINTS) self.CN0_dict[key][-1] = s.cn0 GUI.invoke_later(self.update_plot)
def tracking_state_callback(self, sbp_msg, **metadata): n_channels = len(sbp_msg.states) if n_channels != self.n_channels: # Update number of channels self.n_channels = n_channels self.states = [ TrackingState(0, 0, 0, 0) for _ in range(n_channels) ] for pl in self.plot.plots.iterkeys(): self.plot.delplot(pl.name) self.plots = [] for n in range(n_channels): self.plot_data.set_data('ch' + str(n), [0.0]) pl = self.plot.plot(('t', 'ch' + str(n)), type='line', color='auto', name='ch' + str(n)) self.plots.append(pl) print 'Number of tracking channels changed to {0}'.format( n_channels) for n, k in enumerate(self.states): s = sbp_msg.states[n] prn = s.sid.sat if code_is_gps(s.sid.code): prn += 1 k.update(s.state, prn, s.cn0, s.sid.code) GUI.invoke_later(self.update_plot)
def ephemeris_callback(self, m, **metadata): prn = m.common.sid.sat if code_is_gps(m.common.sid.code): prn += 1 if self.recording: if self.eph_file is None: self.eph_file = sopen(os.path.join(self.dirname, self.name + self.t.strftime("-%Y%m%d-%H%M%S.eph")), 'w') header = "time, " \ + "tgd, " \ + "crs, crc, cuc, cus, cic, cis, " \ + "dn, m0, ecc, sqrta, omega0, omegadot, w, inc, inc_dot, " \ + "af0, af1, af2, " \ + "toe_tow, toe_wn, toc_tow, toc_wn, " \ + "valid, " \ + "healthy, " \ + "prn\n" self.eph_file.write(header) strout = "%s %10.7f" % (self.t.strftime(" %y %m %d %H %M"), self.t.second + self.t.microsecond * 1e-6) strout += "," + str([m.tgd, m.c_rs, m.c_rc, m.c_uc, m.c_us, m.c_ic, m.c_is, m.dn, m.m0, m.ecc, m.sqrta, m.omega0, m.omegadot, m.w, m.inc, m.inc_dot, m.af0, m.af1, m.af2, m.common.toe.tow, m.common.toe.wn, m.toc.tow, m.toc.wn, m.common.valid, m.common.health_bits, prn])[1: -1] + "\n" self.eph_file.write(strout) self.eph_file.flush()
def get_label(key, extra): code, sat, ch = key lbl = 'Ch {ch:02d}: {code} '.format(ch=ch, code=code_to_str(code)) if code_is_gps(code): lbl += 'PRN {sat:02d}'.format(sat=sat) elif code_is_glo(code): lbl += 'FCN {sat:0=+3d}'.format(sat=sat) if sat in extra: lbl += ' Slot: {slot:02d}'.format(slot=extra[sat]) return lbl
def obs_packed_callback(self, sbp_msg, **metadata): if (sbp_msg.sender is not None and (self.relay ^ (sbp_msg.sender == 0))): return tow = sbp_msg.header.t.tow wn = sbp_msg.header.t.wn seq = sbp_msg.header.n_obs tow = float(tow) / 1000.0 total = seq >> 4 count = seq & ((1 << 4) - 1) # Confirm this packet is good. # Assumes no out-of-order packets if count == 0: self.old_tow = self.gps_tow self.gps_tow = tow self.gps_week = wn self.prev_obs_total = total self.prev_obs_count = 0 self.old_cp = self.new_cp self.new_cp = {} self.obs = {} elif self.gps_tow != tow or\ self.gps_week != wn or\ self.prev_obs_count + 1 != count or\ self.prev_obs_total != total: print "We dropped a packet. Skipping this observation sequence" self.prev_obs_count = -1 return else: self.prev_obs_count = count # Save this packet # See sbp_piksi.h for format for o in sbp_msg.obs: # Handle all the message specific stuff prn = o.sid.sat flags = 0 msdopp = 0 # Old PRN values had to add one for GPS if (code_is_gps(o.sid.code) and sbp_msg.msg_type in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]): prn += 1 prn = '{} ({})'.format(prn, code_to_str(o.sid.code)) # 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 else: divisor = 5e1 if sbp_msg.msg_type not in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]: flags = o.flags msdopp = float(o.D.i) + float(o.D.f) / (1 << 8) self.gps_tow += sbp_msg.header.t.ns_residual * 1e-9 try: ocp = self.old_cp[prn] except: ocp = 0 cp = float(o.L.i) + float(o.L.f) / (1 << 8) # Compute time difference of carrier phase for display, but only if carrier phase is valid if ocp != 0 and ((sbp_msg.msg_type in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]) or (flags & 0x3) == 0x3): # Doppler per RINEX has opposite sign direction to carrier phase if self.gps_tow != self.old_tow: cpdopp = (ocp - cp) / float(self.gps_tow - self.old_tow) else: print "Received two complete observation sets with identical TOW" cpdopp = 0 # Older messages had flipped sign carrier phase values if sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B]: cpdopp = -cpdopp else: cpdopp = 0 # Save carrier phase value, but only if value is valid if (sbp_msg.msg_type in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]) or (flags & 0x3) == 0x3: self.new_cp[prn] = cp flags_str = "0x{:04X} = ".format(flags) # Add some indicators to help understand the flags values # Bit 0 is Pseudorange valid if (flags & 0x01): flags_str += "PR " # Bit 1 is Carrier phase valid if (flags & 0x02): flags_str += "CP " # Bit 2 is Half-cycle ambiguity if (flags & 0x04): flags_str += "1/2C " # Bit 3 is Measured Doppler Valid if (flags & 0x08): flags_str += "MD " pr_str = "{:11.2f}".format(float(o.P) / divisor) cp_str = "{:13.2f}".format(cp) cn0_str = "{:2.1f}".format(float(o.cn0) / 4) msdopp_str = "{:9.2f}".format(msdopp) cpdopp_str = "{:9.2f}".format(cpdopp) lock_str = "{:5d}".format(o.lock) # Sets invalid values to zero if sbp_msg.msg_type not in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]: if not (flags & 0x01): pr_str = EMPTY_STR if not (flags & 0x02): cp_str = EMPTY_STR if not (flags & 0x08): msdopp_str = EMPTY_STR if (cpdopp == 0): cpdopp_str = EMPTY_STR self.obs[prn] = (pr_str, cp_str, cn0_str, msdopp_str, cpdopp_str, lock_str, flags_str) if (count == total - 1): self.t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.gps_week) + \ datetime.timedelta(seconds=self.gps_tow) return
def obs_packed_callback(self, sbp_msg, **metadata): if (sbp_msg.sender is not None and (self.relay ^ (sbp_msg.sender == 0))): return tow = float(sbp_msg.header.t.tow / 1000.0) wn = sbp_msg.header.t.wn seq = sbp_msg.header.n_obs total = seq >> 4 count = seq & ((1 << 4) - 1) def reset(): self.update_scheduler.schedule_update('tow', self.gui_update_tow, wn, tow) self.old_tow = self.gps_tow self.gps_tow = tow self.gps_week = wn self.prev_obs_total = total self.prev_obs_count = 0 self.old_cp = dict(self.new_cp) self.new_cp.clear() self.incoming_obs.clear() # Confirm this packet is good. # Assumes no out-of-order packets # this happens on first packet received of epoch if count == 0: reset() elif (self.gps_tow != tow or self.gps_week != wn or self.prev_obs_count + 1 != count or self.prev_obs_total != total): print("We dropped a packet. Skipping this observation sequence") reset() self.prev_obs_count = count return else: self.prev_obs_count = count # Save this packet # See sbp_piksi.h for format for o in sbp_msg.obs: # Handle all the message specific stuff prn = o.sid.sat flags = 0 msdopp = 0 # Old PRN values had to add one for GPS if (code_is_gps(o.sid.code) and sbp_msg.msg_type in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]): prn += 1 prn = (prn, o.sid.code) # 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 else: divisor = 5e1 if sbp_msg.msg_type not in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]: flags = o.flags msdopp = float(o.D.i) + float(o.D.f) / (1 << 8) self.gps_tow += sbp_msg.header.t.ns_residual * 1e-9 self.update_scheduler.schedule_update('tow', self.gui_update_tow, self.gps_week, self.gps_tow) try: ocp = self.old_cp[prn] except: # noqa ocp = 0 cp = float(o.L.i) + float(o.L.f) / (1 << 8) # Compute time difference of carrier phase for display, but only if # carrier phase is valid if ocp != 0 and ((sbp_msg.msg_type in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]) or (flags & 0x3) == 0x3): # Doppler per RINEX has opposite sign direction to carrier phase if self.gps_tow != self.old_tow: cpdopp = (ocp - cp) / float(self.gps_tow - self.old_tow) else: print( "Received two complete observation sets with identical TOW" ) cpdopp = 0 # Older messages had flipped sign carrier phase values if sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B]: cpdopp = -cpdopp else: cpdopp = 0 # Save carrier phase value, but only if value is valid if (sbp_msg.msg_type in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]) or (flags & 0x3) == 0x3: self.new_cp[prn] = cp flags_str = "0x{:04X} = ".format(flags) # Add some indicators to help understand the flags values # Bit 0 is Pseudorange valid if (flags & 0x01): flags_str += "PR " # Bit 1 is Carrier phase valid if (flags & 0x02): flags_str += "CP " # Bit 2 is Half-cycle ambiguity if (flags & 0x04): flags_str += "1/2C " # Bit 3 is Measured Doppler Valid if (flags & 0x08): flags_str += "MD " pr_str = "{:11.2f}".format(float(o.P) / divisor) cp_str = "{:13.2f}".format(cp) cn0_str = "{:2.1f}".format(float(o.cn0) / 4) msdopp_str = "{:9.2f}".format(msdopp) cpdopp_str = "{:9.2f}".format(cpdopp) lock_str = "{:5d}".format(o.lock) # Sets invalid values to zero if sbp_msg.msg_type not in [ SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C ]: if not (flags & 0x01): pr_str = EMPTY_STR if not (flags & 0x02): cp_str = EMPTY_STR if not (flags & 0x08): msdopp_str = EMPTY_STR if (cpdopp == 0): cpdopp_str = EMPTY_STR self.incoming_obs[prn] = (pr_str, cp_str, cn0_str, msdopp_str, cpdopp_str, lock_str, flags_str) if (count == total - 1): # this is here to let GUI catch up to real time if required if monotonic() - self.last_table_update_time > GUI_UPDATE_PERIOD: 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() return
def azel_callback(self, sbp_msg, **metadata): svazelmsg = MsgSvAzEl(sbp_msg) tracked = self._trk_view.get_tracked_sv_labels() pending_update = { 'x_gps': [], 'x_glo': [], 'x_gal': [], 'x_bds': [], 'x_qzss': [], 'x_sbas': [], 'y_gps': [], 'y_glo': [], 'y_gal': [], 'y_bds': [], 'y_qzss': [], 'y_sbas': [] } # store new label updates, and display at once overlay_update = [] for azel in svazelmsg.azel: sid = azel.sid az = azel.az * 2 el = azel.el x, y = self.azel_to_xy(az, el) sat_string = "" if code_is_gps(sid.code) and self.gps_visible: pending_update['x_gps'].append(x) pending_update['y_gps'].append(y) elif code_is_glo(sid.code) and self.glo_visible: pending_update['x_glo'].append(x) pending_update['y_glo'].append(y) elif code_is_galileo(sid.code) and self.gal_visible: pending_update['x_gal'].append(x) pending_update['y_gal'].append(y) elif code_is_bds(sid.code) and self.bds_visible: pending_update['x_bds'].append(x) pending_update['y_bds'].append(y) elif code_is_qzss(sid.code) and self.qzss_visible: pending_update['x_qzss'].append(x) pending_update['y_qzss'].append(y) elif code_is_sbas(sid.code) and self.sbas_visible: pending_update['x_sbas'].append(x) pending_update['y_sbas'].append(y) sat_string = get_label((sid.code, sid.sat))[2] if sat_string in tracked: sat_string += TRK_SIGN label = DataLabel(component=self.plot, data_point=(x, y), label_text=sat_string, label_position="bottom right", border_visible=False, bgcolor="transparent", marker_visible=False, font='modern 14', arrow_visible=False, show_label_coords=False) overlay_update.append(label) # display label updates self.plot.overlays = (self.axis_overlays + overlay_update + self.default_overlays) self.plot_data.update(pending_update)
def obs_packed_callback(self, sbp_msg, **metadata): if (sbp_msg.sender is not None and (self.relay ^ (sbp_msg.sender == 0))): return tow = sbp_msg.header.t.tow wn = sbp_msg.header.t.wn seq = sbp_msg.header.n_obs tow = float(tow) / 1000.0 total = seq >> 4 count = seq & ((1 << 4) - 1) # Confirm this packet is good. # Assumes no out-of-order packets if count == 0: self.old_tow = self.gps_tow self.gps_tow = tow self.gps_week = wn self.prev_obs_total = total self.prev_obs_count = 0 self.old_obs = copy.deepcopy(self.obs) self.obs = {} elif self.gps_tow != tow or\ self.gps_week != wn or\ self.prev_obs_count + 1 != count or\ self.prev_obs_total != total: print "We dropped a packet. Skipping this observation sequence" self.prev_obs_count = -1 return else: self.prev_obs_count = count # Save this packet # See sbp_piksi.h for format for o in sbp_msg.obs: prn = o.sid.sat # compute time difference of carrier phase for display cp = float(o.L.i) + float(o.L.f) / (1 << 8) if code_is_gps(o.sid.code): prn += 1 prn = '{} ({})'.format(prn, code_to_str(o.sid.code)) if sbp_msg.msg_type == SBP_MSG_OBS_DEP_A or\ sbp_msg.msg_type == SBP_MSG_OBS_DEP_B: divisor = 1e2 else: divisor = 5e1 try: ocp = self.old_obs[prn][1] except: ocp = 0 cf = (cp - ocp) / (self.gps_tow - self.old_tow) self.obs[prn] = (float(o.P) / divisor, float(o.L.i) + float(o.L.f) / (1 << 8), float(o.cn0) / 4, cf) if (count == total - 1): self.t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.gps_week) + \ datetime.timedelta(seconds=self.gps_tow) self.update_obs() self.rinex_save() return
def obs_packed_callback(self, sbp_msg, **metadata): if (sbp_msg.sender is not None and (self.relay ^ (sbp_msg.sender == 0))): return tow = sbp_msg.header.t.tow wn = sbp_msg.header.t.wn seq = sbp_msg.header.n_obs tow = float(tow) / 1000.0 total = seq >> 4 count = seq & ((1 << 4) - 1) # Confirm this packet is good. # Assumes no out-of-order packets if count == 0: self.old_tow = self.gps_tow self.gps_tow = tow self.gps_week = wn self.prev_obs_total = total self.prev_obs_count = 0 self.old_cp = self.new_cp self.new_cp = {} self.obs = {} elif self.gps_tow != tow or\ self.gps_week != wn or\ self.prev_obs_count + 1 != count or\ self.prev_obs_total != total: print "We dropped a packet. Skipping this observation sequence" self.prev_obs_count = -1 return else: self.prev_obs_count = count # Save this packet # See sbp_piksi.h for format for o in sbp_msg.obs: # Handle all the message specific stuff prn = o.sid.sat flags = 0 msdopp = 0 # Old PRN values had to add one for GPS if (code_is_gps(o.sid.code) and sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C]): prn += 1 prn = '{} ({})'.format(prn, code_to_str(o.sid.code)) # 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 else: divisor = 5e1 if sbp_msg.msg_type not in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C]: flags = o.flags msdopp = float(o.D.i) + float(o.D.f) / (1 << 8) self.gps_tow += sbp_msg.header.t.ns * 1e-9 try: ocp = self.old_cp[prn] except: ocp = 0 cp = float(o.L.i) + float(o.L.f) / (1 << 8) # Compute time difference of carrier phase for display, but only if carrier phase is valid if ocp != 0 and ((sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C]) or (flags & 0x3) == 0x3): # Doppler per RINEX has opposite sign direction to carrier phase cpdopp = (ocp - cp) / float(self.gps_tow - self.old_tow) # Older messages had flipped sign carrier phase values if sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B]: cpdopp = -cpdopp else: cpdopp = 0 # Save carrier phase value, but only if value is valid if (sbp_msg.msg_type in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C]) or (flags & 0x3) == 0x3: self.new_cp[prn] = cp flags_str = "0x{:04X} = ".format(flags) # Add some indicators to help understand the flags values # Bit 0 is Pseudorange valid if (flags & 0x01): flags_str += "PR " # Bit 1 is Carrier phase valid if (flags & 0x02): flags_str += "CP " # Bit 2 is Half-cycle ambiguity if (flags & 0x04): flags_str += "1/2C " # Bit 3 is Measured Doppler Valid if (flags & 0x08): flags_str += "MD " pr_str = "{:11.2f}".format(float(o.P) / divisor) cp_str = "{:13.2f}".format(cp) cn0_str = "{:2.1f}".format(float(o.cn0) / 4) msdopp_str = "{:9.2f}".format(msdopp) cpdopp_str = "{:9.2f}".format(cpdopp) lock_str = "{:5d}".format(o.lock) # Sets invalid values to zero if sbp_msg.msg_type not in [SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_OBS_DEP_C]: if not (flags & 0x01): pr_str = EMPTY_STR if not (flags & 0x02): cp_str = EMPTY_STR if not (flags & 0x08): msdopp_str = EMPTY_STR if (cpdopp == 0): cpdopp_str = EMPTY_STR self.obs[prn] = (pr_str, cp_str, cn0_str, msdopp_str, cpdopp_str, lock_str, flags_str) if (count == total - 1): self.t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.gps_week) + \ datetime.timedelta(seconds=self.gps_tow) self.update_obs() return