def __init__(self, tfile=False, outdir=''):
if tfile:
self.logfile = sopen(os.path.join(outdir, LOGFILE), 'w')
self.tfile = True
else:
self.tfile = False
def __init__(self, tfile=False, outdir=''):
if tfile:
self.logfile = sopen(os.path.join(outdir, LOGFILE), 'w')
self.tfile = True
else:
self.tfile = False
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 not self.logging_v:
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(sec),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.update_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 not self.logging_v:
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(sec),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 & 0x7) != 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.update_table()
def _download_file_from_url(self, url):
url = url.encode('ascii')
urlpath = urlparse(url).path
filename = os.path.split(urlparse(url).path)[1]
filename = os.path.join(self.root_dir, filename)
url_file = urlopen(url)
blob = url_file.read()
with sopen(filename, 'wb') as f:
f.write(blob)
url_file.close()
return os.path.abspath(filename)
def _download_file_from_url(self, url):
if not os.path.exists(self.root_dir):
raise RuntimeError("Path to download file {0} to does not exist.".format(self.root_dir))
return
filename = os.path.split(urlparse(url).path)[1]
filename = os.path.join(self.root_dir, filename)
requests_response = requests.get(url)
requests_response.raise_for_status()
blob = requests_response.content
with sopen(filename, 'wb') as f:
f.write(blob)
return os.path.abspath(filename)
def _download_file_from_url(self, url):
if not os.path.exists(self.root_dir):
raise RuntimeError("Path to download file {0} to does not exist.".format(self.root_dir))
return
filename = os.path.split(urlparse(url).path)[1]
filename = os.path.join(self.root_dir, filename)
requests_response = requests.get(url)
requests_response.raise_for_status()
blob = requests_response.content
with sopen(filename, 'wb') as f:
f.write(blob)
return os.path.abspath(filename)
def _download_file_from_url(self, url):
if not os.path.isdir(self.root_dir):
raise IOError("Path to download file to does not exist")
url = url.encode('ascii')
urlpath = urlparse(url).path
filename = os.path.split(urlparse(url).path)[1]
filename = os.path.join(self.root_dir, filename)
url_file = urlopen(url)
blob = url_file.read()
with sopen(filename, 'wb') as f:
f.write(blob)
url_file.close()
return os.path.abspath(filename)
def baseline_callback(self, sbp_msg, **metadata):
soln = MsgBaselineNEDDepA(sbp_msg)
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.utc_time is not None:
((tutc, secutc)) = datetime_2_str(self.utc_time)
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 not self.logging_b:
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(sec),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 Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('GPS Time', EMPTY_STR))
table.append(('UTC Time', EMPTY_STR))
table.append(('UTC Src', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('Horiz Acc', EMPTY_STR))
table.append(('Vert Acc', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Sats Used', EMPTY_STR))
table.append(('Flags', EMPTY_STR))
table.append(('Mode', EMPTY_STR))
table.append(('Heading', EMPTY_STR))
table.append(('Corr. Age [s]', EMPTY_STR))
else:
self.last_btime_update = monotonic()
if self.week is not None:
table.append(('GPS Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
if self.week is not None:
table.append(('GPS Time', "{0}:{1:06.3f}".format(
tgps, float(secgps))))
if self.utc_time is not None:
table.append(('UTC Time', "{0}:{1:06.3f}".format(
tutc, float(secutc))))
table.append(('UTC Src', self.utc_source))
table.append(('N', "{:.12g}".format(soln.n)))
table.append(('E', "{:.12g}".format(soln.e)))
table.append(('D', "{:.12g}".format(soln.d)))
table.append(('Horiz Acc', "{:.12g}".format(soln.h_accuracy)))
table.append(('Vert Acc', "{:.12g}".format(soln.v_accuracy)))
table.append(('Dist.', "{0:.3f}".format(dist)))
table.append(('Sats Used', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
if self.heading is not None:
table.append(('Heading', self.heading))
if self.age_corrections is not None:
table.append(('Corr. Age [s]', self.age_corrections))
else:
table.append(('Corr. Age [s]', EMPTY_STR))
self.table = table
if self.last_mode != 0:
self.last_soln = soln
mode_string = mode_string_dict[self.last_mode]
if mode_string not in self.pending_draw_modes:
# if we don't already have a pending upate for that mode
self.pending_draw_modes.append(mode_string)
self.list_lock.acquire()
self._update_sln_data_by_mode(soln, mode_string)
self.list_lock.release()
else:
self.list_lock.acquire()
self._append_empty_sln_data(soln)
self.list_lock.release()
if monotonic() - self.last_plot_update_time > GUI_UPDATE_PERIOD:
self.update_scheduler.schedule_update('_solution_draw', self._solution_draw)
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)
self.ins_used = ((soln.flags & 0x8) >> 3) == 1
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 not self.logging_p:
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(sec),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', "{0:.3f}".format(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(('INS Used', '{}'.format(self.ins_used)))
pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode]))
if self.age_corrections is not 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.list_lock.acquire()
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:]
self.list_lock.release()
# Updating array plot data is not thread safe, so we have to fire an event
# and have the GUI thread do it
if time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD:
self.last_plot_update_time = time.time()
def baseline_callback(self, sbp_msg, **metadata):
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.utc_time is not None:
((tutc, secutc)) = datetime_2_str(self.utc_time)
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 not self.logging_b:
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(sec),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 Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('GPS Time', EMPTY_STR))
table.append(('UTC Time', EMPTY_STR))
table.append(('UTC Src', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('Horiz Acc', EMPTY_STR))
table.append(('Vert Acc', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Sats Used', 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 Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
if self.week is not None:
table.append(('GPS Time', "{0}:{1:06.3f}".format(
tgps, float(secgps))))
if self.utc_time is not None:
table.append(('UTC Time', "{0}:{1:06.3f}".format(
tutc, float(secutc))))
table.append(('UTC Src', self.utc_source))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('Horiz Acc', soln.h_accuracy))
table.append(('Vert Acc', soln.v_accuracy))
table.append(('Dist.', "{0:.3f}".format(dist)))
table.append(('Sats Used', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
if self.heading is not None:
table.append(('Heading', self.heading))
if self.age_corrections is not None:
table.append(('Corr. Age [s]', self.age_corrections))
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
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_pos_mode = get_mode(soln)
if self.last_pos_mode != 0:
self.last_soln = soln
mode_string = mode_string_dict[self.last_pos_mode]
if mode_string not in self.pending_draw_modes:
# this list allows us to tell GUI thread which solutions to update
# (if we decide not to update at full data rate)
# we use short strings to identify each solution mode
self.pending_draw_modes.append(mode_string)
self.list_lock.acquire()
self._update_sln_data_by_mode(soln, mode_string)
self.list_lock.release()
else:
self.list_lock.acquire()
self._append_empty_sln_data()
self.list_lock.release()
self.ins_used = ((soln.flags & 0x8) >> 3) == 1
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 not self.logging_p:
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(sec),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 = monotonic()
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', "{:.12g}".format(soln.lat)))
pos_table.append(('Lng', "{:.12g}".format(soln.lon)))
pos_table.append(('Height', "{0:.3f}".format(soln.height)))
pos_table.append(('Horiz Acc', "{:.12g}".format(soln.h_accuracy)))
pos_table.append(('Vert Acc', "{:.12g}".format(soln.v_accuracy)))
pos_table.append(('Pos Flags', '0x%03x' % soln.flags))
pos_table.append(('INS Used', '{}'.format(self.ins_used)))
pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode]))
if self.age_corrections is not None:
pos_table.append(('Corr. Age [s]', self.age_corrections))
# only store valid solutions for auto survey and degrees to meter transformation
if self.last_pos_mode != 0:
self.lats.append(soln.lat)
self.lngs.append(soln.lon)
self.alts.append(soln.height)
self.tows.append(soln.tow)
self.modes.append(self.last_pos_mode)
self.auto_survey()
# set-up table variables
self.pos_table = pos_table
self.update_table()
# setup_plot variables
# Updating array plot data is not thread safe, so we have to fire an event
# and have the GUI thread do it
if monotonic() - self.last_plot_update_time > GUI_UPDATE_PERIOD:
self.update_scheduler.schedule_update('_solution_draw', self._solution_draw)
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 not self.logging_p:
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(sec),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', "{0:.3f}".format(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 is not 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 baseline_callback(self, sbp_msg, **metadata):
soln = MsgBaselineNEDDepA(sbp_msg)
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.utc_time is not None:
((tutc, secutc)) = datetime_2_str(self.utc_time)
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 not self.logging_b:
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(sec),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 Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('GPS Time', EMPTY_STR))
table.append(('UTC Time', EMPTY_STR))
table.append(('UTC Src', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('Horiz Acc', EMPTY_STR))
table.append(('Vert Acc', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Sats Used', EMPTY_STR))
table.append(('Flags', EMPTY_STR))
table.append(('Mode', EMPTY_STR))
table.append(('Heading', EMPTY_STR))
table.append(('Corr. Age [s]', EMPTY_STR))
else:
self.last_btime_update = time.time()
if self.week is not None:
table.append(('GPS Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
if self.week is not None:
table.append(('GPS Time', "{0}:{1:06.3f}".format(
tgps, float(secgps))))
if self.utc_time is not None:
table.append(('UTC Time', "{0}:{1:06.3f}".format(
tutc, float(secutc))))
table.append(('UTC Src', self.utc_source))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('Horiz Acc', soln.h_accuracy))
table.append(('Vert Acc', soln.v_accuracy))
table.append(('Dist.', "{0:.3f}".format(dist)))
table.append(('Sats Used', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
if self.heading is not None:
table.append(('Heading', self.heading))
if self.age_corrections is not None:
table.append(('Corr. Age [s]', self.age_corrections))
else:
table.append(('Corr. Age [s]', EMPTY_STR))
self.table = table
if self.last_mode != 0:
self.last_soln = soln
mode_string = mode_string_dict[self.last_mode]
if mode_string not in self.pending_draw_modes:
# if we don't already have a pending upate for that mode
self.pending_draw_modes.append(mode_string)
self.list_lock.acquire()
self._update_sln_data_by_mode(soln, mode_string)
self.list_lock.release()
else:
self.list_lock.acquire()
self._append_empty_sln_data(soln)
self.list_lock.release()
if time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD:
GUI.invoke_later(self._solution_draw)
def baseline_callback(self, sbp_msg, **metadata):
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.utc_time is not None:
((tutc, secutc)) = datetime_2_str(self.utc_time)
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 not self.logging_b:
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(sec),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 Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('GPS Time', EMPTY_STR))
table.append(('UTC Time', EMPTY_STR))
table.append(('UTC Src', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('Horiz Acc', EMPTY_STR))
table.append(('Vert Acc', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Sats Used', 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 Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
if self.week is not None:
table.append(('GPS Time', "{0}:{1:06.3f}".format(
tgps, float(secgps))))
if self.utc_time is not None:
table.append(('UTC Time', "{0}:{1:06.3f}".format(
tutc, float(secutc))))
table.append(('UTC Src', self.utc_source))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('Horiz Acc', soln.h_accuracy))
table.append(('Vert Acc', soln.v_accuracy))
table.append(('Dist.', "{0:.3f}".format(dist)))
table.append(('Sats Used', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
if self.heading is not None:
table.append(('Heading', self.heading))
if self.age_corrections is not None:
table.append(('Corr. Age [s]', self.age_corrections))
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
