Face-down dock-hold (lock attitude + precision gain)

app.py

@@ -110,6 +110,9 @@
 # Initialize setpoint override client (UDP port 5007)
 app.config["SETPOINT_OVERRIDE"] = init_setpoint_override(resource_monitor=app.config["RESOURCE"])
 
+# Give the controller the override client so it can lock attitude for docking
+app.config["CONTROLLER"].set_setpoint_override(app.config["SETPOINT_OVERRIDE"])
+
 # Start control loop telemetry receiver (UDP port 5005)
 app.config["CONTROL_TELEM"] = init_control_telemetry(port=5005)
 

lib/controller.py

@@ -37,6 +37,12 @@ def _controller_errors():
 # Defaults; overridable later via the controller-mapping settings.
 FRAME_TOGGLE_BUTTON = pygame.CONTROLLER_BUTTON_B
 FRAME_TOGGLE_BUTTON_JS = 1
+DOCK_TOGGLE_BUTTON = pygame.CONTROLLER_BUTTON_Y
+DOCK_TOGGLE_BUTTON_JS = 3
+
+# Face-down dock-hold targets.
+DOCK_PITCH_DEG = 90.0  # nose-down face-down target (sign confirmed in-water)
+DOCK_GAIN = 0.5  # precision master gain applied while docked
 
 
 class Controller:
@@ -91,6 +97,12 @@ def __init__(self, bitmask_client: BitmaskClient = None, rate_hz: float = 60.0):
         self._frame_mode = "rov"
         self._ref_yaw = 0.0
         self._prev_frame_button = False
+        # Setpoint override client (injected) for face-down dock-hold
+        self._override = None
+        self._dock_lock = threading.Lock()
+        self._docked = False
+        self._saved_gains = None
+        self._prev_dock_button = False
         self._try_connect()
 
     def _try_connect(self):
@@ -381,6 +393,63 @@ def _apply_frame(self, surge, sway):
         body_sway = -surge * sin_d + sway * cos_d
         return body_surge, body_sway
 
+    # --- Face-down dock-hold API ---
+    def set_setpoint_override(self, client):
+        """Inject the setpoint-override client used to lock attitude for docking."""
+        self._override = client
+
+    def dock_engage(self):
+        """Lock attitude face-down (pitch + level roll + current heading) and
+        apply a precision gain so the pilot can still nudge surge/sway/heave.
+
+        Relies on tuned pitch/roll/yaw PID on the MCU (the override only holds
+        attitude if those gains are non-zero).
+        """
+        captured_yaw = self._current_yaw_fresh()
+        axes = {"pitch": DOCK_PITCH_DEG, "roll": 0.0}
+        if captured_yaw is not None:
+            axes["yaw"] = captured_yaw
+        with self._dock_lock:
+            if self._override is None:
+                return {"ok": False, "error": "Setpoint override client unavailable"}
+            try:
+                self._override.send_override(axes, replay_attempts=5, replay_delay=0.1)
+            except Exception as exc:  # noqa: BLE001 - surface any send failure to caller
+                return {"ok": False, "error": str(exc)}
+            if not self._docked:
+                self._saved_gains = self.get_gains()
+            self.set_gains(master=DOCK_GAIN)
+            self._docked = True
+        return {"ok": True, "docked": True, "setpoints": axes}
+
+    def dock_release(self):
+        """Clear the attitude lock and restore the pre-dock gains."""
+        with self._dock_lock:
+            if self._override is not None:
+                try:
+                    self._override.clear_override()
+                except Exception:  # noqa: BLE001 - release should always succeed locally
+                    pass
+            if self._saved_gains:
+                saved = self._saved_gains
+                self.set_gains(
+                    master=saved.get("master"),
+                    **{k: saved[k] for k in ("surge", "sway", "heave", "roll", "pitch", "yaw") if k in saved},
+                )
+                self._saved_gains = None
+            self._docked = False
+        return {"ok": True, "docked": False}
+
+    def dock_toggle(self):
+        """Engage dock-hold if released, otherwise release it."""
+        with self._dock_lock:
+            docked = self._docked
+        return self.dock_release() if docked else self.dock_engage()
+
+    def is_docked(self):
+        with self._dock_lock:
+            return self._docked
+
     def _reset_command(self):
         """Reset all axes to neutral/zero."""
         if self.bm:
@@ -518,6 +587,12 @@ def update(self):
             self.toggle_frame_mode()
         self._prev_frame_button = frame_pressed
 
+        # Dock-hold toggle (edge-detected): lock/unlock face-down attitude
+        dock_pressed = self._read_button(DOCK_TOGGLE_BUTTON, DOCK_TOGGLE_BUTTON_JS)
+        if dock_pressed and not self._prev_dock_button:
+            self.dock_toggle()
+        self._prev_dock_button = dock_pressed
+
         self._update_input_status(buttons)
 
         # Apply gain to each axis

routes.py

@@ -341,6 +341,7 @@ def get_command_status():
                 "uplink": uplink,
                 "controller": controller_state,
                 "frame": controller.get_frame_mode() if controller else "rov",
+                "docked": controller.is_docked() if controller else False,
                 "udp_rx_count": udp_rx,
                 "udp_rx_errors": udp_err,
                 "override": state,
@@ -731,6 +732,39 @@ def set_frame():
             mode = ctrl.set_frame_mode(data.get("mode", "rov"))
         return jsonify({"ok": True, "frame": mode})
 
+    # --- Dock-hold endpoints ---
+    @app.route("/api/dock/status", methods=["GET"])
+    def dock_status():
+        """Return whether face-down dock-hold is engaged."""
+        ctrl = current_app.config.get("CONTROLLER")
+        return jsonify({"ok": True, "docked": ctrl.is_docked() if ctrl else False})
+
+    @app.route("/api/dock/toggle", methods=["POST"])
+    def dock_toggle():
+        """Toggle face-down dock-hold (lock attitude + precision gain)."""
+        ctrl = current_app.config.get("CONTROLLER")
+        if not ctrl:
+            return jsonify({"ok": False, "error": "Controller not available"}), 503
+        result = ctrl.dock_toggle()
+        return jsonify(result), (200 if result.get("ok") else 503)
+
+    @app.route("/api/dock/engage", methods=["POST"])
+    def dock_engage():
+        """Engage face-down dock-hold."""
+        ctrl = current_app.config.get("CONTROLLER")
+        if not ctrl:
+            return jsonify({"ok": False, "error": "Controller not available"}), 503
+        result = ctrl.dock_engage()
+        return jsonify(result), (200 if result.get("ok") else 503)
+
+    @app.route("/api/dock/release", methods=["POST"])
+    def dock_release():
+        """Release face-down dock-hold."""
+        ctrl = current_app.config.get("CONTROLLER")
+        if not ctrl:
+            return jsonify({"ok": False, "error": "Controller not available"}), 503
+        return jsonify(ctrl.dock_release())
+
     # --- Gain endpoints ---
     @app.route("/api/controller/gains", methods=["GET"])
     def get_gains():

static/js/debug.js

@@ -1053,4 +1053,52 @@
     refreshFrame();
     setInterval(refreshFrame, 1000);
   }
+
+  // --- Face-down Dock-hold ---
+  const btnDockEngage = document.getElementById("btn-dock-engage");
+  const btnDockRelease = document.getElementById("btn-dock-release");
+  const dockStatus = document.getElementById("dock-status");
+  const dockFeedback = document.getElementById("dock-feedback");
+
+  function setDockStatus(docked) {
+    if (!dockStatus) return;
+    dockStatus.textContent = docked ? "LOCKED" : "RELEASED";
+    dockStatus.className = "badge " + (docked ? "bg-warning text-dark" : "bg-secondary");
+  }
+
+  async function refreshDock() {
+    try {
+      var res = await fetch("/api/dock/status");
+      var data = await res.json();
+      if (data.ok) setDockStatus(Boolean(data.docked));
+    } catch (e) {
+      /* ignore */
+    }
+  }
+
+  async function postDock(path) {
+    try {
+      var res = await fetch(path, { method: "POST" });
+      var data = await res.json();
+      if (data.ok) {
+        setDockStatus(Boolean(data.docked));
+        if (dockFeedback) dockFeedback.textContent = data.docked ? "Attitude locked." : "Released.";
+      } else if (dockFeedback) {
+        dockFeedback.textContent = data.error || "Failed";
+      }
+    } catch (e) {
+      if (dockFeedback) dockFeedback.textContent = "Error: " + e.message;
+    }
+  }
+
+  if (btnDockEngage) {
+    btnDockEngage.addEventListener("click", function () { postDock("/api/dock/engage"); });
+  }
+  if (btnDockRelease) {
+    btnDockRelease.addEventListener("click", function () { postDock("/api/dock/release"); });
+  }
+  if (btnDockEngage || btnDockRelease) {
+    refreshDock();
+    setInterval(refreshDock, 1000);
+  }
 })();

static/js/home_controls.js

@@ -1,4 +1,4 @@
-// home_controls.js – home dashboard control-mode buttons (frame toggle).
+// home_controls.js – home dashboard control-mode buttons (frame + dock).
 
 function setFrameButton(mode) {
     const btn = document.getElementById("frame-toggle");
@@ -11,11 +11,21 @@ function setFrameButton(mode) {
     if (status) status.textContent = global ? "World / captured heading" : "Body-relative";
 }
 
-async function refreshFrame() {
+function setDockButton(docked) {
+    const btn = document.getElementById("docking-button");
+    if (!btn) return;
+    btn.textContent = docked ? "Docking: LOCKED" : "Docking";
+    btn.classList.toggle("btn-warning", docked);
+    btn.classList.toggle("btn-primary", !docked);
+}
+
+async function refreshControls() {
     try {
-        const res = await fetch("/api/controller/frame");
+        const res = await fetch("/api/command/status", { cache: "no-store" });
         const data = await res.json();
-        if (data.ok) setFrameButton(data.frame);
+        if (!data.ok) return;
+        setFrameButton(data.frame);
+        setDockButton(Boolean(data.docked));
     } catch (error) {
         /* ignore transient errors */
     }
@@ -35,10 +45,23 @@ async function toggleFrame() {
     }
 }
 
+async function toggleDock() {
+    try {
+        const res = await fetch("/api/dock/toggle", { method: "POST" });
+        const data = await res.json();
+        if (data.ok) setDockButton(Boolean(data.docked));
+        else console.error("Dock toggle failed:", data.error);
+    } catch (error) {
+        console.error("Error toggling dock:", error);
+    }
+}
+
 document.addEventListener("DOMContentLoaded", () => {
     const frameBtn = document.getElementById("frame-toggle");
     if (frameBtn) frameBtn.addEventListener("click", toggleFrame);
-    refreshFrame();
-    // Reflect controller-button toggles made on the gamepad.
-    setInterval(refreshFrame, 1000);
+    const dockBtn = document.getElementById("docking-button");
+    if (dockBtn) dockBtn.addEventListener("click", toggleDock);
+    refreshControls();
+    // Reflect toggles made from the gamepad too.
+    setInterval(refreshControls, 1000);
 });

static/templates/debug.html

@@ -152,6 +152,28 @@ <h5 class="card-title mb-0">Control Frame</h5>
   </div>
 </div>
 
+<!-- Face-down Dock-hold -->
+<div class="row g-4 mt-1">
+  <div class="col-12">
+    <div class="card custom-card">
+      <div class="card-body">
+        <div class="d-flex justify-content-between align-items-center mb-2">
+          <h5 class="card-title mb-0">Face-down Dock-hold</h5>
+          <span id="dock-status" class="badge bg-secondary">RELEASED</span>
+        </div>
+        <p class="text-muted small mb-3">
+          Locks attitude face-down (pitch ~90°, roll level, current heading) via the setpoint override and
+          applies a precision gain so you can still nudge surge/sway/heave into the dock.
+          Requires tuned pitch/roll/yaw PID gains.
+        </p>
+        <button id="btn-dock-engage" class="btn btn-sm btn-warning me-1">Engage Dock-hold</button>
+        <button id="btn-dock-release" class="btn btn-sm btn-outline-secondary">Release</button>
+        <span id="dock-feedback" class="small text-light-muted ms-2"></span>
+      </div>
+    </div>
+  </div>
+</div>
+
 <!-- Accelerometer Axis Mapping -->
 <div class="row g-4 mt-1">
   <div class="col-12">

tests/test_controller.py

@@ -100,6 +100,11 @@ def build_controller(controller=None, joystick=None):
     ctrl._frame_mode = "rov"
     ctrl._ref_yaw = 0.0
     ctrl._prev_frame_button = False
+    ctrl._override = None
+    ctrl._dock_lock = threading.Lock()
+    ctrl._docked = False
+    ctrl._saved_gains = None
+    ctrl._prev_dock_button = False
     return ctrl
 
 
@@ -112,6 +117,20 @@ def get_stats(self):
         return {"age_ms": self.age_ms, "last_data": {"yaw": self.yaw}}
 
 
+class FakeOverride:
+    def __init__(self):
+        self.sent = []
+        self.cleared = 0
+
+    def send_override(self, axes, replay_attempts=1, replay_delay=0.0):
+        self.sent.append(dict(axes))
+        return {"active": True}
+
+    def clear_override(self):
+        self.cleared += 1
+        return {"active": False}
+
+
 def test_sdl_gamecontroller_mapping_normalizes_linux_playstation_layout(monkeypatch):
     monkeypatch.setattr(pygame.event, "get", lambda: [])
     sdl = FakeSdlController(
@@ -250,6 +269,64 @@ def test_toggle_frame_mode_flips_between_rov_and_global():
     assert ctrl.toggle_frame_mode() == "rov"
 
 
+def test_dock_engage_locks_attitude_and_applies_precision_gain():
+    ctrl = build_controller()
+    ctrl.set_imu(FakeIMU(yaw=33.0))
+    override = FakeOverride()
+    ctrl.set_setpoint_override(override)
+    ctrl.set_gains(master=1.0)
+
+    result = ctrl.dock_engage()
+
+    assert result["ok"] is True
+    assert ctrl.is_docked() is True
+    sent = override.sent[-1]
+    assert sent["pitch"] == controller_module.DOCK_PITCH_DEG
+    assert sent["roll"] == 0.0
+    assert sent["yaw"] == pytest.approx(33.0)
+    assert ctrl.get_gains()["master"] == pytest.approx(controller_module.DOCK_GAIN)
+
+
+def test_dock_release_clears_override_and_restores_gains():
+    ctrl = build_controller()
+    ctrl.set_imu(FakeIMU(yaw=0.0))
+    override = FakeOverride()
+    ctrl.set_setpoint_override(override)
+    ctrl.set_gains(master=0.8, surge=0.6)
+
+    ctrl.dock_engage()
+    assert ctrl.get_gains()["master"] == pytest.approx(controller_module.DOCK_GAIN)
+
+    res = ctrl.dock_release()
+
+    assert res["docked"] is False
+    assert ctrl.is_docked() is False
+    assert override.cleared == 1
+    gains = ctrl.get_gains()
+    assert gains["master"] == pytest.approx(0.8)
+    assert gains["surge"] == pytest.approx(0.6)
+
+
+def test_dock_toggle_flips_state():
+    ctrl = build_controller()
+    ctrl.set_imu(FakeIMU(yaw=0.0))
+    ctrl.set_setpoint_override(FakeOverride())
+    assert ctrl.is_docked() is False
+    ctrl.dock_toggle()
+    assert ctrl.is_docked() is True
+    ctrl.dock_toggle()
+    assert ctrl.is_docked() is False
+
+
+def test_dock_engage_without_override_client_reports_error():
+    ctrl = build_controller()
+    ctrl.set_imu(FakeIMU(yaw=0.0))
+    # No override client injected.
+    result = ctrl.dock_engage()
+    assert result["ok"] is False
+    assert ctrl.is_docked() is False
+
+
 def test_non_linux_connection_uses_raw_joystick_without_sdl_probe(monkeypatch):
     class FailingSdlController:
         @staticmethod