Plotting

HMI/CALIBRATION.md

@@ -0,0 +1,121 @@
+# Calibration notes — findings from the existing capture set
+
+## What the logs actually contain
+
+| Log | compass (127250) | COG (129026) | samples >4 m/s |
+|---|---|---|---|
+| candump-fullturns-michelleski | 3425 | 944 | **233** |
+| candump-autopilot-north-in-southbay | 5986 | 1640 | 0 |
+| candump-autopilot in south bay | 1851 | 509 | 0 |
+| candump-autopilot-engaged-heading-bounce | 1499 | 410 | 0 |
+| Figure 8 full turns (.trc.log) | 0 | 239 | 0 |
+| Figure 8 half turns (.trc.log) | 0 | 420 | 0 |
+| ZigZag 1200 rpm (.trc.log) | 0 | 110 | 0 |
+
+**Only one log has both a compass and fast running.** The three `.trc.log`
+figure-8/zigzag captures contain no PGN 127250 at all — the compass either
+wasn't on the bus or wasn't being logged on that day. This is the single
+biggest gap in the dataset.
+
+## Sensor inventory (decoded, not assumed)
+
+* **Compass**, PGN 127250, SA **248**, ~138 ms period.
+  Reference field reads **magnetic** on every frame. Deviation is 0. The
+  variation field reads 187.7°, which is not a valid variation and not a
+  standard sentinel — treat the sender's variation as unusable and estimate
+  the offset in the filter instead.
+* **GPS**, PGNs 129025/129026, SA **28**, ~500 ms period.
+* **PGN 65256 (vehicle direction), SA 28** duplicates COG from the same
+  receiver to within 0.01°. It is *not* an independent heading source and is
+  deliberately not registered as one.
+* **No second GPS and no rate gyro appear in any capture.** SA 248 and SA 28
+  are the only positioning/heading talkers. The registry has slots for both;
+  they are currently empty.
+* One position fix in the fullturns log reads lat −83.88°, lon 186.23° — a
+  bad fix that passes the N2K sentinel check. Range-gate lat/lon before use.
+
+## The compass error is heading-dependent
+
+Correlating compass against COG during **straight, fast** segments only
+(|rate| < 2°/s, SOG > 4 m/s):
+
+```
+compass 0–45°   : n=112  median offset −18.9°
+compass 315–360°: n= 65  median offset  +4.2°
+```
+
+A 23° swing across 90° of heading is a hard-iron signature, not magnetic
+declination (which is ~+8° and constant for Horsetooth). **A single scalar
+offset state cannot represent this** — an early version of this filter used
+one and the estimate wandered as the boat turned. The state vector therefore
+carries a constant term plus a one-cycle harmonic:
+
+```
+compass_reading = psi − (d0 + ds·sin psi + dc·cos psi)
+```
+
+## Honest limits of the current tuning
+
+Straight-and-fast running occurs on only **two heading sectors** in the entire
+dataset (roughly 0–45° and 315–360°). Three unknowns (d0, ds, dc) fitted from
+two sectors of one 474-second log is underdetermined.
+
+A parameter sweep showed the validation score improving monotonically as the
+harmonic priors were loosened, with the estimated iron amplitude running to
+45°. That is the metric being gamed, not the model improving: the filter is
+being scored against COG while simultaneously fitting to COG, so looser
+harmonics always win by absorbing crab angle. Because no held-out log with
+fast compass data exists, that sweep cannot be trusted.
+
+The shipped values are therefore **conservative, not sweep-optimal**:
+
+* `P0_HARM` = (8°)², `Q_HARM` = (0.005°/s)²
+* `MAX_IRON_AMP` = 15°, a hard clamp on √(ds²+dc²)
+
+Validation on the fullturns log with these settings: mean 4.3°, median 2.2°,
+p95 16.1°. The looser sweep reached p95 11°, but at an implausible 45° iron
+amplitude. Prefer the conservative numbers until better data exists.
+
+## The one capture that would fix this
+
+A **slow, steady, full 360° circle at planing speed in flat water** — ideally
+two of them, in both directions. Requirements:
+
+* SOG above ~10 mph throughout, so COG noise is small
+* Yaw rate low and constant — a wide circle, not hard-over turns
+* Calm water and minimal wind, so crab angle stays small
+* Compass (PGN 127250) confirmed present on the bus before you start
+
+That single run gives full heading coverage and lets `fit_iron.py` solve d0,
+ds, dc directly by least squares. Seed the filter with those values, tighten
+`P0_HARM`, and the low-speed performance improves too — the harmonic model is
+shared across the whole speed range.
+
+The existing "Attempted Compass Calibration ... 2 circles" capture only spans
+compass 21–142°, so it does not close the circle. It agrees with the fullturns
+numbers over the sectors it covers, which is a useful consistency check but
+not enough to fit on.
+
+## Second GPS and gyro
+
+Both are handled by adding registry entries — no filter changes:
+
+```python
+kf.register(SensorSpec("cog_aux", "cog", SRC_GPS2,
+                       H=[1,0,0,0,0,0], r_fn=kf._cog_variance,
+                       gate=Params.GATE_COG))
+kf.register(SensorSpec("gyro", "rot", SRC_GYRO,
+                       H=[0,1,1,0,0,0], r_fn=lambda o,f: Params.R_GYRO,
+                       gate=Params.GATE_GYRO))
+```
+
+With two GPS units the chi-squared gate does redundancy management for free:
+whichever agrees with the propagated state is accepted, the outlier is gated.
+Watch `gate_pct` per sensor in `health_report()` — a channel that starts
+gating heavily is failing before it fails hard.
+
+Note the gyro row is `[0,1,1,0,0,0]`, measuring rate **plus** bias, which is
+what makes the bias state observable. Without a gyro, `x[BIAS]` stays at its
+prior and does nothing; that is harmless but means the state is currently
+carrying an unobservable element. Do not be alarmed by a flat bias trace until
+a gyro is actually on the bus.

HMI/app.py

@@ -6,19 +6,31 @@
 # canplayer -l i -v -I logs/test_data-21July2025.log vcan0=can0
 
 #
+import time
+import asyncio
 import sys
-sys.stdout.flush()
-
-
 import tornado.web
 import tornado.websocket
 import tornado.ioloop
 import tornado.httpserver
+import tornado.autoreload
 import os
 import json
 import logging
 import subprocess
 
+# ---- OFFLINE TEST SETTINGS (hardcoded) ----
+OFFLINE_TEST = True
+OFFLINE_BACKEND = "virtual"        # use python-can virtual bus
+OFFLINE_CHANNEL = "vcan0"          # arbitrary name for virtual bus
+CANDUMP_PATH = r"logs\test_data-21July2025.log"   # <-- set your file here
+CANDUMP_PATH = r"logs\candump-2025-08-06_16442_horsetooth_firstConstants.log"
+REPLAY_SPEED = 1.0                 # 1.0 = realtime
+REPLAY_LOOP = True                 # loop file
+
+
+sys.stdout.flush()
+
 from can_interface import CANinterface
 from autopilot import Autopilot
 
@@ -27,14 +39,19 @@
 logging.basicConfig(level=logging.INFO)
 
 clients = set()
-can_interface = CANinterface(channel='can0')
+can_interface = CANinterface(channel='can0', bitrate=250000, backend=OFFLINE_BACKEND if OFFLINE_TEST else None )
 autopilot = Autopilot(can_interface)
 autopilot.start()
 
+
 class MainHandler(tornado.web.RequestHandler):
     def get(self):
         self.render("autopilot.html")
 
+class PlotHandler(tornado.web.RequestHandler):
+    def get(self):
+        self.render("plot_data.html")
+
 class ExitBrowserHandler(tornado.web.RequestHandler):
     def post(self):
         # Kill Chromium process
@@ -103,25 +120,103 @@ def handle_client_command(command):
         autopilot.left_turn_engaged = False
         logger.info("Stop Left Turn.")
 
-
+def _safe_broadcast(payload: dict):
+    dead = []
+    msg = json.dumps(payload, separators=(",", ":"))
+    for c in list(clients):
+        try:
+            c.write_message(msg)
+        except Exception:
+            dead.append(c)
+    for c in dead:
+        clients.discard(c)
 
 def broadcast_can_message(data):
-    if 'steering_goal' in data and "min_steering_angle" in data and "min_steering_angle" in data:
+    if 'steering_goal' in data and "max_steering_angle" in data and "min_steering_angle" in data:
         autopilot.set_steering_shaft_limits(data["min_steering_angle"], data["max_steering_angle"])
     elif 'COG' in data:
         autopilot.set_heading(data['COG'])
     elif "rudder_value_min" in data and "rudder_value_max" in data:
         autopilot.set_rudder_counts(data["rudder_value_min"], data["rudder_value_max"])
-
 
-    for c in clients:
-        c.write_message(json.dumps(data))
+    _safe_broadcast(data)
+
+def _getattr(obj, name, default=None):
+    try:
+        v = getattr(obj, name)
+        # Call if it's a method like get_rpm()
+        return v() if callable(v) else v
+    except Exception:
+        return default
+
+def make_telemetry_snapshot():
+    """
+    Builds a single JSON-friendly dict of latest values.
+    Tweak attribute names here to match your classes.
+    """
+    # Pull from CANinterface (raw) — rename as needed
+    rpm         = _getattr(can_interface, "rpm", None)
+    speed       = _getattr(can_interface, "boat_speed", None)  # wheel/GPS-derived speed
+    sog_mps     = _getattr(can_interface, "sog_mps", None)
+    cog_deg     = _getattr(can_interface, "COG", None)
+    compass_deg = _getattr(can_interface, "compass_heading", None)
+    lat         = _getattr(can_interface, "lat", None)
+    lon         = _getattr(can_interface, "lon", None)
+    shaft_goal = _getattr(autopilot, "shaft_goal", None)
+
+    # Pull from Autopilot (fused/commanded)
+    heading_deg   = _getattr(autopilot, "current_heading", None)     # fused/estimated heading
+    heading_goal  = _getattr(autopilot, "heading_goal", None)
+    heading_error = _getattr(autopilot, "heading_error", None)
+    rudder_counts = _getattr(autopilot, "rudder_goal", None)
+    rudder_angle  = _getattr(autopilot, "current_rudder", None)
+    autopilot_on  = _getattr(autopilot, "autopilot_engaged", None)
+    servo_on      = _getattr(can_interface, "servo_enabled", None)
+    compass_offset = _getattr(can_interface, "compass_offset", None)
+
+    snap = {
+        "ts": time.time(),
+        "rpm": rpm,
+        "speed": speed,
+        "heading_deg": heading_deg,
+        "heading_goal": heading_goal,
+        "compass_deg": compass_deg,
+        "cog_deg": cog_deg,
+        "sog_mps": sog_mps,
+        "lat": lat,
+        "lon": lon,
+        "heading_goal": heading_goal,
+        "headingErr": heading_error,
+        "rudder_counts": rudder_counts,
+        "autopilot_engaged": bool(autopilot_on) if autopilot_on is not None else None,
+        "servo_enabled": bool(servo_on) if servo_on is not None else None,
+        "shaft_goal": shaft_goal,
+        "compass_offset": compass_offset
+    }
+    return snap
+
+async def telemetry_broadcaster(rate_hz: int = 10):
+    """
+    Periodically pushes a consolidated telemetry frame to all WebSocket clients.
+    """
+    period = 1.0 / max(rate_hz, 1)
+    while True:
+        try:
+            payload = make_telemetry_snapshot()
+            _safe_broadcast(payload)
+        except Exception:
+            logger.exception("telemetry_broadcaster failed to build/broadcast snapshot")
+        await asyncio.sleep(period)
 
 def make_app():
     return tornado.web.Application([
         (r"/", MainHandler),
         (r"/ws", WSHandler),
-        (r"/exit-browser", ExitBrowserHandler)
+        (r"/exit-browser", ExitBrowserHandler),
+        (r"/plot",PlotHandler),
+        (r"/sw.js", tornado.web.StaticFileHandler,
+            {"path": os.path.join(os.path.dirname(__file__), "static"),
+             "default_filename": "sw.js"}),
     ],
     template_path="templates",
     static_path="static",   
@@ -133,7 +228,26 @@ async def main():
     server.listen(5000)
 
     can_interface.add_listener(broadcast_can_message)
+
+    # Start the periodic telemetry stream (10–20 Hz is plenty)
+    asyncio.create_task(telemetry_broadcaster(rate_hz=5))
+
+    # Kick off offline replay if enabled
+    if OFFLINE_TEST:
+        logger.info("Setting up Offline Test for replay_candump")
+        asyncio.create_task(
+            can_interface.replay_candump(
+                CANDUMP_PATH, speed=REPLAY_SPEED, loop=REPLAY_LOOP
+            )
+        )
+
     await can_interface.read_loop()
 
 if __name__ == "__main__":
+    tornado.autoreload.watch("templates/plot_data.html")   # any file you like
+    tornado.autoreload.watch("autopilot.py")   # any file you like
+    tornado.autoreload.watch("can_interface.py")   # any file you like
+    tornado.autoreload.watch("app.py")   # any file you like
+    tornado.autoreload.watch("templates/autopilot.html")   # any file you like
+    tornado.autoreload.start()            # will re-exec the process on change
     tornado.ioloop.IOLoop.current().run_sync(main)

HMI/autopilot.py

@@ -10,6 +10,9 @@
 import queue
 import statistics
 
+AUTOPILOT_CAN_ID = 0x18FF50E0
+
+
 # Setup logger
 logger = logging.getLogger('autopilot')
 logger.setLevel(logging.INFO)
@@ -133,12 +136,12 @@ def broadcast_status_message(self):
 
             data = struct.pack("<BHHHB", engaged_byte, heading_goal, heading_error, rudder_goal, (self.counter & 0xFF))
 
-            msg = can.Message(arbitration_id=0x18FF50E0,  # example PGN (can change)
+            msg = can.Message(arbitration_id=AUTOPILOT_CAN_ID,  # example PGN (can change)
                             data=data,
                             is_extended_id=True)
 
             self.bus.send(msg)
-            logger.debug(f"Sent Autopilot status: {0x18FF50E0:08X} {data.hex()}")
+            logger.debug(f"Sent Autopilot status: {AUTOPILOT_CAN_ID:08X} {data.hex()}")
 
         except can.CanError:
             logger.exception("CAN message failed to send")
@@ -164,7 +167,7 @@ def set_rudder_counts(self, min_val, max_val):
     def set_steering_shaft_limits(self, min_val, max_val):
         self.steering_shaft_min = float(min_val)
         self.steering_shaft_max = float(max_val)
-        logger.info(f"Set steering shaft limits: min={self.steering_shaft_min}, max={self.steering_shaft_max}")
+        logger.debug(f"Set steering shaft limits: min={self.steering_shaft_min}, max={self.steering_shaft_max}")
 
 
     def run(self):
@@ -212,7 +215,7 @@ def run(self):
             self.broadcast_status_message()
             logger.debug(f"heading error: {self.heading_error:.2f}, Computed shaft goal: {self.shaft_goal:.2f}")
             #if self.autopilot_engaged == True:
-            if not self.autopilot_engaged_event.isSet():
+            if not self.autopilot_engaged_event.is_set():
                 self.heading_goal = self.current_heading
                 self.error_list = []
                 self.time_list = []
@@ -224,7 +227,7 @@ def run(self):
                 if abs(self.shaft_goal_mean - self.last_shaft_goal) > 100: # deadband
                     # Generate a command and broadcast the steering
                     self.last_shaft_goal = self.shaft_goal_mean
-                    if self.autopilot_engaged_event.isSet():
+                    if self.autopilot_engaged_event.is_set():
                         self.can_interface.set_shaft_goal(self.shaft_goal)
                         self.can_interface.send_shaft_goal()
                         logger.debug(f"Sent command for shaft position of {self.can_interface.shaft_goal}")