class SolutionView(HasTraits):
python_console_cmds = Dict()
# we need to doubleup on Lists to store the psuedo absolutes separately
# without rewriting everything
"""
logging_v : toggle logging for velocity files
directory_name_v : location and name of velocity files
logging_p : toggle logging for position files
directory_name_p : location and name of velocity files
"""
plot_history_max = Int(1000)
last_plot_update_time = Float()
logging_v = Bool(False)
display_units = Enum(["degrees", "meters"])
directory_name_v = File
logging_p = Bool(False)
directory_name_p = File
lats_psuedo_abs = List()
lngs_psuedo_abs = List()
alts_psuedo_abs = List()
table = List()
dops_table = List()
pos_table = List()
vel_table = List()
rtk_pos_note = Str(
"It is necessary to enter the \"Surveyed Position\" settings for the base station in order to view the RTK Positions in this tab."
)
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
# Store plots we care about for legend
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(
label='',
tooltip='Clear',
filename=resource_filename('console/images/iconic/x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(
label='',
tooltip='Zoom All',
toggle=True,
filename=resource_filename('console/images/iconic/fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(
label='',
tooltip='Center on Solution',
toggle=True,
filename=resource_filename('console/images/iconic/target.svg'),
width=16,
height=16)
paused_button = SVGButton(
label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
width=16,
height=16)
traits_view = View(
HSplit(
VGroup(
Item('table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
Item('rtk_pos_note',
show_label=False,
resizable=True,
editor=MultilineTextEditor(TextEditor(multi_line=True)),
style='readonly',
width=0.3,
height=-40),
),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
Item('display_units', label="Display Units"),
),
Item('plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8))),
)))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_remove_current(self):
self.plot_data.update_data(self._get_update_current())
def _get_update_current(self, current_dict={}):
out_dict = {
'cur_lat_spp': [],
'cur_lng_spp': [],
'cur_lat_dgnss': [],
'cur_lng_dgnss': [],
'cur_lat_float': [],
'cur_lng_float': [],
'cur_lat_fixed': [],
'cur_lng_fixed': [],
'cur_lat_sbas': [],
'cur_lng_sbas': [],
'cur_lat_dr': [],
'cur_lng_dr': []
}
out_dict.update(current_dict)
return out_dict
def _synchronize_plot_data_by_mode(self, mode_string):
# do all required plot_data updates for a single
# new solution with mode defined by mode_string
pending_update = {
'lat_' + mode_string: self.slns['lat_' + mode_string],
'lng_' + mode_string: self.slns['lng_' + mode_string]
}
current = {}
if len(self.slns['lat_' + mode_string]) != 0:
current = {
'cur_lat_' + mode_string:
[self.slns['lat_' + mode_string][-1]],
'cur_lng_' + mode_string:
[self.slns['lng_' + mode_string][-1]]
}
pending_update.update(self._get_update_current(current))
self.plot_data.update_data(pending_update)
def _update_sln_data_by_mode(self, soln, mode_string):
# do backend deque updates for a new solution of type
# mode string
self.scaling_lock.acquire()
lat = (soln.lat - self.offset[0]) * self.sf[0]
lng = (soln.lon - self.offset[1]) * self.sf[1]
self.slns['lat_' + mode_string].append(lat)
self.slns['lng_' + mode_string].append(lng)
self.scaling_lock.release()
def _clr_sln_data(self):
for each in self.slns:
self.slns[each].clear()
def _clear_history(self):
for each in self.slns:
self.slns[each].clear()
pending_update = {
'lat_spp': [],
'lng_spp': [],
'alt_spp': [],
'lat_dgnss': [],
'lng_dgnss': [],
'alt_dgnss': [],
'lat_float': [],
'lng_float': [],
'alt_float': [],
'lat_fixed': [],
'lng_fixed': [],
'alt_fixed': [],
'lat_sbas': [],
'lng_sbas': [],
'alt_sbas': [],
'lat_dr': [],
'lng_dr': [],
'alt_dr': []
}
pending_update.update(self._get_update_current())
self.plot_data.update(pending_update)
def _clear_button_fired(self):
self._clear_history()
def age_corrections_callback(self, sbp_msg, **metadata):
age_msg = MsgAgeCorrections(sbp_msg)
if age_msg.age != 0xFFFF:
self.age_corrections = age_msg.age / 10.0
else:
self.age_corrections = None
def update_table(self):
self.table = self.pos_table + self.vel_table + self.dops_table
def auto_survey(self):
if len(self.latitude_list) != 0:
self.latitude = sum(self.lats) / len(self.lats)
self.altitude = sum(self.alts) / len(self.alts)
self.longitude = sum(self.lngs) / len(self.lng)
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)
# this list allows us to tell GUI thread which solutions to update
# (if we decide not to update at full data rate)
# we move into using short strings for each solution mode instead of
# numbers for developer friendliness
if self.last_pos_mode != 0:
mode_string = mode_string_dict[self.last_pos_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()
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))
# 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 time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD:
GUI.invoke_later(self._solution_draw)
def _display_units_changed(self):
self.recenter = True # recenter flag tells _solution_draw to update view extents
# we store current extents of plot and current scalefactlrs
self.prev_extents = (self.plot.index_range.low_setting,
self.plot.index_range.high_setting,
self.plot.value_range.low_setting,
self.plot.value_range.high_setting)
self.prev_offsets = (self.offset[0], self.offset[1])
self.prev_sfs = (self.sf[0], self.sf[1])
self.scaling_lock.acquire()
if self.display_units == "meters":
self.offset = (
np.mean(
np.array(self.lats)[~(np.equal(np.array(self.modes), 0))]),
np.mean(
np.array(self.lngs)[~(np.equal(np.array(self.modes), 0))]),
np.mean(
np.array(self.alts)[~(np.equal(np.array(self.modes), 0))]))
(self.meters_per_lat, self.meters_per_lon) = meters_per_deg(
np.mean(
np.array(self.lats)[~(np.equal(np.array(self.modes), 0))]))
self.sf = (self.meters_per_lat, self.meters_per_lon)
self.plot.value_axis.title = 'Latitude (meters)'
self.plot.index_axis.title = 'Longitude (meters)'
else:
self.offset = (0, 0, 0)
self.sf = (1, 1)
self.plot.value_axis.title = 'Latitude (degrees)'
self.plot.index_axis.title = 'Longitude (degrees)'
self.scaling_lock.release()
# now we update the existing sln deques to go from meters back to degrees or vice versa
for each_array in self.slns:
index = 0 if 'lat' else 1
# going from degrees to meters; do scaling with new offset and sf
if self.display_units == "meters":
self.slns[each_array] = deque(
(np.array(self.slns[each_array]) - self.offset[index]) *
self.sf[index],
maxlen=PLOT_HISTORY_MAX)
# going from degrees to meters; do inverse scaling with former offset and sf
if self.display_units == "degrees":
self.slns[each_array] = deque(
np.array(self.slns[each_array]) / self.prev_sfs[index] +
self.prev_offsets[index],
maxlen=PLOT_HISTORY_MAX)
def rescale_for_units_change(self):
# Chaco scales view automatically when 'auto' is stored
if self.prev_extents[0] != 'auto':
# Otherwise use has used mousewheel zoom and we need to transform
if self.display_units == 'meters':
new_scaling = (
(self.prev_extents[0] - self.offset[1]) * self.sf[1],
(self.prev_extents[1] - self.offset[1]) * self.sf[1],
(self.prev_extents[2] - self.offset[0]) * self.sf[0],
(self.prev_extents[3] - self.offset[0]) * self.sf[0])
else:
new_scaling = (self.prev_extents[0] / self.prev_sfs[1] +
self.prev_offsets[1],
self.prev_extents[1] / self.prev_sfs[1] +
self.prev_offsets[1],
self.prev_extents[2] / self.prev_sfs[0] +
self.prev_offsets[0],
self.prev_extents[3] / self.prev_sfs[0] +
self.prev_offsets[0])
# set plot scaling accordingly
self.plot.index_range.low_setting = new_scaling[0]
self.plot.index_range.high_setting = new_scaling[1]
self.plot.value_range.low_setting = new_scaling[2]
self.plot.value_range.high_setting = new_scaling[3]
def _solution_draw(self):
self.last_plot_update_time = time.time()
self.list_lock.acquire()
# update our "current solution" icon
for mode_string in list(self.pending_draw_modes):
self._synchronize_plot_data_by_mode(mode_string)
self.pending_draw_modes.remove(mode_string)
self.list_lock.release()
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(
(self.last_soln.lon - self.offset[1]) * self.sf[1] - d,
(self.last_soln.lon - self.offset[1]) * self.sf[1] + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(
(self.last_soln.lat - self.offset[0]) * self.sf[0] - d,
(self.last_soln.lat - self.offset[0]) * self.sf[0] + d)
if self.zoomall:
self.recenter = False
plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float',
'lng_fixed', 'lng_sbas', 'lng_dr'),
('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed',
'lat_sbas', 'lat_dr'))
if self.recenter:
try:
self.rescale_for_units_change()
self.recenter = False
except AttributeError:
pass
def dops_callback(self, sbp_msg, **metadata):
flags = 0
if sbp_msg.msg_type == SBP_MSG_DOPS_DEP_A:
dops = MsgDopsDepA(sbp_msg)
flags = 1
else:
dops = MsgDops(sbp_msg)
flags = dops.flags
if flags != 0:
self.dops_table = [('PDOP', '%.1f' % (dops.pdop * 0.01)),
('GDOP', '%.1f' % (dops.gdop * 0.01)),
('TDOP', '%.1f' % (dops.tdop * 0.01)),
('HDOP', '%.1f' % (dops.hdop * 0.01)),
('VDOP', '%.1f' % (dops.vdop * 0.01))]
else:
self.dops_table = [('PDOP', EMPTY_STR), ('GDOP', EMPTY_STR),
('TDOP', EMPTY_STR), ('HDOP', EMPTY_STR),
('VDOP', EMPTY_STR)]
self.dops_table.append(('DOPS Flags', '0x%03x' % flags))
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 gps_time_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_GPS_TIME_DEP_A:
time_msg = MsgGPSTimeDepA(sbp_msg)
flags = 1
elif sbp_msg.msg_type == SBP_MSG_GPS_TIME:
time_msg = MsgGPSTime(sbp_msg)
flags = time_msg.flags
if flags != 0:
self.week = time_msg.wn
self.nsec = time_msg.ns_residual
def utc_time_callback(self, sbp_msg, **metadata):
tmsg = MsgUtcTime(sbp_msg)
seconds = math.floor(tmsg.seconds)
microseconds = int(tmsg.ns / 1000.00)
if tmsg.flags & 0x1 == 1:
dt = datetime.datetime(tmsg.year, tmsg.month, tmsg.day, tmsg.hours,
tmsg.minutes, tmsg.seconds, microseconds)
self.utc_time = dt
self.utc_time_flags = tmsg.flags
if (tmsg.flags >> 3) & 0x3 == 0:
self.utc_source = "Factory Default"
elif (tmsg.flags >> 3) & 0x3 == 1:
self.utc_source = "Non Volatile Memory"
elif (tmsg.flags >> 3) & 0x3 == 2:
self.utc_source = "Decoded this Session"
else:
self.utc_source = "Unknown"
else:
self.utc_time = None
self.utc_source = None
def __init__(self, link, dirname=''):
super(SolutionView, self).__init__()
self.pending_draw_modes = []
self.recenter = False
self.offset = (0, 0, 0)
self.sf = (1, 1)
self.list_lock = threading.Lock()
self.scaling_lock = threading.Lock()
self.slns = {
'lat_spp': deque(maxlen=PLOT_HISTORY_MAX),
'lng_spp': deque(maxlen=PLOT_HISTORY_MAX),
'alt_spp': deque(maxlen=PLOT_HISTORY_MAX),
'lat_dgnss': deque(maxlen=PLOT_HISTORY_MAX),
'lng_dgnss': deque(maxlen=PLOT_HISTORY_MAX),
'alt_dgnss': deque(maxlen=PLOT_HISTORY_MAX),
'lat_float': deque(maxlen=PLOT_HISTORY_MAX),
'lng_float': deque(maxlen=PLOT_HISTORY_MAX),
'alt_float': deque(maxlen=PLOT_HISTORY_MAX),
'lat_fixed': deque(maxlen=PLOT_HISTORY_MAX),
'lng_fixed': deque(maxlen=PLOT_HISTORY_MAX),
'alt_fixed': deque(maxlen=PLOT_HISTORY_MAX),
'lat_sbas': deque(maxlen=PLOT_HISTORY_MAX),
'lng_sbas': deque(maxlen=PLOT_HISTORY_MAX),
'alt_sbas': deque(maxlen=PLOT_HISTORY_MAX),
'lat_dr': deque(maxlen=PLOT_HISTORY_MAX),
'lng_dr': deque(maxlen=PLOT_HISTORY_MAX),
'alt_dr': deque(maxlen=PLOT_HISTORY_MAX)
}
self.lats = deque(maxlen=PLOT_HISTORY_MAX)
self.lngs = deque(maxlen=PLOT_HISTORY_MAX)
self.alts = deque(maxlen=PLOT_HISTORY_MAX)
self.tows = deque(maxlen=PLOT_HISTORY_MAX)
self.modes = deque(maxlen=PLOT_HISTORY_MAX)
self.log_file = None
self.directory_name_v = dirname
self.directory_name_p = dirname
self.vel_log_file = None
self.last_stime_update = 0
self.last_soln = None
self.counter = 0
self.latitude_list = []
self.longitude_list = []
self.altitude_list = []
self.altitude = 0
self.longitude = 0
self.latitude = 0
self.last_pos_mode = 0
self.ins_used = False
self.last_plot_update_time = 0
self.plot_data = ArrayPlotData(lat_spp=[],
lng_spp=[],
alt_spp=[],
cur_lat_spp=[],
cur_lng_spp=[],
lat_dgnss=[],
lng_dgnss=[],
alt_dgnss=[],
cur_lat_dgnss=[],
cur_lng_dgnss=[],
lat_float=[],
lng_float=[],
alt_float=[],
cur_lat_float=[],
cur_lng_float=[],
lat_fixed=[],
lng_fixed=[],
alt_fixed=[],
cur_lat_fixed=[],
cur_lng_fixed=[],
lat_sbas=[],
lng_sbas=[],
cur_lat_sbas=[],
cur_lng_sbas=[],
lng_dr=[],
lat_dr=[],
cur_lat_dr=[],
cur_lng_dr=[])
self.plot = Plot(self.plot_data)
# 1000 point buffer
self.plot.plot(('lng_spp', 'lat_spp'),
type='line',
line_width=0.1,
name='',
color=color_dict[SPP_MODE])
self.plot.plot(('lng_spp', 'lat_spp'),
type='scatter',
name='',
color=color_dict[SPP_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_dgnss', 'lat_dgnss'),
type='line',
line_width=0.1,
name='',
color=color_dict[DGNSS_MODE])
self.plot.plot(('lng_dgnss', 'lat_dgnss'),
type='scatter',
name='',
color=color_dict[DGNSS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_float', 'lat_float'),
type='line',
line_width=0.1,
name='',
color=color_dict[FLOAT_MODE])
self.plot.plot(('lng_float', 'lat_float'),
type='scatter',
name='',
color=color_dict[FLOAT_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_fixed', 'lat_fixed'),
type='line',
line_width=0.1,
name='',
color=color_dict[FIXED_MODE])
self.plot.plot(('lng_fixed', 'lat_fixed'),
type='scatter',
name='',
color=color_dict[FIXED_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_sbas', 'lat_sbas'),
type='line',
line_width=0.1,
name='',
color=color_dict[SBAS_MODE])
self.plot.plot(('lng_sbas', 'lat_sbas'),
type='scatter',
name='',
color=color_dict[SBAS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_dr', 'lat_dr'),
type='line',
line_width=0.1,
name='',
color=color_dict[DR_MODE])
self.plot.plot(('lng_dr', 'lat_dr'),
type='scatter',
color=color_dict[DR_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
# current values
spp = self.plot.plot(('cur_lng_spp', 'cur_lat_spp'),
type='scatter',
name=mode_dict[SPP_MODE],
color=color_dict[SPP_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
dgnss = self.plot.plot(('cur_lng_dgnss', 'cur_lat_dgnss'),
type='scatter',
name=mode_dict[DGNSS_MODE],
color=color_dict[DGNSS_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
rtkfloat = self.plot.plot(('cur_lng_float', 'cur_lat_float'),
type='scatter',
name=mode_dict[FLOAT_MODE],
color=color_dict[FLOAT_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
rtkfix = self.plot.plot(('cur_lng_fixed', 'cur_lat_fixed'),
type='scatter',
name=mode_dict[FIXED_MODE],
color=color_dict[FIXED_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
sbas = self.plot.plot(('cur_lng_sbas', 'cur_lat_sbas'),
type='scatter',
name=mode_dict[SBAS_MODE],
color=color_dict[SBAS_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
dr = self.plot.plot(('cur_lng_dr', 'cur_lat_dr'),
type='scatter',
name=mode_dict[DR_MODE],
color=color_dict[DR_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
plot_labels = ['SPP', 'SBAS', 'DGPS', 'RTK float', 'RTK fixed', 'DR']
plots_legend = dict(
zip(plot_labels, [spp, sbas, dgnss, rtkfloat, rtkfix, dr]))
self.plot.legend.plots = plots_legend
self.plot.legend.labels = plot_labels # sets order
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'Longitude (degrees)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'Latitude (degrees)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.link = link
self.link.add_callback(self.pos_llh_callback,
[SBP_MSG_POS_LLH_DEP_A, SBP_MSG_POS_LLH])
self.link.add_callback(self.vel_ned_callback,
[SBP_MSG_VEL_NED_DEP_A, SBP_MSG_VEL_NED])
self.link.add_callback(self.dops_callback,
[SBP_MSG_DOPS_DEP_A, SBP_MSG_DOPS])
self.link.add_callback(self.gps_time_callback,
[SBP_MSG_GPS_TIME_DEP_A, SBP_MSG_GPS_TIME])
self.link.add_callback(self.utc_time_callback, [SBP_MSG_UTC_TIME])
self.link.add_callback(self.age_corrections_callback,
SBP_MSG_AGE_CORRECTIONS)
self.week = None
self.utc_time = None
self.age_corrections = None
self.nsec = 0
self.meters_per_lat = None
self.meters_per_lon = None
self.python_console_cmds = {
'solution': self,
}
class SettingsView(HasTraits):
"""Traits-defined console settings view.
link : object
Serial driver object.
read_finished_functions : list
Callbacks to call on finishing a settings read.
name_of_yaml_file : str
Settings to read from (defaults to settings.yaml)
expert : bool
Show expert settings (defaults to False)
gui_mode : bool
??? (defaults to True)
skip : bool
Skip reading of the settings (defaults to False). Intended for
use when reading from network connections.
"""
show_auto_survey = Bool(False)
settings_yaml = list()
auto_survey = SVGButton(
label='Auto Survey',
tooltip='Auto populate surveyed lat, lon and alt fields',
filename='',
width=16,
height=20)
settings_read_button = SVGButton(
label='Reload',
tooltip='Reload settings from Piksi',
filename=os.path.join(determine_path(), 'images', 'fontawesome',
'refresh.svg'),
width=16,
height=20)
settings_save_button = SVGButton(
label='Save to Flash',
tooltip='Save settings to Flash',
filename=os.path.join(determine_path(), 'images', 'fontawesome',
'download.svg'),
width=16,
height=20)
factory_default_button = SVGButton(
label='Reset to Defaults',
tooltip='Reset to Factory Defaults',
filename=os.path.join(determine_path(), 'images', 'fontawesome',
'exclamation-triangle.svg'),
width=16,
height=20)
settings_list = List(SettingBase)
expert = Bool()
selected_setting = Instance(SettingBase)
traits_view = View(
HSplit(
Item(
'settings_list',
editor=TabularEditor(
adapter=SimpleAdapter(),
editable_labels=False,
auto_update=True,
editable=False,
selected='selected_setting'),
show_label=False, ),
VGroup(
HGroup(
Item('settings_read_button', show_label=False),
Item('settings_save_button', show_label=False),
Item('factory_default_button', show_label=False),
Item(
'auto_survey',
show_label=False,
visible_when='show_auto_survey'), ),
HGroup(
Item(
'expert',
label="Show Advanced Settings",
show_label=True)),
Item('selected_setting', style='custom', show_label=False), ),
))
def _selected_setting_changed(self):
if self.selected_setting:
if (self.selected_setting.name in [
'surveyed_position', 'broadcast', 'surveyed_lat',
'surveyed_lon', 'surveyed_alt'
] and self.lat != 0 and self.lon != 0):
self.show_auto_survey = True
else:
self.show_auto_survey = False
def _expert_changed(self, info):
try:
self.settings_display_setup(do_read_finished=False)
except AttributeError:
pass
def _settings_read_button_fired(self):
self.enumindex = 0
self.ordering_counter = 0
self.link(MsgSettingsReadByIndexReq(index=self.enumindex))
def _settings_save_button_fired(self):
self.link(MsgSettingsSave())
def _factory_default_button_fired(self):
confirm_prompt = prompt.CallbackPrompt(
title="Reset to Factory Defaults?",
actions=[prompt.close_button, prompt.reset_button],
callback=self.reset_factory_defaults)
confirm_prompt.text = "This will erase all settings and then reset the device.\n" \
+ "Are you sure you want to reset to factory defaults?"
confirm_prompt.run(block=False)
def reset_factory_defaults(self):
# Reset the Piksi, with flag set to restore default settings
self.link(MsgReset(flags=1))
def _auto_survey_fired(self):
confirm_prompt = prompt.CallbackPrompt(
title="Auto populate surveyed position?",
actions=[prompt.close_button, prompt.auto_survey_button],
callback=self.auto_survey_fn)
confirm_prompt.text = "\n" \
+ "This will set the Surveyed Position section to the \n" \
+ "mean position of the last 1000 position solutions.\n \n" \
+ "The fields that will be auto-populated are: \n" \
+ "Surveyed Lat \n" \
+ "Surveyed Lon \n" \
+ "Surveyed Alt \n \n" \
+ "The surveyed position will be an approximate value. \n" \
+ "This may affect the relative accuracy of Piksi. \n \n" \
+ "Are you sure you want to auto-populate the Surveyed Position section?"
confirm_prompt.run(block=False)
def auto_survey_fn(self):
lat_value = str(self.lat)
lon_value = str(self.lon)
alt_value = str(self.alt)
self.settings['surveyed_position']['surveyed_lat'].value = lat_value
self.settings['surveyed_position']['surveyed_lon'].value = lon_value
self.settings['surveyed_position']['surveyed_alt'].value = alt_value
self.settings_display_setup(do_read_finished=False)
# Callbacks for receiving messages
def settings_display_setup(self, do_read_finished=True):
self.settings_list = []
sections = sorted(self.settings.keys())
for sec in sections:
this_section = []
for name, setting in sorted(
self.settings[sec].iteritems(),
key=lambda n_s: n_s[1].ordering):
if not setting.expert or (self.expert and setting.expert):
this_section.append(setting)
if this_section:
self.settings_list.append(SectionHeading(sec))
self.settings_list += this_section
# call read_finished_functions as needed
if do_read_finished:
for cb in self.read_finished_functions:
if self.gui_mode:
GUI.invoke_later(cb)
else:
cb()
def settings_read_by_index_done_callback(self, sbp_msg, **metadata):
self.settings_display_setup()
def settings_read_resp_callback(self, sbp_msg, **metadata):
confirmed_set = True
settings_list = sbp_msg.setting.split("\0")
if len(settings_list) <= 3:
print("Received malformed settings read response {0}".format(
sbp_msg))
confirmed_set = False
try:
if self.settings[settings_list[0]][settings_list[1]].value != settings_list[2]:
try:
float_val = float(self.settings[settings_list[0]][
settings_list[1]].value)
float_val2 = float(settings_list[2])
if abs(float_val - float_val2) > 0.000001:
confirmed_set = False
except ValueError:
confirmed_set = False
if not confirmed_set:
pass
# We pass if the new value doesn't match current console value. It would be nice to update it, but that may cause side effects.
self.settings[settings_list[0]][settings_list[
1]].confirmed_set = confirmed_set
except KeyError:
return
def settings_read_by_index_callback(self, sbp_msg, **metadata):
section, setting, value, format_type = sbp_msg.payload[2:].split(
'\0')[:4]
self.ordering_counter += 1
if format_type == '':
format_type = None
else:
setting_type, setting_format = format_type.split(':')
if section not in self.settings:
self.settings[section] = {}
if format_type is None:
# Plain old setting, no format information
self.settings[section][setting] = Setting(
setting,
section,
value,
ordering=self.ordering_counter,
settings=self)
else:
if setting_type == 'enum':
enum_values = setting_format.split(',')
self.settings[section][setting] = EnumSetting(
setting,
section,
value,
ordering=self.ordering_counter,
values=enum_values,
settings=self)
else:
# Unknown type, just treat is as a string
self.settings[section][setting] = Setting(
setting, section, value, settings=self)
if self.enumindex == sbp_msg.index:
self.enumindex += 1
self.link(MsgSettingsReadByIndexReq(index=self.enumindex))
def piksi_startup_callback(self, sbp_msg, **metadata):
self.settings.clear()
self._settings_read_button_fired()
def set(self, section, name, value):
self.link(
MsgSettingsWrite(setting='%s\0%s\0%s\0' % (section, name, value)))
def cleanup(self):
""" Remove callbacks from serial link. """
self.link.remove_callback(self.piksi_startup_callback, SBP_MSG_STARTUP)
self.link.remove_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.link.remove_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.link.remove_callback(self.settings_read_by_index_done_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
def __enter__(self):
return self
def __exit__(self, *args):
self.cleanup()
def __init__(self,
link,
read_finished_functions=[],
name_of_yaml_file="settings.yaml",
expert=False,
gui_mode=True,
skip=False):
super(SettingsView, self).__init__()
self.expert = expert
self.show_auto_survey = False
self.gui_mode = gui_mode
self.enumindex = 0
self.settings = {}
self.link = link
self.link.add_callback(self.piksi_startup_callback, SBP_MSG_STARTUP)
self.link.add_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.link.add_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.link.add_callback(self.settings_read_by_index_done_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
self.link.add_callback(self.settings_read_resp_callback,
SBP_MSG_SETTINGS_READ_RESP)
# Read in yaml file for setting metadata
self.settings_yaml = SettingsList(name_of_yaml_file)
# List of functions to be executed after all settings are read.
# No support for arguments currently.
self.read_finished_functions = read_finished_functions
self.setting_detail = SettingBase()
if not skip:
try:
self._settings_read_button_fired()
except IOError:
print(
"IOError in settings_view startup call of _settings_read_button_fired."
)
print("Verify that write permissions exist on the port.")
self.python_console_cmds = {'settings': self}
class BaselineView(HasTraits):
python_console_cmds = Dict()
ns = List()
es = List()
ds = List()
table = List()
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
running = Bool(True)
position_centered = Bool(False)
clear_button = SVGButton(label='',
tooltip='Clear',
filename=os.path.join(os.path.dirname(__file__),
'images', 'iconic',
'x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(label='',
tooltip='Zoom All',
filename=os.path.join(os.path.dirname(__file__),
'images', 'iconic',
'fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(label='',
tooltip='Center on Baseline',
toggle=True,
filename=os.path.join(os.path.dirname(__file__),
'images', 'iconic',
'target.svg'),
width=16,
height=16)
paused_button = SVGButton(
label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=os.path.join(os.path.dirname(__file__), 'images', 'iconic',
'pause.svg'),
toggle_filename=os.path.join(os.path.dirname(__file__), 'images',
'iconic', 'play.svg'),
width=16,
height=16)
reset_button = Button(label='Reset Filters')
reset_iar_button = Button(label='Reset IAR')
init_base_button = Button(label='Init. with known baseline')
traits_view = View(
HSplit(
Item('table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
Item('reset_button', show_label=False),
Item('reset_iar_button', show_label=False),
Item('init_base_button', show_label=False),
),
Item(
'plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8)),
))))
def _zoomall_button_fired(self):
self.plot.index_range.low_setting = 'auto'
self.plot.index_range.high_setting = 'auto'
self.plot.value_range.low_setting = 'auto'
self.plot.value_range.high_setting = 'auto'
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_button_fired(self):
self.link.send_message(sbp_messages.RESET_FILTERS, '\x00')
def _reset_iar_button_fired(self):
self.link.send_message(sbp_messages.RESET_FILTERS, '\x01')
def _init_base_button_fired(self):
self.link.send_message(sbp_messages.INIT_BASE, '')
def _clear_button_fired(self):
self.ns = []
self.es = []
self.ds = []
self.plot_data.set_data('n', [])
self.plot_data.set_data('e', [])
self.plot_data.set_data('d', [])
self.plot_data.set_data('t', [])
def _baseline_callback_ecef(self, data):
#Don't do anything for ECEF currently
return
def iar_state_callback(self, data):
self.num_hyps = struct.unpack('<I', data)[0]
def _baseline_callback_ned(self, data):
# Updating an ArrayPlotData isn't thread safe (see chaco issue #9), so
# actually perform the update in the UI thread.
if self.running:
GUI.invoke_later(self.baseline_callback, data)
def update_table(self):
self._table_list = self.table.items()
def gps_time_callback(self, data):
self.week = sbp_messages.GPSTime(data).wn
self.nsec = sbp_messages.GPSTime(data).ns
def baseline_callback(self, data):
soln = sbp_messages.BaselineNED(data)
table = []
soln.n = soln.n * 1e-3
soln.e = soln.e * 1e-3
soln.d = soln.d * 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)
table.append(('GPS Time', t))
table.append(('GPS Week', str(self.week)))
if self.log_file is None:
self.log_file = open(
time.strftime("baseline_log_%Y%m%d-%H%M%S.csv"), 'w')
self.log_file.write('%s,%.4f,%.4f,%.4f,%.4f,%d,0x%02x,%d\n' %
(str(t), soln.n, soln.e, soln.d, dist,
soln.n_sats, soln.flags, self.num_hyps))
self.log_file.flush()
table.append(('GPS ToW', tow))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('Dist.', dist))
table.append(('Num. Sats.', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
if soln.flags & 1:
table.append(('Mode', 'Fixed RTK'))
else:
table.append(('Mode', 'Float'))
table.append(('IAR Num. Hyps.', self.num_hyps))
self.ns.append(soln.n)
self.es.append(soln.e)
self.ds.append(soln.d)
self.ns = self.ns[-1000:]
self.es = self.es[-1000:]
self.ds = self.ds[-1000:]
self.plot_data.set_data('n', self.ns)
self.plot_data.set_data('e', self.es)
self.plot_data.set_data('d', self.ds)
self.plot_data.set_data('ref_n', [0.0, soln.n])
self.plot_data.set_data('ref_e', [0.0, soln.e])
self.plot_data.set_data('ref_d', [0.0, soln.d])
t = range(len(self.ns))
self.plot_data.set_data('t', t)
if self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.n - d, soln.n + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.e - d, soln.e + d)
self.table = table
def __init__(self, link):
super(BaselineView, self).__init__()
self.log_file = None
self.num_hyps = 0
self.plot_data = ArrayPlotData(n=[0.0],
e=[0.0],
d=[0.0],
t=[0.0],
ref_n=[0.0],
ref_e=[0.0],
ref_d=[0.0])
self.plot = Plot(self.plot_data)
self.plot.plot(('e', 'n'),
type='line',
name='line',
color=(0, 0, 0, 0.1))
self.plot.plot(('e', 'n'),
type='scatter',
name='points',
color='blue',
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('ref_e', 'ref_n'),
type='scatter',
color='red',
marker='plus',
marker_size=5,
line_width=1.5)
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.padding = (0, 1, 0, 1)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.week = None
self.nsec = 0
self.link = link
self.link.add_callback(sbp_messages.SBP_BASELINE_NED,
self._baseline_callback_ned)
self.link.add_callback(sbp_messages.SBP_BASELINE_ECEF,
self._baseline_callback_ecef)
self.link.add_callback(sbp_messages.IAR_STATE, self.iar_state_callback)
self.link.add_callback(sbp_messages.SBP_GPS_TIME,
self.gps_time_callback)
self.python_console_cmds = {'baseline': self}
class SbpRelayView(HasTraits):
"""
Class allows user to specify port, IP address, and message set
to relay over UDP.
"""
running = Bool(False)
_network_info = List()
configured = Bool(False)
broadcasting = Bool(False)
msg_enum = Enum('Observations', 'All')
ip_ad = String(DEFAULT_UDP_ADDRESS)
port = Int(DEFAULT_UDP_PORT)
information = String(
'UDP Streaming\n\nBroadcast SBP information received by'
' the console to other machines or processes over UDP. With the \'Observations\''
' radio button selected, the console will broadcast the necessary information'
' for a rover Piksi to acheive an RTK solution.'
'\n\nThis can be used to stream observations to a remote Piksi through'
' aircraft telemetry via ground control software such as MAVProxy or'
' Mission Planner.')
start = Button(label='Start', toggle=True, width=32)
stop = Button(label='Stop', toggle=True, width=32)
network_refresh_button = SVGButton(
label='Refresh Network Status',
tooltip='Refresh Network Status',
filename=resource_filename('console/images/fontawesome/refresh.svg'),
width=16,
height=16,
aligment='center')
cell_modem_view = Instance(CellModemView)
view = View(
VGroup(
spring,
HGroup(
VGroup(
Item('msg_enum',
label="Messages to broadcast",
style='custom',
enabled_when='not running'),
Item('ip_ad',
label='IP Address',
enabled_when='not running'),
Item('port', label="Port", enabled_when='not running'),
HGroup(
spring,
UItem('start',
enabled_when='not running',
show_label=False),
UItem('stop', enabled_when='running',
show_label=False), spring)),
VGroup(
Item('information',
label="Notes",
height=10,
editor=MultilineTextEditor(
TextEditor(multi_line=True)),
style='readonly',
show_label=False,
resizable=True,
padding=15),
spring,
)), spring,
HGroup(
Item('cell_modem_view', style='custom', show_label=False),
VGroup(Item(
'_network_info',
style='readonly',
editor=TabularEditor(adapter=SimpleNetworkAdapter()),
show_label=False,
),
Item('network_refresh_button',
show_label=False,
width=0.50),
show_border=True,
label="Network"),
)))
def _network_callback(self, m, **metadata):
txstr = sizeof_fmt(m.tx_bytes),
rxstr = sizeof_fmt(m.rx_bytes)
if m.interface_name.startswith(
b'ppp0'): # Hack for ppp tx and rx which doesn't work
txstr = "---"
rxstr = "---"
elif m.interface_name.startswith(b'lo') or m.interface_name.startswith(
b'sit0'):
return
table_row = ((m.interface_name.decode('ascii'),
ip_bytes_to_string(m.ipv4_address),
((m.flags & (1 << 6)) != 0), txstr, rxstr))
exists = False
for i, each in enumerate(self._network_info):
if each[0][0] == table_row[0][0]:
self._network_info[i] = table_row
exists = True
if not exists:
self._network_info.append(table_row)
def __init__(self, link):
"""
Traits tab with UI for UDP broadcast of SBP.
Parameters
----------
link : sbp.client.handler.Handler
Link for SBP transfer to/from Piksi.
device_uid : str
Piksi Device UUID (defaults to None)
whitelist : [int] | None
Piksi Device UUID (defaults to None)
"""
self.link = link
# Whitelist used for UDP broadcast view
self.cell_modem_view = CellModemView(link)
self.msgs = OBS_MSGS
# register a callback when the msg_enum trait changes
self.on_trait_change(self.update_msgs, 'msg_enum')
self.python_console_cmds = {'update': self}
self.cellmodem_interface_name = "ppp0"
self.link.add_callback(self._network_callback,
SBP_MSG_NETWORK_STATE_RESP)
def update_msgs(self):
"""Updates the instance variable msgs which store the msgs that we
will send over UDP.
"""
if self.msg_enum == 'Observations':
self.msgs = OBS_MSGS
elif self.msg_enum == 'All':
self.msgs = [None]
else:
raise NotImplementedError
def _prompt_setting_error(self, text):
"""Nonblocking prompt for a device setting error.
Parameters
----------
text : str
Helpful error message for the user
"""
prompt = CallbackPrompt(title="Setting Error", actions=[close_button])
prompt.text = text
prompt.run(block=False)
def update_network_state(self):
self._network_refresh_button_fired()
def _network_refresh_button_fired(self):
self._network_info = []
self.link(MsgNetworkStateReq())
def _start_fired(self):
"""Handle start udp broadcast button. Registers callbacks on
self.link for each of the self.msgs If self.msgs is None, it
registers one generic callback for all messages.
"""
self.running = True
try:
self.func = UdpLogger(self.ip_ad, self.port)
self.link.add_callback(self.func, self.msgs)
except: # noqa
import traceback
print(traceback.format_exc())
def _stop_fired(self):
"""Handle the stop udp broadcast button. It uses the self.funcs and
self.msgs to remove the callbacks that were registered when the
start button was pressed.
"""
try:
self.link.remove_callback(self.func, self.msgs)
self.func.__exit__()
self.func = None
self.running = False
except: # noqa
import traceback
print(traceback.format_exc())
class PCBaseControl(NoPlotControl):
eq_axis = Bool(False)
# vis_toggle = Button('Visibility')
y_down = SVGButton(filename=pjoin(img_path, 'y_down.svg'),
width=32,
height=32)
y_up = SVGButton(filename=pjoin(img_path, 'y_up.svg'), width=32, height=32)
x_down = SVGButton(filename=pjoin(img_path, 'x_down.svg'),
width=32,
height=32)
x_up = SVGButton(filename=pjoin(img_path, 'x_up.svg'), width=32, height=32)
reset_xy = SVGButton(filename=pjoin(img_path, 'reset_xy.svg'),
width=32,
height=32)
subset_groups = List()
traits_view = View(
Group(Item('model',
editor=ComponentEditor(bgcolor=bg_color),
show_label=False),
Label('Scroll to zoom and drag to pan in plot.'),
Include('plot_controllers'),
orientation="vertical"))
# @on_trait_change('vis_toggle')
# def switch_visibility(self, obj, name, new):
# obj.model.show_points()
@on_trait_change('eq_axis')
def switch_axis(self, obj, name, new):
obj.model.toggle_eq_axis(new)
@on_trait_change('reset_xy')
def pc_axis_reset(self, obj, name, new):
obj.model.set_x_y_pc(1, 2)
@on_trait_change('x_up')
def pc_axis_x_up(self, obj, name, new):
x, y, n = obj.model.get_x_y_status()
if x < n:
x += 1
else:
x = 1
obj.model.set_x_y_pc(x, y)
@on_trait_change('x_down')
def pc_axis_x_down(self, obj, name, new):
x, y, n = obj.model.get_x_y_status()
if x > 1:
x -= 1
else:
x = n
obj.model.set_x_y_pc(x, y)
@on_trait_change('y_up')
def pc_axis_y_up(self, obj, name, new):
x, y, n = obj.model.get_x_y_status()
if y < n:
y += 1
else:
y = 1
obj.model.set_x_y_pc(x, y)
@on_trait_change('y_down')
def pc_axis_y_down(self, obj, name, new):
x, y, n = obj.model.get_x_y_status()
if y > 1:
y -= 1
else:
y = n
obj.model.set_x_y_pc(x, y)
class BaselineView(HasTraits):
# This mapping should match the flag definitions in libsbp for
# the MsgBaselineNED message. While this isn't strictly necessary
# it helps avoid confusion
python_console_cmds = Dict()
last_plot_update_time = Float()
last_stale_update_time = Float()
table = List()
logging_b = Bool(False)
directory_name_b = File
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(
label='',
tooltip='Clear',
filename=resource_filename('console/images/iconic/x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(
label='',
tooltip='Zoom All',
toggle=True,
filename=resource_filename('console/images/iconic/fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(
label='',
tooltip='Center on Baseline',
toggle=True,
filename=resource_filename('console/images/iconic/target.svg'),
width=16,
height=16)
paused_button = SVGButton(
label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
width=16,
height=16)
reset_button = Button(label='Reset Filters')
traits_view = View(
HSplit(
Item('table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
Item('reset_button', show_label=False),
),
Item(
'plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8)),
))))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_button_fired(self):
self.link(MsgResetFilters(filter=0))
def _get_update_current(self, current_dict={}):
out_dict = {
'cur_n_fixed': [],
'cur_e_fixed': [],
'cur_d_fixed': [],
'cur_n_float': [],
'cur_e_float': [],
'cur_d_float': [],
'cur_n_dgnss': [],
'cur_e_dgnss': [],
'cur_d_dgnss': []
}
out_dict.update(current_dict)
return out_dict
def _synchronize_plot_data_by_mode(self,
mode_string,
update_current=False):
# do all required plot_data updates for a single
# new solution with mode defined by mode_string
pending_update = {
'n_' + mode_string:
[n for n in self.slns['n_' + mode_string] if not np.isnan(n)],
'e_' + mode_string:
[e for e in self.slns['e_' + mode_string] if not np.isnan(e)]
}
if update_current:
current = {}
if len(pending_update['n_' + mode_string]) != 0:
current = {
'cur_n_' + mode_string:
[pending_update['n_' + mode_string][-1]],
'cur_e_' + mode_string:
[pending_update['e_' + mode_string][-1]]
}
else:
current = {
'cur_n_' + mode_string: [],
'cur_e_' + mode_string: []
}
pending_update.update(self._get_update_current(current))
self.plot_data.update_data(pending_update)
def _append_empty_sln_data(self, exclude_mode=None):
for each_mode in mode_string_dict.values():
if exclude_mode is None or each_mode != exclude_mode:
self.slns['n_' + each_mode].append(np.nan)
self.slns['e_' + each_mode].append(np.nan)
def _update_sln_data_by_mode(self, soln, mode_string):
# do backend deque updates for a new solution of type
# mode string
self.slns['n_' + mode_string].append(soln.n)
self.slns['e_' + mode_string].append(soln.e)
# Rotate old data out by appending to deque
self._append_empty_sln_data(exclude_mode=mode_string)
def _clr_sln_data(self):
for each in self.slns:
self.slns[each].clear()
def _reset_remove_current(self):
self.plot_data.update_data(self._get_update_current())
def _clear_history(self):
self._clr_sln_data()
pending_update = {
'n_fixed': [],
'e_fixed': [],
'd_fixed': [],
'n_float': [],
'e_float': [],
'd_float': [],
'n_dgnss': [],
'e_dgnss': [],
'd_dgnss': []
}
pending_update.update(self._get_update_current())
self.plot_data.update(pending_update)
def _clear_button_fired(self):
self._clear_history()
def age_corrections_callback(self, sbp_msg, **metadata):
age_msg = MsgAgeCorrections(sbp_msg)
if age_msg.age != 0xFFFF:
self.age_corrections = age_msg.age / 10.0
else:
self.age_corrections = None
def gps_time_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_GPS_TIME_DEP_A:
time_msg = MsgGPSTimeDepA(sbp_msg)
flags = 1
elif sbp_msg.msg_type == SBP_MSG_GPS_TIME:
time_msg = MsgGPSTime(sbp_msg)
flags = time_msg.flags
if flags != 0:
self.week = time_msg.wn
self.nsec = time_msg.ns_residual
def utc_time_callback(self, sbp_msg, **metadata):
tmsg = MsgUtcTime(sbp_msg)
microseconds = int(tmsg.ns / 1000.00)
if tmsg.flags & 0x1 == 1:
dt = datetime.datetime(tmsg.year, tmsg.month, tmsg.day, tmsg.hours,
tmsg.minutes, tmsg.seconds, microseconds)
self.utc_time = dt
self.utc_time_flags = tmsg.flags
if (tmsg.flags >> 3) & 0x3 == 0:
self.utc_source = "Factory Default"
elif (tmsg.flags >> 3) & 0x3 == 1:
self.utc_source = "Non Volatile Memory"
elif (tmsg.flags >> 3) & 0x3 == 2:
self.utc_source = "Decoded this Session"
else:
self.utc_source = "Unknown"
else:
self.utc_time = None
self.utc_source = None
def baseline_heading_callback(self, sbp_msg, **metadata):
headingMsg = MsgBaselineHeading(sbp_msg)
if headingMsg.flags & 0x7 != 0:
self.heading = headingMsg.heading * 1e-3
else:
self.heading = "---"
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 _solution_draw(self):
self.list_lock.acquire()
current_time = monotonic()
self.last_plot_update_time = current_time
pending_draw_modes = self.pending_draw_modes
current_mode = pending_draw_modes[-1] if len(
pending_draw_modes) > 0 else None
# Periodically, we make sure to redraw older data to expire old plot data
if current_time - self.last_stale_update_time > STALE_DATA_PERIOD:
# we don't update old solution modes every timestep to try and save CPU
pending_draw_modes = list(mode_string_dict.values())
self.last_stale_update_time = current_time
for mode_string in pending_draw_modes:
if self.running:
update_current = mode_string == current_mode if current_mode else True
self._synchronize_plot_data_by_mode(mode_string,
update_current)
if mode_string in self.pending_draw_modes:
self.pending_draw_modes.remove(mode_string)
self.list_lock.release()
# make the zoomall win over the position centered button
if not self.zoomall and self.position_centered and self.running:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(self.last_soln.e - d,
self.last_soln.e + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(self.last_soln.n - d,
self.last_soln.n + d)
if self.zoomall:
plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'),
('n_fixed', 'n_float', 'n_dgnss'))
def __init__(self, link, plot_history_max=1000, dirname=''):
super(BaselineView, self).__init__()
self.pending_draw_modes = []
self.log_file = None
self.directory_name_b = dirname
self.num_hyps = 0
self.last_hyp_update = 0
self.last_btime_update = 0
self.last_soln = None
self.last_mode = 0
self.last_plot_update_time = 0
self.last_stale_update_time = 0
self.slns = {
'n_fixed': deque(maxlen=PLOT_HISTORY_MAX),
'e_fixed': deque(maxlen=PLOT_HISTORY_MAX),
'd_fixed': deque(maxlen=PLOT_HISTORY_MAX),
'n_float': deque(maxlen=PLOT_HISTORY_MAX),
'e_float': deque(maxlen=PLOT_HISTORY_MAX),
'd_float': deque(maxlen=PLOT_HISTORY_MAX),
'n_dgnss': deque(maxlen=PLOT_HISTORY_MAX),
'e_dgnss': deque(maxlen=PLOT_HISTORY_MAX),
'd_dgnss': deque(maxlen=PLOT_HISTORY_MAX)
}
self.plot_data = ArrayPlotData(n_fixed=[],
e_fixed=[],
n_float=[],
e_float=[],
n_dgnss=[],
e_dgnss=[],
t=[0.0],
ref_n=[0.0],
ref_e=[0.0],
cur_e_fixed=[],
cur_n_fixed=[],
cur_e_float=[],
cur_n_float=[],
cur_e_dgnss=[],
cur_n_dgnss=[])
self.list_lock = threading.Lock()
self.plot = Plot(self.plot_data)
pts_float = self.plot.plot( # noqa: F841
('e_float', 'n_float'),
type='scatter',
color=color_dict[FLOAT_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
pts_fixed = self.plot.plot( # noqa: F841
('e_fixed', 'n_fixed'),
type='scatter',
color=color_dict[FIXED_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
pts_dgnss = self.plot.plot( # noqa: F841
('e_dgnss', 'n_dgnss'),
type='scatter',
color=color_dict[DGNSS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
ref = self.plot.plot(('ref_e', 'ref_n'),
type='scatter',
color='red',
marker='plus',
marker_size=5,
line_width=1.5)
cur_fixed = self.plot.plot(('cur_e_fixed', 'cur_n_fixed'),
type='scatter',
color=color_dict[FIXED_MODE],
marker='plus',
marker_size=5,
line_width=1.5)
cur_float = self.plot.plot(('cur_e_float', 'cur_n_float'),
type='scatter',
color=color_dict[FLOAT_MODE],
marker='plus',
marker_size=5,
line_width=1.5)
cur_dgnss = self.plot.plot(('cur_e_dgnss', 'cur_n_dgnss'),
type='scatter',
color=color_dict[DGNSS_MODE],
marker='plus',
line_width=1.5,
marker_size=5)
plot_labels = [' Base Position', 'DGPS', 'RTK Float', 'RTK Fixed']
plots_legend = dict(
zip(plot_labels, [ref, cur_dgnss, cur_float, cur_fixed]))
self.plot.legend.plots = plots_legend
self.plot.legend.labels = plot_labels # sets order
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'E (meters)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'N (meters)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.week = None
self.utc_time = None
self.age_corrections = None
self.heading = "---"
self.nsec = 0
self.link = link
self.link.add_callback(
self.baseline_callback,
[SBP_MSG_BASELINE_NED, SBP_MSG_BASELINE_NED_DEP_A])
self.link.add_callback(self.baseline_heading_callback,
[SBP_MSG_BASELINE_HEADING])
self.link.add_callback(self.gps_time_callback,
[SBP_MSG_GPS_TIME, SBP_MSG_GPS_TIME_DEP_A])
self.link.add_callback(self.utc_time_callback, [SBP_MSG_UTC_TIME])
self.link.add_callback(self.age_corrections_callback,
SBP_MSG_AGE_CORRECTIONS)
self.python_console_cmds = {'baseline': self}
self.update_scheduler = UpdateScheduler()
class SolutionView(HasTraits):
python_console_cmds = Dict()
# we need to doubleup on Lists to store the psuedo absolutes separately
# without rewriting everything
lats = List()
lngs = List()
alts = List()
"""
logging_v : toggle logging for velocity files
directory_name_v : location and name of velocity files
logging_p : toggle logging for position files
directory_name_p : location and name of velocity files
"""
logging_v = Bool(False)
directory_name_v = File
logging_p = Bool(False)
directory_name_p = File
lats_psuedo_abs = List()
lngs_psuedo_abs = List()
alts_psuedo_abs = List()
table_spp = List()
table_psuedo_abs = List()
dops_table = List()
pos_table_spp = List()
vel_table = List()
rtk_pos_note = Str(
"It is necessary to enter the \"Surveyed Position\" settings for the base station in order to view the psuedo-absolute RTK Positions in this tab."
)
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
# Store plots we care about for legend
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(label='',
tooltip='Clear',
filename=os.path.join(determine_path(), 'images',
'iconic', 'x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(label='',
tooltip='Zoom All',
toggle=True,
filename=os.path.join(determine_path(),
'images', 'iconic',
'fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(label='',
tooltip='Center on Solution',
toggle=True,
filename=os.path.join(determine_path(), 'images',
'iconic', 'target.svg'),
width=16,
height=16)
paused_button = SVGButton(label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=os.path.join(determine_path(), 'images',
'iconic', 'pause.svg'),
toggle_filename=os.path.join(
determine_path(), 'images', 'iconic',
'play.svg'),
width=16,
height=16)
traits_view = View(
HSplit(
Tabbed(
VGroup(Item('',
label='Single Point Position (SPP)',
emphasized=True),
Item('table_spp',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
label='Single Point Position'),
VGroup(Item('', label='RTK Position', emphasized=True),
Item('table_psuedo_abs',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3,
height=0.9),
Item('rtk_pos_note',
show_label=False,
resizable=True,
editor=MultilineTextEditor(
TextEditor(multi_line=True)),
style='readonly',
width=0.3,
height=-40),
label='RTK Position')),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
),
Item('plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8))),
)))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _clear_button_fired(self):
self.lats = []
self.lngs = []
self.alts = []
self.lats_psuedo_abs = []
self.lngs_psuedo_abs = []
self.alts_psuedo_abs = []
self.plot_data.set_data('lat', [])
self.plot_data.set_data('lng', [])
self.plot_data.set_data('alt', [])
self.plot_data.set_data('t', [])
self.plot_data.set_data('lat_ps', [])
self.plot_data.set_data('lng_ps', [])
self.plot_data.set_data('alt_ps', [])
self.plot_data.set_data('t_ps', [])
def _pos_llh_callback(self, sbp_msg, **metadata):
# Updating an ArrayPlotData isn't thread safe (see chaco issue #9), so
# actually perform the update in the UI thread.
if self.running:
GUI.invoke_later(self.pos_llh_callback, sbp_msg)
def mode_string(self, msg):
if msg:
if (msg.flags & 0xff) == 0:
return 'SPP (single point position)'
elif (msg.flags & 0xff) == 1:
return 'Fixed RTK'
elif (msg.flags & 0xff) == 2:
return 'Float RTK'
return 'None'
def update_table(self):
self._table_list = self.table_spp.items()
def auto_survey(self):
if self.counter < 1000:
self.counter = self.counter + 1
self.latitude_list.append(self.last_soln.lat)
self.longitude_list.append(self.last_soln.lon)
self.altitude_list.append(self.last_soln.height)
self.latitude_list = self.latitude_list[-1000:]
self.longitude_list = self.longitude_list[-1000:]
self.altitude_list = self.altitude_list[-1000:]
self.latitude = (sum(self.latitude_list)) / self.counter
self.altitude = (sum(self.altitude_list)) / self.counter
self.longitude = (sum(self.longitude_list)) / self.counter
def pos_llh_callback(self, sbp_msg, **metadata):
self.last_stime_update = time.time()
soln = MsgPosLLH(sbp_msg)
self.last_soln = soln
masked_flag = soln.flags & 0x7
if masked_flag == 0:
psuedo_absolutes = False
else:
psuedo_absolutes = True
pos_table = []
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)
pos_table.append(('GPS Time', t))
pos_table.append(('GPS Week', str(self.week)))
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),n_sats,flags\n"
)
self.log_file.write('%s,%.10f,%.10f,%.4f,%d,%d\n' %
(str(t), soln.lat, soln.lon, soln.height,
soln.n_sats, soln.flags))
self.log_file.flush()
pos_table.append(('GPS ToW', tow))
pos_table.append(('Num. sats', soln.n_sats))
pos_table.append(('Lat', soln.lat))
pos_table.append(('Lng', soln.lon))
pos_table.append(('Alt', soln.height))
pos_table.append(('Flags', '0x%02x' % soln.flags))
pos_table.append(('Mode', self.mode_string(soln)))
self.auto_survey()
if psuedo_absolutes:
# setup_plot variables
self.lats_psuedo_abs.append(soln.lat)
self.lngs_psuedo_abs.append(soln.lon)
self.alts_psuedo_abs.append(soln.height)
self.lats_psuedo_abs = self.lats_psuedo_abs[-1000:]
self.lngs_psuedo_abs = self.lngs_psuedo_abs[-1000:]
self.alts_psuedo_abs = self.alts_psuedo_abs[-1000:]
self.plot_data.set_data('lat_ps', self.lats_psuedo_abs)
self.plot_data.set_data('lng_ps', self.lngs_psuedo_abs)
self.plot_data.set_data('alt_ps', self.alts_psuedo_abs)
self.plot_data.set_data('cur_lat_ps', [soln.lat])
self.plot_data.set_data('cur_lng_ps', [soln.lon])
t_psuedo_abs = range(len(self.lats))
if t is not None:
self.plot_data.set_data('t', t)
self.plot_data.set_data('t_ps', t_psuedo_abs)
# set-up table variables
self.table_psuedo_abs = pos_table
else:
# setup_plot variables
self.lats.append(soln.lat)
self.lngs.append(soln.lon)
self.alts.append(soln.height)
self.lats = self.lats[-1000:]
self.lngs = self.lngs[-1000:]
self.alts = self.alts[-1000:]
self.plot_data.set_data('lat', self.lats)
self.plot_data.set_data('lng', self.lngs)
self.plot_data.set_data('alt', self.alts)
self.plot_data.set_data('cur_lat', [soln.lat])
self.plot_data.set_data('cur_lng', [soln.lon])
t = range(len(self.lats))
self.plot_data.set_data('t', t)
# set-up table variables
self.pos_table_spp = pos_table
self.table_spp = self.pos_table_spp + self.vel_table + self.dops_table
# TODO: figure out how to center the graph now that we have two separate messages
# when we selectivtely send only SPP, the centering function won't work anymore
if 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', 'lat')
def dops_callback(self, sbp_msg, **metadata):
dops = MsgDops(sbp_msg)
self.dops_table = [('PDOP', '%.1f' % (dops.pdop * 0.01)),
('GDOP', '%.1f' % (dops.gdop * 0.01)),
('TDOP', '%.1f' % (dops.tdop * 0.01)),
('HDOP', '%.1f' % (dops.hdop * 0.01)),
('VDOP', '%.1f' % (dops.vdop * 0.01))]
self.table_spp = self.pos_table_spp + self.vel_table + self.dops_table
def vel_ned_callback(self, sbp_msg, **metadata):
vel_ned = MsgVelNED(sbp_msg)
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)
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 = open(filepath_v, 'w')
self.vel_log_file.write(
'time,north(m/s),east(m/s),down(m/s),speed(m/s),num_sats\n'
)
self.vel_log_file.write(
'%s,%.6f,%.6f,%.6f,%.6f,%d\n' %
(str(t), 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, vel_ned.n_sats))
self.vel_log_file.flush()
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)),
]
self.table_spp = self.pos_table_spp + self.vel_table + self.dops_table
def gps_time_callback(self, sbp_msg, **metadata):
self.week = MsgGPSTime(sbp_msg).wn
self.nsec = MsgGPSTime(sbp_msg).ns
def __init__(self, link, dirname=''):
super(SolutionView, self).__init__()
self.log_file = None
self.directory_name_v = dirname
self.directory_name_p = dirname
self.vel_log_file = None
self.last_stime_update = 0
self.last_soln = None
self.counter = 0
self.latitude_list = []
self.longitude_list = []
self.altitude_list = []
self.altitude = 0
self.longitude = 0
self.latitude = 0
self.plot_data = ArrayPlotData(lat=[],
lng=[],
alt=[],
t=[],
cur_lat=[],
cur_lng=[],
cur_lat_ps=[],
cur_lng_ps=[],
lat_ps=[],
lng_ps=[],
alt_ps=[],
t_ps=[])
self.plot = Plot(self.plot_data)
# 1000 point buffer
self.plot.plot(('lng', 'lat'),
type='line',
name='',
color=(0, 0, 0.9, 0.1))
self.plot.plot(('lng', 'lat'),
type='scatter',
name='',
color='blue',
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_ps', 'lat_ps'),
type='line',
name='',
color=(1, 0.4, 0, 0.1))
self.plot.plot(('lng_ps', 'lat_ps'),
type='scatter',
name='',
color='orange',
marker='diamond',
line_width=0.0,
marker_size=1.0)
# current values
spp = self.plot.plot(('cur_lng', 'cur_lat'),
type='scatter',
name='SPP',
color='blue',
marker='plus',
line_width=1.5,
marker_size=5.0)
rtk = self.plot.plot(('cur_lng_ps', 'cur_lat_ps'),
type='scatter',
name='RTK',
color='orange',
marker='plus',
line_width=1.5,
marker_size=5.0)
plot_labels = ['SPP', 'RTK']
plots_legend = dict(zip(plot_labels, [spp, rtk]))
self.plot.legend.plots = plots_legend
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'Longitude (degrees)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'Latitude (degrees)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.link = link
self.link.add_callback(self._pos_llh_callback, SBP_MSG_POS_LLH)
self.link.add_callback(self.vel_ned_callback, SBP_MSG_VEL_NED)
self.link.add_callback(self.dops_callback, SBP_MSG_DOPS)
self.link.add_callback(self.gps_time_callback, SBP_MSG_GPS_TIME)
self.week = None
self.nsec = 0
self.python_console_cmds = {
'solution': self,
}
class ObservationView(HasTraits):
python_console_cmds = Dict()
_obs_table_list = List()
obs = Dict()
name = 'Rover'
recording = Bool(False)
record_button = SVGButton(
label='Record', tooltip='Record Raw Observations',
toggle_tooltip='Stop Recording', toggle=True,
filename=os.path.join(os.path.dirname(__file__), 'images', 'fontawesome', 'floppy-o.svg'),
toggle_filename=os.path.join(os.path.dirname(__file__), 'images', 'fontawesome', 'stop.svg'),
width=16, height=16
)
def trait_view(self, view):
return View(
HGroup(
Item('_obs_table_list', style = 'readonly', editor = TabularEditor(adapter=SimpleAdapter()), show_label=False),
VGroup(
Item('record_button', show_label=False),
),
label = self.name,
show_border = True
)
)
def _record_button_fired(self):
self.recording = not self.recording
if not self.recording:
if self.rinex_file is not None:
self.rinex_file.close()
self.rinex_file = None
def update_obs(self):
self._obs_table_list = [(prn + 1,) + obs for prn, obs in sorted(self.obs.items(), key=lambda x: x[0])]
def obs_callback(self, data, sender=None):
if (sender is not None and
(self.relay ^ (sender == 0))):
return
if self.rinex_file is None and self.recording:
self.rinex_file = open(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
4 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)
self.rinex_file.flush()
hdr_fmt = "<dH"
hdr_size = struct.calcsize(hdr_fmt)
tow, wn = struct.unpack("<dH", data[:hdr_size])
self.gps_tow = tow
self.gps_week = wn
self.t = datetime.datetime(1980, 1, 5) + \
datetime.timedelta(weeks=self.gps_week) + \
datetime.timedelta(seconds=self.gps_tow)
obs_fmt = '<ddfB'
"""
double P; /**< Pseudorange (m) */
double L; /**< Carrier-phase (cycles) */
float snr; /**< Signal-to-Noise ratio */
u8 prn; /**< Satellite number. */
"""
obs_size = struct.calcsize(obs_fmt)
self.n_obs = (len(data) - hdr_size) / obs_size
obs_data = data[hdr_size:]
self.obs = {}
for i in range(self.n_obs):
P, L, snr, prn = struct.unpack(obs_fmt, obs_data[:obs_size])
obs_data = obs_data[obs_size:]
self.obs[prn] = (P, L, snr)
if self.recording:
prns = list(self.obs.iterkeys())
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(prns) > 0:
prns_ = prns[:12]
prns = prns[12:]
for prn in prns_:
self.rinex_file.write('G%2d' % (prn+1))
self.rinex_file.write(' ' * (12 - len(prns_)))
self.rinex_file.write('\n')
for prn in list(self.obs.iterkeys()):
# 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()
self.update_obs()
def __init__(self, link, name='Rover', relay=False):
super(ObservationView, self).__init__()
self.obs_count = 0
self.n_obs = 1
self.relay = relay
self.name = name
self.rinex_file = None
self.link = link
self.link.add_callback(ids.NEW_OBS, self.obs_callback)
self.python_console_cmds = {
'obs': self
}
class SolutionView(HasTraits):
python_console_cmds = Dict()
# we need to doubleup on Lists to store the psuedo absolutes separately
# without rewriting everything
"""
logging_v : toggle logging for velocity files
directory_name_v : location and name of velocity files
logging_p : toggle logging for position files
directory_name_p : location and name of velocity files
"""
plot_history_max = Int(1000)
logging_v = Bool(False)
directory_name_v = File
logging_p = Bool(False)
directory_name_p = File
lats_psuedo_abs = List()
lngs_psuedo_abs = List()
alts_psuedo_abs = List()
table = List()
dops_table = List()
pos_table = List()
vel_table = List()
rtk_pos_note = Str(
"It is necessary to enter the \"Surveyed Position\" settings for the base station in order to view the RTK Positions in this tab."
)
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
# Store plots we care about for legend
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(label='',
tooltip='Clear',
filename=os.path.join(determine_path(), 'images',
'iconic', 'x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(label='',
tooltip='Zoom All',
toggle=True,
filename=os.path.join(determine_path(),
'images', 'iconic',
'fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(label='',
tooltip='Center on Solution',
toggle=True,
filename=os.path.join(determine_path(), 'images',
'iconic', 'target.svg'),
width=16,
height=16)
paused_button = SVGButton(label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=os.path.join(determine_path(), 'images',
'iconic', 'pause.svg'),
toggle_filename=os.path.join(
determine_path(), 'images', 'iconic',
'play.svg'),
width=16,
height=16)
traits_view = View(
HSplit(
VGroup(
Item('table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
Item('rtk_pos_note',
show_label=False,
resizable=True,
editor=MultilineTextEditor(TextEditor(multi_line=True)),
style='readonly',
width=0.3,
height=-40),
),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
),
Item('plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8))),
)))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_remove_current(self):
self.plot_data.set_data('cur_lat_spp', [])
self.plot_data.set_data('cur_lng_spp', [])
self.plot_data.set_data('cur_alt_spp', [])
self.plot_data.set_data('cur_lat_dgnss', [])
self.plot_data.set_data('cur_lng_dgnss', [])
self.plot_data.set_data('cur_alt_dgnss', [])
self.plot_data.set_data('cur_lat_float', [])
self.plot_data.set_data('cur_lng_float', [])
self.plot_data.set_data('cur_alt_float', [])
self.plot_data.set_data('cur_lat_fixed', [])
self.plot_data.set_data('cur_lng_fixed', [])
self.plot_data.set_data('cur_alt_fixed', [])
def _clear_button_fired(self):
self.tows = np.empty(self.plot_history_max)
self.lats = np.empty(self.plot_history_max)
self.lngs = np.empty(self.plot_history_max)
self.alts = np.empty(self.plot_history_max)
self.modes = np.empty(self.plot_history_max)
self.plot_data.set_data('lat_spp', [])
self.plot_data.set_data('lng_spp', [])
self.plot_data.set_data('alt_spp', [])
self.plot_data.set_data('lat_dgnss', [])
self.plot_data.set_data('lng_dgnss', [])
self.plot_data.set_data('alt_dgnss', [])
self.plot_data.set_data('lat_float', [])
self.plot_data.set_data('lng_float', [])
self.plot_data.set_data('alt_float', [])
self.plot_data.set_data('lat_fixed', [])
self.plot_data.set_data('lng_fixed', [])
self.plot_data.set_data('alt_fixed', [])
self._reset_remove_current()
def _pos_llh_callback(self, sbp_msg, **metadata):
# Updating an ArrayPlotData isn't thread safe (see chaco issue #9), so
# actually perform the update in the UI thread.
if self.running:
GUI.invoke_later(self.pos_llh_callback, sbp_msg)
def update_table(self):
self._table_list = self.table_spp.items()
def auto_survey(self):
if self.counter < 1000:
self.counter = self.counter + 1
self.latitude_list.append(self.last_soln.lat)
self.longitude_list.append(self.last_soln.lon)
self.altitude_list.append(self.last_soln.height)
self.latitude_list = self.latitude_list[-1000:]
self.longitude_list = self.longitude_list[-1000:]
self.altitude_list = self.altitude_list[-1000:]
self.latitude = (sum(self.latitude_list)) / self.counter
self.altitude = (sum(self.altitude_list)) / self.counter
self.longitude = (sum(self.longitude_list)) / self.counter
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 dops_callback(self, sbp_msg, **metadata):
flags = 0
if sbp_msg.msg_type == SBP_MSG_DOPS_DEP_A:
dops = MsgDopsDepA(sbp_msg)
flags = 1
else:
dops = MsgDops(sbp_msg)
flags = dops.flags
if flags != 0:
self.dops_table = [('PDOP', '%.1f' % (dops.pdop * 0.01)),
('GDOP', '%.1f' % (dops.gdop * 0.01)),
('TDOP', '%.1f' % (dops.tdop * 0.01)),
('HDOP', '%.1f' % (dops.hdop * 0.01)),
('VDOP', '%.1f' % (dops.vdop * 0.01))]
else:
self.dops_table = [('PDOP', EMPTY_STR), ('GDOP', EMPTY_STR),
('TDOP', EMPTY_STR), ('HDOP', EMPTY_STR),
('VDOP', EMPTY_STR)]
self.dops_table.append(('DOPS 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 gps_time_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_GPS_TIME_DEP_A:
time_msg = MsgGPSTimeDepA(sbp_msg)
flags = 1
elif sbp_msg.msg_type == SBP_MSG_GPS_TIME:
time_msg = MsgGPSTime(sbp_msg)
flags = time_msg.flags
if flags != 0:
self.week = time_msg.wn
self.nsec = time_msg.ns
def __init__(self, link, dirname=''):
super(SolutionView, self).__init__()
self.lats = np.zeros(self.plot_history_max)
self.lngs = np.zeros(self.plot_history_max)
self.alts = np.zeros(self.plot_history_max)
self.tows = np.zeros(self.plot_history_max)
self.modes = np.zeros(self.plot_history_max)
self.log_file = None
self.directory_name_v = dirname
self.directory_name_p = dirname
self.vel_log_file = None
self.last_stime_update = 0
self.last_soln = None
self.counter = 0
self.latitude_list = []
self.longitude_list = []
self.altitude_list = []
self.altitude = 0
self.longitude = 0
self.latitude = 0
self.last_pos_mode = 0
self.plot_data = ArrayPlotData(lat_spp=[],
lng_spp=[],
alt_spp=[],
cur_lat_spp=[],
cur_lng_spp=[],
lat_dgnss=[],
lng_dgnss=[],
alt_dgnss=[],
cur_lat_dgnss=[],
cur_lng_dgnss=[],
lat_float=[],
lng_float=[],
alt_float=[],
cur_lat_float=[],
cur_lng_float=[],
lat_fixed=[],
lng_fixed=[],
alt_fixed=[],
cur_lat_fixed=[],
cur_lng_fixed=[])
self.plot = Plot(self.plot_data)
# 1000 point buffer
self.plot.plot(('lng_spp', 'lat_spp'),
type='line',
line_width=0.1,
name='',
color=color_dict[SPP_MODE])
self.plot.plot(('lng_spp', 'lat_spp'),
type='scatter',
name='',
color=color_dict[SPP_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_dgnss', 'lat_dgnss'),
type='line',
line_width=0.1,
name='',
color=color_dict[DGNSS_MODE])
self.plot.plot(('lng_dgnss', 'lat_dgnss'),
type='scatter',
name='',
color=color_dict[DGNSS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_float', 'lat_float'),
type='line',
line_width=0.1,
name='',
color=color_dict[FLOAT_MODE])
self.plot.plot(('lng_float', 'lat_float'),
type='scatter',
name='',
color=color_dict[FLOAT_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_fixed', 'lat_fixed'),
type='line',
line_width=0.1,
name='',
color=color_dict[FIXED_MODE])
self.plot.plot(('lng_fixed', 'lat_fixed'),
type='scatter',
name='',
color=color_dict[FIXED_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
# current values
spp = self.plot.plot(('cur_lng_spp', 'cur_lat_spp'),
type='scatter',
name=mode_dict[SPP_MODE],
color=color_dict[SPP_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
dgnss = self.plot.plot(('cur_lng_dgnss', 'cur_lat_dgnss'),
type='scatter',
name=mode_dict[DGNSS_MODE],
color=color_dict[DGNSS_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
rtkfloat = self.plot.plot(('cur_lng_float', 'cur_lat_float'),
type='scatter',
name=mode_dict[FLOAT_MODE],
color=color_dict[FLOAT_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
rtkfix = self.plot.plot(('cur_lng_fixed', 'cur_lat_fixed'),
type='scatter',
name=mode_dict[FIXED_MODE],
color=color_dict[FIXED_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
plot_labels = ['SPP', 'DGPS', "RTK float", "RTK fixed"]
plots_legend = dict(zip(plot_labels, [spp, dgnss, rtkfloat, rtkfix]))
self.plot.legend.plots = plots_legend
self.plot.legend.labels = plot_labels # sets order
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'Longitude (degrees)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'Latitude (degrees)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.link = link
self.link.add_callback(self._pos_llh_callback,
[SBP_MSG_POS_LLH_DEP_A, SBP_MSG_POS_LLH])
self.link.add_callback(self.vel_ned_callback,
[SBP_MSG_VEL_NED_DEP_A, SBP_MSG_VEL_NED])
self.link.add_callback(self.dops_callback,
[SBP_MSG_DOPS_DEP_A, SBP_MSG_DOPS])
self.link.add_callback(self.gps_time_callback,
[SBP_MSG_GPS_TIME_DEP_A, SBP_MSG_GPS_TIME])
self.week = None
self.nsec = 0
self.python_console_cmds = {
'solution': self,
}
class BaselineView(HasTraits):
python_console_cmds = Dict()
table = List()
logging_b = Bool(False)
directory_name_b = File
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(label='',
tooltip='Clear',
filename=os.path.join(determine_path(), 'images',
'iconic', 'x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(label='',
tooltip='Zoom All',
toggle=True,
filename=os.path.join(determine_path(),
'images', 'iconic',
'fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(label='',
tooltip='Center on Baseline',
toggle=True,
filename=os.path.join(determine_path(), 'images',
'iconic', 'target.svg'),
width=16,
height=16)
paused_button = SVGButton(label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=os.path.join(determine_path(), 'images',
'iconic', 'pause.svg'),
toggle_filename=os.path.join(
determine_path(), 'images', 'iconic',
'play.svg'),
width=16,
height=16)
reset_button = Button(label='Reset Filters')
reset_iar_button = Button(label='Reset IAR')
init_base_button = Button(label='Init. with known baseline')
traits_view = View(
HSplit(
Item('table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
Item('reset_button', show_label=False),
Item('reset_iar_button', show_label=False),
Item('init_base_button', show_label=False),
),
Item(
'plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8)),
))))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_button_fired(self):
self.link(MsgResetFilters(filter=0))
def _reset_iar_button_fired(self):
self.link(MsgResetFilters(filter=1))
def _init_base_button_fired(self):
self.link(MsgInitBase())
def _clear_button_fired(self):
self.neds[:] = np.NAN
self.fixeds[:] = False
self.plot_data.set_data('n_fixed', [])
self.plot_data.set_data('e_fixed', [])
self.plot_data.set_data('d_fixed', [])
self.plot_data.set_data('n_float', [])
self.plot_data.set_data('e_float', [])
self.plot_data.set_data('d_float', [])
self.plot_data.set_data('t', [])
self.plot_data.set_data('cur_fixed_n', [])
self.plot_data.set_data('cur_fixed_e', [])
self.plot_data.set_data('cur_fixed_d', [])
self.plot_data.set_data('cur_float_n', [])
self.plot_data.set_data('cur_float_e', [])
self.plot_data.set_data('cur_float_d', [])
def iar_state_callback(self, sbp_msg, **metadata):
self.num_hyps = sbp_msg.num_hyps
self.last_hyp_update = time.time()
def _baseline_callback_ned(self, sbp_msg, **metadata):
# Updating an ArrayPlotData isn't thread safe (see chaco issue #9), so
# actually perform the update in the UI thread.
if self.running:
GUI.invoke_later(self.baseline_callback, sbp_msg)
def update_table(self):
self._table_list = self.table.items()
def gps_time_callback(self, sbp_msg, **metadata):
self.week = MsgGPSTime(sbp_msg).wn
self.nsec = MsgGPSTime(sbp_msg).ns
def mode_string(self, msg):
if msg:
self.fixed = (msg.flags & 1) == 1
if self.fixed:
return 'Fixed RTK'
else:
return 'Float'
return 'None'
def baseline_callback(self, sbp_msg):
self.last_btime_update = time.time()
soln = MsgBaselineNED(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
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)
table.append(('GPS Time', t))
table.append(('GPS Week', str(self.week)))
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 = open(filepath, 'w')
self.log_file.write(
'time,north(meters),east(meters),down(meters),distance(meters),num_sats,flags,num_hypothesis\n'
)
self.log_file.write('%s,%.4f,%.4f,%.4f,%.4f,%d,0x%02x,%d\n' %
(str(t), soln.n, soln.e, soln.d, dist,
soln.n_sats, soln.flags, self.num_hyps))
self.log_file.flush()
table.append(('GPS ToW', tow))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('Dist.', dist))
table.append(('Num. Sats.', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', self.mode_string(soln)))
if time.time() - self.last_hyp_update < 10 and self.num_hyps != 1:
table.append(('IAR Num. Hyps.', self.num_hyps))
else:
table.append(('IAR Num. Hyps.', "None"))
# Rotate array, deleting oldest entries to maintain
# no more than N in plot
self.neds[1:] = self.neds[:-1]
self.fixeds[1:] = self.fixeds[:-1]
# Insert latest position
self.neds[0][:] = [soln.n, soln.e, soln.d]
self.fixeds[0] = self.fixed
neds_fixed = self.neds[self.fixeds]
neds_float = self.neds[np.logical_not(self.fixeds)]
if not all(map(any, np.isnan(neds_fixed))):
self.plot_data.set_data('n_fixed', neds_fixed.T[0])
self.plot_data.set_data('e_fixed', neds_fixed.T[1])
self.plot_data.set_data('d_fixed', neds_fixed.T[2])
if not all(map(any, np.isnan(neds_float))):
self.plot_data.set_data('n_float', neds_float.T[0])
self.plot_data.set_data('e_float', neds_float.T[1])
self.plot_data.set_data('d_float', neds_float.T[2])
if self.fixed:
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])
self.plot_data.set_data('cur_float_n', [])
self.plot_data.set_data('cur_float_e', [])
self.plot_data.set_data('cur_float_d', [])
else:
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])
self.plot_data.set_data('cur_fixed_n', [])
self.plot_data.set_data('cur_fixed_e', [])
self.plot_data.set_data('cur_fixed_d', [])
self.plot_data.set_data('ref_n', [0.0])
self.plot_data.set_data('ref_e', [0.0])
self.plot_data.set_data('ref_d', [0.0])
if 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'),
('n_fixed', 'n_float'))
self.table = table
def __init__(self, link, plot_history_max=1000, dirname=''):
super(BaselineView, self).__init__()
self.log_file = None
self.directory_name_b = dirname
self.num_hyps = 0
self.last_hyp_update = 0
self.last_btime_update = 0
self.last_soln = None
self.plot_data = ArrayPlotData(n_fixed=[0.0],
e_fixed=[0.0],
d_fixed=[0.0],
n_float=[0.0],
e_float=[0.0],
d_float=[0.0],
t=[0.0],
ref_n=[0.0],
ref_e=[0.0],
ref_d=[0.0],
cur_fixed_e=[],
cur_fixed_n=[],
cur_fixed_d=[],
cur_float_e=[],
cur_float_n=[],
cur_float_d=[])
self.plot_history_max = plot_history_max
self.neds = np.empty((plot_history_max, 3))
self.neds[:] = np.NAN
self.fixeds = np.zeros(plot_history_max, dtype=bool)
self.plot = Plot(self.plot_data)
color_float = (0.5, 0.5, 1.0)
color_fixed = 'orange'
pts_float = self.plot.plot(('e_float', 'n_float'),
type='scatter',
color=color_float,
marker='dot',
line_width=0.0,
marker_size=1.0)
pts_fixed = self.plot.plot(('e_fixed', 'n_fixed'),
type='scatter',
color=color_fixed,
marker='dot',
line_width=0.0,
marker_size=1.0)
lin = self.plot.plot(('e_fixed', 'n_fixed'),
type='line',
color=(1, 0.65, 0, 0.1))
ref = self.plot.plot(('ref_e', 'ref_n'),
type='scatter',
color='red',
marker='plus',
marker_size=5,
line_width=1.5)
cur_fixed = self.plot.plot(('cur_fixed_e', 'cur_fixed_n'),
type='scatter',
color=color_fixed,
marker='plus',
marker_size=5,
line_width=1.5)
cur_float = self.plot.plot(('cur_float_e', 'cur_float_n'),
type='scatter',
color=color_float,
marker='plus',
marker_size=5,
line_width=1.5)
plot_labels = ['Base Position', 'RTK Fixed', 'RTK Float']
plots_legend = dict(zip(plot_labels, [ref, cur_fixed, cur_float]))
self.plot.legend.plots = plots_legend
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'E (meters)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'N (meters)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.week = None
self.nsec = 0
self.link = link
self.link.add_callback(self._baseline_callback_ned,
SBP_MSG_BASELINE_NED)
self.link.add_callback(self.iar_state_callback, SBP_MSG_IAR_STATE)
self.link.add_callback(self.gps_time_callback, SBP_MSG_GPS_TIME)
self.python_console_cmds = {'baseline': self}
class SystemMonitorView(HasTraits):
python_console_cmds = Dict()
_threads_table_list = List()
threads = List()
uart_a_crc_error_count = Int(0)
uart_a_io_error_count = Int(0)
uart_a_rx_buffer = Float(0)
uart_a_tx_buffer = Float(0)
uart_a_tx_KBps = Float(0)
uart_a_rx_KBps = Float(0)
uart_b_crc_error_count = Int(0)
uart_b_io_error_count = Int(0)
uart_b_rx_buffer = Float(0)
uart_b_tx_buffer = Float(0)
uart_b_tx_KBps = Float(0)
uart_b_rx_KBps = Float(0)
ftdi_crc_error_count = Int(0)
ftdi_io_error_count = Int(0)
ftdi_rx_buffer = Float(0)
ftdi_tx_buffer = Float(0)
ftdi_tx_KBps = Float(0)
ftdi_rx_KBps = Float(0)
msg_obs_avg_latency_ms = Int(0)
msg_obs_min_latency_ms = Int(0)
msg_obs_max_latency_ms = Int(0)
msg_obs_window_latency_ms = Int(0)
piksi_reset_button = SVGButton(label='Reset Piksi',
tooltip='Reset Piksi',
filename=os.path.join(
os.path.dirname(__file__), 'images',
'fontawesome', 'power27.svg'),
width=16,
height=16,
aligment='center')
traits_view = View(
VGroup(
Item(
'_threads_table_list',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.85,
),
HGroup(
VGroup(
VGroup(
Item('msg_obs_window_latency_ms',
label='Obs Latency',
style='readonly',
format_str='%dms'),
Item('msg_obs_avg_latency_ms',
label='Obs Latency (Avg ms)',
style='readonly',
format_str='%dms'),
Item('msg_obs_min_latency_ms',
label='Obs Latency (Min ms)',
style='readonly',
format_str='%dms'),
Item('msg_obs_max_latency_ms',
label='Obs Latency (Max ms)',
style='readonly',
format_str='%dms'),
label='Connection Monitor',
show_border=True,
),
HGroup(
Spring(width=50, springy=False),
Item('piksi_reset_button',
show_label=False,
width=0.50),
),
),
VGroup(
Item('uart_a_crc_error_count',
label='CRC Errors',
style='readonly'),
Item('uart_a_io_error_count',
label='IO Errors',
style='readonly'),
Item('uart_a_tx_buffer',
label='TX Buffer %',
style='readonly',
format_str='%.1f'),
Item('uart_a_rx_buffer',
label='RX Buffer %',
style='readonly',
format_str='%.1f'),
Item('uart_a_tx_KBps',
label='TX KBytes/s',
style='readonly',
format_str='%.2f'),
Item('uart_a_rx_KBps',
label='RX KBytes/s',
style='readonly',
format_str='%.2f'),
label='UART A',
show_border=True,
),
VGroup(
Item('uart_b_crc_error_count',
label='CRC Errors',
style='readonly'),
Item('uart_b_io_error_count',
label='IO Errors',
style='readonly'),
Item('uart_b_tx_buffer',
label='TX Buffer %',
style='readonly',
format_str='%.1f'),
Item('uart_b_rx_buffer',
label='RX Buffer %',
style='readonly',
format_str='%.1f'),
Item('uart_b_tx_KBps',
label='TX KBytes/s',
style='readonly',
format_str='%.2f'),
Item('uart_b_rx_KBps',
label='RX KBytes/s',
style='readonly',
format_str='%.2f'),
label='UART B',
show_border=True,
),
VGroup(
Item('ftdi_crc_error_count',
label='CRC Errors',
style='readonly'),
Item('ftdi_io_error_count',
label='IO Errors',
style='readonly'),
Item('ftdi_tx_buffer',
label='TX Buffer %',
style='readonly',
format_str='%.1f'),
Item('ftdi_rx_buffer',
label='RX Buffer %',
style='readonly',
format_str='%.1f'),
Item('ftdi_tx_KBps',
label='TX KBytes/s',
style='readonly',
format_str='%.2f'),
Item('ftdi_rx_KBps',
label='RX KBytes/s',
style='readonly',
format_str='%.2f'),
label='USB UART',
show_border=True,
),
),
), )
def update_threads(self):
self._threads_table_list = [
(thread_name, state.cpu, state.stack_free)
for thread_name, state in sorted(
self.threads, key=lambda x: x[1].cpu, reverse=True)
]
def heartbeat_callback(self, sbp_msg, **metadata):
self.update_threads()
self.threads = []
def thread_state_callback(self, sbp_msg, **metadata):
if sbp_msg.name == '':
sbp_msg.name = '(no name)'
sbp_msg.cpu /= 10.
self.threads.append((sbp_msg.name, sbp_msg))
def _piksi_reset_button_fired(self):
self.link(MsgReset())
def uart_state_callback(self, m, **metadata):
self.uart_a_tx_KBps = m.uart_a.tx_throughput
self.uart_a_rx_KBps = m.uart_a.rx_throughput
self.uart_a_crc_error_count = m.uart_a.crc_error_count
self.uart_a_io_error_count = m.uart_a.io_error_count
self.uart_a_tx_buffer = 100 * m.uart_a.tx_buffer_level / 255.0
self.uart_a_rx_buffer = 100 * m.uart_a.rx_buffer_level / 255.0
self.uart_b_tx_KBps = m.uart_b.tx_throughput
self.uart_b_rx_KBps = m.uart_b.rx_throughput
self.uart_b_crc_error_count = m.uart_b.crc_error_count
self.uart_b_io_error_count = m.uart_b.io_error_count
self.uart_b_tx_buffer = 100 * m.uart_b.tx_buffer_level / 255.0
self.uart_b_rx_buffer = 100 * m.uart_b.rx_buffer_level / 255.0
self.uart_ftdi_tx_KBps = m.uart_ftdi.tx_throughput
self.uart_ftdi_rx_KBps = m.uart_ftdi.rx_throughput
self.uart_ftdi_crc_error_count = m.uart_ftdi.crc_error_count
self.uart_ftdi_io_error_count = m.uart_ftdi.io_error_count
self.uart_ftdi_tx_buffer = 100 * m.uart_ftdi.tx_buffer_level / 255.0
self.uart_ftdi_rx_buffer = 100 * m.uart_ftdi.rx_buffer_level / 255.0
self.msg_obs_avg_latency_ms = m.latency.avg
self.msg_obs_min_latency_ms = m.latency.lmin
self.msg_obs_max_latency_ms = m.latency.lmax
self.msg_obs_window_latency_ms = m.latency.current
def __init__(self, link):
super(SystemMonitorView, self).__init__()
self.link = link
self.link.add_callback(self.heartbeat_callback, SBP_MSG_HEARTBEAT)
self.link.add_callback(self.thread_state_callback,
SBP_MSG_THREAD_STATE)
self.link.add_callback(self.uart_state_callback, SBP_MSG_UART_STATE)
self.python_console_cmds = {'mon': self}
class ObservationView(HasTraits):
python_console_cmds = Dict()
_obs_table_list = List()
obs = Dict()
name = Str('Rover')
recording = Bool(False)
record_button = SVGButton(
label='Record', tooltip='Record Raw Observations',
toggle_tooltip='Stop Recording', toggle=True,
filename=os.path.join(determine_path(),
'images', 'fontawesome', 'floppy-o.svg'),
toggle_filename=os.path.join(determine_path(),
'images', 'fontawesome', 'stop.svg'),
width=16, height=16
)
def trait_view(self, view):
return View(
HGroup(
Item('_obs_table_list', style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()), show_label=False),
VGroup(
Item('record_button', show_label=False),
),
label=self.name,
show_border=True
)
)
def _record_button_fired(self):
self.recording = not self.recording
if not self.recording:
if self.rinex_file is not None:
self.rinex_file.close()
self.rinex_file = None
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 = open(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 update_obs(self):
self._obs_table_list =\
[(prn,) + obs for prn, obs in sorted(self.obs.items(),
key=lambda x: x[0])]
def obs_packed_callback(self, sbp_msg, **metadata):
if (sbp_msg.sender is not None and
(self.relay ^ (sbp_msg.sender == 0))):
return
tow = sbp_msg.header.t.tow
wn = sbp_msg.header.t.wn
seq = sbp_msg.header.n_obs
tow = float(tow) / 1000.0
total = seq >> 4
count = seq & ((1 << 4) - 1)
# Confirm this packet is good.
# Assumes no out-of-order packets
if count == 0:
self.old_tow = self.gps_tow
self.gps_tow = tow
self.gps_week = wn
self.prev_obs_total = total
self.prev_obs_count = 0
self.old_obs = copy.deepcopy(self.obs)
self.obs = {}
elif self.gps_tow != tow or\
self.gps_week != wn or\
self.prev_obs_count + 1 != count or\
self.prev_obs_total != total:
print "We dropped a packet. Skipping this observation sequence"
self.prev_obs_count = -1
return
else:
self.prev_obs_count = count
# Save this packet
# See sbp_piksi.h for format
for o in sbp_msg.obs:
prn = o.sid.sat
# compute time difference of carrier phase for display
cp = float(o.L.i) + float(o.L.f) / (1 << 8)
if o.sid.code == 0 or o.sid.code == 1:
prn += 1
prn = '{} ({})'.format(prn, code_to_str(o.sid.code))
if sbp_msg.msg_type == SBP_MSG_OBS_DEP_A or\
sbp_msg.msg_type == SBP_MSG_OBS_DEP_B:
divisor = 1e2
else:
divisor = 5e1
try:
ocp = self.old_obs[prn][1]
except:
ocp = 0
cf = (cp - ocp) / (self.gps_tow - self.old_tow)
self.obs[prn] = (float(o.P) / divisor,
float(o.L.i) + float(o.L.f) / (1 << 8),
float(o.cn0) / 4,
cf)
if (count == total - 1):
self.t = datetime.datetime(1980, 1, 6) + \
datetime.timedelta(weeks=self.gps_week) + \
datetime.timedelta(seconds=self.gps_tow)
self.update_obs()
self.rinex_save()
return
def ephemeris_callback(self, m, **metadata):
prn = m.sid.sat
if m.sid.code == 0 or m.sid.code == 1:
prn += 1
if self.recording:
if self.eph_file is None:
self.eph_file = open(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.toe_tow, m.toe_wn, m.toc_tow, m.toc_wn,
m.valid,
m.healthy,
prn])[1: -1] + "\n"
self.eph_file.write(strout)
self.eph_file.flush()
def __init__(self, link, name='Rover', relay=False):
super(ObservationView, self).__init__()
self.obs_count = 0
self.gps_tow = 0.0
self.gps_week = 0
self.relay = relay
self.name = name
self.rinex_file = None
self.eph_file = None
self.link = link
self.link.add_callback(self.obs_packed_callback,
[SBP_MSG_OBS, SBP_MSG_OBS_DEP_B])
self.link.add_callback(self.ephemeris_callback, SBP_MSG_EPHEMERIS)
self.python_console_cmds = {'obs': self}
class SwiftConsole(HasTraits):
"""Traits-defined Swift Console.
link : object
Serial driver
update : bool
Update the firmware
log_level_filter : str
Syslog string, one of "ERROR", "WARNING", "INFO", "DEBUG".
"""
link = Instance(sbpc.Handler)
console_output = Instance(OutputList())
python_console_env = Dict
device_serial = Str('')
dev_id = Str('')
tracking_view = Instance(TrackingView)
solution_view = Instance(SolutionView)
baseline_view = Instance(BaselineView)
skyplot_view = Instance(SkyplotView)
observation_view = Instance(ObservationView)
networking_view = Instance(SbpRelayView)
observation_view_base = Instance(ObservationView)
system_monitor_view = Instance(SystemMonitorView)
settings_view = Instance(SettingsView)
update_view = Instance(UpdateView)
ins_view = Instance(INSView)
mag_view = Instance(MagView)
spectrum_analyzer_view = Instance(SpectrumAnalyzerView)
log_level_filter = Enum(list(SYSLOG_LEVELS.values()))
""""
mode : baseline and solution view - SPP, Fixed or Float
num_sat : baseline and solution view - number of satellites
port : which port is Swift Device is connected to
directory_name : location of logged files
json_logging : enable JSON logging
csv_logging : enable CSV logging
"""
pos_mode = Str('')
rtk_mode = Str('')
ins_status_string = Str('')
num_sats_str = Str('')
cnx_desc = Str('')
age_of_corrections = Str('')
uuid = Str('')
directory_name = Directory
json_logging = Bool(True)
csv_logging = Bool(False)
show_csv_log = Bool(False)
cnx_icon = Str('')
heartbeat_count = Int()
last_timer_heartbeat = Int()
driver_data_rate = Str()
solid_connection = Bool(False)
csv_logging_button = SVGButton(
toggle=True,
label='CSV log',
tooltip='start CSV logging',
toggle_tooltip='stop CSV logging',
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
orientation='vertical',
width=2,
height=2,
)
json_logging_button = SVGButton(
toggle=True,
label='JSON log',
tooltip='start JSON logging',
toggle_tooltip='stop JSON logging',
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
orientation='vertical',
width=2,
height=2,
)
paused_button = SVGButton(
label='',
tooltip='Pause console update',
toggle_tooltip='Resume console update',
toggle=True,
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
width=8,
height=8)
clear_button = SVGButton(
label='',
tooltip='Clear console buffer',
filename=resource_filename('console/images/iconic/x.svg'),
width=8,
height=8)
view = View(VSplit(
Tabbed(Tabbed(Item('tracking_view',
style='custom',
label='Signals',
show_label=False),
Item('skyplot_view',
style='custom',
label='Sky Plot',
show_label=False),
label="Tracking"),
Item('solution_view', style='custom', label='Solution'),
Item('baseline_view', style='custom', label='Baseline'),
VSplit(
Item('observation_view', style='custom', show_label=False),
Item('observation_view_base',
style='custom',
show_label=False),
label='Observations',
),
Item('settings_view', style='custom', label='Settings'),
Item('update_view', style='custom', label='Update'),
Tabbed(Item('system_monitor_view',
style='custom',
label='System Monitor'),
Item('ins_view', style='custom', label='INS'),
Item('mag_view', style='custom', label='Magnetometer'),
Item('networking_view',
label='Networking',
style='custom',
show_label=False),
Item('spectrum_analyzer_view',
label='Spectrum Analyzer',
style='custom'),
label='Advanced',
show_labels=False),
show_labels=False),
VGroup(
VGroup(
HGroup(
Spring(width=4, springy=False),
Item('paused_button',
show_label=False,
padding=0,
width=8,
height=8),
Item('clear_button', show_label=False, width=8, height=8),
Item('', label='Console Log', emphasized=True),
Item('csv_logging_button',
emphasized=True,
show_label=False,
visible_when='show_csv_log',
width=12,
height=-30,
padding=0),
Item('json_logging_button',
emphasized=True,
show_label=False,
width=12,
height=-30,
padding=0),
Item(
'directory_name',
show_label=False,
springy=True,
tooltip=
'Choose location for file logs. Default is home/SwiftNav.',
height=-25,
enabled_when='not(json_logging or csv_logging)',
editor_args={'auto_set': True}),
UItem(
'log_level_filter',
style='simple',
padding=0,
height=8,
show_label=True,
tooltip=
'Show log levels up to and including the selected level of severity.\nThe CONSOLE log level is always visible.'
),
),
Item('console_output',
style='custom',
editor=InstanceEditor(),
height=125,
show_label=False,
full_size=True),
),
HGroup(
Spring(width=4, springy=False),
Item('',
label='Port:',
emphasized=True,
tooltip='Interface for communicating with Swift device'),
Item('cnx_desc', show_label=False, style='readonly'),
Item('',
label='Pos:',
emphasized=True,
tooltip='Device Position Mode: SPS, DGNSS, or RTK'),
Item('pos_mode', show_label=False, style='readonly'),
Item('',
label='RTK:',
emphasized=True,
tooltip='Device RTK Mode: Float or Fixed'),
Item('rtk_mode', show_label=False, style='readonly'),
Item('',
label='Sats:',
emphasized=True,
tooltip='Number of satellites used in solution'),
Item('num_sats_str',
padding=2,
show_label=False,
style='readonly'),
Item('',
label='Corr Age:',
emphasized=True,
tooltip=
'Age of corrections (-- means invalid / not present)'),
Item('age_of_corrections',
padding=2,
show_label=False,
style='readonly'),
Item('',
label='INS:',
emphasized=True,
tooltip='INS Status String'),
Item('ins_status_string',
padding=2,
show_label=False,
style='readonly',
width=6),
Spring(springy=True),
Item('driver_data_rate', style='readonly', show_label=False),
Item('cnx_icon',
show_label=False,
padding=0,
width=8,
height=8,
visible_when='solid_connection',
springy=False,
editor=ImageEditor(
allow_clipping=False,
image=ImageResource(
resource_filename(
'console/images/iconic/arrows_blue.png')))),
Item('cnx_icon',
show_label=False,
padding=0,
width=8,
height=8,
visible_when='not solid_connection',
springy=False,
editor=ImageEditor(
allow_clipping=False,
image=ImageResource(
resource_filename(
'console/images/iconic/arrows_grey.png')))),
Spring(width=4, height=-2, springy=False),
),
Spring(height=1, springy=False),
),
),
icon=icon,
resizable=True,
width=800,
height=600,
handler=ConsoleHandler(),
title=CONSOLE_TITLE)
def print_message_callback(self, sbp_msg, **metadata):
try:
encoded = sbp_msg.payload.encode('ascii', 'ignore')
for eachline in reversed(encoded.split('\n')):
self.console_output.write_level(
eachline, str_to_log_level(eachline.split(':')[0]))
except UnicodeDecodeError as e:
print("Error encoding msg_print: {}".format(e))
def log_message_callback(self, sbp_msg, **metadata):
encoded = sbp_msg.text.decode('utf8')
for eachline in reversed(encoded.split('\n')):
self.console_output.write_level(eachline, sbp_msg.level)
def ext_event_callback(self, sbp_msg, **metadata):
e = MsgExtEvent(sbp_msg)
print(
'External event: %s edge on pin %d at wn=%d, tow=%d, time qual=%s'
% ("Rising" if
(e.flags &
(1 << 0)) else "Falling", e.pin, e.wn, e.tow, "good" if
(e.flags & (1 << 1)) else "unknown"))
def cmd_resp_callback(self, sbp_msg, **metadata):
r = MsgCommandResp(sbp_msg)
print("Received a command response message with code {0}".format(
r.code))
def _paused_button_fired(self):
self.console_output.paused = not self.console_output.paused
def _log_level_filter_changed(self):
"""
Takes log level enum and translates into the mapped integer.
Integer stores the current filter value inside OutputList.
"""
self.console_output.log_level_filter = str_to_log_level(
self.log_level_filter)
def _clear_button_fired(self):
self.console_output.clear()
def _directory_name_changed(self):
if self.baseline_view and self.solution_view:
self.baseline_view.directory_name_b = self.directory_name
self.solution_view.directory_name_p = self.directory_name
self.solution_view.directory_name_v = self.directory_name
if self.observation_view and self.observation_view_base:
self.observation_view.dirname = self.directory_name
self.observation_view_base.dirname = self.directory_name
def update_on_heartbeat(self, sbp_msg, **metadata):
self.heartbeat_count += 1
def check_heartbeat(self):
# if our heartbeat hasn't changed since the last timer interval the connection must have dropped
if self.heartbeat_count == self.last_timer_heartbeat and self.heartbeat_count != 0:
self.solid_connection = False
self.ins_status_string = "None"
self.pos_mode = "None"
self.ins_mode = "None"
self.num_sats_str = EMPTY_STR
self.last_timer_heartbeat = self.heartbeat_count
else:
self.solid_connection = True
self.last_timer_heartbeat = self.heartbeat_count
bytes_diff = self.driver.bytes_read_since(self.last_driver_bytes_read)
# 1024 bytes per KiloByte
self.driver_data_rate = "{0:.2f} KB/s".format(
(bytes_diff) / (HEARTBEAT_CHECK_PERIOD_SECONDS * 1024))
self.last_driver_bytes_read = self.driver.total_bytes_read
# if we aren't getting heartbeats and we have at some point gotten heartbeats, we don't have
# a good connection and we should age out the data from the views
if not self.solid_connection or self.driver_data_rate == 0:
return
# -- grab current time to use in logic
current_time = monotonic()
# --- determining which mode, llh or baseline, to show in the status bar ---
llh_display_mode = "None"
llh_num_sats = 0
llh_is_rtk = False
baseline_display_mode = "None"
ins_status_string = EMPTY_STR
# determine the latest llh solution mode
if self.solution_view and (current_time -
self.solution_view.last_stime_update) < 1:
llh_solution_mode = self.solution_view.last_pos_mode
llh_display_mode = pos_mode_dict.get(llh_solution_mode, EMPTY_STR)
if llh_solution_mode > 0 and self.solution_view.last_soln:
llh_num_sats = self.solution_view.last_soln.n_sats
llh_is_rtk = (llh_solution_mode in RTK_MODES)
if getattr(self.solution_view, 'ins_used',
False) and llh_solution_mode != DR_MODE:
llh_display_mode += "+INS"
# determine the latest baseline solution mode
if (self.baseline_view and self.settings_view
and self.settings_view.dgnss_enabled
and (current_time - self.baseline_view.last_btime_update) < 1):
baseline_solution_mode = self.baseline_view.last_mode
baseline_display_mode = rtk_mode_dict.get(baseline_solution_mode,
EMPTY_STR)
# if baseline solution mode is empty and POS mode is RTK, get the RTK mode from pos
if baseline_display_mode not in rtk_mode_dict.values() and llh_is_rtk:
baseline_display_mode = rtk_mode_dict.get(llh_solution_mode)
# determine the latest INS mode
if self.solution_view and (
current_time -
self.solution_view.last_ins_status_receipt_time) < 1:
ins_flags = self.solution_view.ins_status_flags
ins_mode = ins_flags & 0x7
ins_type = (ins_flags >> 29) & 0x7
odo_status = (ins_flags >> 8) & 0x3
# ins_status has bug in odo.
# If it says no odo, check if we have had tics in last 10 seconds from INS_UPDATES
if odo_status != 1:
if (current_time -
self.solution_view.last_odo_update_time) < 10:
odo_status = 1
ins_error = (ins_flags >> 4) & 0xF
if ins_error != 0:
ins_status_string = ins_error_dict.get(ins_error, "Unk Error")
else:
ins_status_string = ins_type_dict.get(ins_type, "unk") + "-"
ins_status_string += ins_mode_dict.get(ins_mode, "unk")
if odo_status == 1:
ins_status_string += "+Odo"
# get age of corrections from baseline view
if self.baseline_view:
if (self.baseline_view.age_corrections is not None
and (current_time -
self.baseline_view.last_age_corr_receipt_time) < 1):
self.age_of_corrections = "{0} s".format(
self.baseline_view.age_corrections)
else:
self.age_of_corrections = EMPTY_STR
# populate modes and #sats on status bar
self.ins_status_string = ins_status_string
self.rtk_mode = baseline_display_mode
self.pos_mode = llh_display_mode
self.num_sats_str = "{}".format(llh_num_sats)
# --- end of status bar mode determination section ---
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
def _csv_logging_button_action(self):
if self.csv_logging and self.baseline_view.logging_b and self.solution_view.logging_p and self.solution_view.logging_v:
print("Stopped CSV logging")
self.csv_logging = False
self.baseline_view.logging_b = False
self.solution_view.logging_p = False
self.solution_view.logging_v = False
else:
print("Started CSV logging at %s" % self.directory_name)
self.csv_logging = True
self.baseline_view.logging_b = True
self.solution_view.logging_p = True
self.solution_view.logging_v = True
def _start_json_logging(self, override_filename=None):
if override_filename:
filename = override_filename
else:
filename = time.strftime("swift-gnss-%Y%m%d-%H%M%S.sbp.json",
time.localtime())
filename = os.path.normpath(
os.path.join(self.directory_name, filename))
self.logger = s.get_logger(True, filename, self.expand_json)
self.forwarder = sbpc.Forwarder(self.link, self.logger)
self.forwarder.start()
if self.settings_view:
self.settings_view._settings_read_all()
def _stop_json_logging(self):
fwd = self.forwarder
fwd.stop()
self.logger.flush()
self.logger.close()
def _json_logging_button_action(self):
if self.first_json_press and self.json_logging:
print(
"JSON Logging initiated via CMD line. Please press button again to stop logging"
)
elif self.json_logging:
self._stop_json_logging()
self.json_logging = False
print("Stopped JSON logging")
else:
self._start_json_logging()
self.json_logging = True
self.first_json_press = False
def _json_logging_button_fired(self):
if not os.path.exists(self.directory_name) and not self.json_logging:
print(
"The selected logging directory does not exist and will be created."
)
self._json_logging_button_action()
def _csv_logging_button_fired(self):
if not os.path.exists(self.directory_name) and not self.csv_logging:
print(
"The selected logging directory does not exist and will be created."
)
self._csv_logging_button_action()
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, traceback):
self.console_output.close()
def __init__(self,
link,
driver,
update,
log_level_filter,
error=False,
cnx_desc=None,
json_logging=False,
show_csv_log=False,
log_dirname=None,
override_filename=None,
log_console=False,
connection_info=None,
expand_json=False,
hide_legend=False):
self.error = error
self.cnx_desc = cnx_desc
self.connection_info = connection_info
self.show_csv_log = show_csv_log
self.dev_id = cnx_desc
self.num_sats_str = EMPTY_STR
self.mode = ''
self.ins_status_string = "None"
self.forwarder = None
self.age_of_corrections = '--'
self.expand_json = expand_json
# if we have passed a logfile, we set our directory to it
override_filename = override_filename
self.last_status_update_time = 0
self.last_driver_bytes_read = 0
self.driver = driver
if log_dirname:
self.directory_name = log_dirname
if override_filename:
override_filename = os.path.join(log_dirname,
override_filename)
else:
self.directory_name = swift_path
# Start swallowing sys.stdout and sys.stderr
self.console_output = OutputList(tfile=log_console,
outdir=self.directory_name)
sys.stdout = self.console_output
self.console_output.write("Console: " + CONSOLE_VERSION +
" starting...")
if not error:
sys.stderr = self.console_output
self.log_level_filter = log_level_filter
self.console_output.log_level_filter = str_to_log_level(
log_level_filter)
try:
self.link = link
self.link.add_callback(self.print_message_callback,
SBP_MSG_PRINT_DEP)
self.link.add_callback(self.log_message_callback, SBP_MSG_LOG)
self.link.add_callback(self.ext_event_callback, SBP_MSG_EXT_EVENT)
self.link.add_callback(self.cmd_resp_callback,
SBP_MSG_COMMAND_RESP)
self.link.add_callback(self.update_on_heartbeat, SBP_MSG_HEARTBEAT)
self.dep_handler = DeprecatedMessageHandler(link)
settings_read_finished_functions = []
self.tracking_view = TrackingView(self.link,
legend_visible=(not hide_legend))
self.solution_view = SolutionView(self.link,
dirname=self.directory_name)
self.baseline_view = BaselineView(self.link,
dirname=self.directory_name)
self.skyplot_view = SkyplotView(self.link, self.tracking_view)
self.observation_view = ObservationView(
self.link,
name='Local',
relay=False,
dirname=self.directory_name,
tracking_view=self.tracking_view)
self.observation_view_base = ObservationView(
self.link,
name='Remote',
relay=True,
dirname=self.directory_name)
self.system_monitor_view = SystemMonitorView(self.link)
self.update_view = UpdateView(self.link,
download_dir=swift_path,
prompt=update,
connection_info=self.connection_info)
self.ins_view = INSView(self.link)
self.mag_view = MagView(self.link)
self.spectrum_analyzer_view = SpectrumAnalyzerView(self.link)
settings_read_finished_functions.append(
self.update_view.compare_versions)
self.networking_view = SbpRelayView(self.link)
self.json_logging = json_logging
self.csv_logging = False
self.first_json_press = True
if json_logging:
self._start_json_logging(override_filename)
self.json_logging = True
# we set timer interval to 1200 milliseconds because we expect a heartbeat each second
self.timer_cancel = call_repeatedly(HEARTBEAT_CHECK_PERIOD_SECONDS,
self.check_heartbeat)
# Once we have received the settings, update device_serial with
# the Swift serial number which will be displayed in the window
# title. This callback will also update the header route as used
# by the networking view.
def update_serial():
mfg_id = None
try:
self.uuid = self.settings_view.settings['system_info'][
'uuid'].value
mfg_id = self.settings_view.settings['system_info'][
'serial_number'].value
except KeyError:
pass
if mfg_id:
self.device_serial = 'PK' + str(mfg_id)
skip_settings_read = False
if 'mode' in self.connection_info:
if self.connection_info['mode'] == 'file':
skip_settings_read = True
settings_read_finished_functions.append(update_serial)
self.settings_view = SettingsView(self.link,
settings_read_finished_functions,
skip_read=skip_settings_read)
self.update_view.settings = self.settings_view.settings
self.python_console_env = {
'send_message': self.link,
'link': self.link,
}
self.python_console_env.update(
self.tracking_view.python_console_cmds)
self.python_console_env.update(
self.solution_view.python_console_cmds)
self.python_console_env.update(
self.baseline_view.python_console_cmds)
self.python_console_env.update(
self.skyplot_view.python_console_cmds)
self.python_console_env.update(
self.observation_view.python_console_cmds)
self.python_console_env.update(
self.networking_view.python_console_cmds)
self.python_console_env.update(
self.system_monitor_view.python_console_cmds)
self.python_console_env.update(
self.update_view.python_console_cmds)
self.python_console_env.update(self.ins_view.python_console_cmds)
self.python_console_env.update(self.mag_view.python_console_cmds)
self.python_console_env.update(
self.settings_view.python_console_cmds)
self.python_console_env.update(
self.spectrum_analyzer_view.python_console_cmds)
except: # noqa
import traceback
traceback.print_exc()
if self.error:
os._exit(1)
class SettingsView(HasTraits):
settings_yaml = list()
settings_read_button = SVGButton(
label='Reload', tooltip='Reload settings from Piksi',
filename=os.path.join(os.path.dirname(__file__), 'images', 'fontawesome', 'refresh.svg'),
width=16, height=16
)
settings_save_button = SVGButton(
label='Save to Flash', tooltip='Save settings to Flash',
filename=os.path.join(os.path.dirname(__file__), 'images', 'fontawesome', 'download.svg'),
width=16, height=16
)
factory_default_button = SVGButton(
label='Reset to Defaults', tooltip='Reset to Factory Defaults',
filename=os.path.join(os.path.dirname(__file__), 'images', 'fontawesome', 'exclamation-triangle.svg'),
width=16, height=16
)
settings_list = List(SettingBase)
selected_setting = Instance(SettingBase)
traits_view = View(
HSplit(
Item('settings_list',
editor = TabularEditor(
adapter=SimpleAdapter(),
editable_labels=False,
auto_update=True,
selected='selected_setting'
),
show_label=False,
),
VGroup(
HGroup(
Item('settings_read_button', show_label=False),
Item('settings_save_button', show_label=False),
Item('factory_default_button', show_label=False),
),
Item('selected_setting', style='custom', show_label=False),
),
)
)
def _settings_read_button_fired(self):
self.enumindex = 0
self.ordering_counter = 0
self.link(MsgSettingsReadByIndexReq(index=self.enumindex))
def _settings_save_button_fired(self):
self.link(MsgSettingsSave())
def _factory_default_button_fired(self):
confirm_prompt = prompt.CallbackPrompt(
title="Reset to Factory Defaults?",
actions=[prompt.close_button, prompt.reset_button],
callback=self.reset_factory_defaults
)
confirm_prompt.text = "This will erase all settings and then reset the device.\n" \
+ "Are you sure you want to reset to factory defaults?"
confirm_prompt.run(block=False)
def reset_factory_defaults(self):
# Delete settings file
fio = FileIO(self.link)
fio.remove('config')
# Reset the Piksi
self.link(MsgReset())
##Callbacks for receiving messages
def settings_display_setup(self):
self.settings_list = []
sections = sorted(self.settings.keys())
for sec in sections:
this_section = []
for name, setting in sorted(self.settings[sec].iteritems(),
key=lambda (n, s): s.ordering):
if not (self.hide_expert and setting.expert):
this_section.append(setting)
if this_section:
self.settings_list.append(SectionHeading(sec))
self.settings_list += this_section
# call read_finished_functions as needed
for cb in self.read_finished_functions:
if self.gui_mode:
GUI.invoke_later(cb)
else:
cb()
return
def settings_read_by_index_done_callback(self, sbp_msg, **metadata):
self.settings_display_setup()
return
def settings_read_by_index_callback(self, sbp_msg, **metadata):
if not sbp_msg.payload:
# Settings output from Piksi is terminated by an empty message.
# Bundle up our list and display it.
self.settings_display_setup()
return
section, setting, value, format_type = sbp_msg.payload[2:].split('\0')[:4]
self.ordering_counter += 1
if format_type == '':
format_type = None
else:
setting_type, setting_format = format_type.split(':')
if not self.settings.has_key(section):
self.settings[section] = {}
if format_type is None:
# Plain old setting, no format information
self.settings[section][setting] = Setting(setting, section, value,
ordering=self.ordering_counter,
settings=self
)
else:
if setting_type == 'enum':
enum_values = setting_format.split(',')
self.settings[section][setting] = EnumSetting(setting, section, value,
ordering=self.ordering_counter,
values=enum_values,
settings=self
)
else:
# Unknown type, just treat is as a string
self.settings[section][setting] = Setting(setting, section, value,
settings=self
)
self.enumindex += 1
self.link(MsgSettingsReadByIndexReq(index=self.enumindex))
def piksi_startup_callback(self, sbp_msg, **metadata):
self._settings_read_button_fired()
def set(self, section, name, value):
self.link(MsgSettingsWrite(setting='%s\0%s\0%s\0' % (section, name, value)))
def cleanup(self):
""" Remove callbacks from serial link. """
self.link.remove_callback(self.piksi_startup_callback, SBP_MSG_STARTUP)
self.link.remove_callback(self.settings_read_by_index_callback, SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.link.remove_callback(self.settings_read_by_index_callback, SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.link.remove_callback(self.settings_read_by_index_done_callback, SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
def __enter__(self):
return self
def __exit__(self, *args):
self.cleanup()
def __init__(self, link, read_finished_functions=[], name_of_yaml_file="settings.yaml", hide_expert=False, gui_mode=True):
super(SettingsView, self).__init__()
self.hide_expert = hide_expert
self.gui_mode = gui_mode
self.enumindex = 0
self.settings = {}
self.link = link
self.link.add_callback(self.piksi_startup_callback, SBP_MSG_STARTUP)
self.link.add_callback(self.settings_read_by_index_callback, SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.link.add_callback(self.settings_read_by_index_callback, SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.link.add_callback(self.settings_read_by_index_done_callback, SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
# Read in yaml file for setting metadata
self.settings_yaml = SettingsList(name_of_yaml_file)
# List of functions to be executed after all settings are read.
# No support for arguments currently.
self.read_finished_functions = read_finished_functions
self.setting_detail = SettingBase()
self._settings_read_button_fired()
self.python_console_cmds = {
'settings': self
}
class SbpRelayView(HasTraits):
"""
SBP Relay view- Class allows user to specify port, IP address, and message set
to relay over UDP and to configure a http connection
"""
running = Bool(False)
_network_info = List()
configured = Bool(False)
broadcasting = Bool(False)
msg_enum = Enum('Observations', 'All')
ip_ad = String(DEFAULT_UDP_ADDRESS)
port = Int(DEFAULT_UDP_PORT)
information = String(
'UDP Streaming\n\nBroadcast SBP information received by'
' the console to other machines or processes over UDP. With the \'Observations\''
' radio button selected, the console will broadcast the necessary information'
' for a rover Piksi to acheive an RTK solution.'
'\n\nThis can be used to stream observations to a remote Piksi through'
' aircraft telemetry via ground control software such as MAVProxy or'
' Mission Planner.')
show_networking = Bool(False)
http_information = String(
'Experimental Piksi Networking\n\n'
"Use this widget to connect Piksi receivers to http servers.\n\n")
start = Button(label='Start', toggle=True, width=32)
stop = Button(label='Stop', toggle=True, width=32)
connected_rover = Bool(False)
connect_rover = Button(label='Connect', toggle=True, width=32)
disconnect_rover = Button(label='Disconnect', toggle=True, width=32)
url = String()
base_pragma = String()
rover_pragma = String()
base_device_uid = String()
rover_device_uid = String()
toggle = True
network_refresh_button = SVGButton(
label='Refresh Network Status',
tooltip='Refresh Network Status',
filename=resource_filename('console/images/fontawesome/refresh.svg'),
width=16,
height=16,
aligment='center')
cell_modem_view = Instance(CellModemView)
view = View(
VGroup(spring,
HGroup(
VGroup(
Item(
'msg_enum',
label="Messages to broadcast",
style='custom',
enabled_when='not running'),
Item(
'ip_ad',
label='IP Address',
enabled_when='not running'),
Item('port', label="Port", enabled_when='not running'),
HGroup(spring,
UItem(
'start',
enabled_when='not running',
show_label=False),
UItem(
'stop',
enabled_when='running',
show_label=False), spring)),
VGroup(
Item(
'information',
label="Notes",
height=10,
editor=MultilineTextEditor(
TextEditor(multi_line=True)),
style='readonly',
show_label=False,
resizable=True,
padding=15),
spring,
),
visible_when='not show_networking'
),
spring,
HGroup(
VGroup(
HGroup(spring,
UItem(
'connect_rover',
enabled_when='not connected_rover',
show_label=False),
UItem(
'disconnect_rover',
enabled_when='connected_rover',
show_label=False), spring),
HGroup(
Spring(springy=False, width=2),
Item(
'url',
enabled_when='not connected_rover',
show_label=True), Spring(
springy=False, width=2)),
HGroup(spring,
Item('base_pragma', label='Base option '),
Item('base_device_uid', label='Base device '),
spring),
HGroup(spring,
Item('rover_pragma', label='Rover option'),
Item('rover_device_uid', label='Rover device'),
spring), ),
VGroup(
Item(
'http_information',
label="Notes",
height=10,
editor=MultilineTextEditor(
TextEditor(multi_line=True)),
style='readonly',
show_label=False,
resizable=True,
padding=15),
spring, ),
visible_when='show_networking'),
HGroup(Item('cell_modem_view', style='custom', show_label=False),
VGroup(
Item(
'_network_info',
style='readonly',
editor=TabularEditor(
adapter=SimpleNetworkAdapter()),
show_label=False, ),
Item(
'network_refresh_button', show_label=False,
width=0.50),
show_border=True,
label="Network"),
)
)
)
def _network_callback(self, m, **metadata):
txstr = sizeof_fmt(m.tx_bytes),
rxstr = sizeof_fmt(m.rx_bytes)
if m.interface_name.startswith('ppp0'): # Hack for ppp tx and rx which doesn't work
txstr = "---"
rxstr = "---"
elif m.interface_name.startswith('lo') or m.interface_name.startswith('sit0'):
return
table_row = ((m.interface_name, ip_bytes_to_string(m.ipv4_address),
((m.flags & (1 << 6)) != 0), txstr, rxstr))
exists = False
for i, each in enumerate(self._network_info):
if each[0][0] == table_row[0][0]:
self._network_info[i] = table_row
exists = True
if not exists:
self._network_info.append(table_row)
def __init__(self,
link,
show_networking=False,
device_uid=None,
url='',
whitelist=None,
rover_pragma='',
base_pragma='',
rover_uuid='',
base_uuid='',
connect=False,
verbose=False):
"""
Traits tab with UI for UDP broadcast of SBP.
Parameters
----------
link : sbp.client.handler.Handler
Link for SBP transfer to/from Piksi.
device_uid : str
Piksi Device UUID (defaults to None)
base : str
HTTP endpoint
whitelist : [int] | None
Piksi Device UUID (defaults to None)
"""
self.link = link
# Whitelist used for UDP broadcast view
self.cell_modem_view = CellModemView(link)
self.msgs = OBS_MSGS
# register a callback when the msg_enum trait changes
self.on_trait_change(self.update_msgs, 'msg_enum')
# Whitelist used for broadcasting
self.whitelist = whitelist
self.device_uid = None
self.python_console_cmds = {'update': self}
self.rover_pragma = rover_pragma
self.base_pragma = base_pragma
self.rover_device_uid = rover_uuid
self.base_device_uid = base_uuid
self.verbose = verbose
self.http_watchdog_thread = None
self.url = url
self.show_networking = show_networking
if connect:
self.connect_when_uuid_received = True
else:
self.connect_when_uuid_received = False
self.cellmodem_interface_name = "ppp0"
self.link.add_callback(self._network_callback,
SBP_MSG_NETWORK_STATE_RESP)
def update_msgs(self):
"""Updates the instance variable msgs which store the msgs that we
will send over UDP.
"""
if self.msg_enum == 'Observations':
self.msgs = OBS_MSGS
elif self.msg_enum == 'All':
self.msgs = [None]
else:
raise NotImplementedError
def set_route(self, uuid=None, serial_id=None, channel=CHANNEL_UUID):
"""Sets serial_id hash for HTTP headers.
Parameters
----------
uuid: str
real uuid of device
serial_id : int
Piksi device ID
channel : str
UUID namespace for device UUID
"""
if uuid:
device_uid = uuid
elif serial_id:
device_uid = str(get_uuid(channel, serial_id % 1000))
else:
print(
"Improper call of set_route, either a serial number or UUID should be passed"
)
device_uid = str(get_uuid(channel, 1234))
print("Setting UUID to default value of {0}".format(device_uid))
self.device_uid = device_uid
def _prompt_setting_error(self, text):
"""Nonblocking prompt for a device setting error.
Parameters
----------
text : str
Helpful error message for the user
"""
prompt = CallbackPrompt(title="Setting Error", actions=[close_button])
prompt.text = text
prompt.run(block=False)
def update_network_state(self):
self._network_refresh_button_fired()
def _network_refresh_button_fired(self):
self._network_info = []
self.link(MsgNetworkStateReq())
def _disconnect_rover_fired(self):
"""Handle callback for HTTP rover disconnects.
"""
try:
if (isinstance(self.http_watchdog_thread, threading.Thread) and
not self.http_watchdog_thread.stopped()):
self.http_watchdog_thread.stop()
else:
print(("Unable to disconnect: Http watchdog thread "
"inititalized at {0} and connected since {1} has "
"already been stopped").format(
self.http_watchdog_thread.get_init_time(),
self.http_watchdog_thread.get_connect_time()))
self.connected_rover = False
except: # noqa
self.connected_rover = False
import traceback
print(traceback.format_exc())
def _connect_rover_fired(self):
"""Handle callback for HTTP rover connections. Launches an instance of http_watchdog_thread.
"""
if not self.device_uid:
msg = "\nDevice ID not found!\n\nConnection requires a valid Piksi device ID."
self._prompt_setting_error(msg)
return
try:
_base_device_uid = self.base_device_uid or self.device_uid
_rover_device_uid = self.rover_device_uid or self.device_uid
config = HttpConsoleConnectConfig(
self.link, self.device_uid, self.url, self.whitelist,
self.rover_pragma, self.base_pragma, _rover_device_uid,
_base_device_uid)
self.http_watchdog_thread = HttpWatchdogThread(
link=self.link,
http_config=config,
stopped_callback=self._disconnect_rover_fired,
verbose=self.verbose)
self.connected_rover = True
self.http_watchdog_thread.start()
except: # noqa
if (isinstance(self.http_watchdog_thread, threading.Thread) and self.http_watchdog_thread.stopped()):
self.http_watchdog_thread.stop()
self.connected_rover = False
import traceback
print(traceback.format_exc())
def _start_fired(self):
"""Handle start udp broadcast button. Registers callbacks on
self.link for each of the self.msgs If self.msgs is None, it
registers one generic callback for all messages.
"""
self.running = True
try:
self.func = UdpLogger(self.ip_ad, self.port)
self.link.add_callback(self.func, self.msgs)
except: # noqa
import traceback
print(traceback.format_exc())
def _stop_fired(self):
"""Handle the stop udp broadcast button. It uses the self.funcs and
self.msgs to remove the callbacks that were registered when the
start button was pressed.
"""
try:
self.link.remove_callback(self.func, self.msgs)
self.func.__exit__()
self.func = None
self.running = False
except: # noqa
import traceback
print(traceback.format_exc())
class SettingsView(HasTraits):
"""Traits-defined console settings view.
link : object
Serial driver object.
read_finished_functions : list
Callbacks to call on finishing a settings read.
name_of_yaml_file : str
Settings to read from (defaults to settings.yaml)
expert : bool
Show expert settings (defaults to False)
gui_mode : bool
??? (defaults to True)
skip : bool
Skip reading of the settings (defaults to False). Intended for
use when reading from network connections.
"""
show_auto_survey = Bool(False)
settings_yaml = list()
auto_survey = SVGButton(
label='Auto\nSurvey',
tooltip='Auto populate surveyed lat, lon and alt fields',
filename='',
width=20,
height=20)
settings_read_button = SVGButton(
tooltip='Reload settings from Piksi',
filename=resource_filename('console/images/fontawesome/refresh.svg'),
allow_clipping=False,
width_padding=4,
height_padding=4)
settings_save_button = SVGButton(
label='Save to\nDevice',
tooltip='Save settings to persistent storage on device.',
filename=resource_filename('console/images/fontawesome/floppy-o.svg'),
width=20,
height=20)
settings_export_to_file_button = SVGButton(
label='Export to\nFile',
tooltip='Export settings from device to a file on this PC.',
filename=resource_filename('console/images/fontawesome/download.svg'),
width=20,
height=20)
settings_import_from_file_button = SVGButton(
label='Import\nfrom File',
tooltip='Import settings to device from a file on this PC.',
filename=resource_filename('console/images/fontawesome/upload.svg'),
width=20,
height=20)
factory_default_button = SVGButton(
label='Reset to\nDefaults',
tooltip='Reset to Factory Defaults',
filename=resource_filename(
'console/images/fontawesome/exclamation-triangle.svg'),
width=20,
height=20)
settings_list = List(SettingBase)
expert = Bool()
selected_setting = Instance(SettingBase)
traits_view = View(
HSplit(
Item(
'settings_list',
editor=TabularEditor(adapter=SimpleAdapter(),
editable_labels=False,
auto_update=True,
editable=False,
selected='selected_setting'),
show_label=False,
),
VGroup(
HGroup(
Item('settings_save_button', show_label=False),
Item('settings_export_to_file_button', show_label=False),
Item('settings_import_from_file_button', show_label=False),
Item('factory_default_button', show_label=False),
Item('auto_survey',
show_label=False,
visible_when='show_auto_survey'),
),
HGroup(
Item('settings_read_button',
show_label=False,
padding=0,
height=-20,
width=-20),
Item('', label="Refresh settings\nfrom device", padding=0),
Item('expert', show_label=False),
Item('', label="Show Advanced\nSettings", padding=0),
),
Item('selected_setting', style='custom', show_label=False),
),
))
def _selected_setting_changed(self):
if self.selected_setting:
if (self.selected_setting.name in [
'surveyed_position', 'broadcast', 'surveyed_lat',
'surveyed_lon', 'surveyed_alt'
] and self.lat != 0 and self.lon != 0):
self.show_auto_survey = True
else:
self.show_auto_survey = False
def _expert_changed(self, info):
try:
self.settings_display_setup(do_read_finished=False)
except AttributeError:
pass
def update_required_smoothpose_settings(self):
"""
Update any recommended settings for smoothpose
"""
list = self._determine_smoothpose_recommended_settings()
for each_setting in list:
self.settings[each_setting.section][
each_setting.name].value = each_setting.rec_value
def _determine_smoothpose_recommended_settings(self):
"""
Returns a list of settings that should change for smoothpose
"""
recommended_settings = {
'imu_raw_output': Setting('imu_raw_output', 'imu', 'True'),
'gyro_range': Setting('gyro_range', 'imu', '1000'),
'acc_range': Setting('acc_range', 'imu', '8g'),
'imu_rate': Setting('imu_rate', 'imu', '100')
}
settings_wrong_list = []
for each_key in recommended_settings.keys():
if recommended_settings[each_key].value != self.settings['imu'][
each_key].value:
self.settings['imu'][
each_key].rec_value = recommended_settings[each_key].value
settings_wrong_list.append(self.settings['imu'][each_key])
return settings_wrong_list
def _save_and_reset(self):
self._settings_save_button_fired()
self.link(MsgReset(flags=0))
def _display_ins_settings_hint(self):
"""
Display helpful hint messages to help a user set up inertial product
"""
settings_list = self._determine_smoothpose_recommended_settings()
if len(settings_list) > 0:
confirm_prompt = prompt.CallbackPrompt(
title="Update Recommended Inertial Navigation Settings?",
actions=[prompt.close_button, prompt.update_button],
callback=self.update_required_smoothpose_settings)
confirm_prompt.settings_list = settings_list
confirm_prompt.text = "\n\n" \
" In order to enable INS output, it is necessary to enable and configure the imu. \n" \
" Your current settings indicate that your imu raw ouptut is disabled and/or improperly configured. \n\n" \
" Choose \"Update\" to allow the console to change the following settings on your device to help enable INS output. \n" \
" Choose \"Close\" to ignore this recommendation and not update any device settings. \n\n"
# from objbrowser import browse
# browse(confirm_prompt)
confirm_prompt.view.content.content[0].content.append(
Item(
"settings_list",
editor=TabularEditor(adapter=SimpleChangeAdapter(),
editable_labels=False,
auto_update=True,
editable=False),
# Only make pop-up as tall as necessary
height=-(len(confirm_prompt.settings_list) * 25 + 40),
label='Recommended Settings'))
confirm_prompt.run(block=False)
while (confirm_prompt.thread.is_alive()):
# Wait until first popup is closed before opening second popup
time.sleep(1)
# even if we didn't need to change any settings, we still have to save settings and restart
self.display_ins_output_hint()
def display_ins_output_hint(self):
confirm_prompt2 = prompt.CallbackPrompt(
title="Restart Device?",
actions=[prompt.close_button, prompt.ok_button],
callback=self._save_and_reset)
confirm_prompt2.text = "\n\n" \
" In order for the \"Ins Output Mode\" setting to take effect, it is necessary to save the \n" \
" current settings to device flash and then power cycle your device. \n\n" \
" Choose \"OK\" to immediately save settings to device flash and send the software reset command. \n" \
" The software reset will temporarily interrupt the console's connection to the device but it \n" \
" will recover on its own. \n\n"
confirm_prompt2.run(block=False)
def _send_pending_settings_by_index(self):
for eachindex in self.pending_settings:
self.link(MsgSettingsReadByIndexReq(index=eachindex))
def _restart_retry_thread(self):
if self.retry_pending_read_index_thread:
self.retry_pending_read_index_thread.stop()
self.retry_pending_read_index_thread = TimedDelayStoppableThread(
SETTINGS_RETRY_TIMEOUT,
target=self._send_pending_settings_by_index,
args=[])
self.retry_pending_read_index_thread.start()
def _settings_read_by_index(self):
self.enumindex = 0 # next index to ask for
self.pending_settings = [] # list of settings idices we've asked for
self.ordering_counter = 0 # helps make deterministic order of settings
self.setup_pending = True # guards against receipt of multiple "done" msgs
# queue up BATCH_WINDOW settings indices to read
self.pending_settings = range(self.enumindex,
self.enumindex + BATCH_WINDOW)
self.enumindex += BATCH_WINDOW
self._send_pending_settings_by_index()
# start a thread that will resend any read indexes that haven't come
self._restart_retry_thread()
def _settings_read_button_fired(self):
self.settings.clear()
self._settings_read_by_index()
def _settings_save_button_fired(self):
self.link(MsgSettingsSave())
def _factory_default_button_fired(self):
confirm_prompt = prompt.CallbackPrompt(
title="Reset to Factory Defaults?",
actions=[prompt.close_button, prompt.reset_button],
callback=self.reset_factory_defaults)
confirm_prompt.text = "This will erase all settings and then reset the device.\n" \
+ "Are you sure you want to reset to factory defaults?"
confirm_prompt.run(block=False)
def reset_factory_defaults(self):
# Reset the Piksi, with flag set to restore default settings
self.link(MsgReset(flags=1))
def _settings_export_to_file_button_fired(self):
"""Exports current gui settings to INI file. Prompts user for file name and location.
Should handle all expected error cases. Defaults to file "config.ini" in the swift_path
"""
# Prompt user for location and name of file
file = FileDialog(action='save as',
default_directory=swift_path,
default_filename='config.ini',
wildcard='*.ini')
is_ok = file.open()
if is_ok == OK:
print('Exporting settings to local path {0}'.format(file.path))
# copy settings so we can modify dict in place to write for configparser
settings_out = {}
# iterate over nested dict and set inner value to a bare string rather than dict
for section in self.settings:
settings_out[section] = {}
for setting, inner_dict in self.settings[section].iteritems():
settings_out[section][setting] = str(inner_dict.value)
# write out with config parser
parser = configparser.RawConfigParser()
# the optionxform is needed to handle case sensitive settings
parser.optionxform = str
parser.read_dict(settings_out)
# write to ini file
try:
with open(file.path, "w") as f:
parser.write(f)
except IOError as e:
print('Unable to export settings to file due to IOError: {}'.
format(e))
else: # No error message because user pressed cancel and didn't choose a file
pass
def _settings_import_from_file_button_fired(self):
"""Imports settings from INI file and sends settings write to device for each entry.
Prompts user for input file. Should handle all expected error cases.
"""
# Prompt user for file
file = FileDialog(action='open',
default_directory=swift_path,
default_filename='config.ini',
wildcard='*.ini')
is_ok = file.open()
if is_ok == OK: # file chosen successfully
print('Importing settings from local path {} to device.'.format(
file.path))
parser = configparser.ConfigParser()
# the optionxform is needed to handle case sensitive settings
parser.optionxform = str
try:
with open(file.path, 'r') as f:
parser.read_file(f)
except configparser.ParsingError as e: # file formatted incorrectly
print(
'Unable to parse ini file due to ParsingError: {}.'.format(
e))
print('Unable to import settings to device.')
return
except IOError as e: # IOError (likely a file permission issue)
print('Unable to read ini file due to IOError: {}'.format(e))
print('Unable to import settings to device.')
return
# Iterate over each setting and set in the GUI.
# Use the same mechanism as GUI to do settings write to device
for section, settings in parser.items():
this_section = self.settings.get(section, None)
for setting, value in settings.items():
if this_section:
this_setting = this_section.get(setting, None)
if this_setting:
this_setting.value = value
else:
print((
"Unable to import settings from file. Setting \"{0}\" in section \"{1}\""
" has not been sent from device.").format(
setting, section))
return
else:
print((
"Unable to import settings from file."
" Setting section \"{0}\" has not been sent from device."
).format(section))
return
# Double check that no settings had a write failure. All settings should exist if we get to this point.
a = TimedDelayStoppableThread(
SETTINGS_REVERT_TIMEOUT + 0.1,
target=self._wait_for_any_write_failures,
args=(dict(parser)))
a.start()
else:
pass # No error message here because user likely pressed cancel when choosing file
def _auto_survey_fired(self):
confirm_prompt = prompt.CallbackPrompt(
title="Auto populate surveyed position?",
actions=[prompt.close_button, prompt.auto_survey_button],
callback=self.auto_survey_fn)
confirm_prompt.text = "\n" \
+ "This will set the Surveyed Position section to the \n" \
+ "mean position of the last 1000 position solutions.\n \n" \
+ "The fields that will be auto-populated are: \n" \
+ "Surveyed Lat \n" \
+ "Surveyed Lon \n" \
+ "Surveyed Alt \n \n" \
+ "The surveyed position will be an approximate value. \n" \
+ "This may affect the relative accuracy of Piksi. \n \n" \
+ "Are you sure you want to auto-populate the Surveyed Position section?"
confirm_prompt.run(block=False)
def auto_survey_fn(self):
lat_value = str(self.lat)
lon_value = str(self.lon)
alt_value = str(self.alt)
self.settings['surveyed_position']['surveyed_lat'].value = lat_value
self.settings['surveyed_position']['surveyed_lon'].value = lon_value
self.settings['surveyed_position']['surveyed_alt'].value = alt_value
self.settings_display_setup(do_read_finished=False)
def _wait_for_any_write_failures(self, parser):
"""Checks for any settings write failures for which a successful write is expected.
If no failures have occurred, we prompt the user whether to save settings to the device's flash.
Args:
parser (dict): A dict of dicts with setting sections then names for keys
"""
write_failures = 0
for section, settings in parser.items():
for setting, _ in settings.items():
if self.settings[section][setting].write_failure:
write_failures += 1
self.settings[section][setting].write_failure = False
if write_failures == 0:
print("Successfully imported settings from file.")
confirm_prompt = prompt.CallbackPrompt(
title="Save to device flash?",
actions=[prompt.close_button, prompt.ok_button],
callback=self._settings_save_button_fired)
confirm_prompt.text = "\n" \
" Settings import from file complete. Click OK to save the settings \n" \
" to the device's persistent storage. \n"
confirm_prompt.run(block=False)
else:
print(
"Unable to import settings from file: {0} settings write failures occurred."
.format(write_failures))
# Callbacks for receiving messages
def settings_display_setup(self, do_read_finished=True):
self.settings_list = []
sections = sorted(self.settings.keys())
for sec in sections:
this_section = []
for name, setting in sorted(self.settings[sec].iteritems(),
key=lambda n_s: n_s[1].ordering):
if not setting.expert or (self.expert and setting.expert):
this_section.append(setting)
if this_section:
self.settings_list.append(SectionHeading(sec))
self.settings_list += this_section
# call read_finished_functions as needed
if do_read_finished:
for cb in self.read_finished_functions:
if self.gui_mode:
GUI.invoke_later(cb)
else:
cb()
def settings_read_by_index_done_callback(self, sbp_msg, **metadata):
if self.retry_pending_read_index_thread:
self.retry_pending_read_index_thread.stop()
# we should only setup the display once per iteration to avoid races
if self.setup_pending:
self.settings_display_setup()
self.setup_pending = False
def settings_read_resp_callback(self, sbp_msg, **metadata):
confirmed_set = True
settings_list = sbp_msg.setting.split("\0")
if len(settings_list) <= 3:
print("Received malformed settings read response {0}".format(
sbp_msg))
confirmed_set = False
try:
if self.settings[settings_list[0]][
settings_list[1]].value != settings_list[2]:
try:
float_val = float(self.settings[settings_list[0]][
settings_list[1]].value)
float_val2 = float(settings_list[2])
if abs(float_val - float_val2) > 0.000001:
confirmed_set = False
except ValueError:
confirmed_set = False
if confirmed_set:
# If we verify the new values matches our expectation, we cancel the revert thread
if self.settings[settings_list[0]][
settings_list[1]].timed_revert_thread:
self.settings[settings_list[0]][
settings_list[1]].timed_revert_thread.stop()
self.settings[settings_list[0]][
settings_list[1]].confirmed_set = True
except KeyError:
return
def settings_write_resp_callback(self, sbp_msg, **metadata):
if sbp_msg.status == 2:
# Setting was rejected. This shouldn't happen because we'll only
# send requests for settings enumerated using read by index.
return
settings_list = sbp_msg.setting.split("\0")
if len(settings_list) <= 3:
print("Received malformed settings write response {0}".format(
sbp_msg))
return
try:
setting = self.settings[settings_list[0]][settings_list[1]]
except KeyError:
return
if setting.timed_revert_thread:
setting.timed_revert_thread.stop()
if sbp_msg.status == 1:
# Value was rejected. Inform the user and revert display to the
# old value.
new = setting.value
old = settings_list[2]
setting.revert_to_prior_value(setting.name, old, new)
return
# Write accepted. Use confirmed value in display without sending settings write.
setting._prevent_revert_thread = True
setting.value = settings_list[2]
setting._prevent_revert_thread = False
setting.confirmed_set = True
def settings_read_by_index_callback(self, sbp_msg, **metadata):
section, setting, value, format_type = sbp_msg.payload[2:].split(
'\0')[:4]
self.ordering_counter += 1
if format_type == '':
format_type = None
else:
setting_type, setting_format = format_type.split(':')
if section not in self.settings:
self.settings[section] = {}
# setting exists, we won't reinitilize it but rather update existing setting
dict_setting = self.settings[section].get(setting, False)
if dict_setting:
dict_setting._prevent_revert_thread = True
dict_setting.value = value
dict_setting._prevent_revert_thread = False
dict_setting.ordering = self.ordering_counter
if format_type is not None and setting_type == 'enum':
enum_values = setting_format.split(',')
dict_setting.enum_values = enum_values
else:
if format_type is None:
# Plain old setting, no format information
self.settings[section][setting] = Setting(
setting,
section,
value,
ordering=self.ordering_counter,
settings=self)
else:
if setting_type == 'enum':
enum_values = setting_format.split(',')
self.settings[section][setting] = EnumSetting(
setting,
section,
value,
enum_values,
ordering=self.ordering_counter,
settings=self)
else:
# Unknown type, just treat is as a string
self.settings[section][setting] = Setting(
setting,
section,
value,
settings=self,
ordering=self.ordering_counter)
# remove index from list of pending items
if sbp_msg.index in self.pending_settings:
self.pending_settings.remove(sbp_msg.index)
if len(self.pending_settings) == 0:
self.pending_settings = range(self.enumindex,
self.enumindex + BATCH_WINDOW)
self.enumindex += BATCH_WINDOW
self._send_pending_settings_by_index()
self._restart_retry_thread()
def piksi_startup_callback(self, sbp_msg, **metadata):
self._settings_read_by_index()
def set(self, section, name, value):
self.link(
MsgSettingsWrite(setting='%s\0%s\0%s\0' % (section, name, value)))
def cleanup(self):
""" Remove callbacks from serial link. """
self.link.remove_callback(self.piksi_startup_callback, SBP_MSG_STARTUP)
self.link.remove_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.link.remove_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.link.remove_callback(self.settings_read_by_index_done_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
def __enter__(self):
return self
def __exit__(self, *args):
self.cleanup()
def __init__(self,
link,
read_finished_functions=[],
name_of_yaml_file="settings.yaml",
expert=False,
gui_mode=True,
skip=False):
super(SettingsView, self).__init__()
self.ordering_counter = 0
self.expert = expert
self.show_auto_survey = False
self.gui_mode = gui_mode
self.enumindex = 0
self.settings = {}
self.link = link
self.link.add_callback(self.piksi_startup_callback, SBP_MSG_STARTUP)
self.link.add_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.link.add_callback(self.settings_read_by_index_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.link.add_callback(self.settings_read_by_index_done_callback,
SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
self.link.add_callback(self.settings_read_resp_callback,
SBP_MSG_SETTINGS_READ_RESP)
self.link.add_callback(self.settings_write_resp_callback,
SBP_MSG_SETTINGS_WRITE_RESP)
# Read in yaml file for setting metadata
self.settings_yaml = SettingsList(name_of_yaml_file)
# List of functions to be executed after all settings are read.
# No support for arguments currently.
self.read_finished_functions = read_finished_functions
self.setting_detail = SettingBase()
self.pending_settings = []
self.retry_pending_read_index_thread = None
self.setup_pending = False
if not skip:
try:
self._settings_read_by_index()
except IOError:
print(
"IOError in settings_view startup call of _settings_read_by_index."
)
print("Verify that write permissions exist on the port.")
self.python_console_cmds = {'settings': self}
class SolutionView(HasTraits):
python_console_cmds = Dict()
# we need to doubleup on Lists to store the psuedo absolutes separately
# without rewriting everything
"""
logging_v : toggle logging for velocity files
directory_name_v : location and name of velocity files
logging_p : toggle logging for position files
directory_name_p : location and name of velocity files
"""
plot_history_max = Int(1000)
logging_v = Bool(False)
display_units = Enum(["degrees", "meters"])
directory_name_v = File
logging_p = Bool(False)
directory_name_p = File
lats_psuedo_abs = List()
lngs_psuedo_abs = List()
alts_psuedo_abs = List()
table = List()
dops_table = List()
pos_table = List()
vel_table = List()
rtk_pos_note = Str(
"It is necessary to enter the \"Surveyed Position\" settings for the base station in order to view the RTK Positions in this tab."
)
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
# Store plots we care about for legend
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(
label='',
tooltip='Clear',
filename=resource_filename('console/images/iconic/x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(
label='',
tooltip='Zoom All',
toggle=True,
filename=resource_filename('console/images/iconic/fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(
label='',
tooltip='Center on Solution',
toggle=True,
filename=resource_filename('console/images/iconic/target.svg'),
width=16,
height=16)
paused_button = SVGButton(
label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
width=16,
height=16)
traits_view = View(
HSplit(
VGroup(
Item('table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
Item('rtk_pos_note',
show_label=False,
resizable=True,
editor=MultilineTextEditor(TextEditor(multi_line=True)),
style='readonly',
width=0.3,
height=-40),
),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
Item('display_units', label="Display Units"),
),
Item('plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8))),
)))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_remove_current(self):
self.plot_data.set_data('cur_lat_spp', [])
self.plot_data.set_data('cur_lng_spp', [])
self.plot_data.set_data('cur_alt_spp', [])
self.plot_data.set_data('cur_lat_dgnss', [])
self.plot_data.set_data('cur_lng_dgnss', [])
self.plot_data.set_data('cur_alt_dgnss', [])
self.plot_data.set_data('cur_lat_float', [])
self.plot_data.set_data('cur_lng_float', [])
self.plot_data.set_data('cur_alt_float', [])
self.plot_data.set_data('cur_lat_fixed', [])
self.plot_data.set_data('cur_lng_fixed', [])
self.plot_data.set_data('cur_alt_fixed', [])
self.plot_data.set_data('cur_lat_sbas', [])
self.plot_data.set_data('cur_lng_sbas', [])
self.plot_data.set_data('cur_alt_sbas', [])
def _clear_history(self):
self.plot_data.set_data('lat_spp', [])
self.plot_data.set_data('lng_spp', [])
self.plot_data.set_data('alt_spp', [])
self.plot_data.set_data('lat_dgnss', [])
self.plot_data.set_data('lng_dgnss', [])
self.plot_data.set_data('alt_dgnss', [])
self.plot_data.set_data('lat_float', [])
self.plot_data.set_data('lng_float', [])
self.plot_data.set_data('alt_float', [])
self.plot_data.set_data('lat_fixed', [])
self.plot_data.set_data('lng_fixed', [])
self.plot_data.set_data('alt_fixed', [])
self.plot_data.set_data('lat_sbas', [])
self.plot_data.set_data('lng_sbas', [])
self.plot_data.set_data('alt_sbas', [])
def _clear_button_fired(self):
self.tows = np.zeros(self.plot_history_max)
self.lats = np.zeros(self.plot_history_max)
self.lngs = np.zeros(self.plot_history_max)
self.alts = np.zeros(self.plot_history_max)
self.modes = np.zeros(self.plot_history_max)
self._clear_history()
self._reset_remove_current()
def _pos_llh_callback(self, sbp_msg, **metadata):
# Updating an ArrayPlotData isn't thread safe (see chaco issue #9), so
# actually perform the update in the UI thread.
if self.running:
GUI.invoke_later(self.pos_llh_callback, sbp_msg)
def age_corrections_callback(self, sbp_msg, **metadata):
age_msg = MsgAgeCorrections(sbp_msg)
if age_msg.age != 0xFFFF:
self.age_corrections = age_msg.age / 10.0
else:
self.age_corrections = None
def update_table(self):
self.table = self.pos_table + self.vel_table + self.dops_table
def auto_survey(self):
if self.last_soln.flags != 0:
self.latitude_list.append(self.last_soln.lat)
self.longitude_list.append(self.last_soln.lon)
self.altitude_list.append(self.last_soln.height)
if len(self.latitude_list) > 1000:
self.latitude_list = self.latitude_list[-1000:]
self.longitude_list = self.longitude_list[-1000:]
self.altitude_list = self.altitude_list[-1000:]
if len(self.latitude_list) != 0:
self.latitude = sum(self.latitude_list) / len(self.latitude_list)
self.altitude = sum(self.altitude_list) / len(self.latitude_list)
self.longitude = sum(self.longitude_list) / len(self.latitude_list)
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 solution_draw(self):
if self.running:
GUI.invoke_later(self._solution_draw)
def _solution_draw(self):
spp_indexer, dgnss_indexer, float_indexer, fixed_indexer, sbas_indexer = None, None, None, None, None
self._clear_history()
soln = self.last_soln
if np.any(self.modes):
if self.display_units == "meters":
offset = (np.mean(self.lats[~(np.equal(self.modes, 0))]),
np.mean(self.lngs[~(np.equal(self.modes, 0))]),
np.mean(self.alts[~(np.equal(self.modes, 0))]))
if not self.meters_per_lat:
(self.meters_per_lat,
self.meters_per_lon) = meters_per_deg(soln.lat)
sf = (self.meters_per_lat, self.meters_per_lon)
self.plot.value_axis.title = 'Latitude (meters)'
self.plot.index_axis.title = 'Longitude (meters)'
else:
offset = (0, 0, 0)
sf = (1, 1)
self.plot.value_axis.title = 'Latitude (degrees)'
self.plot.index_axis.title = 'Longitude (degrees)'
spp_indexer = (self.modes == SPP_MODE)
dgnss_indexer = (self.modes == DGNSS_MODE)
sbas_indexer = (self.modes == SBAS_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] - offset[0]) * sf[0])
self.plot_data.set_data(
'lng_spp', (self.lngs[spp_indexer] - offset[1]) * sf[1])
self.plot_data.set_data('alt_spp',
(self.alts[spp_indexer] - offset[2]))
if any(dgnss_indexer):
self.plot_data.set_data(
'lat_dgnss',
(self.lats[dgnss_indexer] - offset[0]) * sf[0])
self.plot_data.set_data(
'lng_dgnss',
(self.lngs[dgnss_indexer] - offset[1]) * sf[1])
self.plot_data.set_data('alt_dgnss',
(self.alts[dgnss_indexer] - offset[2]))
if any(float_indexer):
self.plot_data.set_data(
'lat_float',
(self.lats[float_indexer] - offset[0]) * sf[0])
self.plot_data.set_data(
'lng_float',
(self.lngs[float_indexer] - offset[1]) * sf[1])
self.plot_data.set_data('alt_float',
(self.alts[float_indexer] - offset[2]))
if any(fixed_indexer):
self.plot_data.set_data(
'lat_fixed',
(self.lats[fixed_indexer] - offset[0]) * sf[0])
self.plot_data.set_data(
'lng_fixed',
(self.lngs[fixed_indexer] - offset[1]) * sf[1])
self.plot_data.set_data('alt_fixed',
(self.alts[fixed_indexer] - offset[2]))
if any(sbas_indexer):
self.plot_data.set_data(
'lat_sbas', (self.lats[sbas_indexer] - offset[0]) * sf[0])
self.plot_data.set_data(
'lng_sbas', (self.lngs[sbas_indexer] - offset[1]) * sf[1])
self.plot_data.set_data('alt_sbas',
(self.alts[sbas_indexer] - offset[2]))
# 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 - offset[0]) * sf[0]])
self.plot_data.set_data('cur_lng_spp',
[(soln.lon - offset[1]) * sf[1]])
elif self.last_pos_mode == DGNSS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_dgnss',
[(soln.lat - offset[0]) * sf[0]])
self.plot_data.set_data('cur_lng_dgnss',
[(soln.lon - offset[1]) * sf[1]])
elif self.last_pos_mode == FLOAT_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_float',
[(soln.lat - offset[0]) * sf[0]])
self.plot_data.set_data('cur_lng_float',
[(soln.lon - offset[1]) * sf[1]])
elif self.last_pos_mode == FIXED_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_fixed',
[(soln.lat - offset[0]) * sf[0]])
self.plot_data.set_data('cur_lng_fixed',
[(soln.lon - offset[1]) * sf[1]])
elif self.last_pos_mode == SBAS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_lat_sbas',
[(soln.lat - offset[0]) * sf[0]])
self.plot_data.set_data('cur_lng_sbas',
[(soln.lon - offset[1]) * sf[1]])
else:
pass
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 - offset[1]) * sf[1] - d,
(soln.lon - offset[1]) * sf[1] + d)
d = (self.plot.value_range.high -
self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(
(soln.lat - offset[0]) * sf[0] - d,
(soln.lat - offset[0]) * sf[0] + d)
if self.zoomall:
plot_square_axes(self.plot,
('lng_spp', 'lng_dgnss', 'lng_float',
'lng_fixed', 'lng_sbas'),
('lat_spp', 'lat_dgnss', 'lat_float',
'lat_fixed', 'lat_sbas'))
def dops_callback(self, sbp_msg, **metadata):
flags = 0
if sbp_msg.msg_type == SBP_MSG_DOPS_DEP_A:
dops = MsgDopsDepA(sbp_msg)
flags = 1
else:
dops = MsgDops(sbp_msg)
flags = dops.flags
if flags != 0:
self.dops_table = [('PDOP', '%.1f' % (dops.pdop * 0.01)),
('GDOP', '%.1f' % (dops.gdop * 0.01)),
('TDOP', '%.1f' % (dops.tdop * 0.01)),
('HDOP', '%.1f' % (dops.hdop * 0.01)),
('VDOP', '%.1f' % (dops.vdop * 0.01))]
else:
self.dops_table = [('PDOP', EMPTY_STR), ('GDOP', EMPTY_STR),
('TDOP', EMPTY_STR), ('HDOP', EMPTY_STR),
('VDOP', EMPTY_STR)]
self.dops_table.append(('DOPS Flags', '0x%03x' % flags))
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 gps_time_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_GPS_TIME_DEP_A:
time_msg = MsgGPSTimeDepA(sbp_msg)
flags = 1
elif sbp_msg.msg_type == SBP_MSG_GPS_TIME:
time_msg = MsgGPSTime(sbp_msg)
flags = time_msg.flags
if flags != 0:
self.week = time_msg.wn
self.nsec = time_msg.ns_residual
def utc_time_callback(self, sbp_msg, **metadata):
tmsg = MsgUtcTime(sbp_msg)
seconds = math.floor(tmsg.seconds)
microseconds = int(tmsg.ns / 1000.00)
if tmsg.flags & 0x1 == 1:
dt = datetime.datetime(tmsg.year, tmsg.month, tmsg.day, tmsg.hours,
tmsg.minutes, tmsg.seconds, microseconds)
self.utc_time = dt
self.utc_time_flags = tmsg.flags
if (tmsg.flags >> 3) & 0x3 == 0:
self.utc_source = "Factory Default"
elif (tmsg.flags >> 3) & 0x3 == 1:
self.utc_source = "Non Volatile Memory"
elif (tmsg.flags >> 3) & 0x3 == 2:
self.utc_source = "Decoded this Session"
else:
self.utc_source = "Unknown"
else:
self.utc_time = None
self.utc_source = None
def __init__(self, link, dirname=''):
super(SolutionView, self).__init__()
self.lats = np.zeros(self.plot_history_max)
self.lngs = np.zeros(self.plot_history_max)
self.alts = np.zeros(self.plot_history_max)
self.tows = np.zeros(self.plot_history_max)
self.modes = np.zeros(self.plot_history_max)
self.log_file = None
self.directory_name_v = dirname
self.directory_name_p = dirname
self.vel_log_file = None
self.last_stime_update = 0
self.last_soln = None
self.counter = 0
self.latitude_list = []
self.longitude_list = []
self.altitude_list = []
self.altitude = 0
self.longitude = 0
self.latitude = 0
self.last_pos_mode = 0
self.plot_data = ArrayPlotData(lat_spp=[],
lng_spp=[],
alt_spp=[],
cur_lat_spp=[],
cur_lng_spp=[],
lat_dgnss=[],
lng_dgnss=[],
alt_dgnss=[],
cur_lat_dgnss=[],
cur_lng_dgnss=[],
lat_float=[],
lng_float=[],
alt_float=[],
cur_lat_float=[],
cur_lng_float=[],
lat_fixed=[],
lng_fixed=[],
alt_fixed=[],
cur_lat_fixed=[],
cur_lng_fixed=[],
lat_sbas=[],
lng_sbas=[],
cur_lat_sbas=[],
cur_lng_sbas=[])
self.plot = Plot(self.plot_data)
# 1000 point buffer
self.plot.plot(('lng_spp', 'lat_spp'),
type='line',
line_width=0.1,
name='',
color=color_dict[SPP_MODE])
self.plot.plot(('lng_spp', 'lat_spp'),
type='scatter',
name='',
color=color_dict[SPP_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_dgnss', 'lat_dgnss'),
type='line',
line_width=0.1,
name='',
color=color_dict[DGNSS_MODE])
self.plot.plot(('lng_dgnss', 'lat_dgnss'),
type='scatter',
name='',
color=color_dict[DGNSS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_float', 'lat_float'),
type='line',
line_width=0.1,
name='',
color=color_dict[FLOAT_MODE])
self.plot.plot(('lng_float', 'lat_float'),
type='scatter',
name='',
color=color_dict[FLOAT_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_fixed', 'lat_fixed'),
type='line',
line_width=0.1,
name='',
color=color_dict[FIXED_MODE])
self.plot.plot(('lng_fixed', 'lat_fixed'),
type='scatter',
name='',
color=color_dict[FIXED_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
self.plot.plot(('lng_sbas', 'lat_sbas'),
type='scatter',
name='',
color=color_dict[SBAS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
# current values
spp = self.plot.plot(('cur_lng_spp', 'cur_lat_spp'),
type='scatter',
name=mode_dict[SPP_MODE],
color=color_dict[SPP_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
dgnss = self.plot.plot(('cur_lng_dgnss', 'cur_lat_dgnss'),
type='scatter',
name=mode_dict[DGNSS_MODE],
color=color_dict[DGNSS_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
rtkfloat = self.plot.plot(('cur_lng_float', 'cur_lat_float'),
type='scatter',
name=mode_dict[FLOAT_MODE],
color=color_dict[FLOAT_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
rtkfix = self.plot.plot(('cur_lng_fixed', 'cur_lat_fixed'),
type='scatter',
name=mode_dict[FIXED_MODE],
color=color_dict[FIXED_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
sbas = self.plot.plot(('cur_lng_sbas', 'cur_lat_sbas'),
type='scatter',
name=mode_dict[SBAS_MODE],
color=color_dict[SBAS_MODE],
marker='plus',
line_width=1.5,
marker_size=5.0)
plot_labels = ['SPP', 'SBAS', 'DGPS', 'RTK float', 'RTK fixed']
plots_legend = dict(
zip(plot_labels, [spp, sbas, dgnss, rtkfloat, rtkfix]))
self.plot.legend.plots = plots_legend
self.plot.legend.labels = plot_labels # sets order
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'Longitude (degrees)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'Latitude (degrees)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot,
zoom_factor=1.1,
tool_mode="box",
always_on=False)
self.plot.overlays.append(zt)
self.link = link
self.link.add_callback(self.pos_llh_callback,
[SBP_MSG_POS_LLH_DEP_A, SBP_MSG_POS_LLH])
self.link.add_callback(self.vel_ned_callback,
[SBP_MSG_VEL_NED_DEP_A, SBP_MSG_VEL_NED])
self.link.add_callback(self.dops_callback,
[SBP_MSG_DOPS_DEP_A, SBP_MSG_DOPS])
self.link.add_callback(self.gps_time_callback,
[SBP_MSG_GPS_TIME_DEP_A, SBP_MSG_GPS_TIME])
self.link.add_callback(self.utc_time_callback, [SBP_MSG_UTC_TIME])
self.link.add_callback(self.age_corrections_callback,
SBP_MSG_AGE_CORRECTIONS)
call_repeatedly(0.2, self.solution_draw)
self.week = None
self.utc_time = None
self.age_corrections = None
self.nsec = 0
self.meters_per_lat = None
self.meters_per_lon = None
self.python_console_cmds = {
'solution': self,
}
class SwiftConsole(HasTraits):
"""Traits-defined Swift Console.
link : object
Serial driver
update : bool
Update the firmware
log_level_filter : str
Syslog string, one of "ERROR", "WARNING", "INFO", "DEBUG".
skip_settings : bool
Don't read the device settings. Set to False when the console is reading
from a network connection only.
"""
link = Instance(sbpc.Handler)
console_output = Instance(OutputList())
python_console_env = Dict
device_serial = Str('')
dev_id = Str('')
tracking_view = Instance(TrackingView)
solution_view = Instance(SolutionView)
baseline_view = Instance(BaselineView)
observation_view = Instance(ObservationView)
networking_view = Instance(SbpRelayView)
observation_view_base = Instance(ObservationView)
system_monitor_view = Instance(SystemMonitorView)
settings_view = Instance(SettingsView)
update_view = Instance(UpdateView)
imu_view = Instance(IMUView)
mag_view = Instance(MagView)
spectrum_analyzer_view = Instance(SpectrumAnalyzerView)
skylark_view = Instance(SkylarkView)
log_level_filter = Enum(list(SYSLOG_LEVELS.itervalues()))
""""
mode : baseline and solution view - SPP, Fixed or Float
num_sat : baseline and solution view - number of satellites
port : which port is Swift Device is connected to
directory_name : location of logged files
json_logging : enable JSON logging
csv_logging : enable CSV logging
"""
mode = Str('')
num_sats = Int(0)
cnx_desc = Str('')
latency = Str('')
uuid = Str('')
directory_name = Directory
json_logging = Bool(True)
csv_logging = Bool(False)
cnx_icon = Str('')
heartbeat_count = Int()
last_timer_heartbeat = Int()
solid_connection = Bool(False)
csv_logging_button = SVGButton(
toggle=True,
label='CSV log',
tooltip='start CSV logging',
toggle_tooltip='stop CSV logging',
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
orientation='vertical',
width=2,
height=2,
)
json_logging_button = SVGButton(
toggle=True,
label='JSON log',
tooltip='start JSON logging',
toggle_tooltip='stop JSON logging',
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
orientation='vertical',
width=2,
height=2,
)
paused_button = SVGButton(
label='',
tooltip='Pause console update',
toggle_tooltip='Resume console update',
toggle=True,
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
width=8,
height=8)
clear_button = SVGButton(
label='',
tooltip='Clear console buffer',
filename=resource_filename('console/images/iconic/x.svg'),
width=8,
height=8)
view = View(VSplit(
Tabbed(Item('tracking_view', style='custom', label='Tracking'),
Item('solution_view', style='custom', label='Solution'),
Item('baseline_view', style='custom', label='Baseline'),
VSplit(
Item('observation_view', style='custom', show_label=False),
Item('observation_view_base',
style='custom',
show_label=False),
label='Observations',
),
Item('settings_view', style='custom', label='Settings'),
Item('update_view', style='custom', label='Update'),
Tabbed(Item('system_monitor_view',
style='custom',
label='System Monitor'),
Item('imu_view', style='custom', label='IMU'),
Item('mag_view', style='custom', label='Magnetometer'),
Item('networking_view',
label='Networking',
style='custom',
show_label=False),
Item('spectrum_analyzer_view',
label='Spectrum Analyzer',
style='custom'),
label='Advanced',
show_labels=False),
Item('skylark_view', style='custom', label='Skylark'),
show_labels=False),
VGroup(
VGroup(
HGroup(
Spring(width=4, springy=False),
Item('paused_button',
show_label=False,
padding=0,
width=8,
height=8),
Item('clear_button', show_label=False, width=8, height=8),
Item('', label='Console Log', emphasized=True),
Item('csv_logging_button',
emphasized=True,
show_label=False,
width=12,
height=-30,
padding=0),
Item('json_logging_button',
emphasized=True,
show_label=False,
width=12,
height=-30,
padding=0),
Item(
'directory_name',
show_label=False,
springy=True,
tooltip=
'Choose location for file logs. Default is home/SwiftNav.',
height=-25,
enabled_when='not(json_logging or csv_logging)',
editor_args={'auto_set': True}),
UItem(
'log_level_filter',
style='simple',
padding=0,
height=8,
show_label=True,
tooltip=
'Show log levels up to and including the selected level of severity.\nThe CONSOLE log level is always visible.'
),
),
Item('console_output',
style='custom',
editor=InstanceEditor(),
height=125,
show_label=False,
full_size=True),
),
HGroup(
Spring(width=4, springy=False),
Item('',
label='Interface:',
emphasized=True,
tooltip='Interface for communicating with Swift device'),
Item('cnx_desc', show_label=False, style='readonly'),
Item('',
label='FIX TYPE:',
emphasized=True,
tooltip='Device Mode: SPS, Float RTK, Fixed RTK'),
Item('mode', show_label=False, style='readonly'),
Item('',
label='#Sats:',
emphasized=True,
tooltip='Number of satellites used in solution'),
Item('num_sats', padding=2, show_label=False,
style='readonly'),
Item('',
label='Base Latency:',
emphasized=True,
tooltip='Corrections latency (-1 means no corrections)'),
Item('latency', padding=2, show_label=False, style='readonly'),
Item('',
label='Device UUID:',
emphasized=True,
tooltip='Universally Unique Device Identifier (UUID)'),
Item('uuid',
padding=2,
show_label=False,
style='readonly',
width=6),
Spring(springy=True),
Item('cnx_icon',
show_label=False,
padding=0,
width=8,
height=8,
visible_when='solid_connection',
springy=False,
editor=ImageEditor(
allow_clipping=False,
image=ImageResource(
resource_filename(
'console/images/iconic/arrows_blue.png')))),
Item('cnx_icon',
show_label=False,
padding=0,
width=8,
height=8,
visible_when='not solid_connection',
springy=False,
editor=ImageEditor(
allow_clipping=False,
image=ImageResource(
resource_filename(
'console/images/iconic/arrows_grey.png')))),
Spring(width=4, height=-2, springy=False),
),
Spring(height=1, springy=False),
),
),
icon=icon,
resizable=True,
width=800,
height=600,
handler=ConsoleHandler(),
title=CONSOLE_TITLE)
def print_message_callback(self, sbp_msg, **metadata):
try:
encoded = sbp_msg.payload.encode('ascii', 'ignore')
for eachline in reversed(encoded.split('\n')):
self.console_output.write_level(
eachline, str_to_log_level(eachline.split(':')[0]))
except UnicodeDecodeError:
print("Critical Error encoding the serial stream as ascii.")
def log_message_callback(self, sbp_msg, **metadata):
try:
encoded = sbp_msg.text.encode('ascii', 'ignore')
for eachline in reversed(encoded.split('\n')):
self.console_output.write_level(eachline, sbp_msg.level)
except UnicodeDecodeError:
print("Critical Error encoding the serial stream as ascii.")
def ext_event_callback(self, sbp_msg, **metadata):
e = MsgExtEvent(sbp_msg)
print(
'External event: %s edge on pin %d at wn=%d, tow=%d, time qual=%s'
% ("Rising" if
(e.flags &
(1 << 0)) else "Falling", e.pin, e.wn, e.tow, "good" if
(e.flags & (1 << 1)) else "unknown"))
def cmd_resp_callback(self, sbp_msg, **metadata):
r = MsgCommandResp(sbp_msg)
print("Received a command response message with code {0}".format(
r.code))
def _paused_button_fired(self):
self.console_output.paused = not self.console_output.paused
def _log_level_filter_changed(self):
"""
Takes log level enum and translates into the mapped integer.
Integer stores the current filter value inside OutputList.
"""
self.console_output.log_level_filter = str_to_log_level(
self.log_level_filter)
def _clear_button_fired(self):
self.console_output.clear()
def _directory_name_changed(self):
if self.baseline_view and self.solution_view:
self.baseline_view.directory_name_b = self.directory_name
self.solution_view.directory_name_p = self.directory_name
self.solution_view.directory_name_v = self.directory_name
if self.observation_view and self.observation_view_base:
self.observation_view.dirname = self.directory_name
self.observation_view_base.dirname = self.directory_name
def check_heartbeat(self):
# if our heartbeat hasn't changed since the last timer interval the connection must have dropped
if self.heartbeat_count == self.last_timer_heartbeat:
self.solid_connection = False
else:
self.solid_connection = True
self.last_timer_heartbeat = self.heartbeat_count
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 _csv_logging_button_action(self):
if self.csv_logging and self.baseline_view.logging_b and self.solution_view.logging_p and self.solution_view.logging_v:
print("Stopped CSV logging")
self.csv_logging = False
self.baseline_view.logging_b = False
self.solution_view.logging_p = False
self.solution_view.logging_v = False
else:
print("Started CSV logging at %s" % self.directory_name)
self.csv_logging = True
self.baseline_view.logging_b = True
self.solution_view.logging_p = True
self.solution_view.logging_v = True
def _start_json_logging(self, override_filename=None):
if override_filename:
filename = override_filename
else:
filename = time.strftime("swift-gnss-%Y%m%d-%H%M%S.sbp.json")
filename = os.path.normpath(
os.path.join(self.directory_name, filename))
self.logger = s.get_logger(True, filename, self.expand_json)
self.forwarder = sbpc.Forwarder(self.link, self.logger)
self.forwarder.start()
if self.settings_view:
self.settings_view._settings_read_button_fired()
def _stop_json_logging(self):
fwd = self.forwarder
fwd.stop()
self.logger.flush()
self.logger.close()
def _json_logging_button_action(self):
if self.first_json_press and self.json_logging:
print(
"JSON Logging initiated via CMD line. Please press button again to stop logging"
)
elif self.json_logging:
self._stop_json_logging()
self.json_logging = False
print("Stopped JSON logging")
else:
self._start_json_logging()
self.json_logging = True
self.first_json_press = False
def _json_logging_button_fired(self):
if not os.path.exists(self.directory_name) and not self.json_logging:
confirm_prompt = CallbackPrompt(
title="Logging directory creation",
actions=[ok_button],
callback=self._json_logging_button_action)
confirm_prompt.text = "\nThe selected logging directory does not exist and will be created."
confirm_prompt.run(block=False)
else:
self._json_logging_button_action()
def _csv_logging_button_fired(self):
if not os.path.exists(self.directory_name) and not self.csv_logging:
confirm_prompt = CallbackPrompt(
title="Logging directory creation",
actions=[ok_button],
callback=self._csv_logging_button_action)
confirm_prompt.text = "\nThe selected logging directory does not exist and will be created."
confirm_prompt.run(block=False)
else:
self._csv_logging_button_action()
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, traceback):
self.console_output.close()
def __init__(self,
link,
update,
log_level_filter,
skip_settings=False,
error=False,
cnx_desc=None,
json_logging=False,
log_dirname=None,
override_filename=None,
log_console=False,
networking=None,
connection_info=None,
expand_json=False):
self.error = error
self.cnx_desc = cnx_desc
self.connection_info = connection_info
self.dev_id = cnx_desc
self.num_sats = 0
self.mode = ''
self.forwarder = None
self.latency = '--'
self.expand_json = expand_json
# if we have passed a logfile, we set our directory to it
override_filename = override_filename
if log_dirname:
self.directory_name = log_dirname
if override_filename:
override_filename = os.path.join(log_dirname,
override_filename)
else:
self.directory_name = swift_path
# Start swallowing sys.stdout and sys.stderr
self.console_output = OutputList(tfile=log_console,
outdir=self.directory_name)
sys.stdout = self.console_output
self.console_output.write("Console: " + CONSOLE_VERSION +
" starting...")
if not error:
sys.stderr = self.console_output
self.log_level_filter = log_level_filter
self.console_output.log_level_filter = str_to_log_level(
log_level_filter)
try:
self.link = link
self.link.add_callback(self.print_message_callback,
SBP_MSG_PRINT_DEP)
self.link.add_callback(self.log_message_callback, SBP_MSG_LOG)
self.link.add_callback(self.ext_event_callback, SBP_MSG_EXT_EVENT)
self.link.add_callback(self.cmd_resp_callback,
SBP_MSG_COMMAND_RESP)
self.link.add_callback(self.update_on_heartbeat, SBP_MSG_HEARTBEAT)
self.dep_handler = DeprecatedMessageHandler(link)
settings_read_finished_functions = []
self.tracking_view = TrackingView(self.link)
self.solution_view = SolutionView(self.link,
dirname=self.directory_name)
self.baseline_view = BaselineView(self.link,
dirname=self.directory_name)
self.observation_view = ObservationView(
self.link,
name='Local',
relay=False,
dirname=self.directory_name)
self.observation_view_base = ObservationView(
self.link,
name='Remote',
relay=True,
dirname=self.directory_name)
self.system_monitor_view = SystemMonitorView(self.link)
self.update_view = UpdateView(self.link,
download_dir=swift_path,
prompt=update,
connection_info=self.connection_info)
self.imu_view = IMUView(self.link)
self.mag_view = MagView(self.link)
self.spectrum_analyzer_view = SpectrumAnalyzerView(self.link)
settings_read_finished_functions.append(
self.update_view.compare_versions)
if networking:
from ruamel.yaml import YAML
yaml = YAML(typ='safe')
try:
networking_dict = yaml.load(networking)
networking_dict.update({'show_networking': True})
except yaml.YAMLError:
print(
"Unable to interpret networking cmdline argument. It will be ignored."
)
import traceback
print(traceback.format_exc())
networking_dict = {'show_networking': True}
else:
networking_dict = {}
networking_dict.update(
{'whitelist': [SBP_MSG_POS_LLH, SBP_MSG_HEARTBEAT]})
self.networking_view = SbpRelayView(self.link, **networking_dict)
self.skylark_view = SkylarkView()
self.json_logging = json_logging
self.csv_logging = False
self.first_json_press = True
if json_logging:
self._start_json_logging(override_filename)
self.json_logging = True
# we set timer interval to 1200 milliseconds because we expect a heartbeat each second
self.timer_cancel = call_repeatedly(1.2, self.check_heartbeat)
# Once we have received the settings, update device_serial with
# the Swift serial number which will be displayed in the window
# title. This callback will also update the header route as used
# by the networking view.
def update_serial():
mfg_id = None
try:
self.uuid = self.settings_view.settings['system_info'][
'uuid'].value
mfg_id = self.settings_view.settings['system_info'][
'serial_number'].value
except KeyError:
pass
if mfg_id:
self.device_serial = 'PK' + str(mfg_id)
self.skylark_view.set_uuid(self.uuid)
self.networking_view.set_route(uuid=self.uuid,
serial_id=mfg_id)
if self.networking_view.connect_when_uuid_received:
self.networking_view._connect_rover_fired()
settings_read_finished_functions.append(update_serial)
self.settings_view = SettingsView(self.link,
settings_read_finished_functions,
skip=skip_settings)
self.update_view.settings = self.settings_view.settings
self.python_console_env = {
'send_message': self.link,
'link': self.link,
}
self.python_console_env.update(
self.tracking_view.python_console_cmds)
self.python_console_env.update(
self.solution_view.python_console_cmds)
self.python_console_env.update(
self.baseline_view.python_console_cmds)
self.python_console_env.update(
self.observation_view.python_console_cmds)
self.python_console_env.update(
self.networking_view.python_console_cmds)
self.python_console_env.update(
self.system_monitor_view.python_console_cmds)
self.python_console_env.update(
self.update_view.python_console_cmds)
self.python_console_env.update(self.imu_view.python_console_cmds)
self.python_console_env.update(self.mag_view.python_console_cmds)
self.python_console_env.update(
self.settings_view.python_console_cmds)
self.python_console_env.update(
self.spectrum_analyzer_view.python_console_cmds)
except: # noqa
import traceback
traceback.print_exc()
if self.error:
sys.exit(1)
class SystemMonitorView(HasTraits):
python_console_cmds = Dict()
_threads_table_list = List()
_csac_telem_list = List()
_csac_received = Bool(False)
threads = List()
msg_obs_avg_latency_ms = Int(0)
msg_obs_min_latency_ms = Int(0)
msg_obs_max_latency_ms = Int(0)
msg_obs_window_latency_ms = Int(0)
msg_obs_avg_period_ms = Int(0)
msg_obs_min_period_ms = Int(0)
msg_obs_max_period_ms = Int(0)
msg_obs_window_period_ms = Float(0)
zynq_temp = Float(0)
fe_temp = Float(0)
piksi_reset_button = SVGButton(
label='Reset Device',
tooltip='Reset Device',
filename=resource_filename('console/images/fontawesome/power27.svg'),
width=16,
height=16,
aligment='center')
traits_view = View(
VGroup(
Item(
'_threads_table_list',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.85,
),
HGroup(
VGroup(
HGroup(VGroup(Item('msg_obs_window_latency_ms',
label='Curr',
style='readonly',
format_str='%dms'),
Item('msg_obs_avg_latency_ms',
label='Avg',
style='readonly',
format_str='%dms'),
Item('msg_obs_min_latency_ms',
label='Min',
style='readonly',
format_str='%dms'),
Item('msg_obs_max_latency_ms',
label='Max',
style='readonly',
format_str='%dms'),
label='Latency',
show_border=True),
VGroup(Item('msg_obs_window_period_ms',
label='Curr',
style='readonly',
format_str='%dms'),
Item('msg_obs_avg_period_ms',
label='Avg',
style='readonly',
format_str='%dms'),
Item('msg_obs_min_period_ms',
label='Min',
style='readonly',
format_str='%dms'),
Item('msg_obs_max_period_ms',
label='Max',
style='readonly',
format_str='%dms'),
label='Period',
show_border=True),
show_border=True,
label="Observation Connection Monitor"),
Item('piksi_reset_button', show_label=False, width=0.50)),
VGroup(
Item('zynq_temp',
label='Zynq CPU Temp',
style='readonly',
format_str='%.1fC'),
Item('fe_temp',
label='RF Frontend Temp',
style='readonly',
format_str='%.1fC'),
show_border=True,
label="Device Monitor",
),
VGroup(Item('_csac_telem_list',
style='readonly',
editor=TabularEditor(adapter=SimpleCSACAdapter()),
show_label=False),
show_border=True,
label="Metrics",
visible_when='_csac_received'))), )
def update_threads(self):
self._threads_table_list = [
(thread_name, state.cpu, state.stack_free)
for thread_name, state in sorted(
self.threads, key=lambda x: x[1].cpu, reverse=True)
]
def update_network_state(self):
self._network_refresh_button_fired()
def heartbeat_callback(self, sbp_msg, **metadata):
if self.threads != []:
self.update_threads()
self.threads = []
def device_callback(self, sbp_msg, **metadata):
self.zynq_temp = float(sbp_msg.cpu_temperature) / 100.
self.fe_temp = float(sbp_msg.fe_temperature) / 100.
def thread_state_callback(self, sbp_msg, **metadata):
if sbp_msg.name == '':
sbp_msg.name = '(no name)'
sbp_msg.cpu /= 10.
self.threads.append((sbp_msg.name, sbp_msg))
def csac_header_callback(self, sbp_msg, **metadata):
self.headers = sbp_msg.telemetry_labels.split(',')
self.telem_header_index = sbp_msg.id
def csac_telem_callback(self, sbp_msg, **metadata):
self._csac_telem_list = []
if self.telem_header_index is not None:
if sbp_msg.id == self.telem_header_index:
self._csac_received = True
metrics_of_interest = [
'Status', 'Alarm', 'Mode', 'Phase', 'DiscOK'
]
telems = sbp_msg.telemetry.split(',')
for i, each in enumerate(self.headers):
if each in metrics_of_interest:
self._csac_telem_list.append((each, telems[i]))
def _piksi_reset_button_fired(self):
self.link(MsgReset(flags=0))
def uart_state_callback(self, m, **metadata):
self.msg_obs_avg_latency_ms = m.latency.avg
self.msg_obs_min_latency_ms = m.latency.lmin
self.msg_obs_max_latency_ms = m.latency.lmax
self.msg_obs_window_latency_ms = m.latency.current
if m.msg_type == SBP_MSG_UART_STATE:
self.msg_obs_avg_period_ms = m.obs_period.avg
self.msg_obs_min_period_ms = m.obs_period.pmin
self.msg_obs_max_period_ms = m.obs_period.pmax
self.msg_obs_window_period_ms = m.obs_period.current
def __init__(self, link):
super(SystemMonitorView, self).__init__()
self.link = link
self.telem_header_index = None
self.link.add_callback(self.heartbeat_callback, SBP_MSG_HEARTBEAT)
self.link.add_callback(self.device_callback, SBP_MSG_DEVICE_MONITOR)
self.link.add_callback(self.thread_state_callback,
SBP_MSG_THREAD_STATE)
self.link.add_callback(self.uart_state_callback,
[SBP_MSG_UART_STATE, SBP_MSG_UART_STATE_DEPA])
self.link.add_callback(self.csac_telem_callback,
SBP_MSG_CSAC_TELEMETRY)
self.link.add_callback(self.csac_header_callback,
SBP_MSG_CSAC_TELEMETRY_LABELS)
self.python_console_cmds = {'mon': self}
class PortChooser(HasTraits):
port = Str(None)
ports = List()
mode = Enum(cnx_type_list)
flow_control = Enum(flow_control_options_list)
ip_port = Int(55555)
ip_address = Str('192.168.0.222')
choose_baud = Bool(True)
baudrate = Int()
refresh_ports_button = SVGButton(
label='',
tooltip='Refresh Port List',
filename=resource_filename(
'console/images/fontawesome/refresh_blue.svg'),
allow_clipping=False,
width_padding=4,
height_padding=4)
traits_view = View(
VGroup(
Spring(height=8),
HGroup(
Spring(width=-2, springy=False),
Item('mode',
style='custom',
editor=EnumEditor(values=cnx_type_list,
cols=2,
format_str='%s'),
show_label=False)),
HGroup(VGroup(
Label('Serial Device:'),
HGroup(
Item('port',
editor=EnumEditor(name='ports'),
show_label=False,
springy=True),
Item('refresh_ports_button',
show_label=False,
padding=0,
height=-20,
width=-20),
),
),
VGroup(
Label('Baudrate:'),
Item('baudrate',
editor=EnumEditor(values=BAUD_LIST),
show_label=False,
visible_when='choose_baud'),
Item('baudrate',
show_label=False,
visible_when='not choose_baud',
style='readonly'),
),
VGroup(
Label('Flow Control:'),
Item('flow_control',
editor=EnumEditor(values=flow_control_options_list,
format_str='%s'),
show_label=False),
),
visible_when="mode==\'Serial/USB\'"),
HGroup(VGroup(
Label('IP Address:'),
Item('ip_address',
label="IP Address",
style='simple',
show_label=False,
height=-24),
),
VGroup(
Label('IP Port:'),
Item('ip_port',
label="IP Port",
style='simple',
show_label=False,
height=-24),
),
Spring(),
visible_when="mode==\'TCP/IP\'"),
),
buttons=['OK', 'Cancel'],
default_button='OK',
close_result=False,
icon=icon,
width=460,
title='Swift Console v{0} - Select Interface'.format(CONSOLE_VERSION))
def refresh_ports(self):
"""
This method refreshes the port list
"""
try:
self.ports = [p for p, _, _ in s.get_ports()]
except TypeError:
pass
def _refresh_ports_button_fired(self):
self.refresh_ports()
def __init__(self, baudrate=None):
self.refresh_ports()
# As default value, use the first city in the list:
try:
self.port = self.ports[0]
except IndexError:
pass
if baudrate not in BAUD_LIST:
self.choose_baud = False
self.baudrate = baudrate
class SolutionView(HasTraits):
python_console_cmds = Dict()
lats = List()
lngs = List()
alts = List()
table = List()
dops_table = List()
pos_table = List()
vel_table = List()
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
running = Bool(True)
position_centered = Bool(False)
clear_button = SVGButton(
label='', tooltip='Clear',
filename=os.path.join(os.path.dirname(__file__), 'images', 'iconic', 'x.svg'),
width=16, height=16
)
zoomall_button = SVGButton(
label='', tooltip='Zoom All',
filename=os.path.join(os.path.dirname(__file__), 'images', 'iconic', 'fullscreen.svg'),
width=16, height=16
)
center_button = SVGButton(
label='', tooltip='Center on Solution', toggle=True,
filename=os.path.join(os.path.dirname(__file__), 'images', 'iconic', 'target.svg'),
width=16, height=16
)
paused_button = SVGButton(
label='', tooltip='Pause', toggle_tooltip='Run', toggle=True,
filename=os.path.join(os.path.dirname(__file__), 'images', 'iconic', 'pause.svg'),
toggle_filename=os.path.join(os.path.dirname(__file__), 'images', 'iconic', 'play.svg'),
width=16, height=16
)
traits_view = View(
HSplit(
Item('table', style = 'readonly', editor = TabularEditor(adapter=SimpleAdapter()), show_label=False, width=0.3),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
),
Item(
'plot',
show_label = False,
editor = ComponentEditor(bgcolor = (0.8,0.8,0.8)),
)
)
)
)
def _zoomall_button_fired(self):
self.plot.index_range.low_setting = 'auto'
self.plot.index_range.high_setting = 'auto'
self.plot.value_range.low_setting = 'auto'
self.plot.value_range.high_setting = 'auto'
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _clear_button_fired(self):
self.lats = []
self.lngs = []
self.alts = []
self.plot_data.set_data('lat', [])
self.plot_data.set_data('lng', [])
self.plot_data.set_data('alt', [])
self.plot_data.set_data('t', [])
def _pos_llh_callback(self, data):
# Updating an ArrayPlotData isn't thread safe (see chaco issue #9), so
# actually perform the update in the UI thread.
if self.running:
GUI.invoke_later(self.pos_llh_callback, data)
def update_table(self):
self._table_list = self.table.items()
def pos_llh_callback(self, data):
soln = sbp_messages.PosLLH(data)
self.pos_table = []
if self.log_file is None:
self.log_file = open(time.strftime("position_log_%Y%m%d-%H%M%S.csv"), 'w')
self.log_file.write('%.2f,%.4f,%.4f,%.4f,%d\n' % (soln.tow * 1e3, soln.lat, soln.lon, soln.height, soln.n_sats))
self.log_file.flush()
if self.week is not None:
t = datetime.datetime(1980, 1, 6) + \
datetime.timedelta(weeks=self.week) + \
datetime.timedelta(seconds=soln.tow/1e3)
self.pos_table.append(('GPS Time', t))
self.pos_table.append(('GPS Week', str(self.week)))
tow = soln.tow*1e-3 + self.nsec*1e-9
if self.nsec is not None:
tow += self.nsec*1e-9
self.pos_table.append(('GPS ToW', tow))
self.pos_table.append(('Num. sats', soln.n_sats))
self.pos_table.append(('Lat', soln.lat))
self.pos_table.append(('Lng', soln.lon))
self.pos_table.append(('Alt', soln.height))
self.lats.append(soln.lat)
self.lngs.append(soln.lon)
self.alts.append(soln.height)
self.lats = self.lats[-1000:]
self.lngs = self.lngs[-1000:]
self.alts = self.alts[-1000:]
self.plot_data.set_data('lat', self.lats)
self.plot_data.set_data('lng', self.lngs)
self.plot_data.set_data('alt', self.alts)
t = range(len(self.lats))
self.plot_data.set_data('t', t)
self.table = self.pos_table + self.vel_table + self.dops_table
if self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.pos_llh[0] - d, soln.pos_llh[0] + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.pos_llh[1] - d, soln.pos_llh[1] + d)
def dops_callback(self, data):
dops = sbp_messages.Dops(data)
self.dops_table = [
('PDOP', '%.1f' % (dops.pdop * 0.01)),
('GDOP', '%.1f' % (dops.gdop * 0.01)),
('TDOP', '%.1f' % (dops.tdop * 0.01)),
('HDOP', '%.1f' % (dops.hdop * 0.01)),
('VDOP', '%.1f' % (dops.vdop * 0.01))
]
self.table = self.pos_table + self.vel_table + self.dops_table
def vel_ned_callback(self, data):
vel_ned = sbp_messages.VelNED(data)
if self.vel_log_file is None:
self.vel_log_file = open(time.strftime("velocity_log_%Y%m%d-%H%M%S.csv"), 'w')
self.vel_log_file.write('%.2f,%.4f,%.4f,%.4f,%.4f,%d\n' % (vel_ned.tow * 1e3, vel_ned.n, vel_ned.e, vel_ned.d, math.sqrt(vel_ned.n*vel_ned.n + vel_ned.e*vel_ned.e),vel_ned.n_sats))
self.vel_log_file.flush()
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)),
]
self.table = self.pos_table + self.vel_table + self.dops_table
def gps_time_callback(self, data):
self.week = sbp_messages.GPSTime(data).wn
self.nsec = sbp_messages.GPSTime(data).ns
def __init__(self, link):
super(SolutionView, self).__init__()
self.log_file = None
self.vel_log_file = None
self.plot_data = ArrayPlotData(lat=[0.0], lng=[0.0], alt=[0.0], t=[0.0], ref_lat=[0.0], ref_lng=[0.0], region_lat=[0.0], region_lng=[0.0])
self.plot = Plot(self.plot_data)
self.plot.plot(('lng', 'lat'), type='line', name='line', color=(0, 0, 0, 0.1))
self.plot.plot(('lng', 'lat'), type='scatter', name='points', color='blue', marker='dot', line_width=0.0, marker_size=1.0)
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.padding = (0, 1, 0, 1)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(self.plot, zoom_factor=1.1, tool_mode="box", always_on=False)
self.plot.overlays.append(zt)
self.link = link
self.link.add_callback(sbp_messages.SBP_POS_LLH, self._pos_llh_callback)
self.link.add_callback(sbp_messages.SBP_VEL_NED, self.vel_ned_callback)
self.link.add_callback(sbp_messages.SBP_DOPS, self.dops_callback)
self.link.add_callback(sbp_messages.SBP_GPS_TIME, self.gps_time_callback)
self.week = None
self.nsec = 0
self.python_console_cmds = {
'solution': self
}
class BaselineView(HasTraits):
# This mapping should match the flag definitions in libsbp for
# the MsgBaselineNED message. While this isn't strictly necessary
# it helps avoid confusion
python_console_cmds = Dict()
table = List()
logging_b = Bool(False)
directory_name_b = File
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
running = Bool(True)
zoomall = Bool(False)
position_centered = Bool(False)
clear_button = SVGButton(
label='',
tooltip='Clear',
filename=resource_filename('console/images/iconic/x.svg'),
width=16,
height=16)
zoomall_button = SVGButton(
label='',
tooltip='Zoom All',
toggle=True,
filename=resource_filename('console/images/iconic/fullscreen.svg'),
width=16,
height=16)
center_button = SVGButton(
label='',
tooltip='Center on Baseline',
toggle=True,
filename=resource_filename('console/images/iconic/target.svg'),
width=16,
height=16)
paused_button = SVGButton(
label='',
tooltip='Pause',
toggle_tooltip='Run',
toggle=True,
filename=resource_filename('console/images/iconic/pause.svg'),
toggle_filename=resource_filename('console/images/iconic/play.svg'),
width=16,
height=16)
reset_button = Button(label='Reset Filters')
traits_view = View(
HSplit(
Item(
'table',
style='readonly',
editor=TabularEditor(adapter=SimpleAdapter()),
show_label=False,
width=0.3),
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('center_button', show_label=False),
Item('reset_button', show_label=False), ),
Item(
'plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8)), ))))
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _center_button_fired(self):
self.position_centered = not self.position_centered
def _paused_button_fired(self):
self.running = not self.running
def _reset_button_fired(self):
self.link(MsgResetFilters(filter=0))
def _reset_remove_current(self):
self.plot_data.set_data('cur_fixed_n', [])
self.plot_data.set_data('cur_fixed_e', [])
self.plot_data.set_data('cur_fixed_d', [])
self.plot_data.set_data('cur_float_n', [])
self.plot_data.set_data('cur_float_e', [])
self.plot_data.set_data('cur_float_d', [])
self.plot_data.set_data('cur_dgnss_n', [])
self.plot_data.set_data('cur_dgnss_e', [])
self.plot_data.set_data('cur_dgnss_d', [])
def _clear_history(self):
self.plot_data.set_data('n_fixed', [])
self.plot_data.set_data('e_fixed', [])
self.plot_data.set_data('d_fixed', [])
self.plot_data.set_data('n_float', [])
self.plot_data.set_data('e_float', [])
self.plot_data.set_data('d_float', [])
self.plot_data.set_data('n_dgnss', [])
self.plot_data.set_data('e_dgnss', [])
self.plot_data.set_data('d_dgnss', [])
def _clear_button_fired(self):
self.n[:] = np.NAN
self.e[:] = np.NAN
self.d[:] = np.NAN
self.mode[:] = np.NAN
self.plot_data.set_data('t', [])
self._clear_history()
self._reset_remove_current()
def iar_state_callback(self, sbp_msg, **metadata):
self.num_hyps = sbp_msg.num_hyps
self.last_hyp_update = time.time()
def age_corrections_callback(self, sbp_msg, **metadata):
age_msg = MsgAgeCorrections(sbp_msg)
if age_msg.age != 0xFFFF:
self.age_corrections = age_msg.age / 10.0
else:
self.age_corrections = None
def gps_time_callback(self, sbp_msg, **metadata):
if sbp_msg.msg_type == SBP_MSG_GPS_TIME_DEP_A:
time_msg = MsgGPSTimeDepA(sbp_msg)
flags = 1
elif sbp_msg.msg_type == SBP_MSG_GPS_TIME:
time_msg = MsgGPSTime(sbp_msg)
flags = time_msg.flags
if flags != 0:
self.week = time_msg.wn
self.nsec = time_msg.ns_residual
def utc_time_callback(self, sbp_msg, **metadata):
tmsg = MsgUtcTime(sbp_msg)
seconds = math.floor(tmsg.seconds)
microseconds = int(tmsg.ns / 1000.00)
if tmsg.flags & 0x1 == 1:
dt = datetime.datetime(tmsg.year, tmsg.month, tmsg.day, tmsg.hours,
tmsg.minutes, tmsg.seconds, microseconds)
self.utc_time = dt
self.utc_time_flags = tmsg.flags
if (tmsg.flags >> 3) & 0x3 == 0:
self.utc_source = "Factory Default"
elif (tmsg.flags >> 3) & 0x3 == 1:
self.utc_source = "Non Volatile Memory"
elif (tmsg.flags >> 3) & 0x3 == 2:
self.utc_source = "Decoded this Session"
else:
self.utc_source = "Unknown"
else:
self.utc_time = None
self.utc_source = None
def baseline_heading_callback(self, sbp_msg, **metadata):
headingMsg = MsgBaselineHeading(sbp_msg)
if headingMsg.flags & 0x7 != 0:
self.heading = headingMsg.heading * 1e-3
else:
self.heading = "---"
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 time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD:
self.solution_draw()
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 solution_draw(self):
if self.running:
GUI.invoke_later(self._solution_draw)
def _solution_draw(self):
self._clear_history()
soln = self.last_soln
self.last_plot_update_time = time.time()
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 __init__(self, link, plot_history_max=1000, dirname=''):
super(BaselineView, self).__init__()
self.log_file = None
self.directory_name_b = dirname
self.num_hyps = 0
self.last_hyp_update = 0
self.last_btime_update = 0
self.last_soln = None
self.last_mode = 0
self.plot_data = ArrayPlotData(
n_fixed=[0.0],
e_fixed=[0.0],
d_fixed=[0.0],
n_float=[0.0],
e_float=[0.0],
d_float=[0.0],
n_dgnss=[0.0],
e_dgnss=[0.0],
d_dgnss=[0.0],
t=[0.0],
ref_n=[0.0],
ref_e=[0.0],
ref_d=[0.0],
cur_fixed_e=[],
cur_fixed_n=[],
cur_fixed_d=[],
cur_float_e=[],
cur_float_n=[],
cur_float_d=[],
cur_dgnss_e=[],
cur_dgnss_n=[],
cur_dgnss_d=[])
self.plot_history_max = plot_history_max
self.n = np.zeros(plot_history_max)
self.e = np.zeros(plot_history_max)
self.d = np.zeros(plot_history_max)
self.mode = np.zeros(plot_history_max)
self.last_plot_update_time = 0
self.plot = Plot(self.plot_data)
pts_float = self.plot.plot(
('e_float', 'n_float'),
type='scatter',
color=color_dict[FLOAT_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
pts_fixed = self.plot.plot( # noqa: F841
('e_fixed', 'n_fixed'),
type='scatter',
color=color_dict[FIXED_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
pts_dgnss = self.plot.plot( # noqa: F841
('e_dgnss', 'n_dgnss'),
type='scatter',
color=color_dict[DGNSS_MODE],
marker='dot',
line_width=0.0,
marker_size=1.0)
ref = self.plot.plot(
('ref_e', 'ref_n'),
type='scatter',
color='red',
marker='plus',
marker_size=5,
line_width=1.5)
cur_fixed = self.plot.plot(
('cur_fixed_e', 'cur_fixed_n'),
type='scatter',
color=color_dict[FIXED_MODE],
marker='plus',
marker_size=5,
line_width=1.5)
cur_float = self.plot.plot(
('cur_float_e', 'cur_float_n'),
type='scatter',
color=color_dict[FLOAT_MODE],
marker='plus',
marker_size=5,
line_width=1.5)
cur_dgnss = self.plot.plot(
('cur_dgnss_e', 'cur_dgnss_n'),
type='scatter',
color=color_dict[DGNSS_MODE],
marker='plus',
line_width=1.5,
marker_size=5)
plot_labels = [' Base Position', 'DGPS', 'RTK Float', 'RTK Fixed']
plots_legend = dict(
zip(plot_labels, [ref, cur_dgnss, cur_float, cur_fixed]))
self.plot.legend.plots = plots_legend
self.plot.legend.labels = plot_labels # sets order
self.plot.legend.visible = True
self.plot.index_axis.tick_label_position = 'inside'
self.plot.index_axis.tick_label_color = 'gray'
self.plot.index_axis.tick_color = 'gray'
self.plot.index_axis.title = 'E (meters)'
self.plot.index_axis.title_spacing = 5
self.plot.value_axis.tick_label_position = 'inside'
self.plot.value_axis.tick_label_color = 'gray'
self.plot.value_axis.tick_color = 'gray'
self.plot.value_axis.title = 'N (meters)'
self.plot.value_axis.title_spacing = 5
self.plot.padding = (25, 25, 25, 25)
self.plot.tools.append(PanTool(self.plot))
zt = ZoomTool(
self.plot, zoom_factor=1.1, tool_mode="box", always_on=False)
self.plot.overlays.append(zt)
self.week = None
self.utc_time = None
self.age_corrections = None
self.heading = "---"
self.nsec = 0
self.link = link
self.link.add_callback(self.baseline_callback, [
SBP_MSG_BASELINE_NED, SBP_MSG_BASELINE_NED_DEP_A
])
self.link.add_callback(self.baseline_heading_callback,
[SBP_MSG_BASELINE_HEADING])
self.link.add_callback(self.iar_state_callback, SBP_MSG_IAR_STATE)
self.link.add_callback(self.gps_time_callback,
[SBP_MSG_GPS_TIME, SBP_MSG_GPS_TIME_DEP_A])
self.link.add_callback(self.utc_time_callback, [SBP_MSG_UTC_TIME])
self.link.add_callback(self.age_corrections_callback,
SBP_MSG_AGE_CORRECTIONS)
self.python_console_cmds = {'baseline': self}
