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.
+