def _solution_draw(self): self.list_lock.acquire() current_time = time.time() self.last_plot_update_time = current_time pending_draw_modes = self.pending_draw_modes current_mode = pending_draw_modes[-1] if len(pending_draw_modes) > 0 else None # Periodically, we make sure to redraw older data to expire old plot data if current_time - self.last_stale_update_time > STALE_DATA_PERIOD: # we don't update old solution modes every timestep to try and save CPU pending_draw_modes = mode_string_dict.values() self.last_stale_update_time = current_time for mode_string in pending_draw_modes: if self.running: update_current = mode_string == current_mode if current_mode else True self._synchronize_plot_data_by_mode(mode_string, update_current) if mode_string in self.pending_draw_modes: self.pending_draw_modes.remove(mode_string) self.list_lock.release() # make the zoomall win over the position centered button if not self.zoomall and self.position_centered and self.running: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds(self.last_soln.e - d, self.last_soln.e + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds(self.last_soln.n - d, self.last_soln.n + d) if self.zoomall: plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'), ('n_fixed', 'n_float', 'n_dgnss'))
def _solution_draw(self): self.list_lock.acquire() current_time = monotonic() self.last_plot_update_time = current_time pending_draw_modes = self.pending_draw_modes current_mode = pending_draw_modes[-1] if len(pending_draw_modes) > 0 else None # Periodically, we make sure to redraw older data to expire old plot data if current_time - self.last_stale_update_time > STALE_DATA_PERIOD: # we don't update old solution modes every timestep to try and save CPU pending_draw_modes = list(mode_string_dict.values()) self.last_stale_update_time = current_time for mode_string in pending_draw_modes: if self.running: update_current = mode_string == current_mode if current_mode else True self._synchronize_plot_data_by_mode(mode_string, update_current) if mode_string in self.pending_draw_modes: self.pending_draw_modes.remove(mode_string) self.list_lock.release() # make the zoomall win over the position centered button if not self.zoomall and self.position_centered and self.running: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds(self.last_soln.e - d, self.last_soln.e + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds(self.last_soln.n - d, self.last_soln.n + d) if self.zoomall: plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'), ('n_fixed', 'n_float', 'n_dgnss'))
def _solution_draw(self): self.last_plot_update_time = time.time() self.list_lock.acquire() # update our "current solution" icon for mode_string in list(self.pending_draw_modes): self._synchronize_plot_data_by_mode(mode_string) self.pending_draw_modes.remove(mode_string) self.list_lock.release() if not self.zoomall and self.position_centered: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds( (self.last_soln.lon - self.offset[1]) * self.sf[1] - d, (self.last_soln.lon - self.offset[1]) * self.sf[1] + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds( (self.last_soln.lat - self.offset[0]) * self.sf[0] - d, (self.last_soln.lat - self.offset[0]) * self.sf[0] + d) if self.zoomall: self.recenter = False plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed', 'lng_sbas', 'lng_dr'), ('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed', 'lat_sbas', 'lat_dr')) if self.recenter: try: self.rescale_for_units_change() self.recenter = False except AttributeError: pass
def _solution_draw(self): spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None self._clear_history() soln = self.last_soln 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 # 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 _solution_draw(self): self._clear_history() soln = self.last_soln self.last_plot_update_time = time.time() if np.any(self.mode): float_indexer = (self.mode == FLOAT_MODE) fixed_indexer = (self.mode == FIXED_MODE) dgnss_indexer = (self.mode == DGNSS_MODE) if np.any(fixed_indexer): self.plot_data.set_data('n_fixed', self.n[fixed_indexer]) self.plot_data.set_data('e_fixed', self.e[fixed_indexer]) self.plot_data.set_data('d_fixed', self.d[fixed_indexer]) if np.any(float_indexer): self.plot_data.set_data('n_float', self.n[float_indexer]) self.plot_data.set_data('e_float', self.e[float_indexer]) self.plot_data.set_data('d_float', self.d[float_indexer]) if np.any(dgnss_indexer): self.plot_data.set_data('n_dgnss', self.n[dgnss_indexer]) self.plot_data.set_data('e_dgnss', self.e[dgnss_indexer]) self.plot_data.set_data('d_dgnss', self.d[dgnss_indexer]) # Update our last solution icon if self.last_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.set_data('cur_fixed_n', [soln.n]) self.plot_data.set_data('cur_fixed_e', [soln.e]) self.plot_data.set_data('cur_fixed_d', [soln.d]) elif self.last_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.set_data('cur_float_n', [soln.n]) self.plot_data.set_data('cur_float_e', [soln.e]) self.plot_data.set_data('cur_float_d', [soln.d]) elif self.last_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.set_data('cur_dgnss_n', [soln.n]) self.plot_data.set_data('cur_dgnss_e', [soln.e]) self.plot_data.set_data('cur_dgnss_d', [soln.d]) else: pass # make the zoomall win over the position centered button # position centered button has no effect when zoom all enabled if not self.zoomall and self.position_centered: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds(soln.e - d, soln.e + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds(soln.n - d, soln.n + d) if self.zoomall: plot_square_axes(self.plot, ('e_fixed', 'e_float', 'e_dgnss'), ('n_fixed', 'n_float', 'n_dgnss'))
def _solution_draw(self): self._clear_history() soln = self.last_soln 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 _solution_draw(self): self.last_plot_update_time = time.time() soln = self.last_soln # update our "current solution" icon for mode_string in list(self.pending_draw_modes): self._synchronize_plot_data_by_mode(mode_string) self.pending_draw_modes.remove(mode_string) # make the zoomall win over the position centered button 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 _solution_draw(self): self.list_lock.acquire() current_time = monotonic() self.last_plot_update_time = current_time pending_draw_modes = self.pending_draw_modes current_mode = pending_draw_modes[-1] if len(pending_draw_modes) > 0 else None # Periodically, we make sure to redraw older data to expire old plot data if current_time - self.last_stale_update_time > STALE_DATA_PERIOD: # we don't update old solution modes every timestep to try and save CPU pending_draw_modes = list(mode_string_dict.values()) self.last_stale_update_time = current_time for mode_string in pending_draw_modes: if self.running: update_current = mode_string == current_mode if current_mode else True self._synchronize_plot_data_by_mode(mode_string, update_current=update_current) if mode_string in self.pending_draw_modes: self.pending_draw_modes.remove(mode_string) self.list_lock.release() if not self.zoomall and self.position_centered and self.running: d = ( self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds( (self.last_soln.lon - self.offset[1]) * self.sf[1] - d, (self.last_soln.lon - self.offset[1]) * self.sf[1] + d) d = ( self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds( (self.last_soln.lat - self.offset[0]) * self.sf[0] - d, (self.last_soln.lat - self.offset[0]) * self.sf[0] + d) if self.zoomall: self.recenter = False plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed', 'lng_sbas', 'lng_dr'), ('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed', 'lat_sbas', 'lat_dr')) if self.recenter: try: self.rescale_for_units_change() self.recenter = False except AttributeError: pass
def _solution_draw(self): self.list_lock.acquire() current_time = monotonic() self.last_plot_update_time = current_time pending_draw_modes = self.pending_draw_modes current_mode = pending_draw_modes[-1] if len(pending_draw_modes) > 0 else None # Periodically, we make sure to redraw older data to expire old plot data if current_time - self.last_stale_update_time > STALE_DATA_PERIOD: # we don't update old solution modes every timestep to try and save CPU pending_draw_modes = list(mode_string_dict.values()) self.last_stale_update_time = current_time for mode_string in pending_draw_modes: if self.running: update_current = mode_string == current_mode if current_mode else True self._synchronize_plot_data_by_mode(mode_string, update_current=update_current) if mode_string in self.pending_draw_modes: self.pending_draw_modes.remove(mode_string) self.list_lock.release() if not self.zoomall and self.position_centered and self.running: d = ( self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds( (self.last_soln.lon - self.offset[1]) * self.sf[1] - d, (self.last_soln.lon - self.offset[1]) * self.sf[1] + d) d = ( self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds( (self.last_soln.lat - self.offset[0]) * self.sf[0] - d, (self.last_soln.lat - self.offset[0]) * self.sf[0] + d) if self.zoomall: self.recenter = False plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed', 'lng_sbas', 'lng_dr'), ('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed', 'lat_sbas', 'lat_dr')) if self.recenter: try: self.rescale_for_units_change() self.recenter = False except AttributeError: pass
def _solution_draw(self): spp_indexer, dgnss_indexer, float_indexer, fixed_indexer, sbas_indexer, dr_indexer = None, None, None, None, None, None soln = self.last_soln if np.any(self.modes): self.list_lock.acquire() spp_indexer = (self.modes == SPP_MODE) dgnss_indexer = (self.modes == DGNSS_MODE) sbas_indexer = (self.modes == SBAS_MODE) float_indexer = (self.modes == FLOAT_MODE) fixed_indexer = (self.modes == FIXED_MODE) dr_indexer = (self.modes == DR_MODE) # make sure that there is at least one true in indexer before setting if any(spp_indexer): self.plot_data.update_data({'lat_spp': (self.lats[spp_indexer] - self.offset[0]) * self.sf[0], 'lng_spp': (self.lngs[spp_indexer] - self.offset[1]) * self.sf[1], 'alt_spp': (self.alts[spp_indexer] - self.offset[2])}) else: self.plot_data.update_data({'lat_spp': [], 'lng_spp': [], 'alt_spp': []}) if any(dgnss_indexer): self.plot_data.update_data({'lat_dgnss': (self.lats[dgnss_indexer] - self.offset[0]) * self.sf[0], 'lng_dgnss': (self.lngs[dgnss_indexer] - self.offset[1]) * self.sf[1], 'alt_dgnss': (self.alts[dgnss_indexer] - self.offset[2])}) else: self.plot_data.update_data({'lat_dgnss': [], 'lng_dgnss': [], 'alt_dgnss': []}) if any(float_indexer): self.plot_data.update_data({'lat_float': (self.lats[float_indexer] - self.offset[0]) * self.sf[0], 'lng_float': (self.lngs[float_indexer] - self.offset[1]) * self.sf[1], 'alt_float': (self.alts[float_indexer] - self.offset[2])}) else: self.plot_data.update_data({'lat_float': [], 'lng_float': [], 'alt_float': []}) if any(fixed_indexer): self.plot_data.update_data({'lat_fixed': (self.lats[fixed_indexer] - self.offset[0]) * self.sf[0], 'lng_fixed': (self.lngs[fixed_indexer] - self.offset[1]) * self.sf[1], 'alt_fixed': (self.alts[fixed_indexer] - self.offset[2])}) else: self.plot_data.update_data({'lat_fixed': [], 'lng_fixed': [], 'alt_fixed': []}) if any(sbas_indexer): self.plot_data.update_data({'lat_sbas': (self.lats[sbas_indexer] - self.offset[0]) * self.sf[0], 'lng_sbas': (self.lngs[sbas_indexer] - self.offset[1]) * self.sf[1], 'alt_sbas': (self.alts[sbas_indexer] - self.offset[2])}) else: self.plot_data.update_data({'lat_sbas': [], 'lng_sbas': [], 'alt_sbas': []}) if any(dr_indexer): self.plot_data.update_data({'lat_dr': (self.lats[dr_indexer] - self.offset[0]) * self.sf[0], 'lng_dr': (self.lngs[dr_indexer] - self.offset[1]) * self.sf[1], 'alt_dr': (self.alts[dr_indexer] - self.offset[2])}) else: self.plot_data.update_data({'lat_dr': [], 'lng_dr': [], 'alt_dr': []}) self.list_lock.release() # update our "current solution" icon if self.last_pos_mode == SPP_MODE: self._reset_remove_current() self.plot_data.update_data( {'cur_lat_spp': [(soln.lat - self.offset[0]) * self.sf[0]], 'cur_lng_spp': [(soln.lon - self.offset[1]) * self.sf[1]]}) elif self.last_pos_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.update_data({'cur_lat_dgnss': [(soln.lat - self.offset[0]) * self.sf[0]], 'cur_lng_dgnss': [(soln.lon - self.offset[1]) * self.sf[1]]}) elif self.last_pos_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.update_data({'cur_lat_float': [(soln.lat - self.offset[0]) * self.sf[0]], 'cur_lng_float': [(soln.lon - self.offset[1]) * self.sf[1]]}) elif self.last_pos_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.update_data({'cur_lat_fixed': [(soln.lat - self.offset[0]) * self.sf[0]], 'cur_lng_fixed': [(soln.lon - self.offset[1]) * self.sf[1]]}) elif self.last_pos_mode == SBAS_MODE: self._reset_remove_current() self.plot_data.update_data({'cur_lat_sbas': [(soln.lat - self.offset[0]) * self.sf[0]], 'cur_lng_sbas': [(soln.lon - self.offset[1]) * self.sf[1]]}) elif self.last_pos_mode == DR_MODE: self._reset_remove_current() self.plot_data.update_data({'cur_lat_dr': [(soln.lat - self.offset[0]) * self.sf[0]], 'cur_lng_dr': [(soln.lon - self.offset[1]) * self.sf[1]]}) else: pass if not self.zoomall and self.position_centered: d = ( self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds( (soln.lon - self.offset[1]) * self.sf[1] - d, (soln.lon - self.offset[1]) * self.sf[1] + d) d = ( self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds( (soln.lat - self.offset[0]) * self.sf[0] - d, (soln.lat - self.offset[0]) * self.sf[0] + d) if self.zoomall: self.recenter = False plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed', 'lng_sbas', 'lng_dr'), ('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed', 'lat_sbas', 'lat_dr')) if self.recenter: try: self.rescale_for_units_change() self.recenter = False except AttributeError: pass
def _solution_draw(self): spp_indexer, dgnss_indexer, float_indexer, fixed_indexer = None, None, None, None self._clear_history() soln = self.last_soln 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 # 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 ((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 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 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.', 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 != None: table.append(('Heading', self.heading)) if self.age_corrections != 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 self._clear_history() 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.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() # 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 self._clear_history() 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 _solution_draw(self): soln = self.last_soln if np.any(self.mode): self.list_lock.acquire() 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.update_data({ 'n_fixed': self.n[fixed_indexer], 'e_fixed': self.e[fixed_indexer], 'd_fixed': self.d[fixed_indexer] }) else: self.plot_data.update_data({ 'n_fixed': [], 'e_fixed': [], 'd_fixed': [] }) if np.any(float_indexer): self.plot_data.update_data({ 'n_float': self.n[float_indexer], 'e_float': self.e[float_indexer], 'd_float': self.d[float_indexer] }) else: self.plot_data.update_data({ 'n_float': [], 'e_float': [], 'd_float': [] }) if np.any(dgnss_indexer): self.plot_data.update_data({ 'n_dgnss': self.n[dgnss_indexer], 'e_dgnss': self.e[dgnss_indexer], 'd_dgnss': self.d[dgnss_indexer] }) else: self.plot_data.update_data({ 'n_dgnss': [], 'e_dgnss': [], 'd_dgnss': [] }) self.list_lock.release() # Update our last solution icon if self.last_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.update_data({ 'cur_fixed_n': [soln.n], 'cur_fixed_e': [soln.e], 'cur_fixed_d': [soln.d] }) elif self.last_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.update_data({ 'cur_float_n': [soln.n], 'cur_float_e': [soln.e], 'cur_float_d': [soln.d] }) elif self.last_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.update_data({ 'cur_dgnss_n': [soln.n], 'cur_dgnss_e': [soln.e], '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 _solution_draw(self): spp_indexer, dgnss_indexer, float_indexer, fixed_indexer, sbas_indexer = None, None, None, None, None self._clear_history() soln = self.last_soln if np.any(self.modes): if self.display_units == "meters": offset = (np.mean(self.lats[~(np.equal(self.modes, 0))]), np.mean(self.lngs[~(np.equal(self.modes, 0))]), np.mean(self.alts[~(np.equal(self.modes, 0))])) if not self.meters_per_lat: (self.meters_per_lat, self.meters_per_lon) = meters_per_deg(soln.lat) sf = (self.meters_per_lat, self.meters_per_lon) self.plot.value_axis.title = 'Latitude (meters)' self.plot.index_axis.title = 'Longitude (meters)' else: offset = (0, 0, 0) sf = (1, 1) self.plot.value_axis.title = 'Latitude (degrees)' self.plot.index_axis.title = 'Longitude (degrees)' spp_indexer = (self.modes == SPP_MODE) dgnss_indexer = (self.modes == DGNSS_MODE) sbas_indexer = (self.modes == SBAS_MODE) float_indexer = (self.modes == FLOAT_MODE) fixed_indexer = (self.modes == FIXED_MODE) # make sure that there is at least one true in indexer before setting if any(spp_indexer): self.plot_data.set_data( 'lat_spp', (self.lats[spp_indexer] - offset[0]) * sf[0]) self.plot_data.set_data( 'lng_spp', (self.lngs[spp_indexer] - offset[1]) * sf[1]) self.plot_data.set_data('alt_spp', (self.alts[spp_indexer] - offset[2])) if any(dgnss_indexer): self.plot_data.set_data( 'lat_dgnss', (self.lats[dgnss_indexer] - offset[0]) * sf[0]) self.plot_data.set_data( 'lng_dgnss', (self.lngs[dgnss_indexer] - offset[1]) * sf[1]) self.plot_data.set_data('alt_dgnss', (self.alts[dgnss_indexer] - offset[2])) if any(float_indexer): self.plot_data.set_data( 'lat_float', (self.lats[float_indexer] - offset[0]) * sf[0]) self.plot_data.set_data( 'lng_float', (self.lngs[float_indexer] - offset[1]) * sf[1]) self.plot_data.set_data('alt_float', (self.alts[float_indexer] - offset[2])) if any(fixed_indexer): self.plot_data.set_data( 'lat_fixed', (self.lats[fixed_indexer] - offset[0]) * sf[0]) self.plot_data.set_data( 'lng_fixed', (self.lngs[fixed_indexer] - offset[1]) * sf[1]) self.plot_data.set_data('alt_fixed', (self.alts[fixed_indexer] - offset[2])) if any(sbas_indexer): self.plot_data.set_data( 'lat_sbas', (self.lats[sbas_indexer] - offset[0]) * sf[0]) self.plot_data.set_data( 'lng_sbas', (self.lngs[sbas_indexer] - offset[1]) * sf[1]) self.plot_data.set_data('alt_sbas', (self.alts[sbas_indexer] - offset[2])) # update our "current solution" icon if self.last_pos_mode == SPP_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_spp', [(soln.lat - offset[0]) * sf[0]]) self.plot_data.set_data('cur_lng_spp', [(soln.lon - offset[1]) * sf[1]]) elif self.last_pos_mode == DGNSS_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_dgnss', [(soln.lat - offset[0]) * sf[0]]) self.plot_data.set_data('cur_lng_dgnss', [(soln.lon - offset[1]) * sf[1]]) elif self.last_pos_mode == FLOAT_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_float', [(soln.lat - offset[0]) * sf[0]]) self.plot_data.set_data('cur_lng_float', [(soln.lon - offset[1]) * sf[1]]) elif self.last_pos_mode == FIXED_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_fixed', [(soln.lat - offset[0]) * sf[0]]) self.plot_data.set_data('cur_lng_fixed', [(soln.lon - offset[1]) * sf[1]]) elif self.last_pos_mode == SBAS_MODE: self._reset_remove_current() self.plot_data.set_data('cur_lat_sbas', [(soln.lat - offset[0]) * sf[0]]) self.plot_data.set_data('cur_lng_sbas', [(soln.lon - offset[1]) * sf[1]]) else: pass if not self.zoomall and self.position_centered: d = (self.plot.index_range.high - self.plot.index_range.low) / 2. self.plot.index_range.set_bounds( (soln.lon - offset[1]) * sf[1] - d, (soln.lon - offset[1]) * sf[1] + d) d = (self.plot.value_range.high - self.plot.value_range.low) / 2. self.plot.value_range.set_bounds( (soln.lat - offset[0]) * sf[0] - d, (soln.lat - offset[0]) * sf[0] + d) if self.zoomall: plot_square_axes(self.plot, ('lng_spp', 'lng_dgnss', 'lng_float', 'lng_fixed', 'lng_sbas'), ('lat_spp', 'lat_dgnss', 'lat_float', 'lat_fixed', 'lat_sbas'))