def _refresh_fired(self):
global labelItems
self.dev_all_list = self.utils.baseline.dev_all_list
self.dev_all_list.remove('Preset')
self.neds_all = self.utils.baseline.neds_all
self.neds_satisfied = self.utils.baseline.neds_satisfied
self.neds_unsatisfied = self.utils.baseline.neds_unsatisfied
self.neds = self.utils.baseline.neds
self.devs = self.utils.baseline.devs
self.times = self.utils.baseline.times
self.presets = self.utils.baseline.presets
self.time = time.time()
if len(self.neds_all) > 0:
self.plot_data.set_data('n_all', self.neds_all.T[0])
self.plot_data.set_data('e_all', self.neds_all.T[1])
if len(self.neds_satisfied) > 0:
self.plot_data.set_data('n_satisfied', self.neds_satisfied.T[0])
self.plot_data.set_data('e_satisfied', self.neds_satisfied.T[1])
if len(self.neds_unsatisfied) > 0:
self.plot_data.set_data('n_unsatisfied', self.neds_unsatisfied.T[0])
self.plot_data.set_data('e_unsatisfied', self.neds_unsatisfied.T[1])
labelItems, csv_text = self._max_depth_time()
path_to_file = os.path.join(determine_path(), 'summary.csv')
summary_file = open(path_to_file, 'w')
summary_file.write(csv_text)
summary_file.close()
def _refresh_fired(self):
global labelItems
self.dev_all_list = self.utils.baseline.dev_all_list
self.dev_all_list.remove('Preset')
self.neds_all = self.utils.baseline.neds_all
self.neds_satisfied = self.utils.baseline.neds_satisfied
self.neds_unsatisfied = self.utils.baseline.neds_unsatisfied
self.neds = self.utils.baseline.neds
self.devs = self.utils.baseline.devs
self.times = self.utils.baseline.times
self.presets = self.utils.baseline.presets
self.time = time.time()
if len(self.neds_all) > 0:
self.plot_data.set_data('n_all', self.neds_all.T[0])
self.plot_data.set_data('e_all', self.neds_all.T[1])
if len(self.neds_satisfied) > 0:
self.plot_data.set_data('n_satisfied', self.neds_satisfied.T[0])
self.plot_data.set_data('e_satisfied', self.neds_satisfied.T[1])
if len(self.neds_unsatisfied) > 0:
self.plot_data.set_data('n_unsatisfied',
self.neds_unsatisfied.T[0])
self.plot_data.set_data('e_unsatisfied',
self.neds_unsatisfied.T[1])
labelItems, csv_text = self._max_depth_time()
path_to_file = os.path.join(determine_path(), 'summary.csv')
summary_file = open(path_to_file, 'w')
summary_file.write(csv_text)
summary_file.close()
def _read_preset_points(self, filename='preset.csv'):
preset_points = {}
px = []
py = []
try:
if os.path.isfile(filename):
path_to_file = filename
else:
path_to_file = os.path.join(determine_path(), filename)
f = open(path_to_file, 'r')
for i in f.readlines():
xy = i.split(',')
if len(xy) < 2:
continue
try:
x = float(xy[0]) * 1e-3
y = float(xy[1]) * 1e-3
px.append(x)
py.append(y)
except:
continue
except:
pass
preset_points['e'] = px
preset_points['n'] = py
return preset_points
def _read_preset_points(self, filename='preset.csv'):
preset_points = {}
px = []
py = []
try:
if os.path.isfile(filename):
path_to_file = filename
else:
path_to_file = os.path.join(determine_path(), filename)
f = open(path_to_file, 'r')
for i in f.readlines():
xy = i.split(',')
if len(xy) < 2:
continue
try:
x = float(xy[0])*1e-3
y = float(xy[1])*1e-3
px.append(x)
py.append(y)
except:
continue
except:
pass
preset_points['e'] = px
preset_points['n'] = py
return preset_points
def __init__(self,filename):
try:
# check if filename exists (absolute or relative path can be given)
if os.path.isfile(filename):
path_to_file = filename
# if it doesn't exist, try and see if it's in the dir as the script
else:
path_to_file = os.path.join(determine_path(), filename)
stram = open(path_to_file, "r")
temp_dict = yaml.load(stram)
self.list_of_dicts = temp_dict
self.warned_dict = {}
# inform user of success or failure
print "Loaded settings yaml file from path " + path_to_file
print "Number of settings loaded {0}".format(len(self.list_of_dicts))
except:
import traceback
traceback.print_exc()
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)
spectrum_analyzer_view = Instance(SpectrumAnalyzerView)
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('')
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=os.path.join(determine_path(), 'images', 'iconic',
'pause.svg'),
toggle_filename=os.path.join(determine_path(), '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=os.path.join(determine_path(), 'images', 'iconic',
'pause.svg'),
toggle_filename=os.path.join(determine_path(), '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=os.path.join(determine_path(), 'images',
'iconic', 'pause.svg'),
toggle_filename=os.path.join(
determine_path(), 'images', 'iconic',
'play.svg'),
width=8,
height=8)
clear_button = SVGButton(label='',
tooltip='Clear console buffer',
filename=os.path.join(determine_path(), '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='Firmware Update'),
Tabbed(Item('system_monitor_view',
style='custom',
label='System Monitor'),
Item('imu_view', style='custom', label='IMU'),
Item('networking_view',
label='Networking',
style='custom',
show_label=False),
Item('spectrum_analyzer_view',
label='Spectrum Analyzer',
style='custom'),
Item('python_console_env',
style='custom',
label='Python Console',
editor=ShellEditor()),
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,
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'),
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(
'arrows_blue.png',
search_path=[
os.path.join(
determine_path(), 'images',
'iconic')
]))),
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(
'arrows_grey.png',
search_path=[
os.path.join(
determine_path(), 'images',
'iconic')
]))),
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.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,
serial_upgrade=False):
self.error = error
self.cnx_desc = cnx_desc
self.dev_id = cnx_desc
self.num_sats = 0
self.mode = ''
self.forwarder = None
self.latency = '--'
# if we have passed a logfile, we set our directory to it
override_filename = override_filename
home = expanduser("~")
swift_path = os.path.normpath(os.path.join(home, 'SwiftNav'))
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,
serial_upgrade=serial_upgrade)
self.imu_view = IMUView(self.link)
self.spectrum_analyzer_view = SpectrumAnalyzerView(self.link)
settings_read_finished_functions.append(
self.update_view.compare_versions)
if networking:
import yaml
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.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():
uuid = None
mfg_id = None
try:
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)[-6:]
self.networking_view.set_route(uuid=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.settings_view.python_console_cmds)
self.python_console_env.update(
self.spectrum_analyzer_view.python_console_cmds)
except:
import traceback
traceback.print_exc()
if self.error:
sys.exit(1)
class SystemMonitorView(HasTraits):
python_console_cmds = Dict()
_threads_table_list = List()
threads = List()
_network_info = 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 = Int(0)
piksi_reset_button = SVGButton(
label='Reset Piksi',
tooltip='Reset Piksi',
filename=os.path.join(determine_path(), 'images', 'fontawesome',
'power27.svg'),
width=16,
height=16,
aligment='center')
network_refresh_button = SVGButton(
label='Refresh Network Status',
tooltip='Refresh Network Status',
filename=os.path.join(determine_path(), 'images', 'fontawesome',
'refresh.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(
'_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 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):
if self.threads != []:
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(flags=0))
def _network_refresh_button_fired(self):
self._network_info = []
self.link(MsgNetworkStateReq())
def _network_callback(self, m, **metadata):
self._network_info.append(
(m.interface_name, ip_bytes_to_string(m.ipv4_address.ipv4_address),
((m.flags & (1 << 6)) != 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.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, SBP_MSG_UART_STATE_DEPA])
self.link.add_callback(self._network_callback,
SBP_MSG_NETWORK_STATE_RESP)
self.python_console_cmds = {'mon': self}
else:
ETSConfig.toolkit = 'qt4'
logging.basicConfig()
pygments.lexers.PythonLexer = PythonLexer
try:
import pygments.lexers.c_cpp
except ImportError:
pass
# These imports seem to be required to make pyinstaller work?
# (usually traitsui would load them automatically)
if ETSConfig.toolkit == 'qt4':
pass
basedir = determine_path()
icon = ImageResource('icon',
search_path=['images',
os.path.join(basedir, 'images')])
CONSOLE_TITLE = 'Swift Console v:' + CONSOLE_VERSION
BAUD_LIST = [57600, 115200, 230400, 921600, 1000000]
class ConsoleHandler(Handler):
"""
Handler that updates the window title with the device serial number
This Handler is used by Traits UI to manage making changes to the GUI in
response to changes in the underlying class/data.
"""
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()
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 update_table(self):
self._table_list = self.table_spp.items()
def pos_llh_callback(self, sbp_msg, **metadata):
soln = MsgPosLLH(sbp_msg)
masked_flag = soln.flags & 0x7
if masked_flag == 0:
psuedo_absolutes = False
else:
psuedo_absolutes = True
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(
"time,latitude(degrees),longitude(degrees),altitude(meters),n_sats,flags\n"
)
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)))
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))
if (soln.flags & 0xff) == 0:
pos_table.append(('Mode', 'SPP (single point position)'))
elif (soln.flags & 0xff) == 1:
pos_table.append(('Mode', 'Fixed RTK'))
elif (soln.flags & 0xff) == 2:
pos_table.append(('Mode', 'Float RTK'))
else:
pos_table.append(('Mode', 'Unknown'))
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))
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)
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(
'time,north(m/s),east(m/s),down(m/s),speed(m/s),num_sats\n')
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)
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):
super(SolutionView, self).__init__()
self.log_file = None
self.vel_log_file = None
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 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,
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.name in [
'surveyed_position', 'broadcast', 'surveyed_lat',
'surveyed_lon', 'surveyed_alt'
]:
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 of the SPP positions of the last 1000 SPP samples.\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): s.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 and not self.called_read_finished:
for cb in self.read_finished_functions:
if self.gui_mode:
GUI.invoke_later(cb)
else:
cb()
self.called_read_finished = True
def settings_read_by_index_done_callback(self, sbp_msg, **metadata):
self.settings_display_setup()
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()
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",
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)
# 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.called_read_finished = False
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 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 = open(filepath_p, 'w')
self.log_file.write("time,latitude(degrees),longitude(degrees),altitude(meters),"
"h_accuracy(meters),v_accuracy(meters),n_sats,flags\n")
self.log_file.write('%s,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % (
str(t),
soln.lat, soln.lon, soln.height,
soln.h_accuracy, soln.v_accuracy,
soln.n_sats, soln.flags)
)
self.log_file.flush()
if self.last_pos_mode == 0:
pos_table.append(('GPS Time', EMPTY_STR))
pos_table.append(('GPS Week', EMPTY_STR))
pos_table.append(('GPS TOW', EMPTY_STR))
pos_table.append(('Num. Signals', EMPTY_STR))
pos_table.append(('Lat', EMPTY_STR))
pos_table.append(('Lng', EMPTY_STR))
pos_table.append(('Height', EMPTY_STR))
pos_table.append(('h_accuracy', EMPTY_STR))
pos_table.append(('v_accuracy', EMPTY_STR))
else:
self.last_stime_update = time.time()
if self.week is not None:
pos_table.append(('GPS Time', "{0}:{1:06.3f}".format(tstr, float(secs))))
pos_table.append(('GPS Week', str(self.week)))
pos_table.append(('GPS TOW', "{:.3f}".format(tow)))
pos_table.append(('Num. Sats', soln.n_sats))
pos_table.append(('Lat', soln.lat))
pos_table.append(('Lng', soln.lon))
pos_table.append(('Height', soln.height))
pos_table.append(('h_accuracy', soln.h_accuracy))
pos_table.append(('v_accuracy', soln.v_accuracy))
pos_table.append(('Pos Flags', '0x%03x' % soln.flags))
pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode]))
self.auto_survey()
# setup_plot variables
self.lats[1:] = self.lats[:-1]
self.lngs[1:] = self.lngs[:-1]
self.alts[1:] = self.alts[:-1]
self.tows[1:] = self.tows[:-1]
self.modes[1:] = self.modes[:-1]
self.lats[0] = soln.lat
self.lngs[0] = soln.lon
self.alts[0] = soln.height
self.tows[0] = soln.tow
self.modes[0] = self.last_pos_mode
self.lats = self.lats[-self.plot_history_max:]
self.lngs = self.lngs[-self.plot_history_max:]
self.alts = self.alts[-self.plot_history_max:]
self.tows = self.tows[-self.plot_history_max:]
self.modes = self.modes[-self.plot_history_max:]
# SPP
spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None
if np.any(self.modes):
spp_indexer = (self.modes == SPP_MODE)
dgnss_indexer = (self.modes == DGNSS_MODE)
float_indexer = (self.modes == FLOAT_MODE)
fixed_indexer = (self.modes == FIXED_MODE)
# make sure that there is at least one true in indexer before setting
if any(spp_indexer):
self.plot_data.set_data('lat_spp', self.lats[spp_indexer])
self.plot_data.set_data('lng_spp', self.lngs[spp_indexer])
self.plot_data.set_data('alt_spp', self.alts[spp_indexer])
if any(dgnss_indexer):
self.plot_data.set_data('lat_dgnss', self.lats[dgnss_indexer])
self.plot_data.set_data('lng_dgnss', self.lngs[dgnss_indexer])
self.plot_data.set_data('alt_dgnss', self.alts[dgnss_indexer])
if any(float_indexer):
self.plot_data.set_data('lat_float', self.lats[float_indexer])
self.plot_data.set_data('lng_float', self.lngs[float_indexer])
self.plot_data.set_data('alt_float', self.alts[float_indexer])
if any(fixed_indexer):
self.plot_data.set_data('lat_fixed', self.lats[fixed_indexer])
self.plot_data.set_data('lng_fixed', self.lngs[fixed_indexer])
self.plot_data.set_data('alt_fixed', self.alts[fixed_indexer])
self._reset_remove_current()
if self.last_pos_mode == SPP_MODE:
self.plot_data.set_data('cur_lat_spp', [soln.lat])
self.plot_data.set_data('cur_lng_spp', [soln.lon])
if self.last_pos_mode == DGNSS_MODE:
self.plot_data.set_data('cur_lat_dgnss', [soln.lat])
self.plot_data.set_data('cur_lng_dgnss', [soln.lon])
if self.last_pos_mode == FLOAT_MODE:
self.plot_data.set_data('cur_lat_float', [soln.lat])
self.plot_data.set_data('cur_lng_float', [soln.lon])
if self.last_pos_mode == FIXED_MODE:
self.plot_data.set_data('cur_lat_fixed', [soln.lat])
self.plot_data.set_data('cur_lng_fixed', [soln.lon])
# set-up table variables
self.pos_table = pos_table
self.table = self.pos_table + self.vel_table + self.dops_table
# TODO: figure out how to center the graph now that we have two separate messages
# when we selectively send only SPP, the centering function won't work anymore
if not self.zoomall and self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.lon - d, soln.lon + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.lat - d, soln.lat + d)
if self.zoomall:
plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float','lng_fixed'),
('lat_spp', 'lat_dgnss', 'lat_float','lat_fixed'))
def 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)
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),flags,num_signals\n')
self.vel_log_file.write('%s,%.6f,%.6f,%.6f,%.6f,%d,%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,
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 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)
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 = Int(0)
piksi_reset_button = SVGButton(label='Reset Piksi',
tooltip='Reset Piksi',
filename=os.path.join(
determine_path(), '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"),
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):
if self.threads != []:
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
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.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, SBP_MSG_UART_STATE_DEPA])
self.python_console_cmds = {'mon': self}
class BaselineView(HasTraits):
python_console_cmds = Dict()
plot = Instance(Plot)
plot_data = Instance(ArrayPlotData)
running = Bool(True)
zoomall = 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)
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)
position_threshold = Str()
depth_threshold = Str()
time_threshold = Str()
focused_dev = Str
dev_all_list = List(['All', 'Preset'])
traits_view = View(
HSplit(
VGroup(
HGroup(
Item('paused_button', show_label=False),
Item('clear_button', show_label=False),
Item('zoomall_button', show_label=False),
Item('focused_dev',
editor=EnumEditor(name='dev_all_list'),
label=u'焦点'), Spring(),
HGroup(
Item('position_threshold',
editor=TextEditor(auto_set=False, enter_set=True),
label=u'位置阈值'),
Item('depth_threshold',
editor=TextEditor(),
label=u'深度阈值'),
Item('time_threshold',
editor=TextEditor(),
label=u'时间阈值'),
)),
Item(
'plot',
show_label=False,
editor=ComponentEditor(bgcolor=(0.8, 0.8, 0.8)),
))), )
def _position_threshold_changed(self):
try:
if int(self.position_threshold) < 0 or int(
self.position_threshold) > 1e6:
self.position_threshold = str(0)
except:
self.position_threshold = str(0)
self.settings_yaml.set_threshold_field('position',
int(self.position_threshold))
self.settings_yaml.dump()
def _depth_threshold_changed(self):
try:
if int(self.depth_threshold) < 0 or int(
self.depth_threshold) > self.plot_history_max:
self.plot_history_max = str(0)
except:
self.plot_history_max = str(0)
self.settings_yaml.set_threshold_field('depth',
int(self.depth_threshold))
self.settings_yaml.dump()
def _time_threshold_changed(self):
try:
if int(self.time_threshold) < 0:
self.time_threshold = str(0)
except:
self.time_threshold = str(0)
self.settings_yaml.set_threshold_field('time',
int(self.time_threshold))
self.settings_yaml.dump()
def _focused_dev_changed(self):
self.zoom_once = True
def _zoomall_button_fired(self):
self.zoomall = not self.zoomall
def _paused_button_fired(self):
self.running = not self.running
def _clear_button_fired(self):
self.neds[:] = np.NAN
self.fixeds[:] = False
self.devs[:] = 0
self.times[:] = 0
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_satisfied', [])
self.plot_data.set_data('e_satisfied', [])
self.plot_data.set_data('n_focused', [])
self.plot_data.set_data('e_focused', [])
self.plot_data.set_data('t', [])
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)
soln = MsgBaselineNED(sbp_msg)
GUI.invoke_later(self.baseline_callback, soln)
cnt = self.cnt % 4
fake_sbp_msg = copy.copy(soln)
if cnt == 3:
fake_sbp_msg.e = 217371
fake_sbp_msg.n = 100837 - (cnt + 1) * 10e3
else:
fake_sbp_msg.e = 217371 + cnt * 20e3
fake_sbp_msg.n = 100837 - cnt * 20e3
fake_sbp_msg.sender = 100 + cnt
fake_sbp_msg.flags = cnt
soln = fake_sbp_msg
self.cnt += 1
GUI.invoke_later(self.baseline_callback, soln)
# _threshold_satisfied()函数计算需要优化
# 或者保持数据发送频率小于2(/s)
time.sleep(0.5)
def baseline_callback(self, sbp_msg):
#soln = MsgBaselineNED(sbp_msg)
soln = sbp_msg
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
#row_data = [soln.sender, soln.n, soln.e, soln.d, soln.n_sats, soln.flags, soln.depth]
row_data = [
soln.sender, soln.n, soln.e, soln.d, soln.n_sats, soln.flags
]
try:
key = int(row_data[0])
self.data_dict[key] = row_data
except:
pass
self.utils.setDataViewTable(self.data_dict)
if soln.sender not in self.dev_list:
self.dev_list.append(soln.sender)
self.dev_all_list.append(str(soln.sender))
# 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]
self.devs[1:] = self.devs[:-1]
self.times[1:] = self.times[:-1]
# Insert latest position
self.neds[0][:] = [soln.n, soln.e, soln.d]
self.fixeds[0] = (soln.flags & 1) == 1
self.devs[0] = int(soln.sender)
self.times[0] = int(time.time())
neds_all = []
neds_fixed = []
neds_float = []
neds_satisfied = []
neds_unsatisfied = []
devs = np.unique(self.devs)
if devs[0] == 0:
devs = devs[1:]
for dev in devs:
is_dev = np.equal(dev, self.devs)
neds_all.append(self.neds[is_dev][0])
try:
neds_fixed.append(self.neds[np.logical_and(
is_dev, self.fixeds)][0])
except:
pass
try:
neds_float.append(self.neds[np.logical_and(
is_dev, np.logical_not(self.fixeds))][0])
except:
pass
position_satisfied, depth_satisfied, time_satisfied = self._threshold_satisfied(
)
is_satisfied = np.logical_and(position_satisfied, depth_satisfied,
time_satisfied)
try:
neds_satisfied.append(self.neds[np.logical_and(
is_dev, is_satisfied)][0])
except:
pass
try:
neds_unsatisfied.append(self.neds[np.logical_and(
is_dev, np.logical_not(is_satisfied))][0])
except:
pass
neds_all = np.array(neds_all)
neds_fixed = np.array(neds_fixed)
neds_float = np.array(neds_float)
neds_satisfied = np.array(neds_satisfied)
neds_unsatisfied = np.array(neds_unsatisfied)
self.neds_all = neds_all
self.neds_satisfied = neds_satisfied
self.neds_unsatisfied = neds_unsatisfied
neds_focused = np.empty((0, 3))
if self.focused_dev == '':
pass
elif self.focused_dev == 'All':
neds_focused = neds_all
elif self.focused_dev != 'Preset':
neds_focused = np.array(
[self.neds[np.equal(self.devs, int(self.focused_dev))][0]])
#if not all(map(any, np.isnan(neds_fixed))):
if len(neds_fixed) > 0:
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))):
if len(neds_float) > 0:
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 len(neds_satisfied) > 0:
self.plot_data.set_data('n_satisfied', neds_satisfied.T[0])
self.plot_data.set_data('e_satisfied', neds_satisfied.T[1])
if len(neds_unsatisfied) > 0:
self.plot_data.set_data('n_unsatisfied', neds_unsatisfied.T[0])
self.plot_data.set_data('e_unsatisfied', neds_unsatisfied.T[1])
if len(self.presets) > 0:
self.plot_data.set_data('n_preset', self.presets['n'])
self.plot_data.set_data('e_preset', self.presets['e'])
if len(neds_focused) > 0:
self.plot_data.set_data('n_focused', neds_focused.T[0])
self.plot_data.set_data('e_focused', neds_focused.T[1])
if self.zoomall:
self._zoomall()
if self.zoom_once:
if self.focused_dev == 'All':
self._zoomall()
elif self.focused_dev == 'Preset':
plot_square_axes(self.plot, 'e_preset', 'n_preset')
else:
plot_square_axes(self.plot, 'e_focused', 'n_focused')
self.zoom_once = False
# 计算阈值，函数功能待测试
def _threshold_satisfied(self):
position_satisfieds = np.zeros(self.plot_history_max, dtype=bool)
depth_satisfieds = np.ones(self.plot_history_max, dtype=bool)
time_satisfieds = np.ones(self.plot_history_max, dtype=bool)
devs = np.unique(self.devs)
for dev in devs:
dev_neds = self.neds[np.equal(self.devs, dev)]
ned_mean = map(np.mean, zip(*dev_neds))
dn = ned_mean[0] - self.presets['n']
de = ned_mean[1] - self.presets['e']
d = np.sqrt(np.add(np.square(dn), np.square(de)))
dmin = np.min(d)
if dmin < int(self.position_threshold):
position_satisfieds[np.equal(dev, self.devs)] = True
ne_depth = self.neds[0:int(self.depth_threshold)]
for ned in ne_depth:
dn = ned[0] - self.presets['n']
de = ned[1] - self.presets['e']
d = np.sqrt(np.add(np.square(dn), np.square(de)))
dmin = np.min(d)
if dmin > int(self.position_threshold):
depth_satisfieds[np.equal(dev, self.devs)] = False
break
cur_time = time.time()
for dev, t, ned in zip(self.devs, self.times, self.neds):
dt = cur_time - t
if dt > int(self.time_threshold):
break
dn = ned[0] - self.presets['n']
de = ned[1] - self.presets['e']
d = np.sqrt(np.add(np.square(dn), np.square(de)))
dmin = np.min(d)
if dmin > int(self.position_threshold):
time_satisfieds[np.equal(dev, self.devs)] = False
return (position_satisfieds, depth_satisfieds, time_satisfieds)
def _zoomall(self):
plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_preset'),
('n_fixed', 'n_float', 'n_preset'))
def _read_preset_points(self, filename='preset.csv'):
preset_points = {}
px = []
py = []
try:
if os.path.isfile(filename):
path_to_file = filename
else:
path_to_file = os.path.join(determine_path(), filename)
f = open(path_to_file, 'r')
for i in f.readlines():
xy = i.split(',')
if len(xy) < 2:
continue
try:
x = float(xy[0]) * 1e-3
y = float(xy[1]) * 1e-3
px.append(x)
py.append(y)
except:
continue
except:
pass
preset_points['e'] = px
preset_points['n'] = py
return preset_points
def set_utils(self, utils):
self.utils = utils
def __init__(self, link, plot_history_max=1000):
super(BaselineView, self).__init__()
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_satisfied=[0.0],
e_satisfied=[0.0],
n_unsatisfied=[0.0],
e_unsatisfied=[0.0],
n_focused=[0.0],
e_focused=[0.0],
t=[0.0],
e_preset=[],
n_preset=[])
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.devs = np.zeros(plot_history_max)
self.times = np.zeros(plot_history_max)
self.plot = Plot(self.plot_data)
color_float = (0.5, 0.5, 1.0)
color_fixed = 'orange'
color_satisfied = (0.3, 1.0, 0.0)
pts_float = self.plot.plot(('e_float', 'n_float'),
type='scatter',
color=color_float,
marker='plus',
line_width=2.0,
marker_size=8.0)
pts_fixed = self.plot.plot(('e_fixed', 'n_fixed'),
type='scatter',
color=color_fixed,
marker='plus',
line_width=2.0,
marker_size=8.0)
threshold_satisfied = self.plot.plot(('e_satisfied', 'n_satisfied'),
type='scatter',
color=color_satisfied,
marker='dot',
line_width=0.0,
marker_size=4.5)
threshold_unsatisfied = self.plot.plot(
('e_unsatisfied', 'n_unsatisfied'),
type='scatter',
color='red',
marker='dot',
line_width=0.0,
marker_size=4.5)
preset = self.plot.plot(('e_preset', 'n_preset'),
type='scatter',
color='black',
marker='plus',
marker_size=1.5,
line_width=0.0)
pts_focused = self.plot.plot(('e_focused', 'n_focused'),
type='scatter',
color='black',
marker='dot',
line_width=0.0,
marker_size=0.0)
#plot_labels = ['RTK Fixed','RTK Float']
#plots_legend = dict(zip(plot_labels, [pts_fixed, pts_float]))
#self.plot.legend.plots = plots_legend
#self.plot.legend.visible = True
self.plot.legend.visible = False
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.cnt = 0
self.dev_list = []
self.data_dict = {}
self.presets = self._read_preset_points()
self.settings_yaml = SettingsList()
self.position_threshold = str(
self.settings_yaml.get_threshold_field('position'))
self.depth_threshold = str(
self.settings_yaml.get_threshold_field('depth'))
self.time_threshold = str(
self.settings_yaml.get_threshold_field('time'))
self.zoom_once = False
self.python_console_cmds = {'baseline': self}
auto_survey_button = Action(name = "Auto Survey", action= "set_execute_callback_true", \
show_label=False)
update_button = Action(name = "Update", action = "set_execute_callback_true", \
show_label=False)
reset_button = Action(name = "Reset", action = "set_execute_callback_true", \
show_label=False)
close_button = Action(name = "Close", action = "set_execute_callback_false", \
show_label=False)
from new import instancemethod
import sys, os
from pyface.image_resource import ImageResource
basedir = determine_path()
icon = ImageResource('icon',
search_path=['images', os.path.join(basedir, 'images')])
# Handler methods that can be associated with buttons.
def set_execute_callback_true(self, info):
info.object.execute_callback = True
info.object.handler_executed = True
def set_execute_callback_false(self, info):
info.object.execute_callback = False
info.object.handler_executed = True
class CallbackHandler(Handler):
# Register action handlers from passed list.
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('')
a = Int
b = Int
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)
log_level_filter = Enum(list(SYSLOG_LEVELS.itervalues()))
paused_button = SVGButton(
label='', tooltip='Pause console update', toggle_tooltip='Resume console update', toggle=True,
filename=os.path.join(determine_path(), 'images', 'iconic', 'pause.svg'),
toggle_filename=os.path.join(determine_path(), 'images', 'iconic', 'play.svg'),
width=8, height=8
)
clear_button = SVGButton(
label='', tooltip='Clear console buffer',
filename=os.path.join(determine_path(), '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='Firmware Update'),
Tabbed(
Item('system_monitor_view', style='custom', label='System Monitor'),
Item('networking_view', label='Networking', style='custom', show_label=False),
Item(
'python_console_env', style='custom',
label='Python Console', editor=ShellEditor()),
label='Advanced',
show_labels=False
),
show_labels=False
),
VGroup(
HGroup(
Spring(width=4, springy=False),
Item('paused_button', show_label=False, width=8, height=8),
Item('clear_button', show_label=False, width=8, height=8),
Item('', label='Console Log', emphasized=True),
Spring(),
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=0.3,
show_label=False,
),
)
),
icon = icon,
resizable = True,
width = 1000,
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 _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 __init__(self, link, update, log_level_filter, skip_settings=False, error=False):
self.console_output = OutputList()
self.console_output.write("Console: starting...")
self.error = error
sys.stdout = self.console_output
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.dep_handler = DeprecatedMessageHandler(link)
settings_read_finished_functions = []
self.tracking_view = TrackingView(self.link)
self.solution_view = SolutionView(self.link)
self.baseline_view = BaselineView(self.link)
self.observation_view = ObservationView(self.link, name='Rover', relay=False)
self.observation_view_base = ObservationView(self.link, name='Base', relay=True)
self.system_monitor_view = SystemMonitorView(self.link)
self.update_view = UpdateView(self.link, prompt=update)
settings_read_finished_functions.append(self.update_view.compare_versions)
self.networking_view = SbpRelayView(self.link)
# Once we have received the settings, update device_serial with
# the Piksi 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():
serial_string = self.settings_view.settings['system_info']['serial_number'].value
self.device_serial = 'PK%04d' % int(serial_string)
if serial_string:
self.networking_view.set_route(int(serial_string))
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.settings_view.python_console_cmds)
except:
import traceback
traceback.print_exc()
if self.error:
sys.exit(1)
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_signals,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. Signals.', 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 ObservationView(HasTraits):
python_console_cmds = Dict()
_obs_table_list = List()
obs = Dict()
name = Str('Local')
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 = sopen(
os.path.join(
self.dirname,
self.name + self.t.strftime("-%Y%m%d-%H%M%S.obs")),
'w')
header = ' ' +\
"""2.11 OBSERVATION DATA G (GPS) RINEX VERSION / TYPE
pyNEX %s UTC PGM / RUN BY / DATE
MARKER NAME
OBSERVER / AGENCY
REC # / TYPE / VERS
ANT # / TYPE
808673.9171 -4086658.5368 4115497.9775 APPROX POSITION XYZ
0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
1 0 WAVELENGTH FACT L1/2
3 C1 L1 S1 # / TYPES OF OBSERV
%s%13.7f GPS TIME OF FIRST OBS
END OF HEADER
""" % (
datetime.datetime.utcnow().strftime("%Y%m%d %H%M%S"),
self.t.strftime(" %Y %m %d %H %M"),
self.t.second + self.t.microsecond * 1e-6)
self.rinex_file.write(header)
# L1CA only
copy_prns = prns = [s for s in self.obs.iterkeys() if L1CA in s]
self.rinex_file.write(
"%s %10.7f 0 %2d" %
(self.t.strftime(" %y %m %d %H %M"),
self.t.second + self.t.microsecond * 1e-6, len(prns)))
while len(copy_prns) > 0:
prns_ = copy_prns[:12]
copy_prns = copy_prns[12:]
for prn in prns_:
# take only the leading prn number
self.rinex_file.write('G%2d' % (int(prn.split()[0])))
self.rinex_file.write(' ' * (12 - len(prns_)))
self.rinex_file.write('\n')
for prn in prns:
# G 3 C1C L1C D1C
self.rinex_file.write("%14.3f " % self.obs[prn][0])
self.rinex_file.write("%14.3f " % self.obs[prn][1])
self.rinex_file.write("%14.3f \n" % self.obs[prn][2])
self.rinex_file.flush()
def 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.common.sid.sat
if m.common.sid.code == 0 or m.common.sid.code == 1:
prn += 1
if self.recording:
if self.eph_file is None:
self.eph_file = sopen(
os.path.join(
self.dirname,
self.name + self.t.strftime("-%Y%m%d-%H%M%S.eph")),
'w')
header = "time, " \
+ "tgd, " \
+ "crs, crc, cuc, cus, cic, cis, " \
+ "dn, m0, ecc, sqrta, omega0, omegadot, w, inc, inc_dot, " \
+ "af0, af1, af2, " \
+ "toe_tow, toe_wn, toc_tow, toc_wn, " \
+ "valid, " \
+ "healthy, " \
+ "prn\n"
self.eph_file.write(header)
strout = "%s %10.7f" % (self.t.strftime(" %y %m %d %H %M"),
self.t.second + self.t.microsecond * 1e-6)
strout += "," + str([
m.tgd, m.c_rs, m.c_rc, m.c_uc, m.c_us, m.c_ic, m.c_is, m.dn,
m.m0, m.ecc, m.sqrta, m.omega0, m.omegadot, m.w, m.inc,
m.inc_dot, m.af0, m.af1, m.af2, m.common.toe.tow,
m.common.toe.wn, m.toc.tow, m.toc.wn, m.common.valid,
m.common.health_bits, prn
])[1:-1] + "\n"
self.eph_file.write(strout)
self.eph_file.flush()
def __init__(self, link, name='Local', relay=False, dirname=None):
super(ObservationView, self).__init__()
self.dirname = dirname
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_GPS)
self.python_console_cmds = {'obs': 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=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')
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_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.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', [])
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 _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 = list(self.table.items())
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 baseline_callback(self, sbp_msg):
soln = MsgBaselineNEDDepA(sbp_msg)
self.last_soln = soln
table = []
soln.n = soln.n * 1e-3
soln.e = soln.e * 1e-3
soln.d = soln.d * 1e-3
soln.h_accuracy = soln.h_accuracy * 1e-3
soln.v_accuracy = soln.v_accuracy * 1e-3
dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2)
tow = soln.tow * 1e-3
if self.nsec is not None:
tow += self.nsec * 1e-9
((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow)
if self.directory_name_b == '':
filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")
else:
filepath = os.path.join(
self.directory_name_b,
time.strftime("baseline_log_%Y%m%d-%H%M%S.csv"))
if self.logging_b == False:
self.log_file = None
if self.logging_b:
if self.log_file is None:
self.log_file = sopen(filepath, 'w')
self.log_file.write(
'pc_time,gps_time,tow(msec),north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),'
'distance(meters),num_sats,flags,num_hypothesis\n')
log_str_gps = ''
if tgps != '' and secgps != 0:
log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps))
self.log_file.write(
'%s,%s,%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' %
("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow,
soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy,
dist, soln.n_sats, soln.flags, self.num_hyps))
self.log_file.flush()
self.last_mode = get_mode(soln)
if self.last_mode < 1:
table.append(('GPS Time', EMPTY_STR))
table.append(('GPS Week', EMPTY_STR))
table.append(('GPS TOW', EMPTY_STR))
table.append(('N', EMPTY_STR))
table.append(('E', EMPTY_STR))
table.append(('D', EMPTY_STR))
table.append(('h_accuracy', EMPTY_STR))
table.append(('v_accuracy', EMPTY_STR))
table.append(('Dist.', EMPTY_STR))
table.append(('Num. Sats', EMPTY_STR))
table.append(('Flags', EMPTY_STR))
table.append(('Mode', EMPTY_STR))
else:
self.last_btime_update = time.time()
if self.week is not None:
table.append(
('GPS Time', "{0}:{1:06.3f}".format(tgps, float(secgps))))
table.append(('GPS Week', str(self.week)))
table.append(('GPS TOW', "{:.3f}".format(tow)))
table.append(('N', soln.n))
table.append(('E', soln.e))
table.append(('D', soln.d))
table.append(('h_accuracy', soln.h_accuracy))
table.append(('v_accuracy', soln.v_accuracy))
table.append(('Dist.', dist))
table.append(('Num. Sats', soln.n_sats))
table.append(('Flags', '0x%02x' % soln.flags))
table.append(('Mode', mode_dict[self.last_mode]))
self.table = table
# Rotate array, deleting oldest entries to maintain
# no more than N in plot
self.n[1:] = self.n[:-1]
self.e[1:] = self.e[:-1]
self.d[1:] = self.d[:-1]
self.mode[1:] = self.mode[:-1]
# Insert latest position
if self.last_mode > 1:
self.n[0], self.e[0], self.d[0] = soln.n, soln.e, soln.d
else:
self.n[0], self.e[0], self.d[0] = [np.NAN, np.NAN, np.NAN]
self.mode[0] = self.last_mode
if np.any(self.mode):
float_indexer = (self.mode == FLOAT_MODE)
fixed_indexer = (self.mode == FIXED_MODE)
dgnss_indexer = (self.mode == DGNSS_MODE)
if np.any(fixed_indexer):
self.plot_data.set_data('n_fixed', self.n[fixed_indexer])
self.plot_data.set_data('e_fixed', self.e[fixed_indexer])
self.plot_data.set_data('d_fixed', self.d[fixed_indexer])
if np.any(float_indexer):
self.plot_data.set_data('n_float', self.n[float_indexer])
self.plot_data.set_data('e_float', self.e[float_indexer])
self.plot_data.set_data('d_float', self.d[float_indexer])
if np.any(dgnss_indexer):
self.plot_data.set_data('n_dgnss', self.n[dgnss_indexer])
self.plot_data.set_data('e_dgnss', self.e[dgnss_indexer])
self.plot_data.set_data('d_dgnss', self.d[dgnss_indexer])
# Update our last solution icon
if self.last_mode == FIXED_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_fixed_n', [soln.n])
self.plot_data.set_data('cur_fixed_e', [soln.e])
self.plot_data.set_data('cur_fixed_d', [soln.d])
elif self.last_mode == FLOAT_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_float_n', [soln.n])
self.plot_data.set_data('cur_float_e', [soln.e])
self.plot_data.set_data('cur_float_d', [soln.d])
elif self.last_mode == DGNSS_MODE:
self._reset_remove_current()
self.plot_data.set_data('cur_dgnss_n', [soln.n])
self.plot_data.set_data('cur_dgnss_e', [soln.e])
self.plot_data.set_data('cur_dgnss_d', [soln.d])
else:
pass
# make the zoomall win over the position centered button
# position centered button has no effect when zoom all enabled
if not self.zoomall and self.position_centered:
d = (self.plot.index_range.high - self.plot.index_range.low) / 2.
self.plot.index_range.set_bounds(soln.e - d, soln.e + d)
d = (self.plot.value_range.high - self.plot.value_range.low) / 2.
self.plot.value_range.set_bounds(soln.n - d, soln.n + d)
if self.zoomall:
plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'),
('n_fixed', 'n_float', 'n_dgnss'))
def __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.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(('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(('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(
list(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.nsec = 0
self.link = link
self.link.add_callback(
self._baseline_callback_ned,
[SBP_MSG_BASELINE_NED, SBP_MSG_BASELINE_NED_DEP_A])
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.python_console_cmds = {'baseline': 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('')
a = Int
b = Int
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)
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 Piksi connected to
directory_name : location of logged files
json_logging : enable JSON logging
csv_logging : enable CSV logging
is_valid_directory : check to see if chosen directory is valid
"""
mode = Str('')
num_sats = Int(0)
port = Str('')
directory_name = Directory
json_logging = Bool(True)
csv_logging = Bool(False)
is_valid_directory = Bool(True)
csv_logging_button = SVGButton(
toggle=True,
label='CSV log',
tooltip='start CSV logging',
toggle_tooltip='stop CSV logging',
filename=os.path.join(determine_path(), 'images', 'iconic',
'pause.svg'),
toggle_filename=os.path.join(determine_path(), '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=os.path.join(determine_path(), 'images', 'iconic',
'pause.svg'),
toggle_filename=os.path.join(determine_path(), '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=os.path.join(determine_path(), 'images',
'iconic', 'pause.svg'),
toggle_filename=os.path.join(
determine_path(), 'images', 'iconic',
'play.svg'),
width=8,
height=8)
clear_button = SVGButton(label='',
tooltip='Clear console buffer',
filename=os.path.join(determine_path(), '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='Firmware Update'),
Tabbed(Item('system_monitor_view',
style='custom',
label='System Monitor'),
Item('networking_view',
label='Networking',
style='custom',
show_label=False),
Item('python_console_env',
style='custom',
label='Python Console',
editor=ShellEditor()),
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,
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)'),
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='Serial Port that Piksi is connected to'),
Item('port', show_label=False, style='readonly'),
Item('',
label='FIX TYPE:',
emphasized=True,
tooltip='Piksi Mode: SPS, Float RTK, Fixed RTK'),
Item('mode', show_label=False, style='readonly'),
Item('',
label='#SATS:',
emphasized=True,
tooltip='Number of satellites acquired by Piksi'),
Item('num_sats', padding=2, show_label=False,
style='readonly'),
),
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 _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 os.path.isdir(self.directory_name):
self.is_valid_directory = True
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
else:
print "Please enter a valid directory!"
self.is_valid_directory = False
def update_on_heartbeat(self, sbp_msg, **metadata):
# 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 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 = view.mode_string(view.last_soln)
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
def _csv_logging_button_fired(self):
if self.is_valid_directory:
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
else:
print "Directory not valid"
def _start_json_logging(self, override_filename=None):
if override_filename:
filename = override_filename
else:
filename = s.logfilename()
filename = os.path.normpath(os.path.join(self.directory_name,
filename))
self.logger = s.get_logger(True, filename)
self.forwarder = sbpc.Forwarder(self.link, self.logger)
self.forwarder.start()
def _stop_json_logging(self):
fwd = self.forwarder
fwd.stop()
self.logger.flush()
self.logger.close()
def _json_logging_button_fired(self):
if self.is_valid_directory:
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
else:
print "Directory not valid"
def __init__(self,
link,
update,
log_level_filter,
skip_settings=False,
error=False,
port=None,
json_logging=False,
log_dirname=None):
self.console_output = OutputList()
self.console_output.write("Console: starting...")
self.error = error
sys.stdout = self.console_output
self.port = port
self.num_sats = 0
self.mode = ''
self.forwarder = None
# if we have passed a logfile, we set our directory to it
override_filename = None
swift_path = None
home = expanduser("~")
swift_path = os.path.normpath(os.path.join(home, 'SwiftNav'))
try:
os.makedirs(swift_path)
except OSError:
if not os.path.isdir(swift_path):
raise
if log_dirname:
self.directory_name = log_dirname
else:
self.directory_name = swift_path
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.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, prompt=update)
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
# Once we have received the settings, update device_serial with
# the Piksi 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():
serial_string = self.settings_view.settings['system_info'][
'serial_number'].value
self.device_serial = 'PK%04d' % int(serial_string)
if serial_string:
self.networking_view.set_route(int(serial_string))
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.settings_view.python_console_cmds)
except:
import traceback
traceback.print_exc()
if self.error:
sys.exit(1)