def __init__(self, tfile=False, outdir=''): if tfile: self.logfile = sopen(os.path.join(outdir, LOGFILE), 'w') self.tfile = True else: self.tfile = False
def vel_ned_callback(self, sbp_msg, **metadata): flags = 0 if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A: vel_ned = MsgVelNEDDepA(sbp_msg) flags = 1 else: vel_ned = MsgVelNED(sbp_msg) flags = vel_ned.flags tow = vel_ned.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.directory_name_v == '': filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv") else: filepath_v = os.path.join( self.directory_name_v, time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")) if not self.logging_v: self.vel_log_file = None if self.logging_v: if self.vel_log_file is None: self.vel_log_file = sopen(filepath_v, 'w') self.vel_log_file.write( 'pc_time,gps_time,tow(sec),north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n' ) log_str_gps = '' if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.vel_log_file.write( '%s,%s,%.3f,%.6f,%.6f,%.6f,%.6f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3, math.sqrt(vel_ned.n * vel_ned.n + vel_ned.e * vel_ned.e ) * 1e-3, flags, vel_ned.n_sats)) self.vel_log_file.flush() if flags != 0: self.vel_table = [ ('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)), ('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)), ('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)), ] else: self.vel_table = [ ('Vel. N', EMPTY_STR), ('Vel. E', EMPTY_STR), ('Vel. D', EMPTY_STR), ] self.vel_table.append(('Vel Flags', '0x%03x' % flags)) self.update_table()
def vel_ned_callback(self, sbp_msg, **metadata): flags = 0 if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A: vel_ned = MsgVelNEDDepA(sbp_msg) flags = 1 else: vel_ned = MsgVelNED(sbp_msg) flags = vel_ned.flags tow = vel_ned.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.directory_name_v == '': filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv") else: filepath_v = os.path.join( self.directory_name_v, time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")) if not self.logging_v: self.vel_log_file = None if self.logging_v: if self.vel_log_file is None: self.vel_log_file = sopen(filepath_v, 'w') self.vel_log_file.write( 'pc_time,gps_time,tow(sec),north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n' ) log_str_gps = '' if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.vel_log_file.write( '%s,%s,%.3f,%.6f,%.6f,%.6f,%.6f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3, math.sqrt(vel_ned.n * vel_ned.n + vel_ned.e * vel_ned.e ) * 1e-3, flags, vel_ned.n_sats)) self.vel_log_file.flush() if (flags & 0x7) != 0: self.vel_table = [ ('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)), ('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)), ('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)), ] else: self.vel_table = [ ('Vel. N', EMPTY_STR), ('Vel. E', EMPTY_STR), ('Vel. D', EMPTY_STR), ] self.vel_table.append(('Vel Flags', '0x%03x' % flags)) self.update_table()
def _download_file_from_url(self, url): url = url.encode('ascii') urlpath = urlparse(url).path filename = os.path.split(urlparse(url).path)[1] filename = os.path.join(self.root_dir, filename) url_file = urlopen(url) blob = url_file.read() with sopen(filename, 'wb') as f: f.write(blob) url_file.close() return os.path.abspath(filename)
def _download_file_from_url(self, url): if not os.path.exists(self.root_dir): raise RuntimeError("Path to download file {0} to does not exist.".format(self.root_dir)) return filename = os.path.split(urlparse(url).path)[1] filename = os.path.join(self.root_dir, filename) requests_response = requests.get(url) requests_response.raise_for_status() blob = requests_response.content with sopen(filename, 'wb') as f: f.write(blob) return os.path.abspath(filename)
def _download_file_from_url(self, url): if not os.path.isdir(self.root_dir): raise IOError("Path to download file to does not exist") url = url.encode('ascii') urlpath = urlparse(url).path filename = os.path.split(urlparse(url).path)[1] filename = os.path.join(self.root_dir, filename) url_file = urlopen(url) blob = url_file.read() with sopen(filename, 'wb') as f: f.write(blob) url_file.close() return os.path.abspath(filename)
def baseline_callback(self, sbp_msg, **metadata): soln = MsgBaselineNEDDepA(sbp_msg) table = [] soln.n = soln.n * 1e-3 soln.e = soln.e * 1e-3 soln.d = soln.d * 1e-3 soln.h_accuracy = soln.h_accuracy * 1e-3 soln.v_accuracy = soln.v_accuracy * 1e-3 dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2) tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time is not None: ((tutc, secutc)) = datetime_2_str(self.utc_time) if self.directory_name_b == '': filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv") else: filepath = os.path.join( self.directory_name_b, time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")) if not self.logging_b: self.log_file = None if self.logging_b: if self.log_file is None: self.log_file = sopen(filepath, 'w') self.log_file.write( 'pc_time,gps_time,tow(sec),north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),' 'distance(meters),num_sats,flags,num_hypothesis\n') log_str_gps = '' if tgps != '' and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy, dist, soln.n_sats, soln.flags, self.num_hyps)) self.log_file.flush() self.last_mode = get_mode(soln) if self.last_mode < 1: table.append(('GPS Week', EMPTY_STR)) table.append(('GPS TOW', EMPTY_STR)) table.append(('GPS Time', EMPTY_STR)) table.append(('UTC Time', EMPTY_STR)) table.append(('UTC Src', EMPTY_STR)) table.append(('N', EMPTY_STR)) table.append(('E', EMPTY_STR)) table.append(('D', EMPTY_STR)) table.append(('Horiz Acc', EMPTY_STR)) table.append(('Vert Acc', EMPTY_STR)) table.append(('Dist.', EMPTY_STR)) table.append(('Sats Used', EMPTY_STR)) table.append(('Flags', EMPTY_STR)) table.append(('Mode', EMPTY_STR)) table.append(('Heading', EMPTY_STR)) table.append(('Corr. Age [s]', EMPTY_STR)) else: self.last_btime_update = monotonic() if self.week is not None: table.append(('GPS Week', str(self.week))) table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) table.append(('UTC Src', self.utc_source)) table.append(('N', "{:.12g}".format(soln.n))) table.append(('E', "{:.12g}".format(soln.e))) table.append(('D', "{:.12g}".format(soln.d))) table.append(('Horiz Acc', "{:.12g}".format(soln.h_accuracy))) table.append(('Vert Acc', "{:.12g}".format(soln.v_accuracy))) table.append(('Dist.', "{0:.3f}".format(dist))) table.append(('Sats Used', soln.n_sats)) table.append(('Flags', '0x%02x' % soln.flags)) table.append(('Mode', mode_dict[self.last_mode])) if self.heading is not None: table.append(('Heading', self.heading)) if self.age_corrections is not None: table.append(('Corr. Age [s]', self.age_corrections)) else: table.append(('Corr. Age [s]', EMPTY_STR)) self.table = table if self.last_mode != 0: self.last_soln = soln mode_string = mode_string_dict[self.last_mode] if mode_string not in self.pending_draw_modes: # if we don't already have a pending upate for that mode self.pending_draw_modes.append(mode_string) self.list_lock.acquire() self._update_sln_data_by_mode(soln, mode_string) self.list_lock.release() else: self.list_lock.acquire() self._append_empty_sln_data(soln) self.list_lock.release() if monotonic() - self.last_plot_update_time > GUI_UPDATE_PERIOD: self.update_scheduler.schedule_update('_solution_draw', self._solution_draw)
def pos_llh_callback(self, sbp_msg, **metadata): if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A: soln = MsgPosLLHDepA(sbp_msg) else: soln = MsgPosLLH(sbp_msg) self.last_soln = soln self.last_pos_mode = get_mode(soln) self.ins_used = ((soln.flags & 0x8) >> 3) == 1 pos_table = [] soln.h_accuracy *= 1e-3 soln.v_accuracy *= 1e-3 tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 # Return the best estimate of my local and receiver time in convenient # format that allows changing precision of the seconds ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time: ((tutc, secutc)) = datetime_2_str(self.utc_time) if (self.directory_name_p == ''): filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv") else: filepath_p = os.path.join( self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv")) if not self.logging_p: self.log_file = None if self.logging_p: if self.log_file is None: self.log_file = sopen(filepath_p, 'w') self.log_file.write( "pc_time,gps_time,tow(sec),latitude(degrees),longitude(degrees),altitude(meters)," "h_accuracy(meters),v_accuracy(meters),n_sats,flags\n") log_str_gps = "" if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.lat, soln.lon, soln.height, soln.h_accuracy, soln.v_accuracy, soln.n_sats, soln.flags)) self.log_file.flush() if self.last_pos_mode == 0: pos_table.append(('GPS Week', EMPTY_STR)) pos_table.append(('GPS TOW', EMPTY_STR)) pos_table.append(('GPS Time', EMPTY_STR)) pos_table.append(('Num. Signals', EMPTY_STR)) pos_table.append(('Lat', EMPTY_STR)) pos_table.append(('Lng', EMPTY_STR)) pos_table.append(('Height', EMPTY_STR)) pos_table.append(('Horiz Acc', EMPTY_STR)) pos_table.append(('Vert Acc', EMPTY_STR)) else: self.last_stime_update = time.time() if self.week is not None: pos_table.append(('GPS Week', str(self.week))) pos_table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: pos_table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: pos_table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) pos_table.append(('UTC Src', self.utc_source)) if self.utc_time is None: pos_table.append(('UTC Time', EMPTY_STR)) pos_table.append(('UTC Src', EMPTY_STR)) pos_table.append(('Sats Used', soln.n_sats)) pos_table.append(('Lat', soln.lat)) pos_table.append(('Lng', soln.lon)) pos_table.append(('Height', "{0:.3f}".format(soln.height))) pos_table.append(('Horiz Acc', soln.h_accuracy)) pos_table.append(('Vert Acc', soln.v_accuracy)) pos_table.append(('Pos Flags', '0x%03x' % soln.flags)) pos_table.append(('INS Used', '{}'.format(self.ins_used))) pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode])) if self.age_corrections is not None: pos_table.append(('Corr. Age [s]', self.age_corrections)) self.auto_survey() # set-up table variables self.pos_table = pos_table self.update_table() # setup_plot variables self.list_lock.acquire() self.lats[1:] = self.lats[:-1] self.lngs[1:] = self.lngs[:-1] self.alts[1:] = self.alts[:-1] self.tows[1:] = self.tows[:-1] self.modes[1:] = self.modes[:-1] self.lats[0] = soln.lat self.lngs[0] = soln.lon self.alts[0] = soln.height self.tows[0] = soln.tow self.modes[0] = self.last_pos_mode self.lats = self.lats[-self.plot_history_max:] self.lngs = self.lngs[-self.plot_history_max:] self.alts = self.alts[-self.plot_history_max:] self.tows = self.tows[-self.plot_history_max:] self.modes = self.modes[-self.plot_history_max:] self.list_lock.release() # Updating array plot data is not thread safe, so we have to fire an event # and have the GUI thread do it if time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD: self.last_plot_update_time = time.time()
def baseline_callback(self, sbp_msg, **metadata): soln = MsgBaselineNEDDepA(sbp_msg) self.last_soln = soln table = [] soln.n = soln.n * 1e-3 soln.e = soln.e * 1e-3 soln.d = soln.d * 1e-3 soln.h_accuracy = soln.h_accuracy * 1e-3 soln.v_accuracy = soln.v_accuracy * 1e-3 dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2) tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time is not None: ((tutc, secutc)) = datetime_2_str(self.utc_time) if self.directory_name_b == '': filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv") else: filepath = os.path.join( self.directory_name_b, time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")) if not self.logging_b: self.log_file = None if self.logging_b: if self.log_file is None: self.log_file = sopen(filepath, 'w') self.log_file.write( 'pc_time,gps_time,tow(sec),north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),' 'distance(meters),num_sats,flags,num_hypothesis\n') log_str_gps = '' if tgps != '' and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy, dist, soln.n_sats, soln.flags, self.num_hyps)) self.log_file.flush() self.last_mode = get_mode(soln) if self.last_mode < 1: table.append(('GPS Week', EMPTY_STR)) table.append(('GPS TOW', EMPTY_STR)) table.append(('GPS Time', EMPTY_STR)) table.append(('UTC Time', EMPTY_STR)) table.append(('UTC Src', EMPTY_STR)) table.append(('N', EMPTY_STR)) table.append(('E', EMPTY_STR)) table.append(('D', EMPTY_STR)) table.append(('Horiz Acc', EMPTY_STR)) table.append(('Vert Acc', EMPTY_STR)) table.append(('Dist.', EMPTY_STR)) table.append(('Sats Used', EMPTY_STR)) table.append(('Flags', EMPTY_STR)) table.append(('Mode', EMPTY_STR)) else: self.last_btime_update = time.time() if self.week is not None: table.append(('GPS Week', str(self.week))) table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) table.append(('UTC Src', self.utc_source)) table.append(('N', soln.n)) table.append(('E', soln.e)) table.append(('D', soln.d)) table.append(('Horiz Acc', soln.h_accuracy)) table.append(('Vert Acc', soln.v_accuracy)) table.append(('Dist.', "{0:.3f}".format(dist))) table.append(('Sats Used', soln.n_sats)) table.append(('Flags', '0x%02x' % soln.flags)) table.append(('Mode', mode_dict[self.last_mode])) if self.heading is not None: table.append(('Heading', self.heading)) if self.age_corrections is not None: table.append(('Corr. Age [s]', self.age_corrections)) self.table = table # Rotate array, deleting oldest entries to maintain # no more than N in plot self.n[1:] = self.n[:-1] self.e[1:] = self.e[:-1] self.d[1:] = self.d[:-1] self.mode[1:] = self.mode[:-1] # Insert latest position if self.last_mode > 1: self.n[0], self.e[0], self.d[0] = soln.n, soln.e, soln.d else: self.n[0], self.e[0], self.d[0] = [np.NAN, np.NAN, np.NAN] self.mode[0] = self.last_mode
def pos_llh_callback(self, sbp_msg, **metadata): if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A: soln = MsgPosLLHDepA(sbp_msg) else: soln = MsgPosLLH(sbp_msg) self.last_pos_mode = get_mode(soln) if self.last_pos_mode != 0: self.last_soln = soln mode_string = mode_string_dict[self.last_pos_mode] if mode_string not in self.pending_draw_modes: # this list allows us to tell GUI thread which solutions to update # (if we decide not to update at full data rate) # we use short strings to identify each solution mode self.pending_draw_modes.append(mode_string) self.list_lock.acquire() self._update_sln_data_by_mode(soln, mode_string) self.list_lock.release() else: self.list_lock.acquire() self._append_empty_sln_data() self.list_lock.release() self.ins_used = ((soln.flags & 0x8) >> 3) == 1 pos_table = [] soln.h_accuracy *= 1e-3 soln.v_accuracy *= 1e-3 tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 # Return the best estimate of my local and receiver time in convenient # format that allows changing precision of the seconds ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time: ((tutc, secutc)) = datetime_2_str(self.utc_time) if (self.directory_name_p == ''): filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv") else: filepath_p = os.path.join( self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv")) if not self.logging_p: self.log_file = None if self.logging_p: if self.log_file is None: self.log_file = sopen(filepath_p, 'w') self.log_file.write( "pc_time,gps_time,tow(sec),latitude(degrees),longitude(degrees),altitude(meters)," "h_accuracy(meters),v_accuracy(meters),n_sats,flags\n") log_str_gps = "" if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.lat, soln.lon, soln.height, soln.h_accuracy, soln.v_accuracy, soln.n_sats, soln.flags)) self.log_file.flush() if self.last_pos_mode == 0: pos_table.append(('GPS Week', EMPTY_STR)) pos_table.append(('GPS TOW', EMPTY_STR)) pos_table.append(('GPS Time', EMPTY_STR)) pos_table.append(('Num. Signals', EMPTY_STR)) pos_table.append(('Lat', EMPTY_STR)) pos_table.append(('Lng', EMPTY_STR)) pos_table.append(('Height', EMPTY_STR)) pos_table.append(('Horiz Acc', EMPTY_STR)) pos_table.append(('Vert Acc', EMPTY_STR)) else: self.last_stime_update = monotonic() if self.week is not None: pos_table.append(('GPS Week', str(self.week))) pos_table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: pos_table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: pos_table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) pos_table.append(('UTC Src', self.utc_source)) if self.utc_time is None: pos_table.append(('UTC Time', EMPTY_STR)) pos_table.append(('UTC Src', EMPTY_STR)) pos_table.append(('Sats Used', soln.n_sats)) pos_table.append(('Lat', "{:.12g}".format(soln.lat))) pos_table.append(('Lng', "{:.12g}".format(soln.lon))) pos_table.append(('Height', "{0:.3f}".format(soln.height))) pos_table.append(('Horiz Acc', "{:.12g}".format(soln.h_accuracy))) pos_table.append(('Vert Acc', "{:.12g}".format(soln.v_accuracy))) pos_table.append(('Pos Flags', '0x%03x' % soln.flags)) pos_table.append(('INS Used', '{}'.format(self.ins_used))) pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode])) if self.age_corrections is not None: pos_table.append(('Corr. Age [s]', self.age_corrections)) # only store valid solutions for auto survey and degrees to meter transformation if self.last_pos_mode != 0: self.lats.append(soln.lat) self.lngs.append(soln.lon) self.alts.append(soln.height) self.tows.append(soln.tow) self.modes.append(self.last_pos_mode) self.auto_survey() # set-up table variables self.pos_table = pos_table self.update_table() # setup_plot variables # Updating array plot data is not thread safe, so we have to fire an event # and have the GUI thread do it if monotonic() - self.last_plot_update_time > GUI_UPDATE_PERIOD: self.update_scheduler.schedule_update('_solution_draw', self._solution_draw)
def pos_llh_callback(self, sbp_msg, **metadata): if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A: soln = MsgPosLLHDepA(sbp_msg) else: soln = MsgPosLLH(sbp_msg) self.last_soln = soln self.last_pos_mode = get_mode(soln) pos_table = [] soln.h_accuracy *= 1e-3 soln.v_accuracy *= 1e-3 tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 # Return the best estimate of my local and receiver time in convenient # format that allows changing precision of the seconds ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time: ((tutc, secutc)) = datetime_2_str(self.utc_time) if (self.directory_name_p == ''): filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv") else: filepath_p = os.path.join( self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv")) if not self.logging_p: self.log_file = None if self.logging_p: if self.log_file is None: self.log_file = sopen(filepath_p, 'w') self.log_file.write( "pc_time,gps_time,tow(sec),latitude(degrees),longitude(degrees),altitude(meters)," "h_accuracy(meters),v_accuracy(meters),n_sats,flags\n") log_str_gps = "" if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.lat, soln.lon, soln.height, soln.h_accuracy, soln.v_accuracy, soln.n_sats, soln.flags)) self.log_file.flush() if self.last_pos_mode == 0: pos_table.append(('GPS Week', EMPTY_STR)) pos_table.append(('GPS TOW', EMPTY_STR)) pos_table.append(('GPS Time', EMPTY_STR)) pos_table.append(('Num. Signals', EMPTY_STR)) pos_table.append(('Lat', EMPTY_STR)) pos_table.append(('Lng', EMPTY_STR)) pos_table.append(('Height', EMPTY_STR)) pos_table.append(('Horiz Acc', EMPTY_STR)) pos_table.append(('Vert Acc', EMPTY_STR)) else: self.last_stime_update = time.time() if self.week is not None: pos_table.append(('GPS Week', str(self.week))) pos_table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: pos_table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: pos_table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) pos_table.append(('UTC Src', self.utc_source)) if self.utc_time is None: pos_table.append(('UTC Time', EMPTY_STR)) pos_table.append(('UTC Src', EMPTY_STR)) pos_table.append(('Sats Used', soln.n_sats)) pos_table.append(('Lat', soln.lat)) pos_table.append(('Lng', soln.lon)) pos_table.append(('Height', "{0:.3f}".format(soln.height))) pos_table.append(('Horiz Acc', soln.h_accuracy)) pos_table.append(('Vert Acc', soln.v_accuracy)) pos_table.append(('Pos Flags', '0x%03x' % soln.flags)) pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode])) if self.age_corrections is not None: pos_table.append(('Corr. Age [s]', self.age_corrections)) self.auto_survey() # set-up table variables self.pos_table = pos_table self.update_table() # setup_plot variables self.lats[1:] = self.lats[:-1] self.lngs[1:] = self.lngs[:-1] self.alts[1:] = self.alts[:-1] self.tows[1:] = self.tows[:-1] self.modes[1:] = self.modes[:-1] self.lats[0] = soln.lat self.lngs[0] = soln.lon self.alts[0] = soln.height self.tows[0] = soln.tow self.modes[0] = self.last_pos_mode self.lats = self.lats[-self.plot_history_max:] self.lngs = self.lngs[-self.plot_history_max:] self.alts = self.alts[-self.plot_history_max:] self.tows = self.tows[-self.plot_history_max:] self.modes = self.modes[-self.plot_history_max:]
def baseline_callback(self, sbp_msg, **metadata): soln = MsgBaselineNEDDepA(sbp_msg) table = [] soln.n = soln.n * 1e-3 soln.e = soln.e * 1e-3 soln.d = soln.d * 1e-3 soln.h_accuracy = soln.h_accuracy * 1e-3 soln.v_accuracy = soln.v_accuracy * 1e-3 dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2) tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time is not None: ((tutc, secutc)) = datetime_2_str(self.utc_time) if self.directory_name_b == '': filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv") else: filepath = os.path.join( self.directory_name_b, time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")) if not self.logging_b: self.log_file = None if self.logging_b: if self.log_file is None: self.log_file = sopen(filepath, 'w') self.log_file.write( 'pc_time,gps_time,tow(sec),north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),' 'distance(meters),num_sats,flags,num_hypothesis\n') log_str_gps = '' if tgps != '' and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy, dist, soln.n_sats, soln.flags, self.num_hyps)) self.log_file.flush() self.last_mode = get_mode(soln) if self.last_mode < 1: table.append(('GPS Week', EMPTY_STR)) table.append(('GPS TOW', EMPTY_STR)) table.append(('GPS Time', EMPTY_STR)) table.append(('UTC Time', EMPTY_STR)) table.append(('UTC Src', EMPTY_STR)) table.append(('N', EMPTY_STR)) table.append(('E', EMPTY_STR)) table.append(('D', EMPTY_STR)) table.append(('Horiz Acc', EMPTY_STR)) table.append(('Vert Acc', EMPTY_STR)) table.append(('Dist.', EMPTY_STR)) table.append(('Sats Used', EMPTY_STR)) table.append(('Flags', EMPTY_STR)) table.append(('Mode', EMPTY_STR)) table.append(('Heading', EMPTY_STR)) table.append(('Corr. Age [s]', EMPTY_STR)) else: self.last_btime_update = time.time() if self.week is not None: table.append(('GPS Week', str(self.week))) table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) table.append(('UTC Src', self.utc_source)) table.append(('N', soln.n)) table.append(('E', soln.e)) table.append(('D', soln.d)) table.append(('Horiz Acc', soln.h_accuracy)) table.append(('Vert Acc', soln.v_accuracy)) table.append(('Dist.', "{0:.3f}".format(dist))) table.append(('Sats Used', soln.n_sats)) table.append(('Flags', '0x%02x' % soln.flags)) table.append(('Mode', mode_dict[self.last_mode])) if self.heading is not None: table.append(('Heading', self.heading)) if self.age_corrections is not None: table.append(('Corr. Age [s]', self.age_corrections)) else: table.append(('Corr. Age [s]', EMPTY_STR)) self.table = table if self.last_mode != 0: self.last_soln = soln mode_string = mode_string_dict[self.last_mode] if mode_string not in self.pending_draw_modes: # if we don't already have a pending upate for that mode self.pending_draw_modes.append(mode_string) self.list_lock.acquire() self._update_sln_data_by_mode(soln, mode_string) self.list_lock.release() else: self.list_lock.acquire() self._append_empty_sln_data(soln) self.list_lock.release() if time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD: GUI.invoke_later(self._solution_draw)