diff --git a/.gitignore b/.gitignore index 72089c9d..ed72d0d0 100644 --- a/.gitignore +++ b/.gitignore @@ -161,3 +161,4 @@ python/tetra3 # users GPS data test_ubx test_ubx/* +python/telemetry_analysis/ diff --git a/default_config.json b/default_config.json index b7bf33ba..471a5e6e 100644 --- a/default_config.json +++ b/default_config.json @@ -177,5 +177,7 @@ "active_telescope_index": 0, "active_eyepiece_index": 0 }, - "imu_threshold_scale": 1 + "imu_threshold_scale": 1, + "telemetry_record": false, + "telemetry_images": false } diff --git a/python/PiFinder/camera_interface.py b/python/PiFinder/camera_interface.py index 1ee3fe32..9c54fe4c 100644 --- a/python/PiFinder/camera_interface.py +++ b/python/PiFinder/camera_interface.py @@ -425,7 +425,17 @@ def get_image_loop( f"Exposure saved and auto-exposure disabled: {self.exposure_time}µs" ) - if command.startswith("save"): + if command.startswith("save_image:"): + # Save current camera frame to specified path + save_path = command.split(":", 1)[1] + try: + img = camera_image.copy() + img.save(save_path, "PNG", compress_level=6) + logger.debug("Telemetry image saved: %s", save_path) + except Exception as e: + logger.error("Failed to save telemetry image: %s", e) + + if command.startswith("save:"): # Set flag to save next capture to this file self._save_next_to = command.split(":")[1] console_queue.put("CAM: Save flag set") diff --git a/python/PiFinder/imu_pi.py b/python/PiFinder/imu_pi.py index 4e00b316..fbe4d58c 100644 --- a/python/PiFinder/imu_pi.py +++ b/python/PiFinder/imu_pi.py @@ -171,6 +171,8 @@ def imu_monitor(shared_state, console_queue, log_queue): 0, 0, 0, 0 ), # Scalar-first numpy quaternion(w, x, y, z) - Init to invalid quaternion "status": 0, # IMU Status: 3=Calibrated + "gyro": None, # Raw gyroscope angular velocity (rad/s) + "accel": None, # Raw linear acceleration (m/s², gravity removed) } while True: @@ -178,12 +180,18 @@ def imu_monitor(shared_state, console_queue, log_queue): imu_data["status"] = imu.calibration # TODO: move_start and move_end don't seem to be used? + # Read raw sensor data for telemetry + try: + imu_data["gyro"] = imu.sensor.gyro + imu_data["accel"] = imu.sensor.linear_acceleration + except Exception: + pass + if imu.moving(): if not imu_data["moving"]: logger.debug("IMU: move start") imu_data["moving"] = True imu_data["move_start"] = time.time() - # DISABLE old method imu_data["quat"] = quaternion.from_float_array( imu.avg_quat ) # Scalar-first (w, x, y, z) diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py index fb96a418..c8807c25 100644 --- a/python/PiFinder/integrator.py +++ b/python/PiFinder/integrator.py @@ -1,68 +1,93 @@ #!/usr/bin/python # -*- coding:utf-8 -*- """ -This module is the solver -* Checks IMU -* Plate solves high-res image - -TODO: -- Rename solved --> pointing_estimate (also includes IMU) -- Rename next_image_solved --> new_solve -- Rename last_image_solve --> prev_solve (previous successful solve) -- Simplify program flow and explain in comments at top -- Refactor into class PointingTracker +Integrator: combines plate solves and IMU dead-reckoning into a single +pointing estimate, then pushes to shared_state. +Telemetry record/replay is handled by TelemetryManager; pointing math +lives in pointing.py. """ -from __future__ import annotations # To support | in typehints (remove this for Python 3.10+) import queue import time import copy import logging -import numpy as np -import quaternion # numpy-quaternion from PiFinder import config from PiFinder import state_utils -import PiFinder.calc_utils as calc_utils from PiFinder.multiproclogging import MultiprocLogging -from PiFinder.types.coordinates import RaDecRoll from PiFinder.solver import get_initialized_solved_dict from PiFinder.pointing_model.imu_dead_reckoning import ImuDeadReckoning -import PiFinder.pointing_model.quaternion_transforms as qt +from PiFinder.pointing import ( + finalize_and_push_solution, + update_imu, + update_plate_solve_and_imu, +) +from PiFinder.telemetry import TelemetryManager logger = logging.getLogger("IMU.Integrator") -# Constants: -# Use IMU tracking if the angle moved is above this -# TODO: May need to adjust this depending on the IMU sensitivity thresholds -IMU_MOVED_ANG_THRESHOLD = np.deg2rad(0.06) - -def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=False): +def integrator( + shared_state, + solver_queue, + console_queue, + log_queue, + is_debug=False, + command_queue=None, + camera_command_queue=None, +): MultiprocLogging.configurer(log_queue) - """ """ if is_debug: logger.setLevel(logging.DEBUG) logger.debug("Starting Integrator") try: - # Dict of RA, Dec, etc. initialized to None: solved = get_initialized_solved_dict() cfg = config.Config() - # Set up dead-reckoning tracking by the IMU: + mount_type = cfg.get_option("mount_type") + logger.debug(f"mount_type = {mount_type}") + imu_dead_reckoning = ImuDeadReckoning(cfg.get_option("screen_direction")) - # imu_dead_reckoning.set_cam2scope_alignment(q_scope2cam) # TODO: Enable when q_scope2cam is available from alignment - # This holds the last image solve position info - # so we can delta for IMU updates last_image_solve = None last_solve_time = time.time() + was_replaying = False + + telemetry = TelemetryManager( + cfg, shared_state, console_queue, camera_command_queue + ) while True: - pointing_updated = False # Flag to track if pointing was updated in this loop + telemetry.poll_commands(command_queue) + + # --- Replay mode --- + if telemetry.replaying: + was_replaying = True + state_utils.sleep_for_framerate(shared_state) + _drain_queue(solver_queue) + event = telemetry.next_replay_event() + if event is not None: + last_image_solve = telemetry.handle_replay_event( + event, + solved, + last_image_solve, + imu_dead_reckoning, + mount_type, + ) + continue + + # Reset integrator state when replay finishes + if was_replaying: + was_replaying = False + last_image_solve = None + solved = get_initialized_solved_dict() + last_solve_time = time.time() + logger.info("Replay ended, integrator state reset") + + # --- Normal mode --- state_utils.sleep_for_framerate(shared_state) # Check for new camera solve in queue @@ -72,16 +97,18 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa except queue.Empty: pass - if type(next_image_solve) is dict: - # TODO: Refactor this bit: + if isinstance(next_image_solve, dict): + telemetry.record_solve( + next_image_solve, + predicted_ra=solved.get("RA"), + predicted_dec=solved.get("Dec"), + ) + # For camera solves, always start from last successful camera solve # NOT from shared_state (which may contain IMU drift) - # This prevents IMU noise accumulation during failed solves if last_image_solve: solved = copy.deepcopy(last_image_solve) - # If no successful solve yet, keep initial solved dict - # TODO: Create a function to update solve? # Update solve metadata (always needed for auto-exposure) for key in [ "Matches", @@ -96,191 +123,41 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa if next_image_solve.get("RA") is not None: solved.update(next_image_solve) - # For failed solves, preserve ALL position data from previous solve - # Don't recalculate from GPS (causes drift from GPS noise) - if solved["RA"] is not None: - # Successfully plate-solved: last_image_solve = copy.deepcopy(solved) solved["solve_source"] = "CAM" 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) - pointing_updated = True + finalize_and_push_solution(shared_state, solved, mount_type) else: - # Failed solve - clear constellation solved["solve_source"] = "CAM_FAILED" - solved["constellation"] = "" # NOTE: This gets over-written by IMU dead-reckoning - # Push failed solved immediately - # This ensures auto-exposure sees Matches=0 for failed solves + solved["constellation"] = "" shared_state.set_solution(solved) shared_state.set_solve_state(False) - if imu_dead_reckoning.tracking and not pointing_updated: - # Previous plate-solve exists so use IMU dead-reckoning from - # the last plate solved coordinates. + elif imu_dead_reckoning.tracking: imu = shared_state.imu() if imu: + telemetry.record_imu(imu) update_imu(imu_dead_reckoning, solved, last_image_solve, imu) - pointing_updated = True - # Update Alt, Az only if newer than last push - if pointing_updated and solved["solve_time"] > last_solve_time: - solved["constellation"] = get_constellation(solved["RA"], solved["Dec"]) + # Push IMU updates only if newer than last push + if solved["RA"] and solved["solve_time"] > last_solve_time: + last_solve_time = time.time() + finalize_and_push_solution(shared_state, solved, mount_type) - # TODO: Altaz doesn't seem to be required for catalogs when in - # EQ mode? Could be disabled in future when in EQ mode? - solved["Alt"], solved["Az"] = get_alt_az(solved["RA"], solved["Dec"], - shared_state.location(), - shared_state.datetime()) - - if (solved["RA"] is not None) and (solved["Dec"] is not None): - # Push new solved to shared state - shared_state.set_solution(solved) - shared_state.set_solve_state(True) - last_solve_time = solved["solve_time"] + telemetry.flush() except EOFError: logger.error("Main no longer running for integrator") + finally: + telemetry.stop() -# ======== Wrapper and helper functions =============================== - -def update_plate_solve_and_imu(imu_dead_reckoning: ImuDeadReckoning, solved: dict): - """ - Wrapper for ImuDeadReckoning.update_plate_solve_and_imu() to - interface angles in degrees to radians. - - This updates the pointing model with the plate-solved coordinates and the - IMU measurements which are assumed to have been taken at the same time. - """ - if (solved["RA"] is None) or (solved["Dec"] is None): - return # No update - else: - # Successfully plate solved & camera pointing exists - if solved["imu_quat"] is None: - q_x2imu = quaternion.quaternion(np.nan) - else: - q_x2imu = solved["imu_quat"] # IMU measurement at the time of plate solving - - # Update: - solved_cam = RaDecRoll( - solved["camera_center"]["RA"], - solved["camera_center"]["Dec"], - solved["camera_center"]["Roll"], - deg=True - ) - imu_dead_reckoning.update_plate_solve_and_imu(solved_cam, q_x2imu) - - # Set alignment. TODO: Do this once at alignment. Move out of here. - set_cam2scope_alignment(imu_dead_reckoning, solved) - - -def update_imu( - imu_dead_reckoning: ImuDeadReckoning, - solved: dict, - last_image_solve: dict, - imu: dict, -): - """ - Updates the solved dictionary using IMU dead-reckoning from the last - solved pointing. - """ - if not (last_image_solve and imu_dead_reckoning.tracking): - return # Need all of these to do IMU dead-reckoning - - assert isinstance( - imu["quat"], quaternion.quaternion - ), "Expecting quaternion.quaternion type" # TODO: Can be removed later - q_x2imu = imu["quat"] # Current IMU measurement (quaternion) - imu_time = time.time() - - # When moving, switch to tracking using the IMU - angle_moved = qt.get_quat_angular_diff(last_image_solve["imu_quat"], q_x2imu) - if angle_moved > IMU_MOVED_ANG_THRESHOLD: - # Estimate camera pointing using IMU dead-reckoning - logger.debug( - "Track using IMU: Angle moved since last_image_solve = " - "{:}(> threshold = {:}) | IMU quat = ({:}, {:}, {:}, {:})".format( - np.rad2deg(angle_moved), - np.rad2deg(IMU_MOVED_ANG_THRESHOLD), - q_x2imu.w, - q_x2imu.x, - q_x2imu.y, - q_x2imu.z, - ) - ) - - # Dead-reckoning using IMU - imu_dead_reckoning.update_imu(q_x2imu) # Latest IMU measurement - - # Store current camera pointing estimate: - cam_eq = imu_dead_reckoning.get_cam_radec() - ( - solved["camera_center"]["RA"], - solved["camera_center"]["Dec"], - solved["camera_center"]["Roll"], - ) = cam_eq.get(deg=True) - - # Store the current scope pointing estimate - scope_eq = imu_dead_reckoning.get_scope_radec() - solved["RA"], solved["Dec"], solved["Roll"] = scope_eq.get(deg=True) - solved["solve_time"] = imu_time - solved["solve_source"] = "IMU" - - # Logging for states updated in solved: - logger.debug( - "IMU update: scope: RA: {:}, Dec: {:}, Roll: {:}".format( - solved["RA"], solved["Dec"], solved["Roll"] - ) - ) - logger.debug( - "IMU update: camera_center: RA: {:}, Dec: {:}, Roll: {:}".format( - solved["camera_center"]["RA"], - solved["camera_center"]["Dec"], - solved["camera_center"]["Roll"], - ) - ) - - -def get_constellation(RA_deg, Dec_deg) -> str: - """ - Get constellation name from the current RA/Dec position. - """ - if RA_deg is None or Dec_deg is None: - return "" - else: - return calc_utils.sf_utils.radec_to_constellation(RA_deg, Dec_deg) - - -def get_alt_az(RA_deg, Dec_deg, location, dt) -> tuple[float | None, float | None]: - """ - Get Alt/Az from RA/Dec, location and datetime. - RETURNS: alt_deg, az_deg - """ - if RA_deg is None or Dec_deg is None or location is None or dt is None: - return None, None - else: - calc_utils.sf_utils.set_location(location.lat, location.lon, location.altitude) - return calc_utils.sf_utils.radec_to_altaz(RA_deg, Dec_deg, dt) - - -def set_cam2scope_alignment(imu_dead_reckoning: ImuDeadReckoning, solved: dict): - """ - Set alignment. - TODO: Do this once at alignment - """ - # RA, Dec of camera center:: - solved_cam = RaDecRoll( - solved["camera_center"]["RA"], - solved["camera_center"]["Dec"], - solved["camera_center"]["Roll"], - deg=True - ) - - # RA, Dec of target (where scope is pointing): - solved["Roll"] = 0 # Target roll isn't calculated by Tetra3. Set to zero here - solved_scope = RaDecRoll(solved["RA"], solved["Dec"], solved["Roll"], deg=True) - - # Set alignment in imu_dead_reckoning - imu_dead_reckoning.set_cam2scope_alignment(solved_cam, solved_scope) +def _drain_queue(q): + """Discard all pending items from a queue.""" + try: + while True: + q.get(block=False) + except queue.Empty: + pass diff --git a/python/PiFinder/main.py b/python/PiFinder/main.py index d6db0427..9e3bdc58 100644 --- a/python/PiFinder/main.py +++ b/python/PiFinder/main.py @@ -123,6 +123,7 @@ def setup_dirs(): utils.create_path(Path(utils.data_dir, "screenshots")) utils.create_path(Path(utils.data_dir, "solver_debug_dumps")) utils.create_path(Path(utils.data_dir, "logs")) + utils.create_path(Path(utils.data_dir, "telemetry")) os.chmod(Path(utils.data_dir), 0o777) @@ -330,6 +331,8 @@ def main( logger.info("PiFinder running on %s, %s, %s", os_detail, platform, arch) # init UI Modes + integrator_command_queue: Queue = Queue() + command_queues = { "camera": camera_command_queue, "console": console_queue, @@ -337,6 +340,7 @@ def main( "align_command": alignment_command_queue, "align_response": alignment_response_queue, "gps": gps_queue, + "integrator": integrator_command_queue, } cfg = config.Config() @@ -491,6 +495,10 @@ def main( integrator_logqueque, verbose, ), + kwargs={ + "command_queue": integrator_command_queue, + "camera_command_queue": camera_command_queue, + }, ) integrator_process.start() diff --git a/python/PiFinder/pointing.py b/python/PiFinder/pointing.py new file mode 100644 index 00000000..7223ec5f --- /dev/null +++ b/python/PiFinder/pointing.py @@ -0,0 +1,192 @@ +""" +Pointing math: IMU dead-reckoning updates, plate-solve integration, +roll/constellation/altaz finalization. + +Pure functions operating on solved dicts and ImuDeadReckoning — no process +state, no queues. Shared by integrator.py and telemetry.py. +""" + +import datetime +import logging +import time + +import numpy as np +import quaternion # numpy-quaternion + +import PiFinder.calc_utils as calc_utils +from PiFinder.types.coordinates import RaDecRoll +from PiFinder.pointing_model.imu_dead_reckoning import ImuDeadReckoning +import PiFinder.pointing_model.quaternion_transforms as qt + +logger = logging.getLogger("IMU.Integrator") + +# Use IMU tracking if the angle moved is above this +IMU_MOVED_ANG_THRESHOLD = np.deg2rad(0.06) + + +def finalize_and_push_solution(shared_state, solved, mount_type): + """Compute roll, constellation, altaz and push solution to shared_state.""" + location = shared_state.location() + dt = shared_state.datetime() + if location: + calc_utils.sf_utils.set_location(location.lat, location.lon, location.altitude) + + solved["Roll"] = get_roll_by_mount_type( + solved["RA"], solved["Dec"], location, dt, mount_type + ) + solved["constellation"] = calc_utils.sf_utils.radec_to_constellation( + solved["RA"], solved["Dec"] + ) + if location and dt: + solved["Alt"], solved["Az"] = calc_utils.sf_utils.radec_to_altaz( + solved["RA"], solved["Dec"], dt + ) + + shared_state.set_solution(solved) + shared_state.set_solve_state(True) + + +def update_plate_solve_and_imu(imu_dead_reckoning: ImuDeadReckoning, solved: dict): + """ + Wrapper for ImuDeadReckoning.update_plate_solve_and_imu() to + interface angles in degrees to radians. + + This updates the pointing model with the plate-solved coordinates and the + IMU measurements which are assumed to have been taken at the same time. + """ + if solved["RA"] is None or solved["Dec"] is None: + return + + if solved["imu_quat"] is None: + q_x2imu = quaternion.quaternion(np.nan) + else: + q_x2imu = solved["imu_quat"] + + solved_cam = RaDecRoll( + solved["camera_center"]["RA"], + solved["camera_center"]["Dec"], + solved["camera_center"]["Roll"], + deg=True, + ) + imu_dead_reckoning.update_plate_solve_and_imu(solved_cam, q_x2imu) + + set_cam2scope_alignment(imu_dead_reckoning, solved) + + +def update_imu( + imu_dead_reckoning: ImuDeadReckoning, + solved: dict, + last_image_solve: dict, + imu: dict, +): + """ + Updates the solved dictionary using IMU dead-reckoning from the last + solved pointing. + """ + if not (last_image_solve and imu_dead_reckoning.tracking): + return + + assert isinstance( + imu["quat"], quaternion.quaternion + ), "Expecting quaternion.quaternion type" + q_x2imu = imu["quat"] + + angle_moved = qt.get_quat_angular_diff(last_image_solve["imu_quat"], q_x2imu) + if angle_moved > IMU_MOVED_ANG_THRESHOLD: + logger.debug( + "Track using IMU: Angle moved since last_image_solve = " + "{:}(> threshold = {:}) | IMU quat = ({:}, {:}, {:}, {:})".format( + np.rad2deg(angle_moved), + np.rad2deg(IMU_MOVED_ANG_THRESHOLD), + q_x2imu.w, + q_x2imu.x, + q_x2imu.y, + q_x2imu.z, + ) + ) + + imu_dead_reckoning.update_imu(q_x2imu) + + cam_eq = imu_dead_reckoning.get_cam_radec() + ( + solved["camera_center"]["RA"], + solved["camera_center"]["Dec"], + solved["camera_center"]["Roll"], + ) = cam_eq.get(deg=True) + + scope_eq = imu_dead_reckoning.get_scope_radec() + solved["RA"], solved["Dec"], solved["Roll"] = scope_eq.get(deg=True) + + solved["solve_time"] = time.time() + solved["solve_source"] = "IMU" + + logger.debug( + "IMU update: scope: RA: {:}, Dec: {:}, Roll: {:}".format( + solved["RA"], solved["Dec"], solved["Roll"] + ) + ) + logger.debug( + "IMU update: camera_center: RA: {:}, Dec: {:}, Roll: {:}".format( + solved["camera_center"]["RA"], + solved["camera_center"]["Dec"], + solved["camera_center"]["Roll"], + ) + ) + + +def set_cam2scope_alignment(imu_dead_reckoning: ImuDeadReckoning, solved: dict): + """Set the camera-to-scope alignment in the dead-reckoning model.""" + solved_cam = RaDecRoll( + solved["camera_center"]["RA"], + solved["camera_center"]["Dec"], + solved["camera_center"]["Roll"], + deg=True, + ) + + solved["Roll"] = 0 # Target roll isn't calculated by Tetra3 + solved_scope = RaDecRoll( + solved["RA"], solved["Dec"], solved["Roll"], deg=True + ) + + imu_dead_reckoning.set_cam2scope_alignment(solved_cam, solved_scope) + + +def get_roll_by_mount_type( + ra_deg: float, + dec_deg: float, + location, + dt: datetime.datetime, + mount_type: str, +) -> float: + """ + Returns the roll (in degrees) depending on the mount type so that the chart + is displayed appropriately for the mount type. + + * Alt/Az mount: Display the chart in the horizontal coordinate so that up + in the chart points to the Zenith. + * EQ mount: Display the chart in the equatorial coordinate system with the + NCP up so roll = 0. + + Assumes that location has already been set in calc_utils.sf_utils. + """ + if mount_type == "Alt/Az": + if location and dt: + roll_deg = calc_utils.sf_utils.radec_to_roll(ra_deg, dec_deg, dt) + + # HACK: The IMU direction flips at a certain point. Could be due to + # an issue in calc_utils.sf_utils.hadec_to_roll(). + ha_deg = calc_utils.sf_utils.ra_to_ha(ra_deg, dt) + roll_deg = roll_deg - np.sign(ha_deg) * 180 + else: + roll_deg = 0.0 + elif mount_type == "EQ": + roll_deg = 0.0 + else: + logger.error(f"Unknown mount type: {mount_type}. Cannot set roll.") + roll_deg = 0.0 + + # Adjust roll for hemisphere + if location and location.lat < 0.0: + roll_deg += 180.0 + + return roll_deg diff --git a/python/PiFinder/telemetry.py b/python/PiFinder/telemetry.py new file mode 100644 index 00000000..d1293439 --- /dev/null +++ b/python/PiFinder/telemetry.py @@ -0,0 +1,620 @@ +""" +Telemetry recording and replay for the integrator. + +Records IMU readings and plate solves with accurate timing to JSONL files +in ~/PiFinder_data/telemetry/. Replay mode feeds recorded data back through +the integrator for bench testing. +""" + +import copy +import json +import logging +import threading +import time +from collections import deque +from datetime import datetime +from pathlib import Path + +import quaternion as quaternion_module + +from PiFinder import utils +from PiFinder import calc_utils +from PiFinder.pointing import ( + finalize_and_push_solution, + update_imu, + update_plate_solve_and_imu, +) + +logger = logging.getLogger("Telemetry") + +TELEMETRY_DIR = Path(utils.data_dir) / "telemetry" + + +_R = 5 # decimal places for float rounding (BNO055 is ~14-bit) + +# Stationary IMU downsampling: record every Nth sample when not moving +_STATIONARY_DECIMATION = 10 + + +def _rf(v): + """Round a float for compact serialization.""" + return round(v, _R) + + +def _serialize_quat(q): + """Serialize a quaternion to a list [w, x, y, z].""" + if q is None: + return None + try: + return [_rf(q.w), _rf(q.x), _rf(q.y), _rf(q.z)] + except (AttributeError, TypeError): + return None + + +def _serialize_vec(v): + """Serialize a 3-tuple/list to rounded list, or None.""" + if v is None: + return None + try: + return [_rf(v[0]), _rf(v[1]), _rf(v[2])] + except (TypeError, IndexError): + return None + + +class TelemetryRecorder: + """ + Records IMU and solve events to a JSONL file. + + Uses a deque buffer flushed every 5 seconds by a background thread. + When disabled, all methods are no-ops. + """ + + def __init__(self): + self.enabled = False + self.images_enabled = False + self._buffer = deque(maxlen=300) + self._file = None + self._flush_thread = None + self._stop_event = threading.Event() + self._session_dir = None + self._last_flush = 0.0 + self._imu_skip_count = 0 + self._last_target_id = None + + def start(self, cfg, shared_state): + """Start a new recording session.""" + if self.enabled: + self.stop() + + TELEMETRY_DIR.mkdir(parents=True, exist_ok=True) + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + self._session_dir = TELEMETRY_DIR / timestamp + self._session_dir.mkdir(parents=True, exist_ok=True) + session_file = self._session_dir / "session.jsonl" + + self._file = open(session_file, "a") + self.enabled = True + self._last_flush = time.time() + + # Write header (no location — written to separate .location file) + dt = shared_state.datetime() + header = { + "t": time.time(), + "e": "hdr", + "dt": dt.isoformat() if dt else None, + "cfg": { + "integrator": cfg.get_option("imu_integrator"), + "mount_type": cfg.get_option("mount_type"), + }, + } + self._buffer.append(json.dumps(header) + "\n") + + # Write location to a separate file to avoid leaking it in shared recordings + location = shared_state.location() + if location: + loc_file = self._session_dir / "session.location" + loc_data = { + "lat": location.lat, + "lon": location.lon, + "altitude": location.altitude, + } + loc_file.write_text(json.dumps(loc_data)) + + # Start flush thread + self._stop_event.clear() + self._flush_thread = threading.Thread( + target=self._flush_loop, daemon=True, name="telemetry-flush" + ) + self._flush_thread.start() + logger.info("Telemetry recording started: %s", session_file) + + def stop(self): + """Stop the current recording session.""" + if not self.enabled: + return + self.enabled = False + self._stop_event.set() + if self._flush_thread: + self._flush_thread.join(timeout=2) + self._flush_thread = None + self._do_flush() + if self._file: + self._file.close() + self._file = None + self._session_dir = None + logger.info("Telemetry recording stopped") + + def record_imu(self, imu): + """Record an IMU reading. No-op if disabled. + + When stationary, only records every _STATIONARY_DECIMATION-th sample + to reduce file size during long sessions. + """ + if not self.enabled or imu is None: + return + moving = imu.get("moving", False) + if not moving: + self._imu_skip_count += 1 + if self._imu_skip_count < _STATIONARY_DECIMATION: + return + self._imu_skip_count = 0 + else: + self._imu_skip_count = 0 + record = { + "t": _rf(time.time()), + "e": "imu", + "q": _serialize_quat(imu.get("quat")), + "pos": _serialize_vec(imu.get("pos")), + "mv": moving, + "st": imu.get("status", 0), + "gyro": _serialize_vec(imu.get("gyro")), + "accel": _serialize_vec(imu.get("accel")), + } + self._buffer.append(json.dumps(record) + "\n") + + def record_solve(self, solve_dict, predicted_ra=None, predicted_dec=None): + """Record a plate solve result. No-op if disabled. + + predicted_ra/predicted_dec are the integrator's IMU-predicted position + just before the solve arrived, enabling drift measurement. + + Returns the timestamp used for the record, or None if not recorded. + """ + if not self.enabled or solve_dict is None: + return None + t = time.time() + cam = solve_dict.get("camera_center", {}) + cam_is_dict = isinstance(cam, dict) + record = { + "t": _rf(t), + "e": "solve", + "ra": _rf(solve_dict["RA"]) if solve_dict.get("RA") is not None else None, + "dec": _rf(solve_dict["Dec"]) + if solve_dict.get("Dec") is not None + else None, + "roll": _rf(solve_dict["Roll"]) + if solve_dict.get("Roll") is not None + else None, + "pred_ra": _rf(predicted_ra) if predicted_ra is not None else None, + "pred_dec": _rf(predicted_dec) if predicted_dec is not None else None, + "cam_ra": _rf(cam["RA"]) + if cam_is_dict and cam.get("RA") is not None + else None, + "cam_dec": _rf(cam["Dec"]) + if cam_is_dict and cam.get("Dec") is not None + else None, + "cam_roll": _rf(cam["Roll"]) + if cam_is_dict and cam.get("Roll") is not None + else None, + "iq": _serialize_quat(solve_dict.get("imu_quat")), + "ip": _serialize_vec(solve_dict.get("imu_pos")), + "matches": solve_dict.get("Matches"), + "rmse": _rf(solve_dict["RMSE"]) + if solve_dict.get("RMSE") is not None + else None, + "lsa": solve_dict.get("last_solve_attempt"), + "lss": solve_dict.get("last_solve_success"), + "src": solve_dict.get("solve_source"), + } + self._buffer.append(json.dumps(record) + "\n") + return t + + def record_target(self, target, alt=None, az=None): + """Record a target change event. Pass None when target is cleared.""" + if not self.enabled: + return + if target is None: + target_id = None + else: + target_id = getattr(target, "object_id", None) + + if target_id == self._last_target_id: + return + self._last_target_id = target_id + + if target is None: + record = { + "t": _rf(time.time()), + "e": "tgt", + "name": None, + "ra": None, + "dec": None, + "alt": None, + "az": None, + } + else: + record = { + "t": _rf(time.time()), + "e": "tgt", + "name": getattr(target, "display_name", None), + "ra": _rf(target.ra) if target.ra is not None else None, + "dec": _rf(target.dec) if target.dec is not None else None, + "alt": _rf(alt) if alt is not None else None, + "az": _rf(az) if az is not None else None, + } + self._buffer.append(json.dumps(record) + "\n") + + def get_session_dir(self): + """Return current session directory path, or None.""" + return self._session_dir + + def flush(self): + """Time-gated flush - only actually flushes every 5 seconds.""" + if not self.enabled: + return + now = time.time() + if now - self._last_flush >= 5.0: + self._do_flush() + self._last_flush = now + + def _do_flush(self): + """Flush the buffer to disk.""" + if not self._file or not self._buffer: + return + lines = [] + while self._buffer: + try: + lines.append(self._buffer.popleft()) + except IndexError: + break + if lines: + self._file.writelines(lines) + self._file.flush() + + def _flush_loop(self): + """Background thread that flushes every 5 seconds.""" + while not self._stop_event.is_set(): + self._stop_event.wait(5.0) + self._do_flush() + + +class TelemetryPlayer: + """ + Reads a recorded JSONL session and replays events with original timing. + """ + + def __init__(self, path): + self.path = Path(path) + self.events = [] + self.header = None + self._index = 0 + self._base_time = None + self._replay_start = None + self._load() + + def _load(self): + """Load all events from the JSONL file.""" + file_path = self.path + if file_path.is_dir(): + file_path = file_path / "session.jsonl" + + with open(file_path) as f: + for line in f: + line = line.strip() + if not line: + continue + event = json.loads(line) + if event.get("e") == "hdr": + self.header = event + else: + self.events.append(event) + + if self.events: + self._base_time = self.events[0]["t"] + logger.info("Loaded telemetry: %d events from %s", len(self.events), file_path) + + def reset(self): + """Reset replay to the beginning.""" + self._index = 0 + self._replay_start = None + + def get_next_event(self): + """ + Return the next event if its relative timestamp has elapsed, + otherwise return None. Call this in a loop. + + Returns (event_dict, done_bool). + """ + if self._index >= len(self.events): + return None, True + + if self._replay_start is None: + self._replay_start = time.time() + + event = self.events[self._index] + event_offset = event["t"] - self._base_time + elapsed = time.time() - self._replay_start + + if elapsed >= event_offset: + self._index += 1 + return event, self._index >= len(self.events) + + return None, False + + @property + def progress(self): + """Return replay progress as a fraction 0.0-1.0.""" + if not self.events: + return 1.0 + return self._index / len(self.events) + + @property + def total_events(self): + return len(self.events) + + @property + def current_index(self): + return self._index + + @staticmethod + def event_to_solve_dict(event): + """Convert a recorded solve event to the fields needed by solved dict.""" + result = { + "RA": event["ra"], + "Dec": event["dec"], + "Roll": event.get("roll"), + "camera_center": { + "RA": event.get("cam_ra"), + "Dec": event.get("cam_dec"), + "Roll": event.get("cam_roll"), + "Alt": None, + "Az": None, + }, + "Matches": event.get("matches"), + "RMSE": event.get("rmse"), + "last_solve_attempt": event.get("lsa"), + "last_solve_success": event.get("lss"), + "solve_source": event.get("src", "CAM"), + "solve_time": event["t"], + "imu_pos": event.get("ip"), + } + iq = event.get("iq") + if iq: + result["imu_quat"] = quaternion_module.quaternion( + iq[0], iq[1], iq[2], iq[3] + ) + return result + + @staticmethod + def event_to_imu_dict(event): + """Convert a recorded IMU event to an imu dict, or None if no quat.""" + q = event.get("q") + if not q: + return None + return { + "quat": quaternion_module.quaternion(q[0], q[1], q[2], q[3]), + "moving": event.get("mv", False), + "status": event.get("st", 0), + } + + +class TelemetryManager: + """ + Facade over TelemetryRecorder and TelemetryPlayer. + + Owns all telemetry I/O: command dispatch, recording, replay state, + image saving, and console/camera queue messaging. The integrator + only needs to call a handful of one-liners. + """ + + def __init__(self, cfg, shared_state, console_queue, camera_command_queue=None): + self._cfg = cfg + self._shared_state = shared_state + self._console_queue = console_queue + self._camera_command_queue = camera_command_queue + self._recorder = TelemetryRecorder() + self._recorder.images_enabled = bool(cfg.get_option("telemetry_images")) + self._player = None + if cfg.get_option("telemetry_record"): + self._recorder.start(cfg, shared_state) + + @property + def replaying(self): + return self._player is not None + + def poll_commands(self, command_queue): + """Check for and dispatch any pending telemetry commands.""" + if command_queue is None: + return + import queue + + try: + cmd = command_queue.get(block=False) + if isinstance(cmd, tuple): + self._handle_command(cmd[0], cmd[1]) + except queue.Empty: + pass + + def _handle_command(self, cmd_name, cmd_arg): + """Dispatch a telemetry command.""" + if cmd_name == "telemetry_record_on": + self._recorder.images_enabled = bool( + self._cfg.get_option("telemetry_images") + ) + self._recorder.start(self._cfg, self._shared_state) + self._console_queue.put("Telemetry: Recording") + + elif cmd_name == "telemetry_record_off": + self._recorder.stop() + self._console_queue.put("Telemetry: Stopped") + + elif cmd_name == "replay": + logger.info("Entering replay mode: %s", cmd_arg) + self._player = TelemetryPlayer(cmd_arg) + if self._player.header: + self._apply_replay_header(self._player.header, self._shared_state) + self._console_queue.put("Telemetry: Replay started") + + elif cmd_name == "replay_stop": + logger.info("Exiting replay mode") + self._player = None + self._restart_camera() + self._console_queue.put("Telemetry: Replay stopped") + + def next_replay_event(self): + """Return the next replay event, or None. + + Automatically clears replay state and restarts camera when done. + """ + if self._player is None: + return None + event, done = self._player.get_next_event() + if done and event is None: + self._player = None + self._restart_camera() + self._console_queue.put("Telemetry: Replay finished") + logger.info("Replay finished") + return None + return event + + def handle_replay_event( + self, event, solved, last_image_solve, imu_dead_reckoning, mount_type + ): + """Process a single replayed event. Returns updated last_image_solve.""" + if event["e"] == "imu": + imu = TelemetryPlayer.event_to_imu_dict(event) + if imu and last_image_solve and imu_dead_reckoning.tracking: + update_imu(imu_dead_reckoning, solved, last_image_solve, imu) + if solved["RA"] is not None: + finalize_and_push_solution(self._shared_state, solved, mount_type) + + elif event["e"] == "solve": + replay_dict = TelemetryPlayer.event_to_solve_dict(event) + + # Always update metadata (needed for auto-exposure) + for key in [ + "Matches", + "RMSE", + "last_solve_attempt", + "last_solve_success", + ]: + if replay_dict.get(key) is not None: + solved[key] = replay_dict[key] + + if event.get("ra") is not None: + # Successful solve — update position and push + solved.update(replay_dict) + self._shared_state.set_solve_state(True) + update_plate_solve_and_imu(imu_dead_reckoning, solved) + finalize_and_push_solution(self._shared_state, solved, mount_type) + return copy.deepcopy(solved) + else: + # Failed solve — mirror normal-mode behavior + solved["solve_source"] = "CAM_FAILED" + solved["constellation"] = "" + self._shared_state.set_solution(solved) + self._shared_state.set_solve_state(False) + + return last_image_solve + + def record_solve(self, solve_dict, predicted_ra=None, predicted_dec=None): + """Record a solve event and send save_image command if enabled.""" + t = self._recorder.record_solve(solve_dict, predicted_ra, predicted_dec) + if ( + t is not None + and self._recorder.images_enabled + and self._camera_command_queue is not None + ): + session_dir = self._recorder.get_session_dir() + if session_dir: + self._camera_command_queue.put( + f"save_image:{session_dir / f'img_{t:.3f}.png'}" + ) + + def record_imu(self, imu): + self._recorder.record_imu(imu) + + def flush(self): + self._poll_target() + self._recorder.flush() + + def _poll_target(self): + """Check if the user's target changed and record it.""" + if not self._recorder.enabled: + return + try: + target = self._shared_state.ui_state().target() + except Exception: + return + alt, az = None, None + if target is not None and target.ra is not None: + try: + location = self._shared_state.location() + dt = self._shared_state.datetime() + if location and dt: + calc_utils.sf_utils.set_location( + location.lat, location.lon, location.altitude + ) + alt, az = calc_utils.sf_utils.radec_to_altaz( + target.ra, target.dec, dt + ) + except Exception: + pass + self._recorder.record_target(target, alt=alt, az=az) + + def stop(self): + self._recorder.stop() + + def _restart_camera(self): + if self._camera_command_queue is not None: + self._camera_command_queue.put("start") + + def _apply_replay_header(self, hdr, shared_state): + """Apply location/datetime from a replay session header.""" + loc_data = self._load_replay_location() + if loc_data: + loc = shared_state.location() + loc.lat = loc_data["lat"] + loc.lon = loc_data["lon"] + loc.altitude = loc_data["altitude"] + loc.lock = True + loc.source = "replay" + shared_state.set_location(loc) + if hdr.get("dt"): + shared_state.set_datetime(datetime.fromisoformat(hdr["dt"])) + + cfg = hdr.get("cfg", {}) + recorded_mount = cfg.get("mount_type") + current_mount = self._cfg.get_option("mount_type") + if recorded_mount and current_mount and recorded_mount != current_mount: + logger.warning( + "Replay mount_type '%s' differs from current config '%s'", + recorded_mount, + current_mount, + ) + + def _load_replay_location(self): + """Load location from the .location sidecar file, or None.""" + if self._player is None: + return None + loc_file = self._player.path + if loc_file.is_file(): + loc_file = loc_file.parent / "session.location" + else: + loc_file = loc_file / "session.location" + if loc_file.exists(): + try: + return json.loads(loc_file.read_text()) + except (json.JSONDecodeError, OSError): + return None + return None diff --git a/python/PiFinder/ui/callbacks.py b/python/PiFinder/ui/callbacks.py index f5e6fc91..79a545b7 100644 --- a/python/PiFinder/ui/callbacks.py +++ b/python/PiFinder/ui/callbacks.py @@ -479,6 +479,20 @@ def generate_custom_object_name(ui_module: UIModule) -> str: return f"CUSTOM {max_num + 1}" +def telemetry_record_toggle(ui_module: UIModule) -> None: + """Toggle telemetry recording on/off via integrator command queue.""" + enabled = ui_module.config_object.get_option("telemetry_record") + if "integrator" in ui_module.command_queues: + if enabled: + ui_module.command_queues["integrator"].put(("telemetry_record_on", None)) + ui_module.message("Telemetry\nRecording", 2) + else: + ui_module.command_queues["integrator"].put(("telemetry_record_off", None)) + ui_module.message("Telemetry\nStopped", 2) + else: + ui_module.message("No integrator\nqueue", 2) + + def update_gpsd_baud_rate(ui_module: UIModule) -> None: """ Updates the GPSD configuration with the current baud rate setting. diff --git a/python/PiFinder/ui/menu_structure.py b/python/PiFinder/ui/menu_structure.py index 9a0cabc0..d174d6aa 100644 --- a/python/PiFinder/ui/menu_structure.py +++ b/python/PiFinder/ui/menu_structure.py @@ -15,6 +15,7 @@ from PiFinder.ui.location_list import UILocationList from PiFinder.ui.locationentry import UILocationEntry from PiFinder.ui.radec_entry import UIRADecEntry +from PiFinder.ui.telemetry_list import UITelemetryList import PiFinder.ui.callbacks as callbacks @@ -1187,6 +1188,57 @@ def _(key: str) -> Any: }, ], }, + { + "name": _("Dev Tools"), + "class": UITextMenu, + "select": "single", + "items": [ + { + "name": _("Telemetry"), + "class": UITextMenu, + "select": "single", + "items": [ + { + "name": _("Record"), + "class": UITextMenu, + "select": "single", + "config_option": "telemetry_record", + "post_callback": callbacks.telemetry_record_toggle, + "items": [ + { + "name": _("Off"), + "value": False, + }, + { + "name": _("On"), + "value": True, + }, + ], + }, + { + "name": _("Images"), + "class": UITextMenu, + "select": "single", + "config_option": "telemetry_images", + "items": [ + { + "name": _("Off"), + "value": False, + }, + { + "name": _("On"), + "value": True, + }, + ], + }, + { + "name": _("Load"), + "class": UITelemetryList, + }, + ], + }, + ], + }, ], }, { diff --git a/python/PiFinder/ui/telemetry_list.py b/python/PiFinder/ui/telemetry_list.py new file mode 100644 index 00000000..b5aefcd2 --- /dev/null +++ b/python/PiFinder/ui/telemetry_list.py @@ -0,0 +1,104 @@ +""" +UI screen for listing and loading telemetry recording sessions. + +Lists .jsonl files from ~/PiFinder_data/telemetry/ with filename and size. +Selecting a file triggers replay via the integrator command queue. +""" + +import logging + +from PiFinder.ui.text_menu import UITextMenu +from PiFinder.telemetry import TELEMETRY_DIR + +from typing import Any, TYPE_CHECKING + +if TYPE_CHECKING: + + def _(a) -> Any: + return a + + +logger = logging.getLogger("UI.TelemetryList") + + +class UITelemetryList(UITextMenu): + """File picker for telemetry sessions.""" + + __title__ = "Telemetry" + + def __init__(self, *args, **kwargs): + self._sessions = self._scan_sessions() + kwargs["item_definition"] = self._create_menu_definition() + super().__init__(*args, **kwargs) + + def _scan_sessions(self): + """Scan telemetry directory for session files.""" + sessions = [] + if not TELEMETRY_DIR.exists(): + return sessions + + # Look for session dirs (contain session.jsonl) and standalone .jsonl files + for entry in sorted(TELEMETRY_DIR.iterdir(), reverse=True): + jsonl_path = None + if entry.is_dir(): + candidate = entry / "session.jsonl" + if candidate.exists(): + jsonl_path = candidate + elif entry.suffix == ".jsonl": + jsonl_path = entry + + if jsonl_path: + size_kb = jsonl_path.stat().st_size / 1024 + label = entry.name + if size_kb >= 1024: + size_str = f"{size_kb / 1024:.1f}MB" + else: + size_str = f"{size_kb:.0f}KB" + sessions.append( + { + "label": label, + "size_str": size_str, + "path": str(entry), + } + ) + return sessions + + def _create_menu_definition(self): + items = [] + for s in self._sessions: + items.append( + { + "name": f"{s['label']} ({s['size_str']})", + "value": s["path"], + } + ) + if not items: + items.append({"name": "No sessions found", "value": None}) + return {"name": "Telemetry", "select": "single", "items": items} + + def key_right(self): + """Select a session to replay.""" + if not self._sessions: + self.message("No sessions", 2) + return False + + idx = self._current_item_index + items = self.item_definition["items"] + if idx >= len(items): + return False + + session_path = items[idx].get("value") + if session_path is None: + return False + + # Send replay command to integrator + if "integrator" in self.command_queues: + self.command_queues["camera"].put("stop") + self.command_queues["integrator"].put(("replay", session_path)) + self.message("Replay\nstarted", 2) + logger.info("Starting telemetry replay: %s", session_path) + else: + self.message("No integrator\nqueue", 2) + logger.warning("Integrator command queue not available") + + return True diff --git a/python/tests/test_integrator_drift.py b/python/tests/test_integrator_drift.py new file mode 100644 index 00000000..68cbdaaf --- /dev/null +++ b/python/tests/test_integrator_drift.py @@ -0,0 +1,409 @@ +""" +Integration tests for IMU dead-reckoning drift through the real integrator +wrapper functions. Replays synthetic telemetry through ImuDeadReckoning and +measures pointing error vs ground truth. + +Catches regressions in: +- Quaternion math (drift explodes) +- Solve incorporation (positions don't update) +- Dead-reckoning (IMU movements not reflected) +- RaDecRoll / quaternion transform correctness +""" + +import copy +from dataclasses import dataclass +from typing import List + +import numpy as np +import pytest +import quaternion + +from PiFinder.pointing import ( + update_imu, + update_plate_solve_and_imu, +) +from PiFinder.pointing_model.imu_dead_reckoning import ImuDeadReckoning +from PiFinder.pointing_model.quaternion_transforms import axis_angle2quat, radec2q_eq +from PiFinder.solver import get_initialized_solved_dict + + +# ── Synthetic telemetry generation ────────────────────────────────── + + +@dataclass +class SolveEvent: + """A plate-solve event with true RA/Dec and IMU quaternion.""" + + timestamp: float + ra_deg: float + dec_deg: float + roll_deg: float + imu_quat: quaternion.quaternion + + +@dataclass +class ImuEvent: + """An IMU-only reading between solves, with ground truth for error measurement.""" + + timestamp: float + imu_quat: quaternion.quaternion + moving: bool + true_ra_deg: float + true_dec_deg: float + + +@dataclass +class Measurement: + """Dead-reckoned vs ground-truth angular error.""" + + error_arcsec: float + timestamp: float + + +def angular_separation_deg(ra1, dec1, ra2, dec2): + """Great-circle angular separation in degrees between two (RA, Dec) pairs in degrees.""" + ra1_r, dec1_r = np.deg2rad(ra1), np.deg2rad(dec1) + ra2_r, dec2_r = np.deg2rad(ra2), np.deg2rad(dec2) + cos_sep = np.sin(dec1_r) * np.sin(dec2_r) + np.cos(dec1_r) * np.cos( + dec2_r + ) * np.cos(ra1_r - ra2_r) + cos_sep = np.clip(cos_sep, -1.0, 1.0) + return np.rad2deg(np.arccos(cos_sep)) + + +def _make_imu_quat_for_radec( + imu_dr: ImuDeadReckoning, ra_rad: float, dec_rad: float, roll_rad: float +) -> quaternion.quaternion: + """ + Compute the IMU quaternion q_x2imu consistent with a given (RA, Dec, Roll), + assuming q_eq2x is identity. + + From: q_eq2cam = q_eq2x * q_x2imu * q_imu2cam + With q_eq2x = I: q_x2imu = q_eq2cam * q_cam2imu + """ + q_eq2cam = radec2q_eq(ra_rad, dec_rad, roll_rad) + q_x2imu = q_eq2cam * imu_dr.q_cam2imu + return q_x2imu.normalized() + + +def generate_stationary_session( + seed: int = 42, + duration_s: float = 10.0, + solve_interval_s: float = 2.0, + imu_rate_hz: float = 10.0, + noise_arcsec: float = 1.0, +) -> list: + """ + Generate a stationary session: scope doesn't move, IMU has small noise. + Returns list of SolveEvent and ImuEvent in chronological order. + """ + rng = np.random.default_rng(seed) + ra_deg, dec_deg, roll_deg = 180.0, 45.0, 0.0 + ra_rad, dec_rad = np.deg2rad(ra_deg), np.deg2rad(dec_deg) + roll_rad = np.deg2rad(0.0) + + tmp_dr = ImuDeadReckoning("flat") + base_quat = _make_imu_quat_for_radec(tmp_dr, ra_rad, dec_rad, roll_rad) + + events = [] + t = 0.0 + imu_dt = 1.0 / imu_rate_hz + next_solve = 0.0 + + while t <= duration_s: + noise_rad = np.deg2rad(noise_arcsec / 3600.0) + axis = rng.normal(size=3) + axis /= np.linalg.norm(axis) + angle = rng.normal(scale=noise_rad) + q_noise = axis_angle2quat(axis, angle) + noisy_quat = (base_quat * q_noise).normalized() + + if t >= next_solve: + events.append( + SolveEvent( + timestamp=t, + ra_deg=ra_deg, + dec_deg=dec_deg, + roll_deg=roll_deg, + imu_quat=noisy_quat, + ) + ) + next_solve = t + solve_interval_s + else: + events.append( + ImuEvent( + timestamp=t, + imu_quat=noisy_quat, + moving=False, + true_ra_deg=ra_deg, + true_dec_deg=dec_deg, + ) + ) + + t += imu_dt + + return events + + +def generate_slew_session( + seed: int = 123, + duration_s: float = 30.0, + solve_interval_s: float = 3.0, + imu_rate_hz: float = 10.0, + slew_speed_deg_s: float = 2.0, + drift_arcsec_s: float = 5.0, +) -> list: + """ + Generate a slewing session: scope moves in RA at constant rate, + IMU has a slow systematic drift added on top. + """ + rng = np.random.default_rng(seed) + start_ra_deg, dec_deg, roll_deg = 90.0, 30.0, 0.0 + + tmp_dr = ImuDeadReckoning("flat") + events = [] + t = 0.0 + imu_dt = 1.0 / imu_rate_hz + next_solve = 0.0 + last_solve_time = 0.0 + + drift_axis = rng.normal(size=3) + drift_axis /= np.linalg.norm(drift_axis) + drift_rate_rad_s = np.deg2rad(drift_arcsec_s / 3600.0) + + while t <= duration_s: + true_ra_deg = start_ra_deg + slew_speed_deg_s * t + true_ra_rad = np.deg2rad(true_ra_deg) + dec_rad = np.deg2rad(dec_deg) + roll_rad = np.deg2rad(roll_deg) + + base_quat = _make_imu_quat_for_radec(tmp_dr, true_ra_rad, dec_rad, roll_rad) + + time_since_solve = t - last_solve_time + drift_angle = drift_rate_rad_s * time_since_solve + q_drift = axis_angle2quat(drift_axis, drift_angle) + + noise_rad = np.deg2rad(1.0 / 3600.0) + noise_axis = rng.normal(size=3) + noise_axis /= np.linalg.norm(noise_axis) + q_noise = axis_angle2quat(noise_axis, rng.normal(scale=noise_rad)) + + drifted_quat = (base_quat * q_drift * q_noise).normalized() + + if t >= next_solve: + events.append( + SolveEvent( + timestamp=t, + ra_deg=true_ra_deg, + dec_deg=dec_deg, + roll_deg=roll_deg, + imu_quat=drifted_quat, + ) + ) + last_solve_time = t + next_solve = t + solve_interval_s + else: + events.append( + ImuEvent( + timestamp=t, + imu_quat=drifted_quat, + moving=True, + true_ra_deg=true_ra_deg, + true_dec_deg=dec_deg, + ) + ) + + t += imu_dt + + return events + + +# ── Replay engine ─────────────────────────────────────────────────── + + +def _populate_solved_from_event(solved: dict, event: SolveEvent): + """Fill the solved dict from a solve event, mirroring the real integrator.""" + solved["RA"] = event.ra_deg + solved["Dec"] = event.dec_deg + solved["Roll"] = event.roll_deg + solved["camera_center"]["RA"] = event.ra_deg + solved["camera_center"]["Dec"] = event.dec_deg + solved["camera_center"]["Roll"] = event.roll_deg + solved["camera_solve"] = { + "RA": event.ra_deg, + "Dec": event.dec_deg, + "Roll": event.roll_deg, + } + solved["imu_quat"] = event.imu_quat + solved["solve_time"] = event.timestamp + solved["solve_source"] = "CAM" + + +def replay_imu_drift(events: list) -> List[Measurement]: + """ + Replay telemetry and measure dead-reckoning error at each IMU update + by comparing the integrator's solved position to ground truth. + """ + imu_dr = ImuDeadReckoning("flat") + solved = get_initialized_solved_dict() + last_image_solve = None + measurements: List[Measurement] = [] + + for event in events: + if isinstance(event, SolveEvent): + _populate_solved_from_event(solved, event) + last_image_solve = copy.deepcopy(solved) + update_plate_solve_and_imu(imu_dr, solved) + + elif isinstance(event, ImuEvent): + if last_image_solve is None: + continue + + imu_dict = {"quat": event.imu_quat, "moving": event.moving} + update_imu(imu_dr, solved, last_image_solve, imu_dict) + + if solved["RA"] is not None: + error_deg = angular_separation_deg( + solved["RA"], + solved["Dec"], + event.true_ra_deg, + event.true_dec_deg, + ) + measurements.append( + Measurement( + error_arcsec=error_deg * 3600.0, + timestamp=event.timestamp, + ) + ) + + return measurements + + +def replay_post_solve_errors(events: list, max_readings: int = 3) -> List[Measurement]: + """ + Measure dead-reckoning error of the first few IMU readings after each solve + vs ground truth. Verifies that solve incorporation resets drift. + """ + imu_dr = ImuDeadReckoning("flat") + solved = get_initialized_solved_dict() + last_image_solve = None + measurements: List[Measurement] = [] + readings_since_solve = max_readings + + for event in events: + if isinstance(event, SolveEvent): + _populate_solved_from_event(solved, event) + last_image_solve = copy.deepcopy(solved) + update_plate_solve_and_imu(imu_dr, solved) + readings_since_solve = 0 + + elif isinstance(event, ImuEvent): + if last_image_solve is None: + continue + + imu_dict = {"quat": event.imu_quat, "moving": event.moving} + update_imu(imu_dr, solved, last_image_solve, imu_dict) + + if readings_since_solve < max_readings and solved["RA"] is not None: + error_deg = angular_separation_deg( + solved["RA"], + solved["Dec"], + event.true_ra_deg, + event.true_dec_deg, + ) + measurements.append( + Measurement( + error_arcsec=error_deg * 3600.0, + timestamp=event.timestamp, + ) + ) + readings_since_solve += 1 + + return measurements + + +# ── Tests ─────────────────────────────────────────────────────────── + + +@pytest.mark.integration +class TestIntegratorDrift: + """ + Integration tests that replay synthetic telemetry through the real + ImuDeadReckoning and integrator wrapper functions, measuring drift + vs ground truth. + """ + + def test_stationary_drift(self): + """ + Scope is stationary, IMU has only tiny noise. + Dead-reckoned position should stay very close to truth. + """ + events = generate_stationary_session(seed=42, duration_s=10.0) + measurements = replay_imu_drift(events) + + assert len(measurements) > 0, "Should have at least one measurement" + errors = [m.error_arcsec for m in measurements] + mean_error = np.mean(errors) + max_error = np.max(errors) + + # Stationary scope with 1 arcsec noise: drift should be tiny + # Baseline: ~0 arcsec (noise below measurement precision) + assert mean_error < 5, ( + f"Stationary mean drift {mean_error:.1f} arcsec exceeds 5 arcsec threshold" + ) + assert max_error < 10, ( + f"Stationary max drift {max_error:.1f} arcsec exceeds 10 arcsec threshold" + ) + + def test_slew_tracking_accuracy(self): + """ + Scope slewing at 2 deg/s with 5 arcsec/s simulated IMU drift. + Dead-reckoning should track the true position with bounded error. + Error should not grow without bound over the session. + """ + events = generate_slew_session(seed=123, duration_s=30.0) + measurements = replay_imu_drift(events) + + assert len(measurements) > 0, "Should have at least one measurement" + errors = [m.error_arcsec for m in measurements] + mean_error = np.mean(errors) + + # With 5 arcsec/s drift and 3s solve intervals, accumulated drift + # between solves is bounded. Baseline: mean ~6, max ~13 arcsec. + assert mean_error < 15, ( + f"Slew mean drift {mean_error:.1f} arcsec exceeds 15 arcsec threshold" + ) + + # Verify errors don't grow without bound across the session + if len(errors) >= 20: + first_quarter = np.mean(errors[: len(errors) // 4]) + last_quarter = np.mean(errors[-len(errors) // 4 :]) + assert last_quarter < first_quarter * 3, ( + f"Drift growing over time: first quarter {first_quarter:.1f}, " + f"last quarter {last_quarter:.1f} arcsec" + ) + + def test_solve_correction_resets_drift(self): + """ + After each solve, the first few IMU dead-reckoned positions should + have near-zero error vs ground truth (solve corrects drift). + """ + events = generate_slew_session( + seed=456, + duration_s=20.0, + solve_interval_s=4.0, + slew_speed_deg_s=1.0, + drift_arcsec_s=2.0, + ) + measurements = replay_post_solve_errors(events, max_readings=3) + + assert len(measurements) > 0, "Should have post-solve measurements" + errors = [m.error_arcsec for m in measurements] + mean_error = np.mean(errors) + + # Right after a solve, error should be just noise + tiny drift. + # Baseline: mean ~7, max ~11 arcsec. + assert mean_error < 20, ( + f"Post-solve mean error {mean_error:.1f} arcsec exceeds 20 arcsec threshold. " + "Solve correction may not be resetting drift properly." + ) diff --git a/python/tests/test_telemetry.py b/python/tests/test_telemetry.py new file mode 100644 index 00000000..546e7ff9 --- /dev/null +++ b/python/tests/test_telemetry.py @@ -0,0 +1,910 @@ +""" +Unit tests for telemetry recording, replay, and the TelemetryManager facade. +""" + +import json +import queue +import time +from pathlib import Path +from unittest.mock import MagicMock, patch + +import numpy as np +import pytest +import quaternion as quaternion_module + +from PiFinder.telemetry import ( + TelemetryManager, + TelemetryPlayer, + TelemetryRecorder, + _serialize_quat, +) + + +# ── Helpers ────────────────────────────────────────────────────────── + + +def _make_quat(w=1.0, x=0.0, y=0.0, z=0.0): + return quaternion_module.quaternion(w, x, y, z) + + +def _make_location(lat=40.0, lon=-74.0, alt=100.0): + loc = MagicMock() + loc.lat = lat + loc.lon = lon + loc.altitude = alt + loc.lock = False + loc.source = "gps" + return loc + + +def _make_shared_state(location=None, dt=None): + ss = MagicMock() + ss.location.return_value = location or _make_location() + ss.datetime.return_value = dt + return ss + + +def _make_cfg( + telemetry_record=False, + telemetry_images=False, + imu_integrator="flat", + mount_type="Alt/Az", +): + cfg = MagicMock() + + def get_option(key): + return { + "telemetry_record": telemetry_record, + "telemetry_images": telemetry_images, + "imu_integrator": imu_integrator, + "mount_type": mount_type, + }.get(key) + + cfg.get_option = get_option + return cfg + + +def _write_session_jsonl(path, events, header=None): + """Write a list of event dicts to a JSONL file.""" + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + with open(path, "w") as f: + if header: + f.write(json.dumps(header) + "\n") + for ev in events: + f.write(json.dumps(ev) + "\n") + + +# ── _serialize_quat ────────────────────────────────────────────────── + + +@pytest.mark.unit +class TestSerializeQuat: + def test_none(self): + assert _serialize_quat(None) is None + + def test_valid(self): + q = _make_quat(1.0, 2.0, 3.0, 4.0) + result = _serialize_quat(q) + assert result == [1.0, 2.0, 3.0, 4.0] + + def test_non_quaternion(self): + assert _serialize_quat("not a quat") is None + + def test_identity(self): + q = _make_quat(1.0, 0.0, 0.0, 0.0) + assert _serialize_quat(q) == [1.0, 0.0, 0.0, 0.0] + + +# ── TelemetryRecorder ─────────────────────────────────────────────── + + +@pytest.mark.unit +class TestTelemetryRecorder: + def test_disabled_by_default(self): + rec = TelemetryRecorder() + assert not rec.enabled + + def test_record_imu_noop_when_disabled(self): + rec = TelemetryRecorder() + rec.record_imu({"quat": _make_quat(), "pos": [0, 0, 0]}) + assert len(rec._buffer) == 0 + + def test_record_solve_noop_when_disabled(self): + rec = TelemetryRecorder() + result = rec.record_solve({"RA": 180.0, "Dec": 45.0}) + assert result is None + assert len(rec._buffer) == 0 + + def test_record_solve_noop_for_none(self): + rec = TelemetryRecorder() + rec.enabled = True + result = rec.record_solve(None) + assert result is None + + def test_start_creates_session(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + cfg = _make_cfg(imu_integrator="flat") + ss = _make_shared_state() + rec.start(cfg, ss) + try: + assert rec.enabled + assert rec._session_dir is not None + assert rec._session_dir.exists() + assert len(rec._buffer) == 1 # header + finally: + rec.stop() + + def test_stop_flushes_and_closes(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + session_dir = rec._session_dir + session_file = session_dir / "session.jsonl" + rec.stop() + assert not rec.enabled + assert rec._file is None + assert rec._session_dir is None + content = session_file.read_text() + assert '"e": "hdr"' in content + # Location should be in separate file, not in header + assert '"loc"' not in content + loc_file = session_dir / "session.location" + assert loc_file.exists() + loc_data = json.loads(loc_file.read_text()) + assert loc_data["lat"] == 40.0 + + def test_record_imu_when_enabled(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + rec.record_imu( + { + "quat": _make_quat(1, 0, 0, 0), + "pos": [1.0, 2.0, 3.0], + "moving": True, + "status": 3, + "gyro": (0.01, -0.02, 0.03), + "accel": (0.1, 0.2, -0.3), + } + ) + assert len(rec._buffer) == 2 # header + imu + line = json.loads(rec._buffer[-1]) + assert line["e"] == "imu" + assert line["q"] == [1.0, 0.0, 0.0, 0.0] + assert line["mv"] is True + assert line["gyro"] == [0.01, -0.02, 0.03] + assert line["accel"] == [0.1, 0.2, -0.3] + finally: + rec.stop() + + def test_record_solve_when_enabled(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + t = rec.record_solve( + { + "RA": 180.0, + "Dec": 45.0, + "Roll": 10.0, + "camera_center": {"RA": 180.1, "Dec": 44.9, "Roll": 10.0}, + "imu_quat": _make_quat(1, 0, 0, 0), + "last_solve_attempt": 1000.4, + "last_solve_success": 1000.5, + }, + predicted_ra=179.5, + predicted_dec=44.8, + ) + assert t is not None + assert len(rec._buffer) == 2 # header + solve + line = json.loads(rec._buffer[-1]) + assert line["e"] == "solve" + assert line["ra"] == 180.0 + assert line["pred_ra"] == 179.5 + assert line["cam_ra"] == 180.1 + assert line["lsa"] == 1000.4 + assert line["lss"] == 1000.5 + finally: + rec.stop() + + def test_flush_time_gated(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + rec._last_flush = time.time() + rec.flush() + # Buffer should NOT be flushed (< 5s elapsed) + assert len(rec._buffer) == 1 # header still in buffer + finally: + rec.stop() + + def test_do_flush_writes_to_file(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + rec.record_imu({"quat": _make_quat(), "pos": None, "moving": True}) + rec._do_flush() + assert len(rec._buffer) == 0 + content = (rec._session_dir / "session.jsonl").read_text() + lines = [l for l in content.strip().split("\n") if l] + assert len(lines) == 2 # header + imu + finally: + rec.stop() + + def test_get_session_dir(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + assert rec.get_session_dir() is None + rec.start(_make_cfg(), _make_shared_state()) + try: + assert rec.get_session_dir() is not None + finally: + rec.stop() + assert rec.get_session_dir() is None + + def test_stop_idempotent(self): + rec = TelemetryRecorder() + rec.stop() # no-op + rec.stop() # still no-op + + def test_record_target(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + target = MagicMock() + target.object_id = 42 + target.display_name = "NGC 7331" + target.ra = 339.267 + target.dec = 34.416 + rec.record_target(target) + assert len(rec._buffer) == 2 # header + target + line = json.loads(rec._buffer[-1]) + assert line["e"] == "tgt" + assert line["name"] == "NGC 7331" + assert line["ra"] == 339.267 + assert line["dec"] == 34.416 + assert "alt" in line + assert "az" in line + finally: + rec.stop() + + def test_record_target_dedup(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + target = MagicMock() + target.object_id = 42 + target.display_name = "NGC 7331" + target.ra = 339.267 + target.dec = 34.416 + rec.record_target(target) + rec.record_target(target) # same target, should be deduped + assert len(rec._buffer) == 2 # header + one target + finally: + rec.stop() + + def test_record_target_change(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + t1 = MagicMock() + t1.object_id = 42 + t1.display_name = "NGC 7331" + t1.ra = 339.267 + t1.dec = 34.416 + t2 = MagicMock() + t2.object_id = 99 + t2.display_name = "M 31" + t2.ra = 10.684 + t2.dec = 41.269 + rec.record_target(t1) + rec.record_target(t2) + assert len(rec._buffer) == 3 # header + 2 targets + finally: + rec.stop() + + def test_record_target_cleared(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + rec.start(_make_cfg(), _make_shared_state()) + try: + target = MagicMock() + target.object_id = 42 + target.display_name = "NGC 7331" + target.ra = 339.267 + target.dec = 34.416 + rec.record_target(target) + rec.record_target(None) + assert len(rec._buffer) == 3 # header + target + clear + line = json.loads(rec._buffer[-1]) + assert line["e"] == "tgt" + assert line["name"] is None + assert line["ra"] is None + assert line["alt"] is None + assert line["az"] is None + finally: + rec.stop() + + +# ── TelemetryPlayer ───────────────────────────────────────────────── + + +@pytest.mark.unit +class TestTelemetryPlayer: + def test_load_from_file(self, tmp_path): + events = [ + {"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0], "mv": False}, + {"t": 1001.0, "e": "solve", "ra": 180.0, "dec": 45.0}, + ] + hdr = {"t": 999.0, "e": "hdr", "loc": [40.0, -74.0, 100.0]} + session_file = tmp_path / "session.jsonl" + _write_session_jsonl(session_file, events, header=hdr) + + player = TelemetryPlayer(session_file) + assert player.header is not None + assert player.header["e"] == "hdr" + assert len(player.events) == 2 + assert player.total_events == 2 + + def test_load_from_directory(self, tmp_path): + events = [{"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events) + player = TelemetryPlayer(tmp_path) + assert len(player.events) == 1 + + def test_progress(self, tmp_path): + events = [ + {"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}, + {"t": 1001.0, "e": "imu", "q": [1, 0, 0, 0]}, + ] + _write_session_jsonl(tmp_path / "session.jsonl", events) + player = TelemetryPlayer(tmp_path) + assert player.progress == 0.0 + assert player.current_index == 0 + + def test_progress_empty(self, tmp_path): + _write_session_jsonl(tmp_path / "session.jsonl", []) + player = TelemetryPlayer(tmp_path) + assert player.progress == 1.0 + + def test_get_next_event_timing(self, tmp_path): + now = time.time() + events = [ + {"t": now, "e": "imu", "q": [1, 0, 0, 0]}, + {"t": now + 100.0, "e": "imu", "q": [1, 0, 0, 0]}, + ] + _write_session_jsonl(tmp_path / "session.jsonl", events) + player = TelemetryPlayer(tmp_path) + + event, done = player.get_next_event() + assert event is not None + assert event["e"] == "imu" + assert not done + + # Second event is 100s in the future, should not be ready + event2, done2 = player.get_next_event() + assert event2 is None + assert not done2 + + def test_get_next_event_done(self, tmp_path): + now = time.time() + events = [{"t": now, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events) + player = TelemetryPlayer(tmp_path) + + event, done = player.get_next_event() + assert event is not None + assert done # last event + + event2, done2 = player.get_next_event() + assert event2 is None + assert done2 + + def test_reset(self, tmp_path): + now = time.time() + events = [{"t": now, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events) + player = TelemetryPlayer(tmp_path) + + player.get_next_event() + assert player.current_index == 1 + player.reset() + assert player.current_index == 0 + assert player._replay_start is None + + def test_event_to_solve_dict(self): + event = { + "t": 1000.5, + "ra": 180.0, + "dec": 45.0, + "roll": 10.0, + "cam_ra": 180.1, + "cam_dec": 44.9, + "cam_roll": 10.0, + "matches": 15, + "rmse": 0.5, + "iq": [1.0, 0.0, 0.0, 0.0], + "ip": [0.1, 0.2, 0.3], + "lsa": 1000.4, + "lss": 1000.5, + "src": "CAM", + } + result = TelemetryPlayer.event_to_solve_dict(event) + assert result["RA"] == 180.0 + assert result["Dec"] == 45.0 + assert result["Roll"] == 10.0 + assert result["camera_center"]["RA"] == 180.1 + assert result["Matches"] == 15 + assert result["solve_source"] == "CAM" + assert result["solve_time"] == 1000.5 + assert result["imu_pos"] == [0.1, 0.2, 0.3] + assert result["last_solve_attempt"] == 1000.4 + assert result["last_solve_success"] == 1000.5 + assert isinstance(result["imu_quat"], quaternion_module.quaternion) + assert result["imu_quat"].w == 1.0 + + def test_event_to_solve_dict_no_imu_quat(self): + event = {"t": 1000.0, "ra": 180.0, "dec": 45.0} + result = TelemetryPlayer.event_to_solve_dict(event) + assert result["RA"] == 180.0 + assert result["solve_time"] == 1000.0 + assert result["solve_source"] == "CAM" + assert result["imu_pos"] is None + assert "imu_quat" not in result + + def test_event_to_solve_dict_uses_recorded_source(self): + event = {"t": 1000.0, "ra": 180.0, "dec": 45.0, "src": "CAM_FAILED"} + result = TelemetryPlayer.event_to_solve_dict(event) + assert result["solve_source"] == "CAM_FAILED" + + def test_event_to_imu_dict(self): + event = {"q": [1.0, 0.0, 0.0, 0.0], "mv": True, "st": 3} + result = TelemetryPlayer.event_to_imu_dict(event) + assert result is not None + assert isinstance(result["quat"], quaternion_module.quaternion) + assert result["moving"] is True + assert result["status"] == 3 + + def test_event_to_imu_dict_no_quat(self): + event = {"mv": False} + assert TelemetryPlayer.event_to_imu_dict(event) is None + + def test_event_to_imu_dict_defaults(self): + event = {"q": [1.0, 0.0, 0.0, 0.0]} + result = TelemetryPlayer.event_to_imu_dict(event) + assert result["moving"] is False + assert result["status"] == 0 + + +# ── TelemetryManager ──────────────────────────────────────────────── + + +@pytest.mark.unit +class TestTelemetryManager: + def test_init_no_auto_record(self): + cfg = _make_cfg(telemetry_record=False) + ss = _make_shared_state() + cq = queue.Queue() + mgr = TelemetryManager(cfg, ss, cq) + assert not mgr.replaying + assert not mgr._recorder.enabled + + def test_init_auto_record(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + cfg = _make_cfg(telemetry_record=True) + ss = _make_shared_state() + cq = queue.Queue() + mgr = TelemetryManager(cfg, ss, cq) + try: + assert mgr._recorder.enabled + finally: + mgr.stop() + + def test_poll_commands_none_queue(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + mgr.poll_commands(None) # no-op + + def test_poll_commands_empty_queue(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + cmd_q = queue.Queue() + mgr.poll_commands(cmd_q) # no-op + + def test_handle_command_record_on(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + cq = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq) + mgr._handle_command("telemetry_record_on", None) + try: + assert mgr._recorder.enabled + assert cq.get_nowait() == "Telemetry: Recording" + finally: + mgr.stop() + + def test_handle_command_record_off(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + cq = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq) + mgr._handle_command("telemetry_record_on", None) + cq.get_nowait() # drain "Recording" msg + mgr._handle_command("telemetry_record_off", None) + assert not mgr._recorder.enabled + assert cq.get_nowait() == "Telemetry: Stopped" + + def test_handle_command_replay(self, tmp_path): + events = [{"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events) + + cq = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq) + mgr._handle_command("replay", str(tmp_path)) + assert mgr.replaying + assert cq.get_nowait() == "Telemetry: Replay started" + + def test_handle_command_replay_with_header(self, tmp_path): + hdr = {"t": 999.0, "e": "hdr", "dt": "2024-01-15T22:30:00"} + events = [{"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events, header=hdr) + loc_file = tmp_path / "session.location" + loc_file.write_text(json.dumps({"lat": 35.0, "lon": -120.0, "altitude": 200.0})) + + ss = _make_shared_state() + cq = queue.Queue() + mgr = TelemetryManager(_make_cfg(), ss, cq) + mgr._handle_command("replay", str(tmp_path)) + + ss.set_location.assert_called_once() + loc_arg = ss.set_location.call_args[0][0] + assert loc_arg.lat == 35.0 + assert loc_arg.source == "replay" + ss.set_datetime.assert_called_once() + + def test_handle_command_replay_stop(self, tmp_path): + events = [{"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events) + + cq = queue.Queue() + cam_q = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq, cam_q) + mgr._handle_command("replay", str(tmp_path)) + cq.get_nowait() # drain "Replay started" + + mgr._handle_command("replay_stop", None) + assert not mgr.replaying + assert cq.get_nowait() == "Telemetry: Replay stopped" + assert cam_q.get_nowait() == "start" + + def test_next_replay_event_not_replaying(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + assert mgr.next_replay_event() is None + + def test_next_replay_event_auto_finish(self, tmp_path): + now = time.time() + events = [{"t": now, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events) + + cq = queue.Queue() + cam_q = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq, cam_q) + mgr._handle_command("replay", str(tmp_path)) + cq.get_nowait() # drain "Replay started" + + # First call returns the event + event = mgr.next_replay_event() + assert event is not None + + # Next call: done → auto-cleanup + event2 = mgr.next_replay_event() + assert event2 is None + assert not mgr.replaying + assert cq.get_nowait() == "Telemetry: Replay finished" + assert cam_q.get_nowait() == "start" + + def test_record_solve_delegates(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + mgr._recorder = MagicMock() + mgr._recorder.record_solve.return_value = None + mgr.record_solve({"RA": 180.0}) + mgr._recorder.record_solve.assert_called_once() + + def test_record_solve_sends_image_command(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + cam_q = queue.Queue() + cfg = _make_cfg(telemetry_images=True) + mgr = TelemetryManager(cfg, _make_shared_state(), queue.Queue(), cam_q) + mgr._recorder.start(_make_cfg(), _make_shared_state()) + mgr._recorder.images_enabled = True + try: + mgr.record_solve({"RA": 180.0, "Dec": 45.0}) + msg = cam_q.get_nowait() + assert msg.startswith("save_image:") + finally: + mgr.stop() + + def test_record_imu_delegates(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + mgr._recorder = MagicMock() + mgr.record_imu({"quat": _make_quat()}) + mgr._recorder.record_imu.assert_called_once() + + def test_flush_delegates(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + mgr._recorder = MagicMock() + mgr._recorder.enabled = False + mgr.flush() + mgr._recorder.flush.assert_called_once() + + def test_stop_delegates(self): + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), queue.Queue()) + mgr._recorder = MagicMock() + mgr.stop() + mgr._recorder.stop.assert_called_once() + + def test_poll_commands_dispatches_tuple(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + cq = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq) + cmd_q = queue.Queue() + cmd_q.put(("telemetry_record_on", None)) + mgr.poll_commands(cmd_q) + try: + assert mgr._recorder.enabled + finally: + mgr.stop() + + def test_poll_commands_ignores_non_tuple(self): + cq = queue.Queue() + mgr = TelemetryManager(_make_cfg(), _make_shared_state(), cq) + cmd_q = queue.Queue() + cmd_q.put("not a tuple") + mgr.poll_commands(cmd_q) # should not crash + + def test_restart_camera_no_queue(self): + mgr = TelemetryManager( + _make_cfg(), _make_shared_state(), queue.Queue(), None + ) + mgr._restart_camera() # no-op, no crash + + def test_handle_replay_event_failed_solve(self): + """Failed solves during replay should set solve_state(False) and CAM_FAILED.""" + ss = _make_shared_state() + mgr = TelemetryManager(_make_cfg(), ss, queue.Queue()) + imu_dr = MagicMock() + + solved = { + "RA": 180.0, + "Dec": 45.0, + "Matches": 15, + "RMSE": 0.5, + "solve_source": "CAM", + "constellation": "Vir", + } + last_image_solve = {"RA": 180.0, "Dec": 45.0} + + failed_event = { + "t": 1000.0, + "e": "solve", + "ra": None, + "dec": None, + "matches": 0, + "rmse": None, + "lsa": 1000.0, + } + + result = mgr.handle_replay_event( + failed_event, solved, last_image_solve, imu_dr, "Alt/Az" + ) + + # last_image_solve should be unchanged (returned as-is) + assert result is last_image_solve + # Metadata updated + assert solved["Matches"] == 0 + assert solved["last_solve_attempt"] == 1000.0 + # Failed solve behavior + assert solved["solve_source"] == "CAM_FAILED" + assert solved["constellation"] == "" + ss.set_solve_state.assert_called_with(False) + ss.set_solution.assert_called_once() + + def test_handle_replay_event_successful_solve(self): + """Successful solves during replay should update position and metadata.""" + ss = _make_shared_state() + mgr = TelemetryManager(_make_cfg(), ss, queue.Queue()) + imu_dr = MagicMock() + + solved = {"RA": None, "Dec": None, "Matches": None, "imu_quat": None} + + event = { + "t": 1000.0, + "e": "solve", + "ra": 180.0, + "dec": 45.0, + "cam_ra": 180.1, + "cam_dec": 44.9, + "cam_roll": 10.0, + "matches": 15, + "rmse": 0.5, + "lsa": 1000.0, + "lss": 1000.0, + "iq": [1.0, 0.0, 0.0, 0.0], + "src": "CAM", + } + + with patch("PiFinder.telemetry.update_plate_solve_and_imu"), patch( + "PiFinder.telemetry.finalize_and_push_solution" + ): + result = mgr.handle_replay_event( + event, solved, None, imu_dr, "Alt/Az" + ) + + assert result is not None + assert result["RA"] == 180.0 + assert result["Matches"] == 15 + assert result["last_solve_attempt"] == 1000.0 + ss.set_solve_state.assert_called_with(True) + + def test_replay_header_mount_type_mismatch_warns(self, tmp_path): + """Replay should warn when header mount_type differs from config.""" + hdr = { + "t": 999.0, + "e": "hdr", + "dt": "2024-01-15T22:30:00", + "cfg": {"integrator": "flat", "mount_type": "EQ"}, + } + events = [{"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events, header=hdr) + + ss = _make_shared_state() + cq = queue.Queue() + cfg = _make_cfg(mount_type="Alt/Az") + mgr = TelemetryManager(cfg, ss, cq) + + with patch("PiFinder.telemetry.logger") as mock_logger: + mgr._handle_command("replay", str(tmp_path)) + mock_logger.warning.assert_called_once() + assert "EQ" in mock_logger.warning.call_args[0][1] + assert "Alt/Az" in mock_logger.warning.call_args[0][2] + + def test_replay_header_mount_type_match_no_warn(self, tmp_path): + """No warning when header mount_type matches config.""" + hdr = { + "t": 999.0, + "e": "hdr", + "cfg": {"integrator": "flat", "mount_type": "Alt/Az"}, + } + events = [{"t": 1000.0, "e": "imu", "q": [1, 0, 0, 0]}] + _write_session_jsonl(tmp_path / "session.jsonl", events, header=hdr) + + cfg = _make_cfg(mount_type="Alt/Az") + mgr = TelemetryManager(cfg, _make_shared_state(), queue.Queue()) + + with patch("PiFinder.telemetry.logger") as mock_logger: + mgr._handle_command("replay", str(tmp_path)) + mock_logger.warning.assert_not_called() + + def test_flush_polls_target(self, tmp_path): + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + ss = _make_shared_state() + target = MagicMock() + target.object_id = 42 + target.display_name = "M 31" + target.ra = 10.684 + target.dec = 41.269 + ui_state = MagicMock() + ui_state.target.return_value = target + ss.ui_state.return_value = ui_state + + mgr = TelemetryManager(_make_cfg(telemetry_record=True), ss, queue.Queue()) + try: + mgr.flush() + assert mgr._recorder._last_target_id == 42 + # Check a target event was buffered (header + target) + assert len(mgr._recorder._buffer) >= 2 + lines = [json.loads(l) for l in mgr._recorder._buffer] + tgt_lines = [l for l in lines if l.get("e") == "tgt"] + assert len(tgt_lines) == 1 + assert tgt_lines[0]["name"] == "M 31" + finally: + mgr.stop() + + def test_poll_target_no_ui_state(self): + """_poll_target should not crash if ui_state() raises.""" + ss = _make_shared_state() + ss.ui_state.side_effect = Exception("no ui state") + mgr = TelemetryManager(_make_cfg(), ss, queue.Queue()) + mgr._recorder.enabled = True + mgr._poll_target() # should not raise + + def test_header_includes_mount_type(self, tmp_path): + """Recording header should include mount_type from config.""" + with patch("PiFinder.telemetry.TELEMETRY_DIR", tmp_path / "telemetry"): + rec = TelemetryRecorder() + cfg = _make_cfg(mount_type="EQ") + ss = _make_shared_state() + rec.start(cfg, ss) + try: + line = json.loads(rec._buffer[0]) + assert line["cfg"]["mount_type"] == "EQ" + finally: + rec.stop() + + +# ── pointing.py ────────────────────────────────────────────────────── + + +@pytest.mark.unit +class TestGetRollByMountType: + def test_eq_mount_returns_zero(self): + from PiFinder.pointing import get_roll_by_mount_type + + roll = get_roll_by_mount_type(180.0, 45.0, None, None, "EQ") + assert roll == 0.0 + + def test_eq_mount_southern_hemisphere(self): + from PiFinder.pointing import get_roll_by_mount_type + + loc = _make_location(lat=-35.0) + roll = get_roll_by_mount_type(180.0, 45.0, loc, None, "EQ") + assert roll == 180.0 + + def test_altaz_no_location_returns_zero(self): + from PiFinder.pointing import get_roll_by_mount_type + + roll = get_roll_by_mount_type(180.0, 45.0, None, None, "Alt/Az") + assert roll == 0.0 + + def test_unknown_mount_returns_zero(self): + from PiFinder.pointing import get_roll_by_mount_type + + roll = get_roll_by_mount_type(180.0, 45.0, None, None, "Dobsonian") + assert roll == 0.0 + + +@pytest.mark.unit +class TestUpdatePlateSolveAndImu: + def test_returns_early_on_none_ra(self): + from PiFinder.pointing import update_plate_solve_and_imu + + imu_dr = MagicMock() + solved = {"RA": None, "Dec": 45.0} + update_plate_solve_and_imu(imu_dr, solved) + imu_dr.update_plate_solve_and_imu.assert_not_called() + + def test_returns_early_on_none_dec(self): + from PiFinder.pointing import update_plate_solve_and_imu + + imu_dr = MagicMock() + solved = {"RA": 180.0, "Dec": None} + update_plate_solve_and_imu(imu_dr, solved) + imu_dr.update_plate_solve_and_imu.assert_not_called() + + +@pytest.mark.unit +class TestUpdateImu: + def test_returns_early_no_last_image_solve(self): + from PiFinder.pointing import update_imu + + imu_dr = MagicMock() + imu_dr.tracking = True + solved = {"RA": 180.0} + imu = {"quat": _make_quat()} + update_imu(imu_dr, solved, None, imu) + imu_dr.update_imu.assert_not_called() + + def test_returns_early_not_tracking(self): + from PiFinder.pointing import update_imu + + imu_dr = MagicMock() + imu_dr.tracking = False + solved = {"RA": 180.0} + last = {"imu_quat": _make_quat()} + imu = {"quat": _make_quat()} + update_imu(imu_dr, solved, last, imu) + imu_dr.update_imu.assert_not_called()