From 5a6ba3939e0870808dc8f65af6a3dc3df11c5daf Mon Sep 17 00:00:00 2001 From: mringsby Date: Fri, 5 Jun 2026 15:16:39 +0200 Subject: [PATCH] Add face-down dock-hold (lock attitude + precision gain) Adds a one-button dock-hold that locks the ROV face-down for docking and gives the pilot finer control for the approach: - Engage: capture current heading and send a setpoint override holding pitch ~90deg nose-down, roll level, and current yaw; save the current gains and drop to a precision master gain. Surge/sway/heave stay under pilot control. - Release: clear the override and restore the saved gains. Relies on the already-tuned pitch/roll/yaw PID (the override only holds attitude when those gains are non-zero) and reuses the existing setpoint-override service (UDP 5007) -- no firmware/protocol change. Toggle from the gamepad (Y button), the home Docking button (previously an unwired stub), or the debug page. New endpoints: /api/dock/toggle|engage|release |status; dock state also surfaced in /api/command/status. Co-Authored-By: Claude Opus 4.8 --- app.py | 3 ++ lib/controller.py | 75 ++++++++++++++++++++++++++++++++++++ routes.py | 34 ++++++++++++++++ static/js/debug.js | 48 +++++++++++++++++++++++ static/js/home_controls.js | 37 ++++++++++++++---- static/templates/debug.html | 22 +++++++++++ tests/test_controller.py | 77 +++++++++++++++++++++++++++++++++++++ 7 files changed, 289 insertions(+), 7 deletions(-) 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. +

+ + + +
+
+
+
+
diff --git a/tests/test_controller.py b/tests/test_controller.py index f420204..e48fb4e 100644 --- a/tests/test_controller.py +++ b/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