Global vs ROV control frame toggle

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()
 

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(

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():

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);
+  }
 })();

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);
+});

static/templates/debug.html

@@ -134,6 +134,24 @@ <h5 class="card-title mb-3">Lights</h5>
   </div>
 </div>
 
+<!-- Control Frame -->
+<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">
+          <h5 class="card-title mb-0">Control Frame</h5>
+          <button id="debug-frame-toggle" class="btn btn-sm btn-outline-info">Frame: ROV</button>
+        </div>
+        <p class="text-muted small mb-0 mt-2">
+          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.
+        </p>
+      </div>
+    </div>
+  </div>
+</div>
+
 <!-- Accelerometer Axis Mapping -->
 <div class="row g-4 mt-1">
   <div class="col-12">

static/templates/layout.html

@@ -176,6 +176,19 @@ <h5 class="card-title">Autonomus missions</h5>
   </div>
 </div>
 
+<div class="row g-4 mt-1">
+  <!-- Control Mode -->
+  <div class="col-12">
+    <div class="card custom-card h-100">
+      <div class="card-body">
+        <h5 class="card-title">Control Mode</h5>
+        <button id="frame-toggle" type="button" class="btn btn-sm btn-outline-info">Frame: ROV</button>
+        <small id="frame-status" class="text-muted ms-2">Body-relative</small>
+      </div>
+    </div>
+  </div>
+</div>
+
 <div class="row g-4 mt-1">
   <!-- Controller Status -->
   <div class="col-12">
@@ -202,4 +215,5 @@ <h5 class="card-title">Controller Status</h5>
 <script src="/static/js/depth.js"></script>
 <script src="/static/js/controller.js"></script>
 <script src="/static/js/resource_monitor.js"></script>
+<script src="/static/js/home_controls.js"></script>
 {% endblock %}

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