Tested manipulator control from both PS4 controller and Topside GUI s…

docs/swagger.yml

@@ -141,6 +141,39 @@ paths:
           schema:
             $ref: "#/definitions/LightsTelemetry"
 
+  /api/manipulator:
+    get:
+      tags: [ROV Command]
+      summary: Get manipulator target and applied telemetry
+      responses:
+        200:
+          description: Current manipulator command state
+          schema:
+            $ref: "#/definitions/ManipulatorTelemetry"
+    post:
+      tags: [ROV Command]
+      summary: Set manipulator target angle
+      parameters:
+        - in: body
+          name: body
+          required: true
+          schema:
+            type: object
+            required: [setpoint_deg]
+            properties:
+              setpoint_deg:
+                type: number
+                minimum: -50
+                maximum: 50
+                example: 15
+      responses:
+        200:
+          description: Updated manipulator command state
+          schema:
+            $ref: "#/definitions/ManipulatorTelemetry"
+        400:
+          description: Missing or invalid setpoint_deg
+
   /api/battery:
     get:
       tags: [Telemetry]
@@ -1121,6 +1154,18 @@ definitions:
         type: integer
         example: 0
 
+  ManipulatorTelemetry:
+    type: object
+    properties:
+      ok: {type: boolean, example: true}
+      target_deg: {type: number, minimum: -50, maximum: 50, example: 15}
+      setpoint_deg: {type: number, minimum: -50, maximum: 50, example: 15}
+      source: {type: string, example: gui}
+      updated_age_ms: {type: number, example: 35.2}
+      applied_deg: {type: number, example: 14.5}
+      pulse_us: {type: integer, example: 1645}
+      telemetry_age_ms: {type: number, example: 82.1}
+
   DepthTelemetry:
     type: object
     properties:
@@ -1166,6 +1211,11 @@ definitions:
         $ref: "#/definitions/AxisValues"
       error:
         $ref: "#/definitions/AxisValues"
+      manipulator:
+        type: object
+        properties:
+          deg: {type: number, example: 14.5}
+          pulse_us: {type: integer, example: 1645}
 
   LogEntry:
     type: object

lib/control_telemetry.py

@@ -8,6 +8,8 @@
     setpoint[6] (float32 little-endian, surge..yaw)
     output[6]   (float32 little-endian)
     error[6]    (float32 little-endian)
+    manipulator_deg (float32 little-endian)
+    manipulator_pulse_us (uint16 little-endian)
     crc32 (u32 big-endian)
 
 The CRC covers the bytes up to but excluding the CRC field.
@@ -30,7 +32,7 @@
 CONTROL_TELEM_PORT = 5005
 AXES = ["surge", "sway", "heave", "roll", "pitch", "yaw"]
 FLOAT_COUNT = len(AXES) * 3
-PACKET_SIZE = 4 + FLOAT_COUNT * 4 + 4
+PACKET_SIZE = 4 + FLOAT_COUNT * 4 + struct.calcsize("<fH") + 4
 HISTORY_CAPACITY = 3000  # 5 minutes @ 10 Hz
 LOG_DIR = logs_dir()
 CONTROL_LOG = log_path("control_telemetry.ndjson")
@@ -96,16 +98,20 @@ def _handle_packet(self, data: bytes, addr: tuple[str, int]):
             print(f"Control telemetry: CRC mismatch (calc=0x{calc:08X}, recv=0x{crc:08X})")
             return
         sequence = struct.unpack("!I", body[:4])[0]
-        floats = struct.unpack("<" + "f" * FLOAT_COUNT, body[4:])
+        float_end = 4 + FLOAT_COUNT * 4
+        floats = struct.unpack("<" + "f" * FLOAT_COUNT, body[4:float_end])
         setpoints = dict(zip(AXES, floats[0:6]))
         outputs = dict(zip(AXES, floats[6:12]))
         errors = dict(zip(AXES, floats[12:18]))
+        manip_deg, manip_pulse_us = struct.unpack("<fH", body[float_end:])
+        manipulator = {"deg": round(manip_deg, 2), "pulse_us": int(manip_pulse_us)}
         snapshot = {
             "sequence": sequence,
             "timestamp": time.time(),
             "setpoint": {k: round(v, 4) for k, v in setpoints.items()},
             "output": {k: round(v, 4) for k, v in outputs.items()},
             "error": {k: round(v, 4) for k, v in errors.items()},
+            "manipulator": manipulator,
         }
         with self._lock:
             self._latest = snapshot

lib/controller.py

@@ -54,6 +54,9 @@ class Controller:
     # Windows) expose the D-pad as buttons instead of a hat. Verified mapping.
     DPAD_UP_BUTTON = 11
     DPAD_DOWN_BUTTON = 12
+    MANIP_MIN_DEG = -50.0
+    MANIP_MAX_DEG = 50.0
+    MANIP_NUDGE_DEG_PER_SEC = 45.0
 
     def __init__(self, bitmask_client: BitmaskClient = None, rate_hz: float = 60.0):
         self.bm = bitmask_client  # Use injected bitmask client from app.py
@@ -68,6 +71,10 @@ def __init__(self, bitmask_client: BitmaskClient = None, rate_hz: float = 60.0):
         self.light = 0  # Initial light value
         self._prev_dpad_up = False  # For edge detection of light increase (D-pad up)
         self._prev_dpad_down = False  # For edge detection of light decrease (D-pad down)
+        self._manipulator_deg = 0.0
+        self._manipulator_source = "neutral"
+        self._manipulator_updated = time.time()
+        self._manipulator_lock = threading.Lock()
         self._stop = threading.Event()
         self._thread = None
         self._reconnect_delay = 0  # Counter for reconnect attempts
@@ -318,8 +325,47 @@ def get_light(self):
         """Return current light brightness as a normalized 0.0-1.0 level."""
         return self.light
 
+    # --- Manipulator API ---
+    def _clamp_manipulator(self, deg):
+        return max(self.MANIP_MIN_DEG, min(self.MANIP_MAX_DEG, float(deg)))
+
+    def _manipulator_norm_locked(self):
+        return self._manipulator_deg / self.MANIP_MAX_DEG
+
+    def set_manipulator(self, setpoint_deg, source="gui"):
+        """Set manipulator position in degrees, clamped to safe servo travel."""
+        with self._manipulator_lock:
+            self._manipulator_deg = self._clamp_manipulator(setpoint_deg)
+            self._manipulator_source = str(source or "gui")
+            self._manipulator_updated = time.time()
+            norm = self._manipulator_norm_locked()
+            state = {
+                "setpoint_deg": self._manipulator_deg,
+                "setpoint_norm": norm,
+                "source": self._manipulator_source,
+                "updated_at": self._manipulator_updated,
+            }
+        if self.bm:
+            self.bm.set_command(manip=int(round(norm * 127)))
+        return state
+
+    def nudge_manipulator(self, direction, dt):
+        with self._manipulator_lock:
+            next_deg = self._manipulator_deg + float(direction) * self.MANIP_NUDGE_DEG_PER_SEC * float(dt)
+        return self.set_manipulator(next_deg, source="controller")
+
+    def get_manipulator(self):
+        with self._manipulator_lock:
+            return {
+                "setpoint_deg": self._manipulator_deg,
+                "setpoint_norm": self._manipulator_norm_locked(),
+                "source": self._manipulator_source,
+                "updated_at": self._manipulator_updated,
+            }
+
     def _reset_command(self):
         """Reset all axes to neutral/zero."""
+        manip = self.get_manipulator()["setpoint_norm"]
         if self.bm:
             self.bm.set_from_axes(
                 surge=0,
@@ -329,7 +375,7 @@ def _reset_command(self):
                 pitch=0,
                 yaw=0,
                 light=self.light,  # Keep light at current level
-                manip=0,
+                manip=manip,
             )
 
     # --- Debug override API ---
@@ -349,6 +395,7 @@ def update(self):
         with self._debug_lock:
             override = self._debug_override.copy() if self._debug_override is not None else None
         if override is not None:
+            manip = self.get_manipulator()["setpoint_norm"]
             # Debug sliders have priority – send their values directly
             if self.bm:
                 self.bm.set_from_axes(
@@ -359,7 +406,7 @@ def update(self):
                     pitch=override.get("pitch", 0),
                     yaw=override.get("yaw", 0),
                     light=self.light,
-                    manip=0,
+                    manip=manip,
                 )
             self._set_input_status(
                 {
@@ -418,10 +465,12 @@ def update(self):
         # Read axes
         heave = -self._read_axis(pygame.CONTROLLER_AXIS_RIGHTY, 3)  # Right Y (inverted)
         yaw = self._read_axis(pygame.CONTROLLER_AXIS_RIGHTX, 2)  # Right X
-        # manip is r2 axis minus l2 axis
         r2 = self._read_trigger(pygame.CONTROLLER_AXIS_TRIGGERRIGHT, 5)  # R2 trigger
         l2 = self._read_trigger(pygame.CONTROLLER_AXIS_TRIGGERLEFT, 4)  # L2 trigger
-        manip = r2 - l2
+        trigger_delta = l2 - r2
+        if abs(trigger_delta) > self.DEADZONE:
+            self.nudge_manipulator(trigger_delta, self.delay_ms / 1000)
+        manip = self.get_manipulator()["setpoint_norm"]
 
         # This runs while button 9 is held down L1 to make
         # surge and sway controls toggleable to pitch and roll

routes.py

@@ -1,6 +1,7 @@
 import json
 import math
 import re
+import time
 from pathlib import Path
 
 from flask import Response, current_app, jsonify, render_template, request, send_from_directory
@@ -87,14 +88,34 @@ def _neutralize_thruster_command():
     """Force topside manual command output to neutral axes."""
     neutral = _neutral_axis_values()
     ctrl = current_app.config.get("CONTROLLER")
+    manip = ctrl.get_manipulator()["setpoint_norm"] if ctrl else 0.0
     if ctrl:
         ctrl.set_debug_override(neutral)
     bm = current_app.config.get("BITMASK")
     if bm:
-        bm.set_from_axes(**neutral)
+        bm.set_from_axes(**neutral, manip=manip)
     return neutral
 
 
+def _manipulator_payload(ctrl, receiver=None):
+    state = ctrl.get_manipulator() if ctrl else {}
+    latest = receiver.get_latest() if receiver else {}
+    manip = latest.get("manipulator") if isinstance(latest, dict) else {}
+    now = time.time()
+    updated_at = state.get("updated_at")
+    telem_ts = latest.get("timestamp") if isinstance(latest, dict) else None
+    return {
+        "ok": bool(ctrl),
+        "target_deg": state.get("setpoint_deg", 0.0),
+        "setpoint_deg": state.get("setpoint_deg", 0.0),
+        "source": state.get("source", "unknown"),
+        "updated_age_ms": None if updated_at is None else max(0.0, (now - updated_at) * 1000.0),
+        "applied_deg": manip.get("deg") if isinstance(manip, dict) else None,
+        "pulse_us": manip.get("pulse_us") if isinstance(manip, dict) else None,
+        "telemetry_age_ms": None if telem_ts is None else max(0.0, (now - telem_ts) * 1000.0),
+    }
+
+
 def _send_full_axis_config():
     """Read all axis settings from config and send to MCU in one packet."""
     imu_axes = config_handler.get_section("imu_axes") or _DEFAULT_IMU_AXES
@@ -307,6 +328,32 @@ def set_lights():
         pct = round(pct)
         return jsonify({"ok": True, "level": pct, "light": pct})
 
+    @app.route("/api/manipulator", methods=["GET"])
+    def get_manipulator():
+        ctrl = current_app.config.get("CONTROLLER")
+        receiver = current_app.config.get("CONTROL_TELEM")
+        if not ctrl:
+            return jsonify({"ok": False, "error": "Controller not available"}), 503
+        return jsonify(_manipulator_payload(ctrl, receiver))
+
+    @app.route("/api/manipulator", methods=["POST"])
+    def set_manipulator():
+        data = request.get_json(force=True, silent=True) or {}
+        ctrl = current_app.config.get("CONTROLLER")
+        receiver = current_app.config.get("CONTROL_TELEM")
+        if not ctrl:
+            return jsonify({"ok": False, "error": "Controller not available"}), 503
+        if "setpoint_deg" not in data:
+            return jsonify({"ok": False, "error": "Missing 'setpoint_deg'"}), 400
+        try:
+            setpoint = float(data["setpoint_deg"])
+        except (TypeError, ValueError):
+            return jsonify({"ok": False, "error": "Invalid 'setpoint_deg'"}), 400
+        if not math.isfinite(setpoint):
+            return jsonify({"ok": False, "error": "Invalid 'setpoint_deg'"}), 400
+        ctrl.set_manipulator(setpoint, source="gui")
+        return jsonify(_manipulator_payload(ctrl, receiver))
+
     @app.route("/api/battery", methods=["GET"])
     def get_battery():
         """API route for battery status."""
@@ -391,7 +438,7 @@ def setpoint_status():
     def set_rov_command():
         """
         JSON body: any subset of
-        surge,sway,heave,roll,pitch,yaw ([-128..127]), light,manip ([0..255])
+        surge,sway,heave,roll,pitch,yaw,manip ([-128..127]), light ([0..255])
         or normalized axes in [-1..1] via "axes": and optional "rate_hz"
         """
         data = request.get_json(force=True, silent=True) or {}
@@ -400,6 +447,10 @@ def set_rov_command():
         # allow normalized axes
         axes = data.get("axes")
         if isinstance(axes, dict):
+            axes = dict(axes)
+            ctrl = current_app.config.get("CONTROLLER")
+            if "manip" not in axes and ctrl:
+                axes["manip"] = ctrl.get_manipulator()["setpoint_norm"]
             bm.set_from_axes(**axes)
 
         # allow raw fields
@@ -550,9 +601,10 @@ def debug_override():
         if not axes:
             return jsonify({"ok": False, "error": "No axes supplied"}), 400
 
-        bm.set_from_axes(**axes)
-
         ctrl = current_app.config.get("CONTROLLER")
+        manip = ctrl.get_manipulator()["setpoint_norm"] if ctrl else 0.0
+        bm.set_from_axes(**axes, manip=manip)
+
         if ctrl:
             ctrl.set_debug_override(axes)
 
@@ -593,7 +645,8 @@ def debug_clear():
         # Zero out the bitmask axes (slider override path)
         bm = current_app.config.get("BITMASK")
         if bm:
-            bm.set_from_axes(surge=0, sway=0, heave=0, roll=0, pitch=0, yaw=0)
+            manip = ctrl.get_manipulator()["setpoint_norm"] if ctrl else 0.0
+            bm.set_from_axes(surge=0, sway=0, heave=0, roll=0, pitch=0, yaw=0, manip=manip)
 
         # Clear any setpoint override on port 5007 (attitude override path)
         client = current_app.config.get("SETPOINT_OVERRIDE")

static/css/pilot.css

@@ -402,6 +402,18 @@
   gap: 8px;
   margin-top: 4px;
 }
+.hud-manipulator-panel {
+  width: 260px;
+}
+.hud-manipulator-readout {
+  display: grid;
+  grid-template-columns: repeat(3, 1fr);
+  gap: 8px;
+  margin: 4px 0 6px;
+}
+.hud-manipulator-slider {
+  margin: 0;
+}
 .hud-thr-item {
   display: flex;
   flex-direction: column;

static/js/controller.js

@@ -55,7 +55,6 @@ function updateControllerFromCommand(command) {
   const roll = getBackendAxis(command, "roll");
   const pitch = getBackendAxis(command, "pitch");
   const yaw = getBackendAxis(command, "yaw");
-  const manip = getBackendAxis(command, "manip");
 
   if (Math.abs(pitch) > 0.01 || Math.abs(roll) > 0.01) {
     updateControllerButton(4, 1);
@@ -66,19 +65,13 @@ function updateControllerFromCommand(command) {
 
   updateStick("controller-b11", yaw, -heave);
 
-  if (manip < -0.01) {
-    updateControllerButton(6, Math.abs(manip));
-  } else if (manip > 0.01) {
-    updateControllerButton(7, manip);
-  }
-
   if (backendButtons) {
     updateControllerButtonsFromValues(backendButtons);
   }
 }
 
 function commandIsActive(command) {
-  return ["surge", "sway", "heave", "roll", "pitch", "yaw", "manip"].some((axis) => Math.abs(command[axis] || 0) > 1);
+  return ["surge", "sway", "heave", "roll", "pitch", "yaw"].some((axis) => Math.abs(command[axis] || 0) > 1);
 }
 
 async function fetchBackendCommand() {