IMU Integrator Selection + Misc Adjustments

default_config.json

@@ -176,5 +176,6 @@
         "active_telescope_index": 0,
         "active_eyepiece_index": 0
     },
-    "imu_threshold_scale": 1
+    "imu_threshold_scale": 1,
+    "imu_integrator": "classic"
 }

python/PiFinder/calc_utils.py

@@ -167,8 +167,6 @@ 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  # TODO: Why should this depend on the screen type?
 
                 alt_diff = target_alt - solution["Alt"]
                 alt_diff = (alt_diff + 180) % 360 - 180

python/PiFinder/camera_interface.py

@@ -8,6 +8,7 @@
 * Takes full res images on demand
 
 """
+
 from typing import Tuple, Optional
 from PIL import Image
 import os
@@ -56,14 +57,20 @@ def capture_file(self, filename) -> None:
     def capture_raw_file(self, filename) -> None:
         pass
 
+    def _blank_capture(self):
+        """
+        Returns a properly formated black frame
+        """
+        return Image.new("L", (512, 512), 0)  # Black 512x512 image
+
     def capture_bias(self):
         """
         Capture a bias frame for pedestal calculation.
         Base implementation returns a black frame (no bias correction).
         Override in subclasses that support bias frames.
         Returns Image.Image or np.ndarray depending on implementation.
         """
-        return Image.new("L", (512, 512), 0)  # Black 512x512 image
+        return self._blank_capture()
 
     def set_camera_config(
         self, exposure_time: float, gain: float
@@ -171,7 +178,7 @@ def get_image_loop(
                         base_image = base_image.convert(
                             "L"
                         )  # Convert to grayscale to match camera output
-                        time.sleep(1)
+                        time.sleep(0.2)
                     image_end_time = time.time()
                     # check imu to make sure we're still static
                     imu_end = shared_state.imu()
@@ -187,7 +194,12 @@ def get_image_loop(
                     else:
                         pointing_diff = 0.0
 
-                    camera_image.paste(base_image)
+                    # Make image available
+                    if debug and abs(pointing_diff) > 0.01:
+                        # Check if we moved and return a blank image
+                        camera_image.paste(self._blank_capture())
+                    else:
+                        camera_image.paste(base_image)
 
                     image_metadata = {
                         "exposure_start": image_start_time,

python/PiFinder/imu_pi_classic.py

@@ -0,0 +1,250 @@
+#!/usr/bin/python
+# -*- coding:utf-8 -*-
+"""
+This module is for IMU related functions
+
+Classic version: Uses axis remapping and euler angle output for the
+classic (alt/az offset) integrator dead-reckoning approach.
+"""
+
+import time
+from PiFinder.multiproclogging import MultiprocLogging
+import board
+import adafruit_bno055
+import logging
+import quaternion  # numpy-quaternion
+
+from scipy.spatial.transform import Rotation
+
+from PiFinder import config
+
+logger = logging.getLogger("IMU.pi")
+
+QUEUE_LEN = 10
+MOVE_CHECK_LEN = 2
+
+
+class Imu:
+    def __init__(self):
+        i2c = board.I2C()
+        self.sensor = adafruit_bno055.BNO055_I2C(i2c)
+        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.__moving = False
+
+        self.last_sample_time = time.time()
+
+        # Calibration settings
+        self.imu_sample_frequency = 1 / 30
+
+        # First value is delta to exceed between samples
+        # to start moving, second is threshold to fall below
+        # to stop moving.
+
+        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
+        with past readings
+        """
+        return self.__moving
+
+    def update(self):
+        # check for update frequency
+        if time.time() - self.last_sample_time < self.imu_sample_frequency:
+            return
+
+        self.last_sample_time = time.time()
+
+        # Throw out non-calibrated data
+        self.calibration = self.sensor.calibration_status[1]
+        if self.calibration == 0:
+            logger.warning("NOIMU CAL")
+            return True
+        quat = self.sensor.quaternion
+        if quat[0] is None:
+            logger.warning("IMU: Failed to get sensor values")
+            return
+
+        _quat_diff = []
+        for i in range(4):
+            _quat_diff.append(abs(quat[i] - self.quat_history[-1][i]))
+
+        self.__reading_diff = sum(_quat_diff)
+
+        # This seems to be some sort of defect / side effect
+        # of the integration system in the BNO055
+        # When not moving quat output will vaccilate
+        # by exactly this amount... so filter this out
+        if self.__reading_diff == 0.0078125:
+            self.__reading_diff = 0
+            return
+
+        # 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
+        if self.__reading_diff > 1.5:
+            self._flip_count += 1
+            if self._flip_count > 10:
+                # with the history initialized to 0,0,0,0 the unit
+                # can get stuck seeing flips if the IMU starts
+                # returning data. This count will reset history
+                # to the current state if it exceeds 10
+                self.quat_history = [quat] * QUEUE_LEN
+                self.__reading_diff = 0
+            else:
+                self.__reading_diff = 0
+                return
+        else:
+            # no flip
+            self._flip_count = 0
+
+        self.avg_quat = quat
+        if len(self.quat_history) == QUEUE_LEN:
+            self.quat_history = self.quat_history[1:]
+        self.quat_history.append(quat)
+
+        if self.__moving:
+            if self.__reading_diff < self.__moving_threshold[1]:
+                self.__moving = False
+        else:
+            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"
+            f"Calibration Status: {self.calibration}\n"
+            f"Quaternion History: {self.quat_history}\n"
+            f"Average Quaternion: {self.avg_quat}\n"
+            f"Moving: {self.moving()}\n"
+            f"Reading Difference: {self.__reading_diff}\n"
+            f"Flip Count: {self._flip_count}\n"
+            f"Last Sample Time: {self.last_sample_time}\n"
+            f"IMU Sample Frequency: {self.imu_sample_frequency}\n"
+            f"Moving Threshold: {self.__moving_threshold}\n"
+        )
+
+
+def imu_monitor(shared_state, console_queue, log_queue):
+    MultiprocLogging.configurer(log_queue)
+    logger.debug("Starting IMU")
+    imu = None
+    try:
+        imu = Imu()
+    except Exception as e:
+        logger.error(f"Error starting phyiscal IMU : {e}")
+        logger.error("Falling back to fake IMU")
+        console_queue.put("IMU: Error starting physical IMU, using fake IMU")
+        console_queue.put("DEGRADED_OPS IMU")
+        from PiFinder.imu_fake import Imu as ImuFake
+
+        imu = ImuFake()
+
+    imu = Imu()
+    imu_calibrated = False
+    imu_data = {
+        "moving": False,
+        "move_start": None,
+        "move_end": None,
+        "pos": [0, 0, 0],
+        "quat": quaternion.quaternion(0, 0, 0, 0),
+        "start_pos": [0, 0, 0],
+        "status": 0,
+    }
+    while True:
+        imu.update()
+        imu_data["status"] = imu.calibration
+        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"] = quaternion.from_float_array(imu.avg_quat)
+
+        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"] = quaternion.from_float_array(imu.avg_quat)
+                imu_data["move_end"] = time.time()
+
+        if not imu_calibrated:
+            if imu_data["status"] == 3:
+                imu_calibrated = True
+                console_queue.put("IMU: NDOF Calibrated!")
+
+        if shared_state is not None and imu_calibrated:
+            shared_state.set_imu(imu_data)
+
+
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.DEBUG)
+    logger.info("Trying to read state from IMU")
+    imu = None
+    try:
+        imu = Imu()
+        for i in range(10):
+            imu.update()
+            time.sleep(0.5)
+    except Exception as e:
+        logger.exception("Error starting phyiscal IMU", e)

python/PiFinder/integrator.py

@@ -111,22 +111,18 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa
                         )
                     )
                     shared_state.set_solve_state(True)
+                    # We have a new image solve: Use plate-solving for RA/Dec
+                    update_plate_solve_and_imu(imu_dead_reckoning, solved)
                 else:
                     # Failed solve - clear constellation
                     solved["solve_source"] = "CAM_FAILED"
                     solved["constellation"] = ""
-                    shared_state.set_solve_state(False)
 
-                # Push all camera solves (success and failure) immediately
-                # This ensures auto-exposure sees Matches=0 for failed solves
-                shared_state.set_solution(solved)
-                shared_state.set_solve_state(True)
-
-                # We have a new image solve: Use plate-solving for RA/Dec
-                update_plate_solve_and_imu(imu_dead_reckoning, solved)
+                    # Push failed solved immediately
+                    # This ensures auto-exposure sees Matches=0 for failed solves
+                    shared_state.set_solution(solved)
+                    shared_state.set_solve_state(False)
 
-                # TODO: main also calculates (alt, az) for target & camera center.
-                # Don't think this is needed because they are done by the update functions?
             elif imu_dead_reckoning.tracking:
                 # Previous plate-solve exists so use IMU dead-reckoning from
                 # the last plate solved coordinates.
@@ -135,7 +131,6 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa
                     update_imu(imu_dead_reckoning, solved, last_image_solve, imu)
 
             # Push IMU updates only if newer than last push
-            # (Camera solves already pushed above at line ~185)  # TODO: Line numbers may have changed in merge
             if (
                 solved["RA"] and solved["solve_time"] > last_solve_time
                 # and solved["solve_source"] == "IMU"