def ephemeris_callback(self, m, **metadata):
prn = m.common.sid.sat
if m.common.sid.code == 0 or m.common.sid.code == 1:
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 vel_ned_callback(self, sbp_msg, **metadata):
flags = 0
if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A:
vel_ned = MsgVelNEDDepA(sbp_msg)
flags = 1
else:
vel_ned = MsgVelNED(sbp_msg)
flags = vel_ned.flags
tow = vel_ned.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
if self.week is not None:
t = datetime.datetime(1980, 1, 6) + \
datetime.timedelta(weeks=self.week) + \
datetime.timedelta(seconds=tow)
tstr = t.strftime('%Y-%m-%d %H:%M')
secs = t.strftime('%S.%f')
if self.directory_name_v == '':
filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")
else:
filepath_v = os.path.join(
self.directory_name_v,
time.strftime("velocity_log_%Y%m%d-%H%M%S.csv"))
if self.logging_v == False:
self.vel_log_file = None
if self.logging_v:
if self.vel_log_file is None:
self.vel_log_file = sopen(filepath_v, 'w')
self.vel_log_file.write(
'time,north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n'
)
self.vel_log_file.write(
'%s,%.6f,%.6f,%.6f,%.6f,%d,%d\n' %
("{0}:{1:06.6f}".format(tstr, float(secs)),
vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3,
math.sqrt(vel_ned.n * vel_ned.n + vel_ned.e * vel_ned.e) *
1e-3, flags, vel_ned.n_sats))
self.vel_log_file.flush()
if flags != 0:
self.vel_table = [
('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)),
('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)),
('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)),
]
else:
self.vel_table = [
('Vel. N', EMPTY_STR),
('Vel. E', EMPTY_STR),
('Vel. D', EMPTY_STR),
]
self.vel_table.append(('Vel Flags', '0x%03x' % flags))
self.table = self.pos_table + self.vel_table + self.dops_table
def vel_ned_callback(self, sbp_msg, **metadata):
flags = 0
if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A:
vel_ned = MsgVelNEDDepA(sbp_msg)
flags = 1
else:
vel_ned = MsgVelNED(sbp_msg)
flags = vel_ned.flags
tow = vel_ned.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow)
if self.directory_name_v == '':
filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")
else:
filepath_v = os.path.join(
self.directory_name_v,
time.strftime("velocity_log_%Y%m%d-%H%M%S.csv"))
if self.logging_v == False:
self.vel_log_file = None
if self.logging_v:
if self.vel_log_file is None:
self.vel_log_file = sopen(filepath_v, 'w')
self.vel_log_file.write(
'pc_time,gps_time,tow,north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n'
)
log_str_gps = ''
if tgps != "" and secgps != 0:
log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps))
self.vel_log_file.write(
'%s,%s,%.3f,%.6f,%.6f,%.6f,%.6f,%d,%d\n' %
("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow,
vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3,
math.sqrt(vel_ned.n * vel_ned.n + vel_ned.e * vel_ned.e) *
1e-3, flags, vel_ned.n_sats))
self.vel_log_file.flush()
if flags != 0:
self.vel_table = [
('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)),
('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)),
('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)),
]
else:
self.vel_table = [
('Vel. N', EMPTY_STR),
('Vel. E', EMPTY_STR),
('Vel. D', EMPTY_STR),
]
self.vel_table.append(('Vel Flags', '0x%03x' % flags))
self.table = self.pos_table + self.vel_table + self.dops_table
def vel_ned_callback(self, sbp_msg, **metadata):
flags = 0
if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A:
vel_ned = MsgVelNEDDepA(sbp_msg)
flags = 1
else:
vel_ned = MsgVelNED(sbp_msg)
flags = vel_ned.flags
tow = vel_ned.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
if self.week is not None:
t = datetime.datetime(1980, 1, 6) + \
datetime.timedelta(weeks=self.week) + \
datetime.timedelta(seconds=tow)
tstr = t.strftime('%Y-%m-%d %H:%M')
secs = t.strftime('%S.%f')
if self.directory_name_v == '':
filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")
else:
filepath_v = os.path.join(self.directory_name_v,time.strftime("velocity_log_%Y%m%d-%H%M%S.csv"))
if self.logging_v == False:
self.vel_log_file = None
if self.logging_v:
if self.vel_log_file is None:
self.vel_log_file = sopen(filepath_v, 'w')
self.vel_log_file.write('time,north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n')
self.vel_log_file.write('%s,%.6f,%.6f,%.6f,%.6f,%d,%d\n' % (
"{0}:{1:06.6f}".format(tstr, float(secs)),
vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3,
math.sqrt(vel_ned.n*vel_ned.n + vel_ned.e*vel_ned.e) * 1e-3,
flags,
vel_ned.n_sats)
)
self.vel_log_file.flush()
if flags != 0:
self.vel_table = [
('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)),
('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)),
('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)),
]
else:
self.vel_table = [
('Vel. N', EMPTY_STR),
('Vel. E', EMPTY_STR),
('Vel. D', EMPTY_STR),
]
self.vel_table.append(('Vel Flags', '0x%03x' % flags))
self.table = self.pos_table + self.vel_table + self.dops_table
def rinex_save(self):
if self.recording:
if self.rinex_file is None:
# If the file is being opened for the first time, write the RINEX header
self.rinex_file = sopen(
os.path.join(
self.dirname,
self.name + self.t.strftime("-%Y%m%d-%H%M%S.obs")),
'w')
header = ' ' +\
"""2.11 OBSERVATION DATA G (GPS) RINEX VERSION / TYPE
pyNEX %s UTC PGM / RUN BY / DATE
MARKER NAME
OBSERVER / AGENCY
REC # / TYPE / VERS
ANT # / TYPE
808673.9171 -4086658.5368 4115497.9775 APPROX POSITION XYZ
0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
1 0 WAVELENGTH FACT L1/2
3 C1 L1 S1 # / TYPES OF OBSERV
%s%13.7f GPS TIME OF FIRST OBS
END OF HEADER
""" % (
datetime.datetime.utcnow().strftime("%Y%m%d %H%M%S"),
self.t.strftime(" %Y %m %d %H %M"),
self.t.second + self.t.microsecond * 1e-6)
self.rinex_file.write(header)
# L1CA only
copy_prns = prns = [s for s in self.obs.iterkeys() if L1CA in s]
self.rinex_file.write(
"%s %10.7f 0 %2d" %
(self.t.strftime(" %y %m %d %H %M"),
self.t.second + self.t.microsecond * 1e-6, len(prns)))
while len(copy_prns) > 0:
prns_ = copy_prns[:12]
copy_prns = copy_prns[12:]
for prn in prns_:
# take only the leading prn number
self.rinex_file.write('G%2d' % (int(prn.split()[0])))
self.rinex_file.write(' ' * (12 - len(prns_)))
self.rinex_file.write('\n')
for prn in prns:
# G 3 C1C L1C D1C
self.rinex_file.write("%14.3f " % self.obs[prn][0])
self.rinex_file.write("%14.3f " % self.obs[prn][1])
self.rinex_file.write("%14.3f \n" % self.obs[prn][2])
self.rinex_file.flush()
def baseline_callback(self, sbp_msg):
soln = MsgBaselineNEDDepA(sbp_msg)
self.last_soln = soln
table = []
soln.n = soln.n * 1e-3
soln.e = soln.e * 1e-3
soln.d = soln.d * 1e-3
soln.h_accuracy = soln.h_accuracy * 1e-3
soln.v_accuracy = soln.v_accuracy * 1e-3
dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2)
tow = soln.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow)
if self.directory_name_b == '':
filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")
else:
filepath = os.path.join(
self.directory_name_b,
time.strftime("baseline_log_%Y%m%d-%H%M%S.csv"))
if self.logging_b == False:
self.log_file = None
if self.logging_b:
if self.log_file is None:
self.log_file = sopen(filepath, 'w')
self.log_file.write(
'pc_time,gps_time,tow(msec),north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),'
'distance(meters),num_sats,flags,num_hypothesis\n')
log_str_gps = ''
if tgps != '' and secgps != 0:
log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps))
self.log_file.write(
'%s,%s,%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' %
("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow,
soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy,
dist, soln.n_sats, soln.flags, self.num_hyps))
self.log_file.flush()
self.last_mode = get_mode(soln)
if self.last_mode < 1:
table.append(('GPS Time', EMPTY_STR))
table.append(('GPS Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('h_accuracy', EMPTY_STR))
table.append(('v_accuracy', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Num. Sats', EMPTY_STR))
table.append(('Flags', EMPTY_STR))
table.append(('Mode', EMPTY_STR))
else:
self.last_btime_update = time.time()
if self.week is not None:
table.append(
('GPS Time', "{0}:{1:06.3f}".format(tgps, float(secgps))))
table.append(('GPS Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('h_accuracy', soln.h_accuracy))
table.append(('v_accuracy', soln.v_accuracy))
table.append(('Dist.', dist))
table.append(('Num. Sats', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
self.table = table
# Rotate array, deleting oldest entries to maintain
# no more than N in plot
self.n[1:] = self.n[:-1]
self.e[1:] = self.e[:-1]
self.d[1:] = self.d[:-1]
self.mode[1:] = self.mode[:-1]
# Insert latest position
if self.last_mode > 1:
self.n[0], self.e[0], self.d[0] = soln.n, soln.e, soln.d
else:
self.n[0], self.e[0], self.d[0] = [np.NAN, np.NAN, np.NAN]
self.mode[0] = self.last_mode
if np.any(self.mode):
float_indexer = (self.mode == FLOAT_MODE)
fixed_indexer = (self.mode == FIXED_MODE)
dgnss_indexer = (self.mode == DGNSS_MODE)
if np.any(fixed_indexer):
self.plot_data.set_data('n_fixed', self.n[fixed_indexer])
self.plot_data.set_data('e_fixed', self.e[fixed_indexer])
self.plot_data.set_data('d_fixed', self.d[fixed_indexer])
if np.any(float_indexer):
self.plot_data.set_data('n_float', self.n[float_indexer])
self.plot_data.set_data('e_float', self.e[float_indexer])
self.plot_data.set_data('d_float', self.d[float_indexer])
if np.any(dgnss_indexer):
self.plot_data.set_data('n_dgnss', self.n[dgnss_indexer])
self.plot_data.set_data('e_dgnss', self.e[dgnss_indexer])
self.plot_data.set_data('d_dgnss', self.d[dgnss_indexer])
# Update our last solution icon
if self.last_mode == FIXED_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_fixed_n', [soln.n])
self.plot_data.set_data('cur_fixed_e', [soln.e])
self.plot_data.set_data('cur_fixed_d', [soln.d])
elif self.last_mode == FLOAT_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_float_n', [soln.n])
self.plot_data.set_data('cur_float_e', [soln.e])
self.plot_data.set_data('cur_float_d', [soln.d])
elif self.last_mode == DGNSS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_dgnss_n', [soln.n])
self.plot_data.set_data('cur_dgnss_e', [soln.e])
self.plot_data.set_data('cur_dgnss_d', [soln.d])
else:
pass
# make the zoomall win over the position centered button
# position centered button has no effect when zoom all enabled
if not self.zoomall and self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.e - d, soln.e + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.n - d, soln.n + d)
if self.zoomall:
plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'),
('n_fixed', 'n_float', 'n_dgnss'))
def pos_llh_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A:
soln = MsgPosLLHDepA(sbp_msg)
else:
soln = MsgPosLLH(sbp_msg)
self.last_soln = soln
self.last_pos_mode = get_mode(soln)
pos_table = []
soln.h_accuracy *= 1e-3
soln.v_accuracy *= 1e-3
tow = soln.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
# Return the best estimate of my local and receiver time in convenient
# format that allows changing precision of the seconds
((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow)
if (self.directory_name_p == ''):
filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv")
else:
filepath_p = os.path.join(
self.directory_name_p,
time.strftime("position_log_%Y%m%d-%H%M%S.csv"))
if self.logging_p == False:
self.log_file = None
if self.logging_p:
if self.log_file is None:
self.log_file = sopen(filepath_p, 'w')
self.log_file.write(
"pc_time,gps_time,tow(msec),latitude(degrees),longitude(degrees),altitude(meters),"
"h_accuracy(meters),v_accuracy(meters),n_sats,flags\n")
log_str_gps = ""
if tgps != "" and secgps != 0:
log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps))
self.log_file.write(
'%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' %
("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow,
soln.lat, soln.lon, soln.height, soln.h_accuracy,
soln.v_accuracy, soln.n_sats, soln.flags))
self.log_file.flush()
if self.last_pos_mode == 0:
pos_table.append(('GPS Time', EMPTY_STR))
pos_table.append(('GPS Week', EMPTY_STR))
pos_table.append(('GPS TOW', EMPTY_STR))
pos_table.append(('Num. Signals', EMPTY_STR))
pos_table.append(('Lat', EMPTY_STR))
pos_table.append(('Lng', EMPTY_STR))
pos_table.append(('Height', EMPTY_STR))
pos_table.append(('h_accuracy', EMPTY_STR))
pos_table.append(('v_accuracy', EMPTY_STR))
else:
self.last_stime_update = time.time()
if self.week is not None:
pos_table.append(
('GPS Time', "{0}:{1:06.3f}".format(tgps, float(secgps))))
pos_table.append(('GPS Week', str(self.week)))
pos_table.append(('GPS TOW', "{:.3f}".format(tow)))
pos_table.append(('Num. Sats', soln.n_sats))
pos_table.append(('Lat', soln.lat))
pos_table.append(('Lng', soln.lon))
pos_table.append(('Height', soln.height))
pos_table.append(('h_accuracy', soln.h_accuracy))
pos_table.append(('v_accuracy', soln.v_accuracy))
pos_table.append(('Pos Flags', '0x%03x' % soln.flags))
pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode]))
self.auto_survey()
# setup_plot variables
self.lats[1:] = self.lats[:-1]
self.lngs[1:] = self.lngs[:-1]
self.alts[1:] = self.alts[:-1]
self.tows[1:] = self.tows[:-1]
self.modes[1:] = self.modes[:-1]
self.lats[0] = soln.lat
self.lngs[0] = soln.lon
self.alts[0] = soln.height
self.tows[0] = soln.tow
self.modes[0] = self.last_pos_mode
self.lats = self.lats[-self.plot_history_max:]
self.lngs = self.lngs[-self.plot_history_max:]
self.alts = self.alts[-self.plot_history_max:]
self.tows = self.tows[-self.plot_history_max:]
self.modes = self.modes[-self.plot_history_max:]
# SPP
spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None
if np.any(self.modes):
spp_indexer = (self.modes == SPP_MODE)
dgnss_indexer = (self.modes == DGNSS_MODE)
float_indexer = (self.modes == FLOAT_MODE)
fixed_indexer = (self.modes == FIXED_MODE)
# make sure that there is at least one true in indexer before setting
if any(spp_indexer):
self.plot_data.set_data('lat_spp', self.lats[spp_indexer])
self.plot_data.set_data('lng_spp', self.lngs[spp_indexer])
self.plot_data.set_data('alt_spp', self.alts[spp_indexer])
if any(dgnss_indexer):
self.plot_data.set_data('lat_dgnss', self.lats[dgnss_indexer])
self.plot_data.set_data('lng_dgnss', self.lngs[dgnss_indexer])
self.plot_data.set_data('alt_dgnss', self.alts[dgnss_indexer])
if any(float_indexer):
self.plot_data.set_data('lat_float', self.lats[float_indexer])
self.plot_data.set_data('lng_float', self.lngs[float_indexer])
self.plot_data.set_data('alt_float', self.alts[float_indexer])
if any(fixed_indexer):
self.plot_data.set_data('lat_fixed', self.lats[fixed_indexer])
self.plot_data.set_data('lng_fixed', self.lngs[fixed_indexer])
self.plot_data.set_data('alt_fixed', self.alts[fixed_indexer])
# update our "current solution" icon
if self.last_pos_mode == SPP_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_spp', [soln.lat])
self.plot_data.set_data('cur_lng_spp', [soln.lon])
elif self.last_pos_mode == DGNSS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_dgnss', [soln.lat])
self.plot_data.set_data('cur_lng_dgnss', [soln.lon])
elif self.last_pos_mode == FLOAT_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_float', [soln.lat])
self.plot_data.set_data('cur_lng_float', [soln.lon])
elif self.last_pos_mode == FIXED_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_fixed', [soln.lat])
self.plot_data.set_data('cur_lng_fixed', [soln.lon])
else:
pass
# set-up table variables
self.pos_table = pos_table
self.table = self.pos_table + self.vel_table + self.dops_table
# TODO: figure out how to center the graph now that we have two separate messages
# when we selectively send only SPP, the centering function won't work anymore
if not self.zoomall and self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.lon - d, soln.lon + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.lat - d, soln.lat + d)
if self.zoomall:
plot_square_axes(
self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed'),
('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed'))
def pos_llh_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A:
soln = MsgPosLLHDepA(sbp_msg)
else:
soln = MsgPosLLH(sbp_msg)
self.last_soln = soln
self.last_pos_mode = get_mode(soln)
pos_table = []
soln.h_accuracy *= 1e-3
soln.v_accuracy *= 1e-3
tow = soln.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
# Return the best estimate of my local and receiver time in convenient
# format that allows changing precision of the seconds
((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow)
if self.utc_time:
((tutc, secutc)) = datetime_2_str(self.utc_time)
if (self.directory_name_p == ''):
filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv")
else:
filepath_p = os.path.join(
self.directory_name_p,
time.strftime("position_log_%Y%m%d-%H%M%S.csv"))
if self.logging_p == False:
self.log_file = None
if self.logging_p:
if self.log_file is None:
self.log_file = sopen(filepath_p, 'w')
self.log_file.write(
"pc_time,gps_time,tow(msec),latitude(degrees),longitude(degrees),altitude(meters),"
"h_accuracy(meters),v_accuracy(meters),n_sats,flags\n")
log_str_gps = ""
if tgps != "" and secgps != 0:
log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps))
self.log_file.write(
'%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' %
("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow,
soln.lat, soln.lon, soln.height, soln.h_accuracy,
soln.v_accuracy, soln.n_sats, soln.flags))
self.log_file.flush()
if self.last_pos_mode == 0:
pos_table.append(('GPS Week', EMPTY_STR))
pos_table.append(('GPS TOW', EMPTY_STR))
pos_table.append(('GPS Time', EMPTY_STR))
pos_table.append(('Num. Signals', EMPTY_STR))
pos_table.append(('Lat', EMPTY_STR))
pos_table.append(('Lng', EMPTY_STR))
pos_table.append(('Height', EMPTY_STR))
pos_table.append(('Horiz Acc', EMPTY_STR))
pos_table.append(('Vert Acc', EMPTY_STR))
else:
self.last_stime_update = time.time()
if self.week is not None:
pos_table.append(('GPS Week', str(self.week)))
pos_table.append(('GPS TOW', "{:.3f}".format(tow)))
if self.week is not None:
pos_table.append(
('GPS Time', "{0}:{1:06.3f}".format(tgps, float(secgps))))
if self.utc_time is not None:
pos_table.append(
('UTC Time', "{0}:{1:06.3f}".format(tutc, float(secutc))))
pos_table.append(('UTC Src', self.utc_source))
if self.utc_time is None:
pos_table.append(('UTC Time', EMPTY_STR))
pos_table.append(('UTC Src', EMPTY_STR))
pos_table.append(('Sats Used', soln.n_sats))
pos_table.append(('Lat', soln.lat))
pos_table.append(('Lng', soln.lon))
pos_table.append(('Height', soln.height))
pos_table.append(('Horiz Acc', soln.h_accuracy))
pos_table.append(('Vert Acc', soln.v_accuracy))
pos_table.append(('Pos Flags', '0x%03x' % soln.flags))
pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode]))
if self.age_corrections != None:
pos_table.append(('Corr. Age [s]', self.age_corrections))
self.auto_survey()
# set-up table variables
self.pos_table = pos_table
self.update_table()
# setup_plot variables
self.lats[1:] = self.lats[:-1]
self.lngs[1:] = self.lngs[:-1]
self.alts[1:] = self.alts[:-1]
self.tows[1:] = self.tows[:-1]
self.modes[1:] = self.modes[:-1]
self.lats[0] = soln.lat
self.lngs[0] = soln.lon
self.alts[0] = soln.height
self.tows[0] = soln.tow
self.modes[0] = self.last_pos_mode
self.lats = self.lats[-self.plot_history_max:]
self.lngs = self.lngs[-self.plot_history_max:]
self.alts = self.alts[-self.plot_history_max:]
self.tows = self.tows[-self.plot_history_max:]
self.modes = self.modes[-self.plot_history_max:]
def pos_llh_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A:
soln = MsgPosLLHDepA(sbp_msg)
else:
soln = MsgPosLLH(sbp_msg)
self.last_soln = soln
self.last_pos_mode = get_mode(soln)
pos_table = []
soln.h_accuracy *= 1e-3
soln.v_accuracy *= 1e-3
tow = soln.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
if self.week is not None:
t = datetime.datetime(1980, 1, 6) + \
datetime.timedelta(weeks=self.week) + \
datetime.timedelta(seconds=tow)
tstr = t.strftime('%Y-%m-%d %H:%M')
secs = t.strftime('%S.%f')
if(self.directory_name_p == ''):
filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv")
else:
filepath_p = os.path.join(self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv"))
if self.logging_p == False:
self.log_file = None
if self.logging_p:
if self.log_file is None:
self.log_file = sopen(filepath_p, 'w')
self.log_file.write("time,latitude(degrees),longitude(degrees),altitude(meters),"
"h_accuracy(meters),v_accuracy(meters),n_sats,flags\n")
self.log_file.write('%s,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % (
"{0}:{1:06.6f}".format(tstr, float(secs)),
soln.lat, soln.lon, soln.height,
soln.h_accuracy, soln.v_accuracy,
soln.n_sats, soln.flags)
)
self.log_file.flush()
if self.last_pos_mode == 0:
pos_table.append(('GPS Time', EMPTY_STR))
pos_table.append(('GPS Week', EMPTY_STR))
pos_table.append(('GPS TOW', EMPTY_STR))
pos_table.append(('Num. Signals', EMPTY_STR))
pos_table.append(('Lat', EMPTY_STR))
pos_table.append(('Lng', EMPTY_STR))
pos_table.append(('Height', EMPTY_STR))
pos_table.append(('h_accuracy', EMPTY_STR))
pos_table.append(('v_accuracy', EMPTY_STR))
else:
self.last_stime_update = time.time()
if self.week is not None:
pos_table.append(('GPS Time', "{0}:{1:06.3f}".format(tstr, float(secs))))
pos_table.append(('GPS Week', str(self.week)))
pos_table.append(('GPS TOW', "{:.3f}".format(tow)))
pos_table.append(('Num. Sats', soln.n_sats))
pos_table.append(('Lat', soln.lat))
pos_table.append(('Lng', soln.lon))
pos_table.append(('Height', soln.height))
pos_table.append(('h_accuracy', soln.h_accuracy))
pos_table.append(('v_accuracy', soln.v_accuracy))
pos_table.append(('Pos Flags', '0x%03x' % soln.flags))
pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode]))
self.auto_survey()
# setup_plot variables
self.lats[1:] = self.lats[:-1]
self.lngs[1:] = self.lngs[:-1]
self.alts[1:] = self.alts[:-1]
self.tows[1:] = self.tows[:-1]
self.modes[1:] = self.modes[:-1]
self.lats[0] = soln.lat
self.lngs[0] = soln.lon
self.alts[0] = soln.height
self.tows[0] = soln.tow
self.modes[0] = self.last_pos_mode
self.lats = self.lats[-self.plot_history_max:]
self.lngs = self.lngs[-self.plot_history_max:]
self.alts = self.alts[-self.plot_history_max:]
self.tows = self.tows[-self.plot_history_max:]
self.modes = self.modes[-self.plot_history_max:]
# SPP
spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None
if np.any(self.modes):
spp_indexer = (self.modes == SPP_MODE)
dgnss_indexer = (self.modes == DGNSS_MODE)
float_indexer = (self.modes == FLOAT_MODE)
fixed_indexer = (self.modes == FIXED_MODE)
# make sure that there is at least one true in indexer before setting
if any(spp_indexer):
self.plot_data.set_data('lat_spp', self.lats[spp_indexer])
self.plot_data.set_data('lng_spp', self.lngs[spp_indexer])
self.plot_data.set_data('alt_spp', self.alts[spp_indexer])
if any(dgnss_indexer):
self.plot_data.set_data('lat_dgnss', self.lats[dgnss_indexer])
self.plot_data.set_data('lng_dgnss', self.lngs[dgnss_indexer])
self.plot_data.set_data('alt_dgnss', self.alts[dgnss_indexer])
if any(float_indexer):
self.plot_data.set_data('lat_float', self.lats[float_indexer])
self.plot_data.set_data('lng_float', self.lngs[float_indexer])
self.plot_data.set_data('alt_float', self.alts[float_indexer])
if any(fixed_indexer):
self.plot_data.set_data('lat_fixed', self.lats[fixed_indexer])
self.plot_data.set_data('lng_fixed', self.lngs[fixed_indexer])
self.plot_data.set_data('alt_fixed', self.alts[fixed_indexer])
# update our "current solution" icon
if self.last_pos_mode == SPP_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_spp', [soln.lat])
self.plot_data.set_data('cur_lng_spp', [soln.lon])
elif self.last_pos_mode == DGNSS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_dgnss', [soln.lat])
self.plot_data.set_data('cur_lng_dgnss', [soln.lon])
elif self.last_pos_mode == FLOAT_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_float', [soln.lat])
self.plot_data.set_data('cur_lng_float', [soln.lon])
elif self.last_pos_mode == FIXED_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_fixed', [soln.lat])
self.plot_data.set_data('cur_lng_fixed', [soln.lon])
else:
pass
# set-up table variables
self.pos_table = pos_table
self.table = self.pos_table + self.vel_table + self.dops_table
# TODO: figure out how to center the graph now that we have two separate messages
# when we selectively send only SPP, the centering function won't work anymore
if not self.zoomall and self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.lon - d, soln.lon + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.lat - d, soln.lat + d)
if self.zoomall:
plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float','lng_fixed'),
('lat_spp', 'lat_dgnss', 'lat_float','lat_fixed'))
def baseline_callback(self, sbp_msg):
soln = MsgBaselineNEDDepA(sbp_msg)
self.last_soln = soln
table = []
soln.n = soln.n * 1e-3
soln.e = soln.e * 1e-3
soln.d = soln.d * 1e-3
soln.h_accuracy = soln.h_accuracy * 1e-3
soln.v_accuracy = soln.v_accuracy * 1e-3
dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2)
tow = soln.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
if self.week is not None:
t = datetime.datetime(1980, 1, 6) + \
datetime.timedelta(weeks=self.week) + \
datetime.timedelta(seconds=tow)
tstr = t.strftime('%Y-%m-%d %H:%M')
secs = t.strftime('%S.%f')
if self.directory_name_b == '':
filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")
else:
filepath = os.path.join(self.directory_name_b, time.strftime("baseline_log_%Y%m%d-%H%M%S.csv"))
if self.logging_b == False:
self.log_file = None
if self.logging_b:
if self.log_file is None:
self.log_file = sopen(filepath, 'w')
self.log_file.write('time,north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),'
'distance(meters),num_sats,flags,num_hypothesis\n')
self.log_file.write('%s,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' % (
"{0}:{1:06.6f}".format(tstr, float(secs)),
soln.n, soln.e, soln.d,
soln.h_accuracy, soln.v_accuracy,
dist,
soln.n_sats,
soln.flags,
self.num_hyps)
)
self.log_file.flush()
self.last_mode = get_mode(soln)
if self.last_mode < 1:
table.append(('GPS Time', EMPTY_STR))
table.append(('GPS Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('h_accuracy', EMPTY_STR))
table.append(('v_accuracy', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Num. Sats', EMPTY_STR))
table.append(('Flags', EMPTY_STR))
table.append(('Mode', EMPTY_STR))
else:
self.last_btime_update = time.time()
if self.week is not None:
table.append(('GPS Time', "{0}:{1:06.3f}".format(tstr, float(secs))))
table.append(('GPS Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('h_accuracy', soln.h_accuracy))
table.append(('v_accuracy', soln.v_accuracy))
table.append(('Dist.', dist))
table.append(('Num. Sats', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
self.table = table
# Rotate array, deleting oldest entries to maintain
# no more than N in plot
self.n[1:] = self.n[:-1]
self.e[1:] = self.e[:-1]
self.d[1:] = self.d[:-1]
self.mode[1:] = self.mode[:-1]
# Insert latest position
if self.last_mode > 1:
self.n[0], self.e[0], self.d[0] = soln.n, soln.e, soln.d
else:
self.n[0], self.e[0], self.d[0] = [np.NAN, np.NAN, np.NAN]
self.mode[0] = self.last_mode
if np.any(self.mode):
float_indexer = (self.mode == FLOAT_MODE)
fixed_indexer = (self.mode == FIXED_MODE)
dgnss_indexer = (self.mode == DGNSS_MODE)
if np.any(fixed_indexer):
self.plot_data.set_data('n_fixed', self.n[fixed_indexer])
self.plot_data.set_data('e_fixed', self.e[fixed_indexer])
self.plot_data.set_data('d_fixed', self.d[fixed_indexer])
if np.any(float_indexer):
self.plot_data.set_data('n_float', self.n[float_indexer])
self.plot_data.set_data('e_float', self.e[float_indexer])
self.plot_data.set_data('d_float', self.d[float_indexer])
if np.any(dgnss_indexer):
self.plot_data.set_data('n_dgnss', self.n[dgnss_indexer])
self.plot_data.set_data('e_dgnss', self.e[dgnss_indexer])
self.plot_data.set_data('d_dgnss', self.d[dgnss_indexer])
# Update our last solution icon
if self.last_mode == FIXED_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_fixed_n', [soln.n])
self.plot_data.set_data('cur_fixed_e', [soln.e])
self.plot_data.set_data('cur_fixed_d', [soln.d])
elif self.last_mode == FLOAT_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_float_n', [soln.n])
self.plot_data.set_data('cur_float_e', [soln.e])
self.plot_data.set_data('cur_float_d', [soln.d])
elif self.last_mode == DGNSS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_dgnss_n', [soln.n])
self.plot_data.set_data('cur_dgnss_e', [soln.e])
self.plot_data.set_data('cur_dgnss_d', [soln.d])
else:
pass
# make the zoomall win over the position centered button
# position centered button has no effect when zoom all enabled
if not self.zoomall and self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.e - d, soln.e + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.n - d, soln.n + d)
if self.zoomall:
plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'), ('n_fixed', 'n_float', 'n_dgnss'))
