diff --git a/app.py b/app.py
index f54a470..0233573 100644
--- a/app.py
+++ b/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)
diff --git a/lib/controller.py b/lib/controller.py
index ba1e17e..e850f60 100644
--- a/lib/controller.py
+++ b/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
diff --git a/routes.py b/routes.py
index 0d5cddb..7ffb9e2 100644
--- a/routes.py
+++ b/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():
diff --git a/static/js/debug.js b/static/js/debug.js
index 1bfe6bc..9e87915 100644
--- a/static/js/debug.js
+++ b/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);
+ }
})();
diff --git a/static/js/home_controls.js b/static/js/home_controls.js
index b279eba..cdb631b 100644
--- a/static/js/home_controls.js
+++ b/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);
});
diff --git a/static/templates/debug.html b/static/templates/debug.html
index 1814843..538d30a 100644
--- a/static/templates/debug.html
+++ b/static/templates/debug.html
@@ -152,6 +152,28 @@
Control Frame
+
+
+
+
+
+
+
Face-down Dock-hold
+ RELEASED
+
+
+ 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.
+