From 74a2e2c22f2a4c702df31eddd3587f5a8145dccc Mon Sep 17 00:00:00 2001 From: mringsby Date: Fri, 5 Jun 2026 15:12:26 +0200 Subject: [PATCH] Add global vs ROV control frame toggle Adds a world-referenced translation mode alongside the default body-relative (ROV) frame. When global mode is enabled the current IMU yaw is captured as a reference and pilot surge/sway are rotated by the live yaw so 'forward' keeps driving the captured heading regardless of how the ROV has yawed. Falls back to ROV frame whenever IMU yaw is missing or stale. Implemented entirely topside (no firmware/protocol change): the Controller reads yaw from the injected IMU receiver. Toggle from the gamepad (B button), the home dashboard, or the debug page; current frame is also surfaced in /api/command/status. New endpoint: GET/POST /api/controller/frame. Co-Authored-By: Claude Opus 4.8 --- app.py | 3 ++ lib/controller.py | 90 ++++++++++++++++++++++++++++++++++++ routes.py | 23 +++++++++ static/js/debug.js | 39 ++++++++++++++++ static/js/home_controls.js | 44 ++++++++++++++++++ static/templates/debug.html | 18 ++++++++ static/templates/layout.html | 14 ++++++ tests/test_controller.py | 52 +++++++++++++++++++++ 8 files changed, 283 insertions(+) create mode 100644 static/js/home_controls.js diff --git a/app.py b/app.py index 660b256..f54a470 100644 --- a/app.py +++ b/app.py @@ -32,6 +32,9 @@ # Start background IMU receiver (UDP port 5002) app.config["IMU"] = init_imu_receiver(port=5002) +# Give the controller the IMU so it can do world-frame (global) translation +app.config["CONTROLLER"].set_imu(app.config["IMU"]) + # Tracks ordered ARUCO markers for the pipeline challenge. app.config["ARUCO_LOGGER"] = ArucoPipelineLogger() diff --git a/lib/controller.py b/lib/controller.py index 082f503..ba1e17e 100644 --- a/lib/controller.py +++ b/lib/controller.py @@ -6,6 +6,7 @@ os.environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" os.environ["SDL_VIDEODRIVER"] = "dummy" # Run pygame without video/window on Linux/MacOS +import math import threading import time @@ -32,6 +33,12 @@ def _controller_errors(): return tuple(errors) +# Controller button bindings: (SDL game-controller button, raw-joystick fallback index). +# Defaults; overridable later via the controller-mapping settings. +FRAME_TOGGLE_BUTTON = pygame.CONTROLLER_BUTTON_B +FRAME_TOGGLE_BUTTON_JS = 1 + + class Controller: AXIS_THRESHOLDS = { "leftx": (0, 0.1), @@ -77,6 +84,13 @@ def __init__(self, bitmask_client: BitmaskClient = None, rate_hz: float = 60.0): "pitch": 1.0, "yaw": 1.0, } + # IMU receiver (injected from app.py) for world-frame translation + self._imu = None + # Control frame: "rov" (body-relative) or "global" (captured heading) + self._frame_lock = threading.Lock() + self._frame_mode = "rov" + self._ref_yaw = 0.0 + self._prev_frame_button = False self._try_connect() def _try_connect(self): @@ -301,6 +315,72 @@ def get_light(self): """Return current light brightness as a normalized 0.0-1.0 level.""" return self.light + # --- Control frame API --- + def set_imu(self, imu): + """Inject the IMU receiver used for world-frame translation.""" + self._imu = imu + + def _current_yaw_fresh(self, max_age_ms=500): + """Return the latest IMU yaw (deg) if fresh, else None.""" + imu = self._imu + if imu is None: + return None + try: + stats = imu.get_stats() + except Exception: + return None + age = stats.get("age_ms") + if age is None or age > max_age_ms: + return None + yaw = (stats.get("last_data") or {}).get("yaw") + try: + return float(yaw) + except (TypeError, ValueError): + return None + + def set_frame_mode(self, mode): + """Set the control frame ('rov' or 'global'); capture heading on global.""" + mode = "global" if str(mode).lower() == "global" else "rov" + captured = self._current_yaw_fresh() + with self._frame_lock: + self._frame_mode = mode + if mode == "global": + self._ref_yaw = captured if captured is not None else 0.0 + return mode + + def toggle_frame_mode(self): + """Flip between 'rov' and 'global' frames.""" + with self._frame_lock: + current = self._frame_mode + return self.set_frame_mode("rov" if current == "global" else "global") + + def get_frame_mode(self): + with self._frame_lock: + return self._frame_mode + + def _apply_frame(self, surge, sway): + """Rotate horizontal translation into the body frame when in global mode. + + Global mode keeps 'forward' pointing at the heading captured when the + mode was enabled, regardless of how the ROV has since yawed. Falls back + to ROV (body) frame when the IMU yaw is missing or stale. The rotation + sign is the single place to flip if in-water testing shows it inverted. + """ + with self._frame_lock: + mode = self._frame_mode + ref = self._ref_yaw + if mode != "global": + return surge, sway + yaw = self._current_yaw_fresh() + if yaw is None: + return surge, sway + delta = math.radians(yaw - ref) + cos_d = math.cos(delta) + sin_d = math.sin(delta) + body_surge = surge * cos_d + sway * sin_d + body_sway = -surge * sin_d + sway * cos_d + return body_surge, body_sway + def _reset_command(self): """Reset all axes to neutral/zero.""" if self.bm: @@ -431,6 +511,13 @@ def update(self): self._prev_dpad_up = dpad_up self._prev_dpad_down = dpad_down + + # Frame toggle (edge-detected): switch between ROV and global frames + frame_pressed = self._read_button(FRAME_TOGGLE_BUTTON, FRAME_TOGGLE_BUTTON_JS) + if frame_pressed and not self._prev_frame_button: + self.toggle_frame_mode() + self._prev_frame_button = frame_pressed + self._update_input_status(buttons) # Apply gain to each axis @@ -441,6 +528,9 @@ def update(self): pitch = self._apply_gain("pitch", pitch) yaw = self._apply_gain("yaw", yaw) + # World/global frame: rotate horizontal translation by IMU yaw if enabled + surge, sway = self._apply_frame(surge, sway) + # Send to ROV! if self.bm: self.bm.set_from_axes( diff --git a/routes.py b/routes.py index 5e33c19..0d5cddb 100644 --- a/routes.py +++ b/routes.py @@ -340,6 +340,7 @@ def get_command_status(): "ok": True, "uplink": uplink, "controller": controller_state, + "frame": controller.get_frame_mode() if controller else "rov", "udp_rx_count": udp_rx, "udp_rx_errors": udp_err, "override": state, @@ -708,6 +709,28 @@ def zero_all_pid(): } ) + # --- Control frame endpoints --- + @app.route("/api/controller/frame", methods=["GET"]) + def get_frame(): + """Return the current control frame ('rov' or 'global').""" + ctrl = current_app.config.get("CONTROLLER") + if not ctrl: + return jsonify({"ok": False, "error": "Controller not available"}), 503 + return jsonify({"ok": True, "frame": ctrl.get_frame_mode()}) + + @app.route("/api/controller/frame", methods=["POST"]) + def set_frame(): + """Set the control frame. JSON: {"mode": "rov"|"global"} or {"toggle": true}.""" + data = request.get_json(force=True, silent=True) or {} + ctrl = current_app.config.get("CONTROLLER") + if not ctrl: + return jsonify({"ok": False, "error": "Controller not available"}), 503 + if data.get("toggle"): + mode = ctrl.toggle_frame_mode() + else: + mode = ctrl.set_frame_mode(data.get("mode", "rov")) + return jsonify({"ok": True, "frame": mode}) + # --- 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 a228a38..1bfe6bc 100644 --- a/static/js/debug.js +++ b/static/js/debug.js @@ -1014,4 +1014,43 @@ }) .catch(function () {}); } + + // --- Control Frame --- + const frameToggle = document.getElementById("debug-frame-toggle"); + + function setFrameButton(mode) { + if (!frameToggle) return; + var global = mode === "global"; + frameToggle.textContent = "Frame: " + (global ? "GLOBAL" : "ROV"); + frameToggle.classList.toggle("btn-info", global); + frameToggle.classList.toggle("btn-outline-info", !global); + } + + async function refreshFrame() { + try { + var res = await fetch("/api/controller/frame"); + var data = await res.json(); + if (data.ok) setFrameButton(data.frame); + } catch (e) { + /* ignore */ + } + } + + if (frameToggle) { + frameToggle.addEventListener("click", async function () { + try { + var res = await fetch("/api/controller/frame", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ toggle: true }), + }); + var data = await res.json(); + if (data.ok) setFrameButton(data.frame); + } catch (e) { + /* ignore */ + } + }); + refreshFrame(); + setInterval(refreshFrame, 1000); + } })(); diff --git a/static/js/home_controls.js b/static/js/home_controls.js new file mode 100644 index 0000000..b279eba --- /dev/null +++ b/static/js/home_controls.js @@ -0,0 +1,44 @@ +// home_controls.js – home dashboard control-mode buttons (frame toggle). + +function setFrameButton(mode) { + const btn = document.getElementById("frame-toggle"); + const status = document.getElementById("frame-status"); + if (!btn) return; + const global = mode === "global"; + btn.textContent = "Frame: " + (global ? "GLOBAL" : "ROV"); + btn.classList.toggle("btn-info", global); + btn.classList.toggle("btn-outline-info", !global); + if (status) status.textContent = global ? "World / captured heading" : "Body-relative"; +} + +async function refreshFrame() { + try { + const res = await fetch("/api/controller/frame"); + const data = await res.json(); + if (data.ok) setFrameButton(data.frame); + } catch (error) { + /* ignore transient errors */ + } +} + +async function toggleFrame() { + try { + const res = await fetch("/api/controller/frame", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ toggle: true }), + }); + const data = await res.json(); + if (data.ok) setFrameButton(data.frame); + } catch (error) { + console.error("Error toggling frame:", 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); +}); diff --git a/static/templates/debug.html b/static/templates/debug.html index 2435999..1814843 100644 --- a/static/templates/debug.html +++ b/static/templates/debug.html @@ -134,6 +134,24 @@
Lights
+ +
+
+
+
+
+
Control Frame
+ +
+

+ Toggle between ROV (body-relative) and global (captured-heading) translation. + Global rotates surge/sway by the live IMU yaw; it falls back to ROV frame if the IMU is stale. +

+
+
+
+
+
diff --git a/static/templates/layout.html b/static/templates/layout.html index 145a2d1..36e6068 100644 --- a/static/templates/layout.html +++ b/static/templates/layout.html @@ -176,6 +176,19 @@
Autonomus missions
+
+ +
+
+
+
Control Mode
+ + Body-relative +
+
+
+
+
@@ -202,4 +215,5 @@
Controller Status
+ {% endblock %} diff --git a/tests/test_controller.py b/tests/test_controller.py index 1d8ea19..f420204 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -95,9 +95,23 @@ def build_controller(controller=None, joystick=None): "pitch": 1.0, "yaw": 1.0, } + ctrl._imu = None + ctrl._frame_lock = threading.Lock() + ctrl._frame_mode = "rov" + ctrl._ref_yaw = 0.0 + ctrl._prev_frame_button = False return ctrl +class FakeIMU: + def __init__(self, yaw=0.0, age_ms=10): + self.yaw = yaw + self.age_ms = age_ms + + def get_stats(self): + return {"age_ms": self.age_ms, "last_data": {"yaw": self.yaw}} + + def test_sdl_gamecontroller_mapping_normalizes_linux_playstation_layout(monkeypatch): monkeypatch.setattr(pygame.event, "get", lambda: []) sdl = FakeSdlController( @@ -198,6 +212,44 @@ def test_set_light_clamps_and_pushes_to_bitmask(): assert ctrl.bm.commands[-1]["light"] == 0 +def test_rov_frame_is_identity(): + ctrl = build_controller() + ctrl.set_imu(FakeIMU(yaw=45.0)) + # Default mode is "rov" -> no rotation regardless of yaw. + assert ctrl._apply_frame(0.7, -0.3) == (0.7, -0.3) + + +def test_global_frame_rotates_translation_by_relative_yaw(): + ctrl = build_controller() + imu = FakeIMU(yaw=0.0) + ctrl.set_imu(imu) + ctrl.set_frame_mode("global") # captures ref_yaw = 0 + # ROV has since yawed +90°; world-forward (surge=1) maps onto body axes. + imu.yaw = 90.0 + body_surge, body_sway = ctrl._apply_frame(1.0, 0.0) + assert body_surge == pytest.approx(0.0, abs=1e-9) + assert body_sway == pytest.approx(-1.0, abs=1e-9) + + +def test_global_frame_falls_back_to_rov_when_imu_stale(): + ctrl = build_controller() + imu = FakeIMU(yaw=0.0) + ctrl.set_imu(imu) + ctrl.set_frame_mode("global") + imu.yaw = 90.0 + imu.age_ms = 5000 # stale -> no rotation + assert ctrl._apply_frame(1.0, 0.0) == (1.0, 0.0) + + +def test_toggle_frame_mode_flips_between_rov_and_global(): + ctrl = build_controller() + ctrl.set_imu(FakeIMU(yaw=0.0)) + assert ctrl.get_frame_mode() == "rov" + assert ctrl.toggle_frame_mode() == "global" + assert ctrl.get_frame_mode() == "global" + assert ctrl.toggle_frame_mode() == "rov" + + def test_non_linux_connection_uses_raw_joystick_without_sdl_probe(monkeypatch): class FailingSdlController: @staticmethod