Choppy panning and zooming working! Not the nicest, but it works.

software/firmware/tower/tower.ino

@@ -163,7 +163,7 @@ void send_imu_stream_line(JsonObject &root) {
   JsonObject& imu_object = root.createNestedObject("imu");
 
   quat = bno.getQuat();
-  linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
+  linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
   angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
 
   imu_object["ox"] = quat.x();

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py

@@ -447,26 +447,15 @@ class OverlayImage(object):
             draw.ellipse((x - size, y - size, x + size, y + size), fill="blue")
 
         self._draw_rover(latitude, longitude, compass)
-        self._draw_coordinate_text(latitude, longitude)
+        # self._draw_coordinate_text(latitude, longitude)
         self.update(latitude, longitude)
 
         return self.display_image
 
     def load_rover_icon(self):
-        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((60, 60))
+        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((40, 40))
 
-    def _draw_coordinate_text(self, latitude, longitude):
-        location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
 
-        font = PIL.ImageFont.truetype("UbuntuMono-R", size=20)
-
-        new_image = PIL.Image.new('RGBA', (200, 45), "black")
-
-        draw = PIL.ImageDraw.Draw(new_image)
-
-        draw.multiline_text((5, 0), location_text, font=font)
-
-        self.display_image.paste(new_image, (0, 0))
 
     def _draw_rover(self, lat, lon, angle=0):
         x, y = self._get_cartesian(lat, lon)
@@ -486,7 +475,7 @@ class OverlayImage(object):
         # self.left_x -= 50
         # self.upper_y -= 50
         self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
-        self._draw_coordinate_text(latitude, longitude)
+        # self._draw_coordinate_text(latitude, longitude)
 
     def connect_signals_and_slots(self):
         pass

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py

@@ -5,6 +5,9 @@
 from PyQt5 import QtCore, QtWidgets, QtGui
 from PIL.ImageQt import ImageQt
 from PIL import Image
+import PIL.ImageDraw
+import PIL.Image
+import PIL.ImageFont
 import numpy
 
 import logging
@@ -17,6 +20,7 @@ from sensor_msgs.msg import Imu
 
 # Custom Imports
 import RoverMap
+from Resources.Settings import Mapping as MappingSettings
 from sensor_msgs.msg import NavSatFix
 
 #####################################
@@ -28,6 +32,11 @@ GPS_POSITION_TOPIC = "/rover_odometry/fix"
 IMU_DATA_TOPIC = "/rover_odometry/imu/data"
 
 
+MAP_WIDGET_WIDTH = float(1280)
+MAP_WIDGET_HEIGHT = float(720)
+MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT
+
+
 class RoverMapCoordinator(QtCore.QThread):
     pixmap_ready_signal = QtCore.pyqtSignal()
     change_waypoint_signal = QtCore.pyqtSignal()
@@ -37,7 +46,7 @@ class RoverMapCoordinator(QtCore.QThread):
 
         self.shared_objects = shared_objects
         self.left_screen = self.shared_objects["screens"]["left_screen"]
-        self.mapping_label = self.left_screen.mapping_label
+        self.mapping_label = self.left_screen.mapping_label  # type:QtWidgets.QLabel
         self.navigation_label = self.left_screen.navigation_waypoints_table_widget
         self.landmark_label = self.left_screen.landmark_waypoints_table_widget
 
@@ -74,6 +83,17 @@ class RoverMapCoordinator(QtCore.QThread):
 
         self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
 
+        self.zoom_pan_x_location = 0
+        self.zoom_pan_y_location = 0
+
+        self.zoom_subtraction = 0
+
+        self.x_drag = 0
+        self.y_drag = 0
+
+        self.x_drag_start = -1
+        self.y_drag_start = -1
+
     def run(self):
         self.logger.debug("Starting Map Coordinator Thread")
         self.pixmap_ready_signal.emit()  # This gets us the loading map
@@ -87,7 +107,7 @@ class RoverMapCoordinator(QtCore.QThread):
                     self.new_imu_data = False
 
                 self._get_map_image()
-            self.msleep(30)
+            self.msleep(10)
 
         self.logger.debug("Stopping Map Coordinator Thread")
 
@@ -96,7 +116,7 @@ class RoverMapCoordinator(QtCore.QThread):
                                                          720,
                                                          44.5675721667,
                                                          -123.2750535,
-                                                         19,  # FIXME: Used to be 18
+                                                         18,  # FIXME: Used to be 18
                                                          'satellite',
                                                          None, 20)
         self.overlay_image_object = (
@@ -120,21 +140,51 @@ class RoverMapCoordinator(QtCore.QThread):
         self.map_image.paste(self.overlay_image_object.display_image,
                              (0, 0),
                              self.overlay_image_object.display_image)
-        # self.map_image = Image.alpha_composite(
+
-        #                   self.google_maps_object.display_image,
+        # ##### Zoom testing! #####
-        #                   self.overlay_image_object.display_image)
+        if self.zoom_subtraction:
-        # get overlay here
+            self.zoom_subtraction = self.constrain(self.zoom_subtraction, 0, 520)
+
+            crop_x_start = ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2) - self.x_drag
+            crop_y_start = (self.zoom_subtraction / 2) - self.y_drag
+            crop_x_end = (MAP_WIDGET_WIDTH - ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2)) - self.x_drag
+            crop_y_end = (MAP_WIDGET_HEIGHT - (self.zoom_subtraction / 2)) - self.y_drag
+            crop_box = (int(crop_x_start), int(crop_y_start), int(crop_x_end), int(crop_y_end))
+
+            self.map_image = self.map_image.crop(crop_box)
+            self.map_image = self.map_image.resize((1280, 720), resample=PIL.Image.BICUBIC)
+
+        # ##### Draw coordinates #####
+        self._draw_coordinate_text(self.latitude, self.longitude)
+
         qim = ImageQt(self.map_image)
         self.map_pixmap = QtGui.QPixmap.fromImage(qim)
 
-        if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key:
+        # if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key:
-            self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey()
+        #     self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey()
-            self.pixmap_ready_signal.emit()
+        self.pixmap_ready_signal.emit()
+
+    def _draw_coordinate_text(self, latitude, longitude):
+        location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
+
+        font = PIL.ImageFont.truetype("UbuntuMono-R", size=20)
+
+        new_image = PIL.Image.new('RGBA', (200, 45), "black")
+
+        draw = PIL.ImageDraw.Draw(new_image)
+
+        draw.multiline_text((5, 0), location_text, font=font)
+
+        self.map_image.paste(new_image, (0, 0))
 
     def connect_signals_and_slots(self):
         self.pixmap_ready_signal.connect(self.pixmap_ready__slot)
         self.change_waypoint_signal.connect(self.update_overlay)
 
+        self.mapping_label.mouseReleaseEvent = self.__mouse_released_event
+        self.mapping_label.wheelEvent = self.__mouse_wheel_event
+        self.mapping_label.mouseMoveEvent = self.__mouse_move_event
+
     def on_kill_threads_requested_slot(self):
         self.run_thread_flag = False
 
@@ -191,4 +241,33 @@ class RoverMapCoordinator(QtCore.QThread):
             self.imu_data.orientation.w,
         )
         self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
-        self.last_heading = self.euler_interpolator(self.yaw) % 360
+        self.last_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360
+
+    def __mouse_released_event(self, event):
+        self.x_drag_start = -1
+        self.y_drag_start = -1
+
+
+    def __mouse_wheel_event(self, event):
+        # print "wheel:", event.angleDelta()
+        self.zoom_subtraction += event.angleDelta().y() / 12
+
+    def __mouse_move_event(self, event):
+        if self.x_drag_start != -1 and self.x_drag_start != -1:
+            buttons = event.buttons()
+
+            if buttons == QtCore.Qt.LeftButton:
+                x_pos = event.pos().x()
+                y_pos = event.pos().y()
+                dx = x_pos - self.x_drag_start
+                dy = y_pos - self.y_drag_start
+
+                self.x_drag = dx
+                self.y_drag = dy
+        else:
+            self.x_drag_start = event.pos().x()
+            self.y_drag_start = event.pos().y()
+
+    @staticmethod
+    def constrain(val, min_val, max_val):
+        return min(max_val, max(min_val, val))

software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py

@@ -14,6 +14,7 @@ from tf import transformations
 from scipy.interpolate import interp1d
 import math
 from sensor_msgs.msg import Imu
+from Resources.Settings import Mapping as MappingSettings
 
 #####################################
 # Global Variables
@@ -107,7 +108,7 @@ class SpeedAndHeadingIndication(QtCore.QThread):
             self.imu_data.orientation.w,
         )
         self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
-        self.current_heading = self.euler_interpolator(self.yaw) % 360
+        self.current_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360
 
     def rotate_compass_if_needed(self):
 

software/ros_packages/ground_station/src/Resources/Settings/Mapping.py

@@ -17,4 +17,7 @@ MAPPING_LOCATIONS = {
         "valid_zoom_options": [15, 16, 17, 18, 19, 20],
         "pre_cache_map_zoom_levels": [18, 19, 20]
     }
-}
+}
+
+# ##### This is the offset from magnetic north to true north
+DECLINATION_OFFSET = 15

software/ros_packages/rover_main/launch/rover/odometry2.launch

@@ -32,7 +32,7 @@
             <!--<param name="base_link_frame" value="base_link"/>-->
             <!--<param name="world_frame"     value="odom"/>-->
 
-            <!--<param name="odom0" value="odometry/gps"/>-->
+            <!--<param name="odom0" value="fix"/>-->
             <!--<param name="imu0"  value="imu/data"/>-->
 
             <!--<rosparam param="odom0_config">[true, true, false,-->