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 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 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 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 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
