Eq-mount support under IMU dead-reckoning tracking

docs/source/dev_arch.rst

@@ -219,12 +219,12 @@ There are three types of shared state in PiFinder
    SharedStateObj(
       power_state=1,
       solve_state=True,
-      solution={'RA': 22.86683471463411, 'Dec': 15.347716050003328, 'imu_pos': [171.39798541261814, 202.7646132036331, 358.2794741322842],
+      solution={'RA': 22.86683471463411, 'Dec': 15.347716050003328, 
                 'solve_time': 1695297930.5532792, 'cam_solve_time': 1695297930.5532837, 'Roll': 306.2951794424281, 'FOV': 10.200729425086111,
                 RMSE': 21.995567413046142, 'Matches': 12, 'Prob': 6.987725483613384e-13, 'T_solve': 15.00384000246413, 'RA_target': 22.86683471463411,
                 'Dec_target': 15.347716050003328, 'T_extract': 75.79255499877036, 'Alt': None, 'Az': None, 'solve_source': 'CAM', 'constellation': 'Psc'},
-      imu={'moving': False, 'move_start': 1695297928.69749, 'move_end': 1695297928.764207, 'pos': [171.39798541261814, 202.7646132036331, 358.2794741322842],
-           'start_pos': [171.4009455613444, 202.76321535004726, 358.2587208386012], 'status': 3},
+      imu={'moving': False, 'move_start': 1695297928.69749, 'move_end': 1695297928.764207,
+            'status': 3},
       location={'lat': 59.05139745, 'lon': 7.987654, 'altitude': 151.4, 'gps_lock': False, 'timezone': 'Europe/Stockholm', 'last_gps_lock': None},
       datetime=None,
       screen=<PIL.Image.Image image mode=RGB size=128x128 at 0xE693C910>,

python/PiFinder/calc_utils.py

@@ -159,7 +159,7 @@ def aim_degrees(shared_state, mount_type, screen_direction, target):
                 az_diff = target_az - solution["Az"]
                 az_diff = (az_diff + 180) % 360 - 180
                 if screen_direction in ["flat", "as_bloom"]:
-                    az_diff *= -1
+                    az_diff *= -1  # TODO: Why should this depend on the screen type?
 
                 alt_diff = target_alt - solution["Alt"]
                 alt_diff = (alt_diff + 180) % 360 - 180
@@ -168,8 +168,11 @@ def aim_degrees(shared_state, mount_type, screen_direction, target):
         else:
             # EQ Mount type
             ra_diff = target.ra - solution["RA"]
+            ra_diff = (ra_diff + 180) % 360 - 180  # Convert to -180 to +180
+
             dec_diff = target.dec - solution["Dec"]
             dec_diff = (dec_diff + 180) % 360 - 180
+
             return ra_diff, dec_diff
     return None, None
 
@@ -247,7 +250,8 @@ def hadec_to_roll(ha_deg, dec_deg, lat_deg):
     if dec_deg <= lat_deg:
         roll_deg = -pa_deg
     else:
-        roll_deg = -pa_deg + np.sign(ha_deg) * 180
+        roll_deg = -pa_deg
+        #roll_deg = -pa_deg + np.sign(ha_deg) * 180  # Disable for testing TODO: Check this
 
     return roll_deg
 

python/PiFinder/camera_interface.py

@@ -8,17 +8,17 @@
 * Takes full res images on demand
 
 """
-
-import datetime
-import logging
-import os
-import queue
-import time
 from typing import Tuple, Optional
-
 from PIL import Image
+import os
+import time
+import datetime
+import numpy as np
+import queue
+import logging
 
 from PiFinder import state_utils, utils
+import PiFinder.pointing_model.quaternion_transforms as qt
 from PiFinder.auto_exposure import (
     ExposurePIDController,
     SweepZeroStarHandler,
@@ -28,6 +28,7 @@
     generate_exposure_sweep,
 )
 
+
 logger = logging.getLogger("Camera.Interface")
 
 
@@ -164,20 +165,23 @@ def get_image_loop(
                     imu_end = shared_state.imu()
 
                     # see if we moved during exposure
-                    reading_diff = 0
                     if imu_start and imu_end:
-                        reading_diff = (
-                            abs(imu_start["pos"][0] - imu_end["pos"][0])
-                            + abs(imu_start["pos"][1] - imu_end["pos"][1])
-                            + abs(imu_start["pos"][2] - imu_end["pos"][2])
+                        # Returns the pointing difference between successive IMU quaternions as
+                        # an angle (radians). Note that this also accounts for rotation around the
+                        # scope axis. Returns an angle in radians.
+                        pointing_diff = qt.get_quat_angular_diff(
+                            imu_start["quat"], imu_end["quat"]
                         )
+                    else:
+                        pointing_diff = 0.0
 
                     camera_image.paste(base_image)
+
                     image_metadata = {
                         "exposure_start": image_start_time,
                         "exposure_end": image_end_time,
                         "imu": imu_end,
-                        "imu_delta": reading_diff,
+                        "imu_delta": np.rad2deg(pointing_diff),
                         "exposure_time": self.exposure_time,
                         "gain": self.gain,
                     }

python/PiFinder/imu_pi.py

@@ -6,65 +6,40 @@
 """
 
 import time
+from PiFinder import config
 from PiFinder.multiproclogging import MultiprocLogging
 import board
 import adafruit_bno055
 import logging
-
-from scipy.spatial.transform import Rotation
-
-from PiFinder import config
+import numpy as np
+import quaternion  # Numpy quaternion
 
 logger = logging.getLogger("IMU.pi")
 
 QUEUE_LEN = 10
-MOVE_CHECK_LEN = 2
 
 
 class Imu:
+    """
+    Previous version modified the IMU axes but the IMU now outputs the
+    measurements using its native axes and the transformation from the IMU
+    axes to the camera frame is done by the IMU dead-reckonig functionality.
+    """
+
     def __init__(self):
         i2c = board.I2C()
         self.sensor = adafruit_bno055.BNO055_I2C(i2c)
+        # IMPLUS mode: Accelerometer + Gyro + Fusion data
         self.sensor.mode = adafruit_bno055.IMUPLUS_MODE
         # self.sensor.mode = adafruit_bno055.NDOF_MODE
-        cfg = config.Config()
-        if (
-            cfg.get_option("screen_direction") == "flat"
-            or cfg.get_option("screen_direction") == "straight"
-            or cfg.get_option("screen_direction") == "flat3"
-        ):
-            self.sensor.axis_remap = (
-                adafruit_bno055.AXIS_REMAP_Y,
-                adafruit_bno055.AXIS_REMAP_X,
-                adafruit_bno055.AXIS_REMAP_Z,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-                adafruit_bno055.AXIS_REMAP_NEGATIVE,
-            )
-        elif cfg.get_option("screen_direction") == "as_bloom":
-            self.sensor.axis_remap = (
-                adafruit_bno055.AXIS_REMAP_X,
-                adafruit_bno055.AXIS_REMAP_Z,
-                adafruit_bno055.AXIS_REMAP_Y,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-            )
-        else:
-            self.sensor.axis_remap = (
-                adafruit_bno055.AXIS_REMAP_Z,
-                adafruit_bno055.AXIS_REMAP_Y,
-                adafruit_bno055.AXIS_REMAP_X,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-                adafruit_bno055.AXIS_REMAP_POSITIVE,
-            )
+
         self.quat_history = [(0, 0, 0, 0)] * QUEUE_LEN
         self._flip_count = 0
         self.calibration = 0
-        self.avg_quat = (0, 0, 0, 0)
+        self.avg_quat = (0, 0, 0, 0)  # Scalar-first quaternion: (w, x, y, z)
         self.__moving = False
-
+        self.__reading_diff = 0.0
+
         self.last_sample_time = time.time()
 
         # Calibration settings
@@ -74,23 +49,12 @@ def __init__(self):
         # to start moving, second is threshold to fall below
         # to stop moving.
 
+        cfg = config.Config()
         imu_threshold_scale = cfg.get_option("imu_threshold_scale", 1)
         self.__moving_threshold = (
             0.0005 * imu_threshold_scale,
             0.0003 * imu_threshold_scale,
         )
-
-    def quat_to_euler(self, quat):
-        if quat[0] + quat[1] + quat[2] + quat[3] == 0:
-            return 0, 0, 0
-        rot = Rotation.from_quat(quat)
-        rot_euler = rot.as_euler("xyz", degrees=True)
-        # convert from -180/180 to 0/360
-        rot_euler[0] += 180
-        rot_euler[1] += 180
-        rot_euler[2] += 180
-        return rot_euler
-
     def moving(self):
         """
         Compares most recent reading
@@ -110,6 +74,7 @@ def update(self):
         if self.calibration == 0:
             logger.warning("NOIMU CAL")
             return True
+        # adafruit_bno055 uses quaternion convention (w, x, y, z)
         quat = self.sensor.quaternion
         if quat[0] is None:
             logger.warning("IMU: Failed to get sensor values")
@@ -132,6 +97,9 @@ def update(self):
         # Sometimes the quat output will 'flip' and change by 2.0+
         # from one reading to another.  This is clearly noise or an
         # artifact, so filter them out
+        #
+        # NOTE: This is probably due to the double-cover property of quaternions
+        # where +q and -q describe the same rotation?
         if self.__reading_diff > 1.5:
             self._flip_count += 1
             if self._flip_count > 10:
@@ -148,7 +116,9 @@ def update(self):
             # no flip
             self._flip_count = 0
 
+        # avg_quat is the latest quaternion measurement, not the average
         self.avg_quat = quat
+        # Write over the quat_hisotry queue FIFO:
         if len(self.quat_history) == QUEUE_LEN:
             self.quat_history = self.quat_history[1:]
         self.quat_history.append(quat)
@@ -160,9 +130,6 @@ def update(self):
             if self.__reading_diff > self.__moving_threshold[0]:
                 self.__moving = True
 
-    def get_euler(self):
-        return list(self.quat_to_euler(self.avg_quat))
-
     def __str__(self):
         return (
             f"IMU Information:\n"
@@ -195,35 +162,40 @@ def imu_monitor(shared_state, console_queue, log_queue):
 
     imu = Imu()
     imu_calibrated = False
+    # TODO: Remove move_start, move_end
     imu_data = {
         "moving": False,
         "move_start": None,
         "move_end": None,
-        "pos": [0, 0, 0],
-        "quat": [0, 0, 0, 0],
-        "start_pos": [0, 0, 0],
+        "quat": quaternion.quaternion(
+            0, 0, 0, 0
+        ),  # Scalar-first numpy quaternion(w, x, y, z) - Init to invalid quaternion
         "status": 0,
     }
+
     while True:
         imu.update()
         imu_data["status"] = imu.calibration
+
+        # TODO: move_start and move_end don't seem to be used?
         if imu.moving():
             if not imu_data["moving"]:
                 logger.debug("IMU: move start")
                 imu_data["moving"] = True
-                imu_data["start_pos"] = imu_data["pos"]
                 imu_data["move_start"] = time.time()
-            imu_data["pos"] = imu.get_euler()
-            imu_data["quat"] = imu.avg_quat
-
+            # DISABLE old method
+            imu_data["quat"] = quaternion.from_float_array(
+                imu.avg_quat
+            )  # Scalar-first (w, x, y, z)
         else:
             if imu_data["moving"]:
                 # If we were moving and we now stopped
                 logger.debug("IMU: move end")
                 imu_data["moving"] = False
-                imu_data["pos"] = imu.get_euler()
-                imu_data["quat"] = imu.avg_quat
                 imu_data["move_end"] = time.time()
+                imu_data["quat"] = quaternion.from_float_array(
+                    imu.avg_quat
+                )  # Scalar-first (w, x, y, z)
 
         if not imu_calibrated:
             if imu_data["status"] == 3:
@@ -242,6 +214,7 @@ def imu_monitor(shared_state, console_queue, log_queue):
         imu = Imu()
         for i in range(10):
             imu.update()
+            print(imu)
             time.sleep(0.5)
     except Exception as e:
         logger.exception("Error starting phyiscal IMU", e)