From 71ab98c5a52d90898dfc5895e8b96d066b160e36 Mon Sep 17 00:00:00 2001 From: Tomas Aas-Hansen Date: Sun, 7 Jun 2026 17:01:07 +0200 Subject: [PATCH 1/2] Tested manipulator control from both PS4 controller and Topside GUI successfully implemented on D9/PD15. --- data/data.json | 34 +++++------ docs/swagger.yml | 50 ++++++++++++++++ lib/control_telemetry.py | 22 +++++-- lib/controller.py | 57 ++++++++++++++++-- routes.py | 63 ++++++++++++++++++-- static/css/pilot.css | 12 ++++ static/js/controller.js | 9 +-- static/js/manipulator.js | 107 ++++++++++++++++++++++++++++++++++ static/templates/layout.html | 21 +++++++ static/templates/pilot.html | 20 +++++++ tests/test_controller.py | 48 +++++++++++++-- tests/test_manipulator_api.py | 65 +++++++++++++++++++++ tests/test_protocols.py | 31 ++++++++++ 13 files changed, 496 insertions(+), 43 deletions(-) create mode 100644 static/js/manipulator.js create mode 100644 tests/test_manipulator_api.py diff --git a/data/data.json b/data/data.json index 3368130..24dcf4e 100644 --- a/data/data.json +++ b/data/data.json @@ -1,14 +1,14 @@ { "imu": { - "yaw": 0.0, - "pitch": 0.0, - "roll": 0.0, + "yaw": 126.16, + "pitch": 5.22, + "roll": -179.87, "yr": 0.0, - "pr": 0.0, - "rr": 0.0, - "ax": 0.0, - "ay": 0.0, - "az": 0.0 + "pr": -0.02, + "rr": 0.05, + "ax": 0.005, + "ay": -0.001, + "az": -0.002 }, "9dof": { "acceleration": { @@ -74,14 +74,14 @@ "dptSet": 0.0 }, "resources": { - "sequence": 0, - "uptime_ms": 0, - "cpu_percent": 0, - "heap_used_percent": 0, - "heap_free_kb": 0, - "heap_total_kb": 0, - "thread_count": 0, - "udp_rx_count": 0, + "sequence": 306, + "uptime_ms": 308703, + "cpu_percent": 4, + "heap_used_percent": 2, + "heap_free_kb": 502, + "heap_total_kb": 512, + "thread_count": 20, + "udp_rx_count": 5899, "udp_rx_errors": 0 }, "control_telemetry": { @@ -123,4 +123,4 @@ "Buttons": { "button_surface": 0 } -} +} \ No newline at end of file diff --git a/docs/swagger.yml b/docs/swagger.yml index 05bf635..f59fe0c 100644 --- a/docs/swagger.yml +++ b/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 diff --git a/lib/control_telemetry.py b/lib/control_telemetry.py index 7c7f987..fd06d0c 100644 --- a/lib/control_telemetry.py +++ b/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, optional on newer firmware) + manipulator_pulse_us (uint16 little-endian, optional on newer firmware) crc32 (u32 big-endian) The CRC covers the bytes up to but excluding the CRC field. @@ -30,7 +32,9 @@ CONTROL_TELEM_PORT = 5005 AXES = ["surge", "sway", "heave", "roll", "pitch", "yaw"] FLOAT_COUNT = len(AXES) * 3 -PACKET_SIZE = 4 + FLOAT_COUNT * 4 + 4 +OLD_PACKET_SIZE = 4 + FLOAT_COUNT * 4 + 4 +MANIPULATOR_SIZE = struct.calcsize(" List[dict]: # Internal helpers ------------------------------------------------- def _handle_packet(self, data: bytes, addr: tuple[str, int]): - if len(data) != PACKET_SIZE: - print(f"Control telemetry: invalid packet size from {addr}: {len(data)} bytes (expected {PACKET_SIZE})") + if len(data) not in (OLD_PACKET_SIZE, PACKET_SIZE): + print( + f"Control telemetry: invalid packet size from {addr}: " + f"{len(data)} bytes (expected {OLD_PACKET_SIZE} or {PACKET_SIZE})" + ) return body = data[:-4] crc = struct.unpack("!I", data[-4:])[0] @@ -96,16 +103,23 @@ 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])) + manipulator = {} + if len(data) == PACKET_SIZE: + manip_offset = float_end + manip_deg, manip_pulse_us = struct.unpack(" 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 diff --git a/routes.py b/routes.py index 5e33c19..126aa59 100644 --- a/routes.py +++ b/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") diff --git a/static/css/pilot.css b/static/css/pilot.css index 1f41f08..e4b9097 100644 --- a/static/css/pilot.css +++ b/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; diff --git a/static/js/controller.js b/static/js/controller.js index a97304e..ea68664 100644 --- a/static/js/controller.js +++ b/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() { diff --git a/static/js/manipulator.js b/static/js/manipulator.js new file mode 100644 index 0000000..29fc0e0 --- /dev/null +++ b/static/js/manipulator.js @@ -0,0 +1,107 @@ +// Shared manipulator controls for Home and Pilot views. + +let _manipPostTimer = null; +let _manipActiveSlider = null; + +const MANIP_SLIDER_IDS = ["manipulator-slider", "hud-manipulator-slider"]; + +function manipFmt(value) { + return typeof value === "number" && Number.isFinite(value) ? `${value.toFixed(0)} deg` : "--"; +} + +function setText(id, value) { + const el = document.getElementById(id); + if (el) el.textContent = value; +} + +function updateManipulatorDisplays(data) { + const target = Number(data.target_deg ?? data.setpoint_deg ?? 0); + const applied = typeof data.applied_deg === "number" ? data.applied_deg : null; + const pulse = typeof data.pulse_us === "number" ? data.pulse_us : null; + + setText("manipulator-target-value", target.toFixed(0)); + setText("manipulator-applied-value", applied == null ? "--" : applied.toFixed(1)); + setText("manipulator-pulse-value", pulse == null ? "--" : String(pulse)); + setText("manipulator-source-value", data.source || "--"); + + setText("hud-manipulator-target", manipFmt(target)); + setText("hud-manipulator-applied", applied == null ? "--" : `${applied.toFixed(1)} deg`); + setText("hud-manipulator-pulse", pulse == null ? "--" : `${pulse} us`); + + MANIP_SLIDER_IDS.forEach((id) => { + const slider = document.getElementById(id); + if (slider && slider !== _manipActiveSlider) { + slider.value = target.toFixed(0); + } + }); +} + +async function fetchManipulator() { + try { + const res = await fetch("/api/manipulator", { cache: "no-store" }); + const data = await res.json(); + if (data && data.ok) updateManipulatorDisplays(data); + } catch (error) { + console.error("Error fetching manipulator:", error); + } +} + +async function postManipulator(setpointDeg) { + try { + const res = await fetch("/api/manipulator", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ setpoint_deg: setpointDeg }), + }); + const data = await res.json(); + if (data && data.ok) updateManipulatorDisplays(data); + } catch (error) { + console.error("Error setting manipulator:", error); + } +} + +function queueManipulatorPost(value) { + if (_manipPostTimer) return; + _manipPostTimer = setTimeout(() => { + _manipPostTimer = null; + }, 100); + postManipulator(value); +} + +function bindManipulatorSlider(id) { + const slider = document.getElementById(id); + if (!slider) return; + + slider.addEventListener("pointerdown", () => { + _manipActiveSlider = slider; + }); + slider.addEventListener("input", () => { + _manipActiveSlider = slider; + const value = parseFloat(slider.value) || 0; + setText("manipulator-target-value", value.toFixed(0)); + setText("hud-manipulator-target", manipFmt(value)); + queueManipulatorPost(value); + }); + slider.addEventListener("pointerup", () => { + const value = parseFloat(slider.value) || 0; + postManipulator(value); + _manipActiveSlider = null; + }); + slider.addEventListener("pointercancel", () => { + _manipActiveSlider = null; + }); + slider.addEventListener("change", () => { + const value = parseFloat(slider.value) || 0; + postManipulator(value); + _manipActiveSlider = null; + }); + slider.addEventListener("blur", () => { + _manipActiveSlider = null; + }); +} + +document.addEventListener("DOMContentLoaded", () => { + MANIP_SLIDER_IDS.forEach(bindManipulatorSlider); + fetchManipulator(); + setInterval(fetchManipulator, 250); +}); diff --git a/static/templates/layout.html b/static/templates/layout.html index 2b56943..56af67e 100644 --- a/static/templates/layout.html +++ b/static/templates/layout.html @@ -40,6 +40,26 @@
Lights
+ + +
+
+
+
Manipulator
+
+ + +
+
+ Applied: --° / + -- us +
+
Source: --
+
+
+
@@ -198,6 +218,7 @@
Controller Status
+ diff --git a/static/templates/pilot.html b/static/templates/pilot.html index 85cdccd..d861a0b 100644 --- a/static/templates/pilot.html +++ b/static/templates/pilot.html @@ -167,6 +167,25 @@
+
+ MANIPULATOR +
+
+ TARGET + 0 deg +
+
+ APPLIED + -- +
+
+ PULSE + -- +
+
+ +
+
@@ -189,4 +208,5 @@ + {% endblock %} diff --git a/tests/test_controller.py b/tests/test_controller.py index 1d8ea19..721756c 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -74,12 +74,17 @@ def get_hat(self, index): def build_controller(controller=None, joystick=None): ctrl = Controller.__new__(Controller) ctrl.bm = FakeBitmask() + ctrl.delay_ms = 16 ctrl.controller = controller ctrl.joystick = joystick or FakeJoystick() ctrl.axis_offsets = {} ctrl.light = 0.0 ctrl._prev_dpad_up = False ctrl._prev_dpad_down = False + ctrl._manipulator_deg = 0.0 + ctrl._manipulator_source = "neutral" + ctrl._manipulator_updated = 0.0 + ctrl._manipulator_lock = threading.Lock() ctrl._reconnect_delay = 0 ctrl._debug_override = None ctrl._debug_lock = threading.Lock() @@ -122,7 +127,7 @@ def test_sdl_gamecontroller_mapping_normalizes_linux_playstation_layout(monkeypa assert command["yaw"] == pytest.approx(0.375, rel=1e-3) assert command["pitch"] == 0.0 assert command["roll"] == 0.0 - assert command["manip"] == 1.0 + assert command["manip"] == pytest.approx(-0.0144, rel=1e-3) assert command["light"] == pytest.approx(0.1, rel=1e-3) status = ctrl.get_input_status() assert status["connected"] is True @@ -175,7 +180,7 @@ def test_raw_joystick_mapping_remains_available_for_unsupported_devices(monkeypa assert command["sway"] == 0.25 assert command["heave"] == 0.2 assert command["yaw"] == 0.4 - assert command["manip"] == 1.0 + assert command["manip"] == pytest.approx(-0.0144, rel=1e-3) assert command["light"] == pytest.approx(0.1, rel=1e-3) status = ctrl.get_input_status() assert status["source"] == "raw_joystick" @@ -189,15 +194,48 @@ def test_set_light_clamps_and_pushes_to_bitmask(): assert ctrl.get_light() == pytest.approx(0.5) assert ctrl.bm.commands[-1]["light"] == 128 - ctrl.set_light(2.0) # clamps to 1.0 -> 255 - assert ctrl.get_light() == 1.0 - assert ctrl.bm.commands[-1]["light"] == 255 + ctrl.set_light(2.0) # clamps to MAX_LIGHT -> 204 + assert ctrl.get_light() == ctrl.MAX_LIGHT + assert ctrl.bm.commands[-1]["light"] == 204 ctrl.set_light(-1.0) # clamps to 0.0 -> 0 assert ctrl.get_light() == 0.0 assert ctrl.bm.commands[-1]["light"] == 0 +def test_set_manipulator_clamps_and_pushes_to_bitmask(): + ctrl = build_controller() + + state = ctrl.set_manipulator(25) + assert state["setpoint_deg"] == 25 + assert ctrl.bm.commands[-1]["manip"] == 64 + + state = ctrl.set_manipulator(100) + assert state["setpoint_deg"] == 50.0 + assert ctrl.bm.commands[-1]["manip"] == 127 + + state = ctrl.set_manipulator(-100) + assert state["setpoint_deg"] == -50.0 + assert ctrl.bm.commands[-1]["manip"] == -127 + + +def test_l2_nudges_manipulator_counterclockwise(monkeypatch): + monkeypatch.setattr(pygame.event, "get", lambda: []) + sdl = FakeSdlController( + axes={ + pygame.CONTROLLER_AXIS_TRIGGERLEFT: 32767, + pygame.CONTROLLER_AXIS_TRIGGERRIGHT: 0, + } + ) + ctrl = build_controller(controller=sdl) + + ctrl.update() + + command = ctrl.bm.calls[-1] + assert command["manip"] == pytest.approx(0.0144, rel=1e-3) + assert ctrl.get_manipulator()["source"] == "controller" + + def test_non_linux_connection_uses_raw_joystick_without_sdl_probe(monkeypatch): class FailingSdlController: @staticmethod diff --git a/tests/test_manipulator_api.py b/tests/test_manipulator_api.py new file mode 100644 index 0000000..fd4f18c --- /dev/null +++ b/tests/test_manipulator_api.py @@ -0,0 +1,65 @@ +from flask import Flask + +from routes import register_routes + + +class FakeController: + def __init__(self): + self.state = { + "setpoint_deg": 0.0, + "setpoint_norm": 0.0, + "source": "neutral", + "updated_at": 100.0, + } + + def set_manipulator(self, setpoint_deg, source="gui"): + deg = max(-50.0, min(50.0, float(setpoint_deg))) + self.state = { + "setpoint_deg": deg, + "setpoint_norm": deg / 50.0, + "source": source, + "updated_at": 100.0, + } + return self.state + + def get_manipulator(self): + return dict(self.state) + + +class FakeTelemetry: + def get_latest(self): + return { + "timestamp": 100.0, + "manipulator": {"deg": 12.5, "pulse_us": 1625}, + } + + +def make_client(): + app = Flask(__name__) + app.config["CONTROLLER"] = FakeController() + app.config["CONTROL_TELEM"] = FakeTelemetry() + register_routes(app) + return app.test_client() + + +def test_manipulator_api_returns_target_and_applied_values(): + client = make_client() + + res = client.get("/api/manipulator") + + assert res.status_code == 200 + data = res.get_json() + assert data["target_deg"] == 0.0 + assert data["applied_deg"] == 12.5 + assert data["pulse_us"] == 1625 + + +def test_manipulator_api_sets_clamped_target(): + client = make_client() + + res = client.post("/api/manipulator", json={"setpoint_deg": 80}) + + assert res.status_code == 200 + data = res.get_json() + assert data["target_deg"] == 50.0 + assert data["source"] == "gui" diff --git a/tests/test_protocols.py b/tests/test_protocols.py index cda4d83..b987e13 100644 --- a/tests/test_protocols.py +++ b/tests/test_protocols.py @@ -32,6 +32,12 @@ def test_bitmask_packet_big_endian(): assert pkt[12:] == struct.pack("!I", expected_crc) +def test_bitmask_manipulator_is_signed_in_top_byte(): + assert (bitmask.encode_payload(bitmask.Command(manip=-127)) >> 56) & 0xFF == 1 + assert (bitmask.encode_payload(bitmask.Command(manip=0)) >> 56) & 0xFF == 128 + assert (bitmask.encode_payload(bitmask.Command(manip=127)) >> 56) & 0xFF == 255 + + def test_system_reset_packet_crc(): pkt = system_control_client.build_reset_packet(0x01020304) assert pkt[:4] == b"RST1" @@ -69,12 +75,37 @@ def test_control_telemetry_history(monkeypatch, tmp_path): assert latest["sequence"] == sequence for axis, value in zip(control_telem.AXES, setpoints): assert latest["setpoint"][axis] == pytest.approx(value, rel=1e-4) + assert latest["manipulator"] == {} history = receiver.get_history(limit=5) assert len(history) == 1 assert history[0]["sequence"] == sequence assert handler.last_update is not None +def test_control_telemetry_includes_manipulator_fields(monkeypatch, tmp_path): + monkeypatch.setattr(control_telem, "LOG_DIR", tmp_path) + monkeypatch.setattr(control_telem, "CONTROL_LOG", tmp_path / "control_telemetry.ndjson") + receiver = control_telem.ControlTelemetryReceiver(data_handler=DummyHandler()) + receiver.disable_capture() + + sequence = 3 + setpoints = [0.0] * 6 + outputs = [0.0] * 6 + errors = [0.0] * 6 + body = ( + struct.pack("!I", sequence) + + struct.pack("<" + "f" * 18, *(setpoints + outputs + errors)) + + struct.pack(" Date: Mon, 8 Jun 2026 16:20:59 +0200 Subject: [PATCH 2/2] fix manipulator review comments --- data/data.json | 34 +++++++++++++++++----------------- lib/control_telemetry.py | 22 +++++++--------------- tests/test_protocols.py | 3 ++- 3 files changed, 26 insertions(+), 33 deletions(-) diff --git a/data/data.json b/data/data.json index 24dcf4e..3368130 100644 --- a/data/data.json +++ b/data/data.json @@ -1,14 +1,14 @@ { "imu": { - "yaw": 126.16, - "pitch": 5.22, - "roll": -179.87, + "yaw": 0.0, + "pitch": 0.0, + "roll": 0.0, "yr": 0.0, - "pr": -0.02, - "rr": 0.05, - "ax": 0.005, - "ay": -0.001, - "az": -0.002 + "pr": 0.0, + "rr": 0.0, + "ax": 0.0, + "ay": 0.0, + "az": 0.0 }, "9dof": { "acceleration": { @@ -74,14 +74,14 @@ "dptSet": 0.0 }, "resources": { - "sequence": 306, - "uptime_ms": 308703, - "cpu_percent": 4, - "heap_used_percent": 2, - "heap_free_kb": 502, - "heap_total_kb": 512, - "thread_count": 20, - "udp_rx_count": 5899, + "sequence": 0, + "uptime_ms": 0, + "cpu_percent": 0, + "heap_used_percent": 0, + "heap_free_kb": 0, + "heap_total_kb": 0, + "thread_count": 0, + "udp_rx_count": 0, "udp_rx_errors": 0 }, "control_telemetry": { @@ -123,4 +123,4 @@ "Buttons": { "button_surface": 0 } -} \ No newline at end of file +} diff --git a/lib/control_telemetry.py b/lib/control_telemetry.py index fd06d0c..d68d293 100644 --- a/lib/control_telemetry.py +++ b/lib/control_telemetry.py @@ -8,8 +8,8 @@ setpoint[6] (float32 little-endian, surge..yaw) output[6] (float32 little-endian) error[6] (float32 little-endian) - manipulator_deg (float32 little-endian, optional on newer firmware) - manipulator_pulse_us (uint16 little-endian, optional on newer firmware) + 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. @@ -32,9 +32,7 @@ CONTROL_TELEM_PORT = 5005 AXES = ["surge", "sway", "heave", "roll", "pitch", "yaw"] FLOAT_COUNT = len(AXES) * 3 -OLD_PACKET_SIZE = 4 + FLOAT_COUNT * 4 + 4 -MANIPULATOR_SIZE = struct.calcsize(" List[dict]: # Internal helpers ------------------------------------------------- def _handle_packet(self, data: bytes, addr: tuple[str, int]): - if len(data) not in (OLD_PACKET_SIZE, PACKET_SIZE): - print( - f"Control telemetry: invalid packet size from {addr}: " - f"{len(data)} bytes (expected {OLD_PACKET_SIZE} or {PACKET_SIZE})" - ) + if len(data) != PACKET_SIZE: + print(f"Control telemetry: invalid packet size from {addr}: {len(data)} bytes (expected {PACKET_SIZE})") return body = data[:-4] crc = struct.unpack("!I", data[-4:])[0] @@ -108,11 +103,8 @@ def _handle_packet(self, data: bytes, addr: tuple[str, int]): setpoints = dict(zip(AXES, floats[0:6])) outputs = dict(zip(AXES, floats[6:12])) errors = dict(zip(AXES, floats[12:18])) - manipulator = {} - if len(data) == PACKET_SIZE: - manip_offset = float_end - manip_deg, manip_pulse_us = struct.unpack("