class TutRunner(RobotInterface): def __init__(self): self.tut = GPS() self.time_plot = RobotPlot("gps time") self.minute_plot = RobotPlot("gps minutes") self.plotter = LivePlotter(2, self.time_plot, self.minute_plot) super(TutRunner, self).__init__( self.tut, debug_prints=True, log_data=False, ) def loop(self): pass def packet_received(self, timestamp, whoiam, packet): if self.did_receive(self.tut): if self.queue_len() < 25: self.time_plot.append(timestamp, self.tut.second) self.minute_plot.append(timestamp, self.tut.minute) if not self.plotter.plot(): return False def close(self): self.plotter.close() def start(self): self.plotter.start_time(self.start_time)
class LidarTest(Robot): def __init__(self, enable_plotting=True): self.turret = LidarTurret() self.slam = SLAM(self.turret) super(LidarTest, self).__init__(self.turret) self.plotter = LivePlotter( 1, self.turret.point_cloud_plot, enabled=enable_plotting ) self.slam_plots = SlamPlots(self.slam.map_size_pixels, self.slam.map_size_meters, skip_count=0) self.lidar_t0 = 0.0 self.link_object(self.turret, self.received_lidar) def received_lidar(self, timestamp, packet, packet_type): if self.turret.did_cloud_update(): if self.turret.is_cloud_ready(): self.slam.update(timestamp, self.turret.distances, [0, 0, self.dt() - self.lidar_t0]) self.slam_plots.update(self.slam.mapbytes, self.slam.get_pos()) self.lidar_t0 = self.dt() return self.plotter.plot() def close(self, reason): if reason == "done": self.plotter.freeze_plot() self.plotter.close() self.slam.make_image(self.logger.full_path + " post map")
def __init__(self): self.gps_plot = RobotPlot("gps", color="red") self.compass_plot = RobotPlot("compass", color="purple") self.sticky_compass_plot = RobotPlot("sticky compass", color="gray") self.sticky_compass_counter = 0 self.sticky_compass_skip = 100 self.accuracy_check_plot = RobotPlotCollection( "Animation", self.sticky_compass_plot, self.gps_plot, self.compass_plot) self.plotter = LivePlotter( 2, self.accuracy_check_plot, matplotlib_events=dict(key_press_event=self.key_press)) self.gps = AdafruitGPS() self.imu = BNO055() # whoiam IDs were different in the old logs, changing for convenience, please don't do this otherwise!! self.gps.whoiam = "gps" self.imu.whoiam = "imu" # file_name, directory = file_sets["rolls day 3"][0] super(AnimationExample, self).__init__(self.imu, self.gps) self.compass_angle = 0.0 self.start_angle = 0.0 self.link(self.gps, self.receive_gps) self.link(self.imu, self.receive_imu)
def __init__(self): self.tut = GPS() self.time_plot = RobotPlot("gps time") self.minute_plot = RobotPlot("gps minutes") self.plotter = LivePlotter(2, self.time_plot, self.minute_plot) super(TutRunner, self).__init__( self.tut, debug_prints=True, log_data=False, )
def __init__(self, enable_plotting=True): self.turret = LidarTurret() self.slam = SLAM(self.turret) super(LidarTest, self).__init__(self.turret) self.plotter = LivePlotter( 1, self.turret.point_cloud_plot, enabled=enable_plotting ) self.slam_plots = SlamPlots(self.slam.map_size_pixels, self.slam.map_size_meters, skip_count=0) self.lidar_t0 = 0.0 self.link_object(self.turret, self.received_lidar)
def __init__(self, animate, enable_plotting, enable_kalman, course_map, inner_map, outer_map, key_press_fn, map_set_name, label_checkpoints): # GPS map based plots self.gps_plot = RobotPlot("gps", color="red", enabled=True) self.corrected_gps_plot = RobotPlot("corrected gps", color="fuchsia", enabled=True) self.map_plot = RobotPlot("map", color="purple", marker='.', put_in_legend=False) self.inner_map_plot = RobotPlot("inner map", enabled=True, put_in_legend=False) self.outer_map_plot = RobotPlot("outer map", enabled=True, put_in_legend=False) # angle based plots self.compass_plot = RobotPlot("quasar filtered heading", color="purple", put_in_legend=False) self.compass_plot_imu_only = RobotPlot("imu filtered heading", color="magenta", put_in_legend=False) self.sticky_compass_plot = RobotPlot("sticky compass", color="gray", enabled=False, put_in_legend=False) self.steering_plot = RobotPlot("steering angle", color="gray", enabled=False, put_in_legend=False) # indicator dots and lines self.checkpoint_plot = RobotPlot("checkpoint", color="green", marker='.', linestyle='', markersize=8, put_in_legend=False) self.goal_plot = RobotPlot("checkpoint goal", color="cyan", put_in_legend=False) self.recorded_goal_plot = RobotPlot("recorded checkpoint goal", "blue", put_in_legend=False) self.current_pos_dot = RobotPlot("current pos dot", "blue", marker='.', markersize=8, put_in_legend=False) # kalman plots self.enable_kalman = enable_kalman self.kalman_recomputed_plot = RobotPlot("kalman recomputed", color="lawngreen", enabled=self.enable_kalman) self.kalman_recorded_plot = RobotPlot("kalman recorded", color="gray", enabled=self.enable_kalman) self.steering_angle_plot = RobotPlot("steering angle", put_in_legend=False) self.sticky_compass_counter = 0 self.sticky_compass_skip = 100 self.label_checkpoints = label_checkpoints # plot collection self.accuracy_check_plot = RobotPlotCollection( "RoboQuasar plot", self.sticky_compass_plot, self.gps_plot, self.corrected_gps_plot, self.checkpoint_plot, self.map_plot, self.inner_map_plot, self.outer_map_plot, self.steering_plot, self.compass_plot_imu_only, self.compass_plot, self.goal_plot, self.current_pos_dot, self.kalman_recomputed_plot, self.kalman_recorded_plot, self.steering_angle_plot, window_resizing=False) if animate: self.plotter = LivePlotter( 2, self.accuracy_check_plot, # self.pipeline_plots, matplotlib_events=dict(key_press_event=key_press_fn), enable_legend=True, enabled=enable_plotting, skip_count=3) else: self.plotter = StaticPlotter( 1, self.accuracy_check_plot, matplotlib_events=dict(key_press_event=key_press_fn), enabled=enable_plotting) # plot maps self.update_maps(course_map, inner_map, outer_map) self.plot_image(map_set_name) self.position_state = "" self.prev_pos_state = self.position_state
def __init__(self, set_num=-1): self.gps = GPS() self.imu = IMU() self.turret = LidarTurret() self.steering = Steering() self.brakes = Brakes() self.underglow = Underglow() joystick = None if only_lidar: self.gps.enabled = False self.imu.enabled = False self.steering.enabled = False self.brakes.enabled = False self.underglow.enabled = False elif not simulated: joystick = WiiUJoystick() if args.includelidar: self.turret.enabled = False # self.imu_plot = RobotPlot("Magnetometer data (x, z vs. time)", flat_plot=True, skip_count=20, # plot_enabled=False) self.kalman_plots = KalmanPlots(not only_lidar, use_filter, animate) if animate: if args.includelidar: self.turret.point_cloud_plot.enabled = False self.live_plot = LivePlotter(2, self.kalman_plots.filter_comparison, self.turret.point_cloud_plot, skip_count=10) if self.turret.point_cloud_plot.enabled: self.live_plot.draw_dot(self.turret.point_cloud_plot, 0, 0, color='orange', markersize=5) elif plots_enabled: self.static_plot = StaticPlotter( 1, self.kalman_plots.filter_comparison) self.filter = None # m = np.array(MapFile("buggy course checkpoints").map) # self.map_plot.update(m[:, 0], m[:, 1]) file_sets = ( ("16;49", "2017_Feb_17"), # default, latest # data day 4 # ("15", "data_days/2017_Feb_14"), # filter explodes, LIDAR interfered by the sun # ("16;20", "data_days/2017_Feb_14"), # shorten run, LIDAR collapsed # ("16;57", "data_days/2017_Feb_14"), # interfered LIDAR # ("17;10", "data_days/2017_Feb_14"), # all data is fine, interfered LIDAR # ("17;33", "data_days/2017_Feb_14"), # data is fine, normal run # data day 3 # ("16;38", "data_days/2017_Feb_08"), # ("17", "data_days/2017_Feb_08"), # ("18", "data_days/2017_Feb_08"), # trackfield no gyro values # ("15;46", "old_data/2016_Dec_02"), # ("15;54", "old_data/2016_Dec_02"), # ("16;10", "old_data/2016_Dec_02"), # ("16;10", "old_data/2016_Dec_02") # rolls day 1 # ("07;22", "old_data/2016_Nov_06"), # bad gyro values # rolls day 2 # ("07;36;03 m", "old_data/2016_Nov_12"), # ("09;12", "old_data/2016_Nov_12"), # invalid values # ("07;04;57", "old_data/2016_Nov_13"), # bad gyro values # rolls day 3 # ("modified 07;04", "old_data/2016_Nov_13"), # ("modified 07;23", "old_data/2016_Nov_13"), # wonky value for mag. # rolling on the cut # ("16;29", "old_data/2016_Dec_09"), # ("16;49", "old_data/2016_Dec_09"), # ("16;5", "old_data/2016_Dec_09"), # ("17;", "old_data/2016_Dec_09"), # nothing wrong, really short # bad data # ("16;07", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short # ("16;09", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short # ("18;00", "old_data/2016_Dec_09/bad_data"), # gps spazzed out # ("18;02", "old_data/2016_Dec_09/bad_data"), # gps spazzed out # ("18;09", "old_data/2016_Dec_09/bad_data"), # gps spazzed out ) self.checkpoint_num = 0 self.lat1, self.long1, self.alt1 = 0, 0, 0 self.lat2, self.long2, self.alt2 = 0, 0, 0 self.num_recv_from_fix = None self.received_fix = False self.imu_t0 = 0 self.gps_t0 = 0 self.lidar_t0 = 0 if simulated: super(RoboQuasar, self).__init__( file_sets[set_num][0], file_sets[set_num][1], self.gps, self.imu, self.turret, start_index=10000, # end_index=2000, ) extension_index = self.parser.full_path.rfind(".") image_path = self.parser.full_path[:extension_index] else: super(RoboQuasar, self).__init__(self.imu, self.gps, self.steering, self.brakes, self.underglow, self.turret, joystick=joystick, debug_prints=args.verbose, log_data=args.log) extension_index = self.logger.full_path.rfind(".") image_path = self.logger.full_path[:extension_index] self.slam = SLAM(self.turret, image_path + " post map") self.slam_plots = SlamPlots(self.slam.map_size_pixels, self.slam.map_size_meters, args.computeslam)
class RoboQuasar(Interface): def __init__(self, set_num=-1): self.gps = GPS() self.imu = IMU() self.turret = LidarTurret() self.steering = Steering() self.brakes = Brakes() self.underglow = Underglow() joystick = None if only_lidar: self.gps.enabled = False self.imu.enabled = False self.steering.enabled = False self.brakes.enabled = False self.underglow.enabled = False elif not simulated: joystick = WiiUJoystick() if args.includelidar: self.turret.enabled = False # self.imu_plot = RobotPlot("Magnetometer data (x, z vs. time)", flat_plot=True, skip_count=20, # plot_enabled=False) self.kalman_plots = KalmanPlots(not only_lidar, use_filter, animate) if animate: if args.includelidar: self.turret.point_cloud_plot.enabled = False self.live_plot = LivePlotter(2, self.kalman_plots.filter_comparison, self.turret.point_cloud_plot, skip_count=10) if self.turret.point_cloud_plot.enabled: self.live_plot.draw_dot(self.turret.point_cloud_plot, 0, 0, color='orange', markersize=5) elif plots_enabled: self.static_plot = StaticPlotter( 1, self.kalman_plots.filter_comparison) self.filter = None # m = np.array(MapFile("buggy course checkpoints").map) # self.map_plot.update(m[:, 0], m[:, 1]) file_sets = ( ("16;49", "2017_Feb_17"), # default, latest # data day 4 # ("15", "data_days/2017_Feb_14"), # filter explodes, LIDAR interfered by the sun # ("16;20", "data_days/2017_Feb_14"), # shorten run, LIDAR collapsed # ("16;57", "data_days/2017_Feb_14"), # interfered LIDAR # ("17;10", "data_days/2017_Feb_14"), # all data is fine, interfered LIDAR # ("17;33", "data_days/2017_Feb_14"), # data is fine, normal run # data day 3 # ("16;38", "data_days/2017_Feb_08"), # ("17", "data_days/2017_Feb_08"), # ("18", "data_days/2017_Feb_08"), # trackfield no gyro values # ("15;46", "old_data/2016_Dec_02"), # ("15;54", "old_data/2016_Dec_02"), # ("16;10", "old_data/2016_Dec_02"), # ("16;10", "old_data/2016_Dec_02") # rolls day 1 # ("07;22", "old_data/2016_Nov_06"), # bad gyro values # rolls day 2 # ("07;36;03 m", "old_data/2016_Nov_12"), # ("09;12", "old_data/2016_Nov_12"), # invalid values # ("07;04;57", "old_data/2016_Nov_13"), # bad gyro values # rolls day 3 # ("modified 07;04", "old_data/2016_Nov_13"), # ("modified 07;23", "old_data/2016_Nov_13"), # wonky value for mag. # rolling on the cut # ("16;29", "old_data/2016_Dec_09"), # ("16;49", "old_data/2016_Dec_09"), # ("16;5", "old_data/2016_Dec_09"), # ("17;", "old_data/2016_Dec_09"), # nothing wrong, really short # bad data # ("16;07", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short # ("16;09", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short # ("18;00", "old_data/2016_Dec_09/bad_data"), # gps spazzed out # ("18;02", "old_data/2016_Dec_09/bad_data"), # gps spazzed out # ("18;09", "old_data/2016_Dec_09/bad_data"), # gps spazzed out ) self.checkpoint_num = 0 self.lat1, self.long1, self.alt1 = 0, 0, 0 self.lat2, self.long2, self.alt2 = 0, 0, 0 self.num_recv_from_fix = None self.received_fix = False self.imu_t0 = 0 self.gps_t0 = 0 self.lidar_t0 = 0 if simulated: super(RoboQuasar, self).__init__( file_sets[set_num][0], file_sets[set_num][1], self.gps, self.imu, self.turret, start_index=10000, # end_index=2000, ) extension_index = self.parser.full_path.rfind(".") image_path = self.parser.full_path[:extension_index] else: super(RoboQuasar, self).__init__(self.imu, self.gps, self.steering, self.brakes, self.underglow, self.turret, joystick=joystick, debug_prints=args.verbose, log_data=args.log) extension_index = self.logger.full_path.rfind(".") image_path = self.logger.full_path[:extension_index] self.slam = SLAM(self.turret, image_path + " post map") self.slam_plots = SlamPlots(self.slam.map_size_pixels, self.slam.map_size_meters, args.computeslam) def init_filter(self, timestamp): self.lat2, self.long2, self.alt2 = self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude roll, pitch, yaw = get_gps_orientation(self.lat1, self.long1, self.alt1, self.lat2, self.long2, self.alt2) yaw = math.atan2(self.lat2 - self.lat1, self.long2 - self.long1) print(math.degrees(roll), math.degrees(pitch), math.degrees(yaw)) self.filter = GrovesKalmanFilter(initial_roll=roll, initial_pitch=pitch, initial_yaw=yaw, initial_lat=self.lat2, initial_long=self.long2, initial_alt=self.alt2, **constants) self.imu_t0 = timestamp self.gps_t0 = timestamp self.lidar_t0 = timestamp self.kalman_plots.update_gps(self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude) self.kalman_plots.update_kalman(self.filter.get_position()) # length = math.sqrt(lat1 ** 2 + long1 ** 2) lat2 = 0.0003 * math.sin(yaw) + self.lat1 long2 = 0.0003 * math.cos(yaw) + self.long1 self.kalman_plots.initial_compass_plot.updated([self.lat1, lat2], [self.long1, long2]) def gps_in_range(self): if not 280 < self.gps.altitude < 310: self.gps.altitude = 300.0 if -80 < self.gps.longitude_deg < -79.8 and 40.4 < self.gps.latitude_deg < 41 and ( 280 < self.gps.altitude < 310 or self.gps.altitude == 0.0): return True else: return False def update_gps_plot(self): if self.gps_in_range(): self.kalman_plots.update_gps(self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude) return True else: return False def update_epoch(self, timestamp): if use_filter: if self.gps.fix and self.gps.latitude is not None and self.received_fix != self.gps.fix: self.received_fix = True self.num_recv_from_fix = self.num_received(self.gps) if self.num_recv_from_fix is not None and self.num_received( self.gps) - self.num_recv_from_fix == 245: self.lat1, self.long1, self.alt1 = self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude elif self.filter is None and self.num_recv_from_fix is not None and \ self.gps_in_range() and self.num_received(self.gps) - self.num_recv_from_fix >= 250: if use_filter: self.init_filter(timestamp) elif self.filter is not None and self.num_recv_from_fix is not None: # if self.num_received(self.gps) % 5 == 0: if self.update_gps_plot(): if use_filter: self.filter.gps_updated(timestamp - self.gps_t0, self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude) self.kalman_plots.update_kalman( self.filter.get_position()) self.gps_t0 = timestamp lat1, long1, alt1 = self.filter.get_position() roll, pitch, yaw = self.filter.get_orientation() yaw = -self.imu.euler.x # declination = 9, 19 # # self.declination = \ # (declination[0] + declination[1] / 60) * math.pi / 180 # # yaw = math.atan2(self.imu.mag.y, self.imu.mag.x) # yaw += self.declination # # Correct for reversed heading # if yaw < 0: # yaw += 2 * math.pi # # Check for wrap and compensate # elif yaw > 2 * math.pi: # yaw -= 2 * math.pi # length = math.sqrt(lat1 ** 2 + long1 ** 2) lat2 = 0.0003 * math.sin(math.radians(yaw)) + lat1 long2 = 0.0003 * math.cos(math.radians(yaw)) + long1 self.kalman_plots.update_compass( lat1, lat2, long1, long2) # print(self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude) # if use_filter: # print(self.filter.get_position(), self.filter.properties.estimated_velocity.T.tolist()) # print(math.degrees(roll), math.degrees(pitch), math.degrees(yaw)) if animate and self.live_plot.should_update(): if self.live_plot.plot() is False: return False def update_ins(self, timestamp): if use_filter and self.filter is not None and self.imu_t0 != timestamp: self.filter.imu_updated(timestamp - self.imu_t0, self.imu.linaccel.x, self.imu.linaccel.y, self.imu.linaccel.z, self.imu.gyro.x, self.imu.gyro.y, self.imu.gyro.z) self.kalman_plots.update_kalman(self.filter.get_position()) self.imu_t0 = timestamp def update_turret(self, timestamp): if not args.includelidar and self.turret.did_cloud_update(): if self.turret.is_cloud_ready() and self.filter is not None and ( self.num_received(self.gps) - self.num_recv_from_fix > 250): v = self.filter.get_velocity() vx = v[0] vy = v[1] ang_v = self.filter.get_angular(timestamp - self.lidar_t0)[2] self.slam.update(timestamp, self.turret.distances, [vx, vy, ang_v]) self.lidar_t0 = timestamp if animate: self.slam_plots.update(self.slam.mapbytes, self.slam.get_pos()) # print(self.slam.get_pos(), (vx, vy, ang_v)) if animate and not self.live_plot.plot(): return False # ----- live methods ----- def packet_received(self, timestamp, whoiam, packet): if self.did_receive(self.imu): if self.update_ins(timestamp) is False: return False elif self.did_receive(self.gps): if self.update_epoch(timestamp) is False: return False print(timestamp) print(self.gps) print(self.imu) elif self.did_receive(self.turret): if self.update_turret(timestamp) is False: return False else: print(packet) # elif self.did_receive(self.steering):# and self.steering.goal_reached: # print(self.steering.current_step) def loop(self): if not simulated: if self.joystick is not None: if self.steering.calibrated: if self.joystick.axis_updated("left x"): self.steering.set_speed( self.joystick.get_axis("left x")) elif self.joystick.button_updated( "A") and self.joystick.get_button("A"): self.steering.set_position(0) if self.joystick.button_updated( "B") and self.joystick.get_button("B"): self.brakes.pull() elif self.joystick.button_updated( "X") and self.joystick.get_button("X"): self.brakes.release() elif self.joystick.button_updated( "Y") and self.joystick.get_button("Y"): self.record( "checkpoint", "%s\t%s\t%s" % (self.checkpoint_num, self.gps.latitude_deg, self.gps.longitude_deg)) if self.gps.latitude_deg is not None and self.gps.longitude_deg is not None: print("checkpoint #%s: %3.6f, %3.6f" % (self.checkpoint_num, self.gps.latitude_deg, self.gps.longitude_deg)) self.checkpoint_num += 1 print("checkpoint #%s" % self.checkpoint_num) # elif self.joystick.dpad_updated(): # if self.joystick.dpad[1] == 1: # self.brakes.set_brake(self.brakes.goal_position + 20) # elif self.joystick.dpad[1] == -1: # self.brakes.set_brake(self.brakes.goal_position - 20) def start(self): if animate: self.live_plot.start_time(self.start_time) # ----- simulator methods ----- def object_packet(self, timestamp): if self.did_receive(self.imu): if self.update_ins(timestamp) is False: return False elif self.did_receive(self.gps): if self.update_epoch(timestamp) is False: return False elif self.did_receive(self.turret): if self.update_turret(timestamp) is False: return False # def user_packet(self, timestamp, packet): # if self.did_receive("checkpoint"): # print(timestamp, packet) # def command_packet(self, timestamp, packet): # if self.did_receive(self.steering): # print(timestamp, packet) def close(self): if plots_enabled: if not animate: self.static_plot.plot() self.static_plot.show() else: if simulated: if not self.error_signalled: print("Finished!") self.live_plot.freeze_plot() self.live_plot.close() if args.computeslam: self.slam.make_image()
class AnimationExample(Robot): def __init__(self): self.gps_plot = RobotPlot("gps", color="red") self.compass_plot = RobotPlot("compass", color="purple") self.sticky_compass_plot = RobotPlot("sticky compass", color="gray") self.sticky_compass_counter = 0 self.sticky_compass_skip = 100 self.accuracy_check_plot = RobotPlotCollection( "Animation", self.sticky_compass_plot, self.gps_plot, self.compass_plot) self.plotter = LivePlotter( 2, self.accuracy_check_plot, matplotlib_events=dict(key_press_event=self.key_press)) self.gps = AdafruitGPS() self.imu = BNO055() # whoiam IDs were different in the old logs, changing for convenience, please don't do this otherwise!! self.gps.whoiam = "gps" self.imu.whoiam = "imu" # file_name, directory = file_sets["rolls day 3"][0] super(AnimationExample, self).__init__(self.imu, self.gps) self.compass_angle = 0.0 self.start_angle = 0.0 self.link(self.gps, self.receive_gps) self.link(self.imu, self.receive_imu) def key_press(self, event): if event.key == " ": self.toggle_pause() self.plotter.toggle_pause() def receive_gps(self, timestamp, packet, packet_type): if self.gps.is_position_valid(): self.gps_plot.append(self.gps.latitude_deg, self.gps.longitude_deg) status = self.plotter.plot() if status is not None: return status def init_compass(self, packet): self.compass_angle = math.radians(float(packet)) - math.pi / 2 def offset_angle(self): if self.start_angle is None: self.start_angle = -self.imu.euler.z return (-self.imu.euler.z + self.start_angle - self.compass_angle) % (2 * math.pi) def compass_coords(self, angle, length=0.0003): lat2 = length * math.sin(angle) + self.gps.latitude_deg long2 = length * math.cos(angle) + self.gps.longitude_deg return lat2, long2 def receive_imu(self, timestamp, packet, packet_type): if self.gps.is_position_valid(): angle = self.offset_angle() lat2, long2 = self.compass_coords(angle) self.compass_plot.update([self.gps.latitude_deg, lat2], [self.gps.longitude_deg, long2]) self.plotter.draw_text(self.accuracy_check_plot, "%0.4f" % angle, self.gps.latitude_deg, self.gps.longitude_deg, text_name="angle text") if self.sticky_compass_skip > 0 and self.sticky_compass_counter % self.sticky_compass_skip == 0: lat2, long2 = self.compass_coords(angle, length=0.0001) self.sticky_compass_plot.append(self.gps.latitude_deg, self.gps.longitude_deg) self.sticky_compass_plot.append(lat2, long2) self.sticky_compass_plot.append(self.gps.latitude_deg, self.gps.longitude_deg) self.sticky_compass_counter += 1 def received(self, timestamp, whoiam, packet, packet_type): if self.did_receive("initial compass"): self.init_compass(packet) print("compass value:", packet) def loop(self): if self.is_paused: status = self.plotter.plot() if status is not None: return status def close(self, reason): if reason == "done": self.plotter.freeze_plot() self.plotter.close() print(self.dt())