def update_on_heartbeat(self, sbp_msg, **metadata):
self.heartbeat_count += 1
# First initialize the state to nothing, if we can't update, it will be none
temp_mode = "None"
temp_num_sats = 0
view = None
if self.baseline_view and self.solution_view:
# If we have a recent baseline update, we use the baseline info
if time.time() - self.baseline_view.last_btime_update < 10:
view = self.baseline_view
# Otherwise, if we have a recent SPP update, we use the SPP
elif time.time() - self.solution_view.last_stime_update < 10:
view = self.solution_view
if view:
if view.last_soln:
# if all is well we update state
temp_mode = mode_dict.get(get_mode(view.last_soln),
EMPTY_STR)
temp_num_sats = view.last_soln.n_sats
self.mode = temp_mode
self.num_sats = temp_num_sats
if self.settings_view: # for auto populating surveyed fields
self.settings_view.lat = self.solution_view.latitude
self.settings_view.lon = self.solution_view.longitude
self.settings_view.alt = self.solution_view.altitude
if self.system_monitor_view:
if self.system_monitor_view.msg_obs_window_latency_ms != -1:
self.latency = "{0} ms".format(
self.system_monitor_view.msg_obs_window_latency_ms)
else:
self.latency = EMPTY_STR
def update_on_heartbeat(self, sbp_msg, **metadata):
self.heartbeat_count += 1
# First initialize the state to nothing, if we can't update, it will be none
temp_mode = "None"
temp_num_sats = 0
view = None
if self.baseline_view and self.solution_view:
# If we have a recent baseline update, we use the baseline info
if time.time() - self.baseline_view.last_btime_update < 10:
view = self.baseline_view
# Otherwise, if we have a recent SPP update, we use the SPP
elif time.time() - self.solution_view.last_stime_update < 10:
view = self.solution_view
if view:
if view.last_soln:
# if all is well we update state
temp_mode = mode_dict.get(
get_mode(view.last_soln), EMPTY_STR)
temp_num_sats = view.last_soln.n_sats
if getattr(view, 'ins_used', False):
temp_mode += "+INS"
self.mode = temp_mode
self.num_sats = temp_num_sats
if self.settings_view: # for auto populating surveyed fields
self.settings_view.lat = self.solution_view.latitude
self.settings_view.lon = self.solution_view.longitude
self.settings_view.alt = self.solution_view.altitude
if self.baseline_view:
if self.baseline_view.age_corrections is not None:
self.age_of_corrections = "{0} s".format(
self.baseline_view.age_corrections)
else:
self.age_of_corrections = EMPTY_STR
def update_on_heartbeat(self, sbp_msg, **metadata):
self.heartbeat_count += 1
# First initialize the state to nothing, if we can't update, it will be none
temp_mode = "None"
temp_num_sats = 0
view = None
if self.baseline_view and self.solution_view:
# If we have a recent baseline update, we use the baseline info
if time.time() - self.baseline_view.last_btime_update < 10:
view = self.baseline_view
# Otherwise, if we have a recent SPP update, we use the SPP
elif time.time() - self.solution_view.last_stime_update < 10:
view = self.solution_view
if view:
if view.last_soln:
# if all is well we update state
temp_mode = mode_dict.get(
get_mode(view.last_soln), EMPTY_STR)
temp_num_sats = view.last_soln.n_sats
self.mode = temp_mode
self.num_sats = temp_num_sats
if self.settings_view: # for auto populating surveyed fields
self.settings_view.lat = self.solution_view.latitude
self.settings_view.lon = self.solution_view.longitude
self.settings_view.alt = self.solution_view.altitude
if self.system_monitor_view:
if self.system_monitor_view.msg_obs_window_latency_ms != -1:
self.latency = "{0} ms".format(
self.system_monitor_view.msg_obs_window_latency_ms)
else:
self.latency = EMPTY_STR
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 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 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 = open(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' % (
str(t),
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])
self._reset_remove_current()
if self.last_pos_mode == SPP_MODE:
self.plot_data.set_data('cur_lat_spp', [soln.lat])
self.plot_data.set_data('cur_lng_spp', [soln.lon])
if self.last_pos_mode == DGNSS_MODE:
self.plot_data.set_data('cur_lat_dgnss', [soln.lat])
self.plot_data.set_data('cur_lng_dgnss', [soln.lon])
if self.last_pos_mode == FLOAT_MODE:
self.plot_data.set_data('cur_lat_float', [soln.lat])
self.plot_data.set_data('cur_lng_float', [soln.lon])
if self.last_pos_mode == FIXED_MODE:
self.plot_data.set_data('cur_lat_fixed', [soln.lat])
self.plot_data.set_data('cur_lng_fixed', [soln.lon])
# 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, **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 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()
# 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
self._clear_history()
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
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'))
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
