Add frame lock PID controls

app.py

@@ -8,6 +8,7 @@
 from lib.camera import init_camera, init_ip_camera, init_rpi_camera
 from lib.control_telemetry import init_control_telemetry
 from lib.controller import Controller
+from lib.frame_control_client import init_frame_control
 from lib.json_data_handler import JSONDataHandler
 from lib.log_udp_receiver import init_log_stream
 from lib.net_transport import DEFAULT_ROV_HOST
@@ -109,6 +110,9 @@
 # Initialize system control client (UDP port 5008)
 app.config["SYSTEM_CONTROL"] = SystemControlClient()
 
+# Initialize frame control client (UDP port 5009)
+app.config["FRAME_CONTROL"] = init_frame_control()
+
 register_routes(app)
 
 
@@ -146,6 +150,9 @@ def _shutdown():
     system_control = app.config.get("SYSTEM_CONTROL")
     if system_control:
         system_control.close()
+    frame_control = app.config.get("FRAME_CONTROL")
+    if frame_control:
+        frame_control.close()
 
 
 atexit.register(_shutdown)

lib/frame_control_client.py

@@ -0,0 +1,69 @@
+"""Frame-control client for world-frame PID support."""
+
+from __future__ import annotations
+
+import struct
+import threading
+import time
+
+from lib.crc import crc32_ieee
+from lib.net_transport import DEFAULT_ROV_HOST, UdpSender
+
+FRAME_CONTROL_PORT = 5009
+FRAME_MAGIC = b"FRM1"
+TYPE_LOCK = 0x01
+TYPE_UNLOCK = 0x02
+
+
+def build_frame_packet(command: int, sequence: int) -> bytes:
+    body = FRAME_MAGIC + struct.pack("!B3xI", command, sequence & 0xFFFFFFFF)
+    return body + struct.pack("!I", crc32_ieee(body))
+
+
+class FrameControlClient:
+    def __init__(self, host: str = DEFAULT_ROV_HOST, port: int = FRAME_CONTROL_PORT):
+        self.host = host
+        self.port = port
+        self.sender = UdpSender(host, port)
+        self._lock = threading.Lock()
+        self._sequence = 0
+        self._active = False
+        self._last_update_ts = 0.0
+        self._last_error: str | None = None
+
+    def close(self) -> None:
+        self.sender.close()
+
+    def _send(self, command: int) -> dict:
+        with self._lock:
+            self._sequence = (self._sequence + 1) & 0xFFFFFFFF
+            sequence = self._sequence
+        self.sender.send(build_frame_packet(command, sequence))
+        with self._lock:
+            self._active = command == TYPE_LOCK
+            self._last_update_ts = time.time()
+            self._last_error = None
+        return self.get_state()
+
+    def lock(self) -> dict:
+        return self._send(TYPE_LOCK)
+
+    def unlock(self) -> dict:
+        return self._send(TYPE_UNLOCK)
+
+    def set_error(self, message: str) -> None:
+        with self._lock:
+            self._last_error = message
+            self._last_update_ts = time.time()
+
+    def get_state(self) -> dict:
+        with self._lock:
+            return {
+                "active": self._active,
+                "last_update_ts": self._last_update_ts,
+                "last_error": self._last_error,
+            }
+
+
+def init_frame_control(host: str = DEFAULT_ROV_HOST, port: int = FRAME_CONTROL_PORT) -> FrameControlClient:
+    return FrameControlClient(host=host, port=port)

lib/ninedof_receiver.py

@@ -225,21 +225,24 @@ def _val(key: str, default: float = float("nan")) -> float:
 
         # Update data.json
         try:
-            self.data_handler.update_data(
-                {
-                    "imu": {
-                        "yaw": _coerce_json_number(yaw),
-                        "pitch": _coerce_json_number(pitch),
-                        "roll": _coerce_json_number(roll),
-                        "yr": _coerce_json_number(raw_yr),
-                        "pr": _coerce_json_number(raw_pr),
-                        "rr": _coerce_json_number(raw_rr),
-                        "ax": _coerce_json_number(mapped_ax, precision=3),
-                        "ay": _coerce_json_number(mapped_ay, precision=3),
-                        "az": _coerce_json_number(mapped_az, precision=3),
-                    }
+            update = {
+                "imu": {
+                    "yaw": _coerce_json_number(yaw),
+                    "pitch": _coerce_json_number(pitch),
+                    "roll": _coerce_json_number(roll),
+                    "yr": _coerce_json_number(raw_yr),
+                    "pr": _coerce_json_number(raw_pr),
+                    "rr": _coerce_json_number(raw_rr),
+                    "ax": _coerce_json_number(mapped_ax, precision=3),
+                    "ay": _coerce_json_number(mapped_ay, precision=3),
+                    "az": _coerce_json_number(mapped_az, precision=3),
                 }
-            )
+            }
+            if isinstance(msg.get("depth"), dict):
+                update["depth"] = msg["depth"]
+            if isinstance(msg.get("frame"), dict):
+                update["frame"] = msg["frame"]
+            self.data_handler.update_data(update)
         except Exception as e:
             print(f"IMU: Error updating data: {e}")
 

routes.py

@@ -95,6 +95,22 @@ def _neutralize_thruster_command():
     return neutral
 
 
+def _current_depth_altitude_setpoint():
+    depth = data_handler.get_section("depth") or {}
+    try:
+        depth_m = float(depth["dpt"])
+    except (KeyError, TypeError, ValueError):
+        raise ValueError("Depth data is unavailable") from None
+    if not math.isfinite(depth_m):
+        raise ValueError("Depth data is invalid")
+    if depth.get("valid") is False:
+        raise ValueError("Depth sensor is not valid")
+    age_ms = depth.get("age_ms")
+    if age_ms is not None and float(age_ms) > 2500:
+        raise ValueError("Depth data is stale")
+    return -depth_m
+
+
 def _send_full_axis_config():
     """Read all axis settings from config and send to MCU in one packet."""
     imu_axes = config_handler.get_section("imu_axes") or _DEFAULT_IMU_AXES
@@ -334,6 +350,35 @@ def setpoint_status():
             return jsonify({"ok": False, "error": "Setpoint override client unavailable"}), 503
         return jsonify({"ok": True, "state": client.get_state()})
 
+    @app.route("/api/frame/status", methods=["GET"])
+    def frame_status():
+        client = current_app.config.get("FRAME_CONTROL")
+        if not client:
+            return jsonify({"ok": False, "error": "Frame control client unavailable"}), 503
+        return jsonify({"ok": True, "state": client.get_state(), "telemetry": data_handler.get_section("frame") or {}})
+
+    @app.route("/api/frame/lock", methods=["POST"])
+    def lock_frame():
+        client = current_app.config.get("FRAME_CONTROL")
+        if not client:
+            return jsonify({"ok": False, "error": "Frame control client unavailable"}), 503
+        try:
+            return jsonify({"ok": True, "state": client.lock()})
+        except Exception as exc:  # pylint: disable=broad-except
+            client.set_error(str(exc))
+            return jsonify({"ok": False, "error": str(exc)}), 503
+
+    @app.route("/api/frame/unlock", methods=["POST"])
+    def unlock_frame():
+        client = current_app.config.get("FRAME_CONTROL")
+        if not client:
+            return jsonify({"ok": False, "error": "Frame control client unavailable"}), 503
+        try:
+            return jsonify({"ok": True, "state": client.unlock()})
+        except Exception as exc:  # pylint: disable=broad-except
+            client.set_error(str(exc))
+            return jsonify({"ok": False, "error": str(exc)}), 503
+
     @app.route("/api/rov/command", methods=["POST"])
     def set_rov_command():
         """
@@ -576,6 +621,10 @@ def start_pid_hold():
             return jsonify({"ok": False, "error": "Current attitude is incomplete"}), 503
 
         neutral = _neutralize_thruster_command()
+        try:
+            neutral["heave"] = _current_depth_altitude_setpoint()
+        except ValueError as exc:
+            return jsonify({"ok": False, "error": str(exc), "neutralized": True}), 503
         setpoints = {**neutral, **attitude_setpoints}
         try:
             client.clear_override()

static/js/pid_tuning.js

@@ -4,18 +4,21 @@
 
   const AXES = ["surge", "sway", "heave", "roll", "pitch", "yaw"];
   const ROT_AXES = ["roll", "pitch", "yaw"];
+  const DISPLAY_AXES = ["heave", "roll", "pitch", "yaw"];
   const PID_GAINS = ["kp", "ki", "kd"];
   const SEND_INTERVAL_MS = 50;
 
   const attitudeLimits = window.pidTuningAttitudeLimits || { roll: 180, pitch: 90, yaw: 180 };
   let overrideActive = false;
   let sendTimer = null;
   let latestImu = {};
+  let latestDepth = {};
   let latestTelemetry = null;
   const localSetpoints = { roll: NaN, pitch: NaN, yaw: NaN };
 
   const pageStatus = document.getElementById("pid-page-status");
   const linkStatus = document.getElementById("pid-link-status");
+  const frameStatus = document.getElementById("frame-lock-status");
   const imuAge = document.getElementById("pid-imu-age");
   const pidStatus = document.getElementById("pid-status");
   const setpointStatus = document.getElementById("setpoint-status");
@@ -85,10 +88,10 @@
   function updateTelemetryTable() {
     if (!telemetryBody) return;
     const frag = document.createDocumentFragment();
-    ROT_AXES.forEach((axis) => {
+    DISPLAY_AXES.forEach((axis) => {
       const setpoint = getTelemetrySetpoint(axis);
-      const position = Number(latestImu[axis]);
-      const error = angleError(setpoint, position);
+      const position = axis === "heave" ? -Number(latestDepth.dpt) : Number(latestImu[axis]);
+      const error = axis === "heave" ? setpoint - position : angleError(setpoint, position);
       const tr = document.createElement("tr");
       const tdAxis = document.createElement("td");
       const tdSet = document.createElement("td");
@@ -98,8 +101,10 @@
       tdSet.textContent = fmt(setpoint, 2);
       tdPos.textContent = fmt(position, 2);
       tdErr.textContent = fmt(error, 2);
-      if (isFiniteNumber(error) && Math.abs(error) > 10) tdErr.className = "text-warning";
-      if (isFiniteNumber(error) && Math.abs(error) > 25) tdErr.className = "text-danger";
+      const warnLimit = axis === "heave" ? 0.15 : 10;
+      const dangerLimit = axis === "heave" ? 0.35 : 25;
+      if (isFiniteNumber(error) && Math.abs(error) > warnLimit) tdErr.className = "text-warning";
+      if (isFiniteNumber(error) && Math.abs(error) > dangerLimit) tdErr.className = "text-danger";
       tr.append(tdAxis, tdSet, tdPos, tdErr);
       frag.appendChild(tr);
     });
@@ -119,7 +124,9 @@
   async function pollImuAndTelemetry() {
     const imuReq = fetch("/api/imu/status").then((res) => res.json());
     const telemetryReq = fetch("/api/control/telemetry").then((res) => res.json());
-    const results = await Promise.allSettled([imuReq, telemetryReq]);
+    const depthReq = fetch("/api/depth").then((res) => res.json());
+    const frameReq = fetch("/api/frame/status").then((res) => res.json());
+    const results = await Promise.allSettled([imuReq, telemetryReq, depthReq, frameReq]);
 
     if (results[0].status === "fulfilled" && results[0].value.ok) {
       const stats = results[0].value.stats || {};
@@ -137,9 +144,38 @@
       updateTelemetryAge(latestTelemetry);
     }
 
+    if (results[2].status === "fulfilled") {
+      latestDepth = results[2].value || {};
+    }
+
+    if (results[3].status === "fulfilled" && results[3].value.ok) {
+      updateFrameStatus(results[3].value.state || {}, results[3].value.telemetry || {});
+    }
+
     updateTelemetryTable();
   }
 
+  function updateFrameStatus(state, telemetry) {
+    const locked = Boolean((telemetry && telemetry.locked) || (state && state.active));
+    setBadge(frameStatus, locked ? "FRAME LOCKED" : "FRAME FREE", locked ? "bg-info text-dark" : "bg-secondary");
+  }
+
+  async function sendFrameCommand(action) {
+    const endpoint = action === "lock" ? "/api/frame/lock" : "/api/frame/unlock";
+    try {
+      const res = await fetch(endpoint, { method: "POST" });
+      const data = await res.json();
+      if (data.ok) {
+        updateFrameStatus(data.state || {}, {});
+      } else {
+        setBadge(frameStatus, "FRAME ERROR", "bg-danger");
+      }
+    } catch (err) {
+      setBadge(frameStatus, "FRAME ERROR", "bg-danger");
+      console.error("Frame command failed:", err);
+    }
+  }
+
   function getSliderValue(axis) {
     const slider = document.getElementById("slider-" + axis);
     return slider ? parseInt(slider.value, 10) / 100 : 0;
@@ -358,7 +394,7 @@
         fillSetpointFields(data.setpoints || {});
         setSetpointStatus("ACTIVE", "bg-danger");
         setPageStatus("PID HOLD ACTIVE", "bg-success");
-        setFeedback("Started from current IMU attitude with neutral manual command axes.", "text-success");
+        setFeedback("Started from current IMU attitude and current depth with neutral manual command axes.", "text-success");
       } else {
         setSetpointStatus("BLOCKED", "bg-danger");
         setPageStatus("START BLOCKED", "bg-danger");
@@ -584,6 +620,11 @@
     if (btnPidRequest) btnPidRequest.addEventListener("click", requestPidGains);
     if (btnPidSend) btnPidSend.addEventListener("click", sendPidGains);
 
+    const btnFrameLock = document.getElementById("btn-frame-lock");
+    const btnFrameUnlock = document.getElementById("btn-frame-unlock");
+    if (btnFrameLock) btnFrameLock.addEventListener("click", () => sendFrameCommand("lock"));
+    if (btnFrameUnlock) btnFrameUnlock.addEventListener("click", () => sendFrameCommand("unlock"));
+
     const btnPidSave = document.getElementById("btn-pid-save");
     const btnPidLoad = document.getElementById("btn-pid-load");
     const btnPidDelete = document.getElementById("btn-pid-delete");

static/templates/pid_tuning.html

@@ -18,9 +18,12 @@
       <span class="pid-kicker">PID Tuning</span>
       <span id="pid-page-status" class="badge bg-secondary">READY</span>
       <span id="pid-link-status" class="badge bg-secondary">LINK IDLE</span>
+      <span id="frame-lock-status" class="badge bg-secondary">FRAME FREE</span>
       <span class="pid-muted">IMU <span id="pid-imu-age">--</span> ms</span>
     </div>
     <div class="pid-action-controls">
+      <button type="button" id="btn-frame-lock" class="btn btn-sm btn-outline-info">Lock Frame</button>
+      <button type="button" id="btn-frame-unlock" class="btn btn-sm btn-outline-secondary">Unlock Frame</button>
       <button type="button" class="btn btn-sm btn-primary js-start-pid">Start PID</button>
       <button type="button" class="btn btn-sm btn-danger pid-zero-btn js-zero-all-pid">ZERO PID + THRUST</button>
     </div>

tests/test_protocols.py

@@ -4,7 +4,15 @@
 
 import lib.control_telemetry as control_telem
 import lib.resource_receiver as resource_telem
-from lib import axis_config_sender, bitmask, crc, net_transport, pid_config_client, system_control_client
+from lib import (
+    axis_config_sender,
+    bitmask,
+    crc,
+    frame_control_client,
+    net_transport,
+    pid_config_client,
+    system_control_client,
+)
 from lib.json_data_handler import JSONDataHandler
 
 
@@ -40,12 +48,22 @@ def test_system_reset_packet_crc():
     assert pkt[8:] == struct.pack("!I", expected_crc)
 
 
+def test_frame_control_packet_crc():
+    pkt = frame_control_client.build_frame_packet(frame_control_client.TYPE_LOCK, 0x01020304)
+    assert pkt[:4] == b"FRM1"
+    assert pkt[4:8] == struct.pack("!B3x", frame_control_client.TYPE_LOCK)
+    assert pkt[8:12] == struct.pack("!I", 0x01020304)
+    expected_crc = crc.crc32_ieee(pkt[:12])
+    assert pkt[12:] == struct.pack("!I", expected_crc)
+
+
 def test_network_defaults_use_current_mcu_address():
     assert net_transport.DEFAULT_ROV_HOST == "10.77.0.2"
     assert net_transport.DEFAULT_BROADCAST == "10.77.0.255"
     assert bitmask.NUCLEO_HOST == net_transport.DEFAULT_ROV_HOST
     assert axis_config_sender.NUCLEO_HOST == net_transport.DEFAULT_ROV_HOST
     assert pid_config_client.MCU_IP == net_transport.DEFAULT_ROV_HOST
+    assert frame_control_client.DEFAULT_ROV_HOST == net_transport.DEFAULT_ROV_HOST
 
 
 def test_control_telemetry_history(monkeypatch, tmp_path):