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 baseline_callback(self, sbp_msg, **metadata): soln = MsgBaselineNEDDepA(sbp_msg) table = [] soln.n = soln.n * 1e-3 soln.e = soln.e * 1e-3 soln.d = soln.d * 1e-3 soln.h_accuracy = soln.h_accuracy * 1e-3 soln.v_accuracy = soln.v_accuracy * 1e-3 dist = np.sqrt(soln.n**2 + soln.e**2 + soln.d**2) tow = soln.tow * 1e-3 if self.nsec is not None: tow += self.nsec * 1e-9 ((tloc, secloc), (tgps, secgps)) = log_time_strings(self.week, tow) if self.utc_time is not None: ((tutc, secutc)) = datetime_2_str(self.utc_time) if self.directory_name_b == '': filepath = time.strftime("baseline_log_%Y%m%d-%H%M%S.csv") else: filepath = os.path.join( self.directory_name_b, time.strftime("baseline_log_%Y%m%d-%H%M%S.csv")) if not self.logging_b: self.log_file = None if self.logging_b: if self.log_file is None: self.log_file = sopen(filepath, 'w') self.log_file.write( 'pc_time,gps_time,tow(sec),north(meters),east(meters),down(meters),h_accuracy(meters),v_accuracy(meters),' 'distance(meters),num_sats,flags,num_hypothesis\n') log_str_gps = '' if tgps != '' and secgps != 0: log_str_gps = "{0}:{1:06.6f}".format(tgps, float(secgps)) self.log_file.write( '%s,%s,%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%d\n' % ("{0}:{1:06.6f}".format(tloc, float(secloc)), log_str_gps, tow, soln.n, soln.e, soln.d, soln.h_accuracy, soln.v_accuracy, dist, soln.n_sats, soln.flags, self.num_hyps)) self.log_file.flush() self.last_mode = get_mode(soln) if self.last_mode < 1: table.append(('GPS Week', EMPTY_STR)) table.append(('GPS TOW', EMPTY_STR)) table.append(('GPS Time', EMPTY_STR)) table.append(('UTC Time', EMPTY_STR)) table.append(('UTC Src', EMPTY_STR)) table.append(('N', EMPTY_STR)) table.append(('E', EMPTY_STR)) table.append(('D', EMPTY_STR)) table.append(('Horiz Acc', EMPTY_STR)) table.append(('Vert Acc', EMPTY_STR)) table.append(('Dist.', EMPTY_STR)) table.append(('Sats Used', EMPTY_STR)) table.append(('Flags', EMPTY_STR)) table.append(('Mode', EMPTY_STR)) table.append(('Heading', EMPTY_STR)) table.append(('Corr. Age [s]', EMPTY_STR)) else: self.last_btime_update = time.time() if self.week is not None: table.append(('GPS Week', str(self.week))) table.append(('GPS TOW', "{:.3f}".format(tow))) if self.week is not None: table.append(('GPS Time', "{0}:{1:06.3f}".format( tgps, float(secgps)))) if self.utc_time is not None: table.append(('UTC Time', "{0}:{1:06.3f}".format( tutc, float(secutc)))) table.append(('UTC Src', self.utc_source)) table.append(('N', soln.n)) table.append(('E', soln.e)) table.append(('D', soln.d)) table.append(('Horiz Acc', soln.h_accuracy)) table.append(('Vert Acc', soln.v_accuracy)) table.append(('Dist.', "{0:.3f}".format(dist))) table.append(('Sats Used', soln.n_sats)) table.append(('Flags', '0x%02x' % soln.flags)) table.append(('Mode', mode_dict[self.last_mode])) if self.heading is not None: table.append(('Heading', self.heading)) if self.age_corrections is not None: table.append(('Corr. Age [s]', self.age_corrections)) else: table.append(('Corr. Age [s]', EMPTY_STR)) self.table = table if self.last_mode != 0: self.last_soln = soln mode_string = mode_string_dict[self.last_mode] if mode_string not in self.pending_draw_modes: # if we don't already have a pending upate for that mode self.pending_draw_modes.append(mode_string) self.list_lock.acquire() self._update_sln_data_by_mode(soln, mode_string) self.list_lock.release() else: self.list_lock.acquire() self._append_empty_sln_data(soln) self.list_lock.release() if time.time() - self.last_plot_update_time > GUI_UPDATE_PERIOD: GUI.invoke_later(self._solution_draw)
def 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