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 vel_ned_callback(self, sbp_msg, **metadata): flags = 0 if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A: vel_ned = MsgVelNEDDepA(sbp_msg) flags = 1 else: vel_ned = MsgVelNED(sbp_msg) flags = vel_ned.flags tow = vel_ned.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 if self.week is not None: t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.week) + \ datetime.timedelta(seconds=tow) tstr = t.strftime('%Y-%m-%d %H:%M') secs = t.strftime('%S.%f') if self.directory_name_v == '': filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv") else: filepath_v = os.path.join( self.directory_name_v, time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")) if self.logging_v == False: self.vel_log_file = None if self.logging_v: if self.vel_log_file is None: self.vel_log_file = sopen(filepath_v, 'w') self.vel_log_file.write( 'time,north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n' ) self.vel_log_file.write( '%s,%.6f,%.6f,%.6f,%.6f,%d,%d\n' % ("{0}:{1:06.6f}".format(tstr, float(secs)), vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3, math.sqrt(vel_ned.n * vel_ned.n + vel_ned.e * vel_ned.e) * 1e-3, flags, vel_ned.n_sats)) self.vel_log_file.flush() if flags != 0: self.vel_table = [ ('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)), ('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)), ('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)), ] else: self.vel_table = [ ('Vel. N', EMPTY_STR), ('Vel. E', EMPTY_STR), ('Vel. D', EMPTY_STR), ] self.vel_table.append(('Vel Flags', '0x%03x' % flags)) self.table = self.pos_table + self.vel_table + self.dops_table
def 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 self.logging_v == False: self.vel_log_file = None if self.logging_v: if self.vel_log_file is None: self.vel_log_file = sopen(filepath_v, 'w') self.vel_log_file.write( 'pc_time,gps_time,tow,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.table = self.pos_table + self.vel_table + self.dops_table
def vel_ned_callback(self, sbp_msg, **metadata): flags = 0 if sbp_msg.msg_type == SBP_MSG_VEL_NED_DEP_A: vel_ned = MsgVelNEDDepA(sbp_msg) flags = 1 else: vel_ned = MsgVelNED(sbp_msg) flags = vel_ned.flags tow = vel_ned.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 if self.week is not None: t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.week) + \ datetime.timedelta(seconds=tow) tstr = t.strftime('%Y-%m-%d %H:%M') secs = t.strftime('%S.%f') if self.directory_name_v == '': filepath_v = time.strftime("velocity_log_%Y%m%d-%H%M%S.csv") else: filepath_v = os.path.join(self.directory_name_v,time.strftime("velocity_log_%Y%m%d-%H%M%S.csv")) if self.logging_v == False: self.vel_log_file = None if self.logging_v: if self.vel_log_file is None: self.vel_log_file = sopen(filepath_v, 'w') self.vel_log_file.write('time,north(m/s),east(m/s),down(m/s),speed(m/s),flags,num_signals\n') self.vel_log_file.write('%s,%.6f,%.6f,%.6f,%.6f,%d,%d\n' % ( "{0}:{1:06.6f}".format(tstr, float(secs)), vel_ned.n * 1e-3, vel_ned.e * 1e-3, vel_ned.d * 1e-3, math.sqrt(vel_ned.n*vel_ned.n + vel_ned.e*vel_ned.e) * 1e-3, flags, vel_ned.n_sats) ) self.vel_log_file.flush() if flags != 0: self.vel_table = [ ('Vel. N', '% 8.4f' % (vel_ned.n * 1e-3)), ('Vel. E', '% 8.4f' % (vel_ned.e * 1e-3)), ('Vel. D', '% 8.4f' % (vel_ned.d * 1e-3)), ] else: self.vel_table = [ ('Vel. N', EMPTY_STR), ('Vel. E', EMPTY_STR), ('Vel. D', EMPTY_STR), ] self.vel_table.append(('Vel Flags', '0x%03x' % flags)) self.table = self.pos_table + self.vel_table + self.dops_table
def 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 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 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.directory_name_p == ''): filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv") else: filepath_p = os.path.join( self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv")) if self.logging_p == False: self.log_file = None if self.logging_p: if self.log_file is None: self.log_file = sopen(filepath_p, 'w') self.log_file.write( "pc_time,gps_time,tow(msec),latitude(degrees),longitude(degrees),altitude(meters)," "h_accuracy(meters),v_accuracy(meters),n_sats,flags\n") log_str_gps = "" if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.lat, soln.lon, soln.height, soln.h_accuracy, soln.v_accuracy, soln.n_sats, soln.flags)) self.log_file.flush() if self.last_pos_mode == 0: pos_table.append(('GPS 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(tgps, float(secgps)))) pos_table.append(('GPS Week', str(self.week))) pos_table.append(('GPS TOW', "{:.3f}".format(tow))) pos_table.append(('Num. Sats', soln.n_sats)) pos_table.append(('Lat', soln.lat)) pos_table.append(('Lng', soln.lon)) pos_table.append(('Height', soln.height)) pos_table.append(('h_accuracy', soln.h_accuracy)) pos_table.append(('v_accuracy', soln.v_accuracy)) pos_table.append(('Pos Flags', '0x%03x' % soln.flags)) pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode])) self.auto_survey() # setup_plot variables self.lats[1:] = self.lats[:-1] self.lngs[1:] = self.lngs[:-1] self.alts[1:] = self.alts[:-1] self.tows[1:] = self.tows[:-1] self.modes[1:] = self.modes[:-1] self.lats[0] = soln.lat self.lngs[0] = soln.lon self.alts[0] = soln.height self.tows[0] = soln.tow self.modes[0] = self.last_pos_mode self.lats = self.lats[-self.plot_history_max:] self.lngs = self.lngs[-self.plot_history_max:] self.alts = self.alts[-self.plot_history_max:] self.tows = self.tows[-self.plot_history_max:] self.modes = self.modes[-self.plot_history_max:] # SPP spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None if np.any(self.modes): spp_indexer = (self.modes == SPP_MODE) dgnss_indexer = (self.modes == DGNSS_MODE) float_indexer = (self.modes == FLOAT_MODE) fixed_indexer = (self.modes == FIXED_MODE) # make sure that there is at least one true in indexer before setting if any(spp_indexer): self.plot_data.set_data('lat_spp', self.lats[spp_indexer]) self.plot_data.set_data('lng_spp', self.lngs[spp_indexer]) self.plot_data.set_data('alt_spp', self.alts[spp_indexer]) if any(dgnss_indexer): self.plot_data.set_data('lat_dgnss', self.lats[dgnss_indexer]) self.plot_data.set_data('lng_dgnss', self.lngs[dgnss_indexer]) self.plot_data.set_data('alt_dgnss', self.alts[dgnss_indexer]) if any(float_indexer): self.plot_data.set_data('lat_float', self.lats[float_indexer]) self.plot_data.set_data('lng_float', self.lngs[float_indexer]) self.plot_data.set_data('alt_float', self.alts[float_indexer]) if any(fixed_indexer): self.plot_data.set_data('lat_fixed', self.lats[fixed_indexer]) self.plot_data.set_data('lng_fixed', self.lngs[fixed_indexer]) self.plot_data.set_data('alt_fixed', self.alts[fixed_indexer]) # update our "current solution" icon if self.last_pos_mode == SPP_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_spp', [soln.lat]) self.plot_data.set_data('cur_lng_spp', [soln.lon]) elif self.last_pos_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_dgnss', [soln.lat]) self.plot_data.set_data('cur_lng_dgnss', [soln.lon]) elif self.last_pos_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_float', [soln.lat]) self.plot_data.set_data('cur_lng_float', [soln.lon]) elif self.last_pos_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_fixed', [soln.lat]) self.plot_data.set_data('cur_lng_fixed', [soln.lon]) else: pass # set-up table variables self.pos_table = pos_table self.table = self.pos_table + self.vel_table + self.dops_table # TODO: figure out how to center the graph now that we have two separate messages # when we selectively send only SPP, the centering function won't work anymore if not self.zoomall and self.position_centered: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds(soln.lon - d, soln.lon + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds(soln.lat - d, soln.lat + d) if self.zoomall: plot_square_axes( self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed'), ('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed'))
def pos_llh_callback(self, sbp_msg, **metadata): if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A: soln = MsgPosLLHDepA(sbp_msg) else: soln = MsgPosLLH(sbp_msg) self.last_soln = soln self.last_pos_mode = get_mode(soln) pos_table = [] soln.h_accuracy *= 1e-3 soln.v_accuracy *= 1e-3 tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 # Return the best estimate of my local and receiver time in convenient # format that allows changing precision of the seconds ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time: ((tutc, secutc)) = datetime_2_str(self.utc_time) if (self.directory_name_p == ''): filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv") else: filepath_p = os.path.join( self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv")) if self.logging_p == False: self.log_file = None if self.logging_p: if self.log_file is None: self.log_file = sopen(filepath_p, 'w') self.log_file.write( "pc_time,gps_time,tow(msec),latitude(degrees),longitude(degrees),altitude(meters)," "h_accuracy(meters),v_accuracy(meters),n_sats,flags\n") log_str_gps = "" if tgps != "" and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.lat, soln.lon, soln.height, soln.h_accuracy, soln.v_accuracy, soln.n_sats, soln.flags)) self.log_file.flush() if self.last_pos_mode == 0: pos_table.append(('GPS Week', EMPTY_STR)) pos_table.append(('GPS TOW', EMPTY_STR)) pos_table.append(('GPS Time', EMPTY_STR)) pos_table.append(('Num. Signals', EMPTY_STR)) pos_table.append(('Lat', EMPTY_STR)) pos_table.append(('Lng', EMPTY_STR)) pos_table.append(('Height', EMPTY_STR)) pos_table.append(('Horiz Acc', EMPTY_STR)) pos_table.append(('Vert Acc', EMPTY_STR)) else: self.last_stime_update = time.time() if self.week is not None: pos_table.append(('GPS Week', str(self.week))) pos_table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: pos_table.append( ('GPS Time', "{0}:{1:06.3f}".format(tgps, float(secgps)))) if self.utc_time is not None: pos_table.append( ('UTC Time', "{0}:{1:06.3f}".format(tutc, float(secutc)))) pos_table.append(('UTC Src', self.utc_source)) if self.utc_time is None: pos_table.append(('UTC Time', EMPTY_STR)) pos_table.append(('UTC Src', EMPTY_STR)) pos_table.append(('Sats Used', soln.n_sats)) pos_table.append(('Lat', soln.lat)) pos_table.append(('Lng', soln.lon)) pos_table.append(('Height', soln.height)) pos_table.append(('Horiz Acc', soln.h_accuracy)) pos_table.append(('Vert Acc', soln.v_accuracy)) pos_table.append(('Pos Flags', '0x%03x' % soln.flags)) pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode])) if self.age_corrections != None: pos_table.append(('Corr. Age [s]', self.age_corrections)) self.auto_survey() # 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 pos_llh_callback(self, sbp_msg, **metadata): if sbp_msg.msg_type == SBP_MSG_POS_LLH_DEP_A: soln = MsgPosLLHDepA(sbp_msg) else: soln = MsgPosLLH(sbp_msg) self.last_soln = soln self.last_pos_mode = get_mode(soln) pos_table = [] soln.h_accuracy *= 1e-3 soln.v_accuracy *= 1e-3 tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 if self.week is not None: t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.week) + \ datetime.timedelta(seconds=tow) tstr = t.strftime('%Y-%m-%d %H:%M') secs = t.strftime('%S.%f') if(self.directory_name_p == ''): filepath_p = time.strftime("position_log_%Y%m%d-%H%M%S.csv") else: filepath_p = os.path.join(self.directory_name_p, time.strftime("position_log_%Y%m%d-%H%M%S.csv")) if self.logging_p == False: self.log_file = None if self.logging_p: if self.log_file is None: self.log_file = sopen(filepath_p, 'w') self.log_file.write("time,latitude(degrees),longitude(degrees),altitude(meters)," "h_accuracy(meters),v_accuracy(meters),n_sats,flags\n") self.log_file.write('%s,%.10f,%.10f,%.4f,%.4f,%.4f,%d,%d\n' % ( "{0}:{1:06.6f}".format(tstr, float(secs)), soln.lat, soln.lon, soln.height, soln.h_accuracy, soln.v_accuracy, soln.n_sats, soln.flags) ) self.log_file.flush() if self.last_pos_mode == 0: pos_table.append(('GPS Time', EMPTY_STR)) pos_table.append(('GPS Week', EMPTY_STR)) pos_table.append(('GPS TOW', EMPTY_STR)) pos_table.append(('Num. Signals', EMPTY_STR)) pos_table.append(('Lat', EMPTY_STR)) pos_table.append(('Lng', EMPTY_STR)) pos_table.append(('Height', EMPTY_STR)) pos_table.append(('h_accuracy', EMPTY_STR)) pos_table.append(('v_accuracy', EMPTY_STR)) else: self.last_stime_update = time.time() if self.week is not None: pos_table.append(('GPS Time', "{0}:{1:06.3f}".format(tstr, float(secs)))) pos_table.append(('GPS Week', str(self.week))) pos_table.append(('GPS TOW', "{:.3f}".format(tow))) pos_table.append(('Num. Sats', soln.n_sats)) pos_table.append(('Lat', soln.lat)) pos_table.append(('Lng', soln.lon)) pos_table.append(('Height', soln.height)) pos_table.append(('h_accuracy', soln.h_accuracy)) pos_table.append(('v_accuracy', soln.v_accuracy)) pos_table.append(('Pos Flags', '0x%03x' % soln.flags)) pos_table.append(('Pos Fix Mode', mode_dict[self.last_pos_mode])) self.auto_survey() # setup_plot variables self.lats[1:] = self.lats[:-1] self.lngs[1:] = self.lngs[:-1] self.alts[1:] = self.alts[:-1] self.tows[1:] = self.tows[:-1] self.modes[1:] = self.modes[:-1] self.lats[0] = soln.lat self.lngs[0] = soln.lon self.alts[0] = soln.height self.tows[0] = soln.tow self.modes[0] = self.last_pos_mode self.lats = self.lats[-self.plot_history_max:] self.lngs = self.lngs[-self.plot_history_max:] self.alts = self.alts[-self.plot_history_max:] self.tows = self.tows[-self.plot_history_max:] self.modes = self.modes[-self.plot_history_max:] # SPP spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None if np.any(self.modes): spp_indexer = (self.modes == SPP_MODE) dgnss_indexer = (self.modes == DGNSS_MODE) float_indexer = (self.modes == FLOAT_MODE) fixed_indexer = (self.modes == FIXED_MODE) # make sure that there is at least one true in indexer before setting if any(spp_indexer): self.plot_data.set_data('lat_spp', self.lats[spp_indexer]) self.plot_data.set_data('lng_spp', self.lngs[spp_indexer]) self.plot_data.set_data('alt_spp', self.alts[spp_indexer]) if any(dgnss_indexer): self.plot_data.set_data('lat_dgnss', self.lats[dgnss_indexer]) self.plot_data.set_data('lng_dgnss', self.lngs[dgnss_indexer]) self.plot_data.set_data('alt_dgnss', self.alts[dgnss_indexer]) if any(float_indexer): self.plot_data.set_data('lat_float', self.lats[float_indexer]) self.plot_data.set_data('lng_float', self.lngs[float_indexer]) self.plot_data.set_data('alt_float', self.alts[float_indexer]) if any(fixed_indexer): self.plot_data.set_data('lat_fixed', self.lats[fixed_indexer]) self.plot_data.set_data('lng_fixed', self.lngs[fixed_indexer]) self.plot_data.set_data('alt_fixed', self.alts[fixed_indexer]) # update our "current solution" icon if self.last_pos_mode == SPP_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_spp', [soln.lat]) self.plot_data.set_data('cur_lng_spp', [soln.lon]) elif self.last_pos_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_dgnss', [soln.lat]) self.plot_data.set_data('cur_lng_dgnss', [soln.lon]) elif self.last_pos_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_float', [soln.lat]) self.plot_data.set_data('cur_lng_float', [soln.lon]) elif self.last_pos_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_fixed', [soln.lat]) self.plot_data.set_data('cur_lng_fixed', [soln.lon]) else: pass # set-up table variables self.pos_table = pos_table self.table = self.pos_table + self.vel_table + self.dops_table # TODO: figure out how to center the graph now that we have two separate messages # when we selectively send only SPP, the centering function won't work anymore if not self.zoomall and self.position_centered: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds(soln.lon - d, soln.lon + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds(soln.lat - d, soln.lat + d) if self.zoomall: plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float','lng_fixed'), ('lat_spp', 'lat_dgnss', 'lat_float','lat_fixed'))
def baseline_callback(self, sbp_msg): soln = MsgBaselineNEDDepA(sbp_msg) self.last_soln = soln table = [] soln.n = soln.n * 1e-3 soln.e = soln.e * 1e-3 soln.d = soln.d * 1e-3 soln.h_accuracy = soln.h_accuracy * 1e-3 soln.v_accuracy = soln.v_accuracy * 1e-3 dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2) tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 if self.week is not None: t = datetime.datetime(1980, 1, 6) + \ datetime.timedelta(weeks=self.week) + \ datetime.timedelta(seconds=tow) tstr = t.strftime('%Y-%m-%d %H:%M') secs = t.strftime('%S.%f') if self.directory_name_b == '': filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv") else: filepath = os.path.join(self.directory_name_b, time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")) if self.logging_b == False: self.log_file = None if self.logging_b: if self.log_file is None: self.log_file = sopen(filepath, 'w') self.log_file.write('time,north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),' 'distance(meters),num_sats,flags,num_hypothesis\n') self.log_file.write('%s,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' % ( "{0}:{1:06.6f}".format(tstr, float(secs)), soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy, dist, soln.n_sats, soln.flags, self.num_hyps) ) self.log_file.flush() self.last_mode = get_mode(soln) if self.last_mode < 1: table.append(('GPS Time', EMPTY_STR)) table.append(('GPS Week', EMPTY_STR)) table.append(('GPS TOW', EMPTY_STR)) table.append(('N', EMPTY_STR)) table.append(('E', EMPTY_STR)) table.append(('D', EMPTY_STR)) table.append(('h_accuracy', EMPTY_STR)) table.append(('v_accuracy', EMPTY_STR)) table.append(('Dist.', EMPTY_STR)) table.append(('Num. Sats', EMPTY_STR)) table.append(('Flags', EMPTY_STR)) table.append(('Mode', EMPTY_STR)) else: self.last_btime_update = time.time() if self.week is not None: table.append(('GPS Time', "{0}:{1:06.3f}".format(tstr, float(secs)))) table.append(('GPS Week', str(self.week))) table.append(('GPS TOW', "{:.3f}".format(tow))) table.append(('N', soln.n)) table.append(('E', soln.e)) table.append(('D', soln.d)) table.append(('h_accuracy', soln.h_accuracy)) table.append(('v_accuracy', soln.v_accuracy)) table.append(('Dist.', dist)) table.append(('Num. Sats', soln.n_sats)) table.append(('Flags', '0x%02x' % soln.flags)) table.append(('Mode', mode_dict[self.last_mode])) self.table = table # Rotate array, deleting oldest entries to maintain # no more than N in plot self.n[1:] = self.n[:-1] self.e[1:] = self.e[:-1] self.d[1:] = self.d[:-1] self.mode[1:] = self.mode[:-1] # Insert latest position if self.last_mode > 1: self.n[0], self.e[0], self.d[0] = soln.n, soln.e, soln.d else: self.n[0], self.e[0], self.d[0] = [np.NAN, np.NAN, np.NAN] self.mode[0] = self.last_mode if np.any(self.mode): float_indexer = (self.mode == FLOAT_MODE) fixed_indexer = (self.mode == FIXED_MODE) dgnss_indexer = (self.mode == DGNSS_MODE) if np.any(fixed_indexer): self.plot_data.set_data('n_fixed', self.n[fixed_indexer]) self.plot_data.set_data('e_fixed', self.e[fixed_indexer]) self.plot_data.set_data('d_fixed', self.d[fixed_indexer]) if np.any(float_indexer): self.plot_data.set_data('n_float', self.n[float_indexer]) self.plot_data.set_data('e_float', self.e[float_indexer]) self.plot_data.set_data('d_float', self.d[float_indexer]) if np.any(dgnss_indexer): self.plot_data.set_data('n_dgnss', self.n[dgnss_indexer]) self.plot_data.set_data('e_dgnss', self.e[dgnss_indexer]) self.plot_data.set_data('d_dgnss', self.d[dgnss_indexer]) # Update our last solution icon if self.last_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.set_data('cur_fixed_n', [soln.n]) self.plot_data.set_data('cur_fixed_e', [soln.e]) self.plot_data.set_data('cur_fixed_d', [soln.d]) elif self.last_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.set_data('cur_float_n', [soln.n]) self.plot_data.set_data('cur_float_e', [soln.e]) self.plot_data.set_data('cur_float_d', [soln.d]) elif self.last_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.set_data('cur_dgnss_n', [soln.n]) self.plot_data.set_data('cur_dgnss_e', [soln.e]) self.plot_data.set_data('cur_dgnss_d', [soln.d]) else: pass # make the zoomall win over the position centered button # position centered button has no effect when zoom all enabled if not self.zoomall and self.position_centered: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds(soln.e - d, soln.e + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds(soln.n - d, soln.n + d) if self.zoomall: plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'), ('n_fixed', 'n_float', 'n_dgnss'))