From 2450bad98c74293ce0674a590bf4b3df4cd1c5d2 Mon Sep 17 00:00:00 2001 From: Tomas Aas-Hansen Date: Fri, 12 Jun 2026 15:56:58 +0200 Subject: [PATCH 1/3] PID-tuning tab overhaul. Topside logical and visual changes. --- app.py | 2 + data/config.json | 16 +- data/data.json | 24 +- lib/bitmask.py | 2 + lib/controller.py | 309 +++++++++++++++++++++--- routes.py | 392 +++++++++++++++++++++++++------ static/css/pid_tuning.css | 144 +++++++++++- static/js/configuration.js | 41 ++++ static/js/pid_tuning.js | 352 +++++++++++++++++++-------- static/templates/config.html | 25 ++ static/templates/pid_tuning.html | 81 ++++--- tests/test_controller.py | 87 +++++++ tests/test_pid_runtime_routes.py | 277 ++++++++++++++++++++++ 13 files changed, 1508 insertions(+), 244 deletions(-) create mode 100644 tests/test_pid_runtime_routes.py diff --git a/app.py b/app.py index 208c59c..de9a120 100644 --- a/app.py +++ b/app.py @@ -120,6 +120,8 @@ # Initialize setpoint override client (UDP port 5007) app.config["SETPOINT_OVERRIDE"] = init_setpoint_override(resource_monitor=app.config["RESOURCE"]) +app.config["CONTROLLER"].set_setpoint_client(app.config["SETPOINT_OVERRIDE"]) +app.config["CONTROLLER"].set_pid_rates(_config.get_section("pid_setpoint_rates") or {}) # Start control loop telemetry receiver (UDP port 5005) app.config["CONTROL_TELEM"] = init_control_telemetry(port=5005) diff --git a/data/config.json b/data/config.json index 9651aa7..1cb47b8 100644 --- a/data/config.json +++ b/data/config.json @@ -15,7 +15,17 @@ "z": "+z" }, "ip_camera": { - "active_ip": "10.77.0.4", - "presets": [] + "active_ip": "10.77.0.3", + "presets": [ + { + "name": "Main IP-camera", + "ip": "10.77.0.3" + } + ] + }, + "pid_setpoint_rates": { + "roll": 90.0, + "pitch": 90.0, + "yaw": 90.0 } -} +} \ No newline at end of file diff --git a/data/data.json b/data/data.json index c8841c4..587e95e 100644 --- a/data/data.json +++ b/data/data.json @@ -1,14 +1,14 @@ { "imu": { - "yaw": 35.51, - "pitch": 4.75, - "roll": -179.7, - "yr": 0.05, - "pr": 0.01, - "rr": 0.12, - "ax": 0.002, + "yaw": -123.53, + "pitch": 1.98, + "roll": -174.45, + "yr": -0.01, + "pr": -0.0, + "rr": 0.14, + "ax": 0.001, "ay": 0.001, - "az": -0.001 + "az": 0.0 }, "9dof": { "acceleration": { @@ -74,14 +74,14 @@ "dptSet": 0.0 }, "resources": { - "sequence": 137, - "uptime_ms": 139172, - "cpu_percent": 4, + "sequence": 3232, + "uptime_ms": 3241146, + "cpu_percent": 5, "heap_used_percent": 2, "heap_free_kb": 502, "heap_total_kb": 512, "thread_count": 20, - "udp_rx_count": 2695, + "udp_rx_count": 40626, "udp_rx_errors": 0 }, "control_telemetry": { diff --git a/lib/bitmask.py b/lib/bitmask.py index b45407d..eea00d1 100644 --- a/lib/bitmask.py +++ b/lib/bitmask.py @@ -120,11 +120,13 @@ def get_uplink_status(self) -> dict: return { "sequence": self._seq, "last_send_age_ms": send_age, + "last_send_timestamp": None if self._last_send_time == 0 else self._last_send_time, "last_ack_age_ms": ack_age, "last_ack_count": self._last_ack_count, "watchdog_timeout": self._watchdog_timeout, "watchdog_resends": self._watchdog_resends, "last_command": self._last_command_snapshot or {}, + "last_packet_hex": self._last_packet.hex() if self._last_packet else None, } # convenience: set from normalized axes diff --git a/lib/controller.py b/lib/controller.py index a12be9f..1c1930e 100644 --- a/lib/controller.py +++ b/lib/controller.py @@ -20,11 +20,39 @@ from lib.bitmask import BitmaskClient +CONTROL_AXES = ("surge", "sway", "heave", "roll", "pitch", "yaw") +ATTITUDE_AXES = ("roll", "pitch", "yaw") +ATTITUDE_LIMITS_DEG = {"roll": 180.0, "pitch": 90.0, "yaw": 180.0} +DEFAULT_PID_SETPOINT_RATES = {axis: 90.0 for axis in ATTITUDE_AXES} + def _use_sdl_gamecontroller(): return sys.platform.startswith("linux") and sdl_controller is not None +def _clamp(value, lower, upper): + return max(lower, min(upper, float(value))) + + +def _normalize_angle_deg(value): + wrapped = ((float(value) + 180.0) % 360.0) - 180.0 + if wrapped == -180.0 and float(value) > 0: + return 180.0 + return wrapped + + +def _clamp_setpoint(axis, value): + value = float(value) + if axis in ("roll", "yaw"): + value = _normalize_angle_deg(value) + limit = ATTITUDE_LIMITS_DEG[axis] + return _clamp(value, -limit, limit) + + +def _neutral_axes(): + return {axis: 0.0 for axis in CONTROL_AXES} + + def _controller_errors(): errors = [pygame.error] if sdl2 is not None: @@ -83,6 +111,17 @@ def __init__(self, bitmask_client: BitmaskClient = None, rate_hz: float = 60.0): self._debug_lock = threading.Lock() self._input_status_lock = threading.Lock() self._input_status = self._empty_input_status() + self._runtime_lock = threading.RLock() + self._killed = False + self._pid_enabled = False + self._pid_setpoints = {} + self._pid_setpoint_rates = dict(DEFAULT_PID_SETPOINT_RATES) + self._last_pid_update = time.monotonic() + self._last_manual_command = _neutral_axes() + self._last_output_command = _neutral_axes() + self._last_runtime_source = "PS4" + self._last_pid_error = None + self._setpoint_client = None self._try_connect() def _try_connect(self): @@ -158,6 +197,146 @@ def get_input_status(self): "buttons": list(self._input_status["buttons"]), } + # --- Control runtime state --- + def set_setpoint_client(self, client): + self._setpoint_client = client + + def is_killed(self): + with self._runtime_lock: + return self._killed + + def is_pid_enabled(self): + with self._runtime_lock: + return self._pid_enabled + + def get_pid_setpoints(self): + with self._runtime_lock: + return dict(self._pid_setpoints) + + def get_pid_rates(self): + with self._runtime_lock: + return dict(self._pid_setpoint_rates) + + def set_pid_rates(self, rates): + with self._runtime_lock: + for axis in ATTITUDE_AXES: + if axis not in rates: + continue + try: + value = float(rates[axis]) + except (TypeError, ValueError): + continue + if value == value: + self._pid_setpoint_rates[axis] = _clamp(value, 0.0, 90.0) + return dict(self._pid_setpoint_rates) + + def get_control_state(self): + with self._debug_lock: + override_active = self._debug_override is not None + with self._runtime_lock: + if self._killed: + control_path = "KILLED" + elif override_active: + control_path = "Override Controls" + else: + control_path = "PS4" + return { + "killed": self._killed, + "pid_enabled": self._pid_enabled, + "pid_setpoints": dict(self._pid_setpoints), + "active_setpoints": dict(self._pid_setpoints) if self._pid_enabled else {}, + "pid_setpoint_rates": dict(self._pid_setpoint_rates), + "control_path": control_path, + "override_active": override_active, + "manual_command_before_pid": dict(self._last_manual_command), + "topside_command": dict(self._last_output_command), + "last_pid_error": self._last_pid_error, + } + + def kill(self): + with self._debug_lock: + self._debug_override = None + with self._runtime_lock: + self._killed = True + self._pid_enabled = False + self._pid_setpoints = {} + self._last_manual_command = _neutral_axes() + self._last_output_command = _neutral_axes() + self._last_runtime_source = "KILLED" + self._reset_command() + self._set_input_status( + { + "connected": self.joystick is not None, + "source": "killed", + "name": self.joystick.get_name() if self.joystick else None, + "buttons": [0.0] * self.VISUALIZER_BUTTON_COUNT, + } + ) + return self.get_control_state() + + def rearm(self): + with self._debug_lock: + self._debug_override = None + with self._runtime_lock: + self._killed = False + self._pid_enabled = False + self._pid_setpoints = {} + self._last_manual_command = _neutral_axes() + self._last_output_command = _neutral_axes() + self._last_runtime_source = "PS4" + self._last_pid_error = None + self._last_pid_update = time.monotonic() + self._reset_command() + return self.get_control_state() + + def start_pid(self, setpoints): + cleaned = {} + for axis in ATTITUDE_AXES: + if axis in setpoints: + cleaned[axis] = _clamp_setpoint(axis, setpoints[axis]) + with self._runtime_lock: + if self._killed: + return None + self._pid_enabled = bool(cleaned) + self._pid_setpoints = cleaned + self._last_pid_update = time.monotonic() + self._last_pid_error = None + return self.get_pid_setpoints() + + def stop_pid(self, clear=True): + with self._runtime_lock: + self._pid_enabled = False + if clear: + self._pid_setpoints = {} + self._last_pid_update = time.monotonic() + return self.get_control_state() + + def set_pid_setpoints(self, setpoints): + cleaned = {} + for axis in ATTITUDE_AXES: + if axis in setpoints: + cleaned[axis] = _clamp_setpoint(axis, setpoints[axis]) + with self._runtime_lock: + if self._killed: + return None + self._pid_setpoints.update(cleaned) + self._last_pid_update = time.monotonic() + self._last_pid_error = None + return dict(self._pid_setpoints) + + def clear_pid_setpoint(self, axis): + if axis not in ATTITUDE_AXES: + return None + with self._runtime_lock: + self._pid_setpoints.pop(axis, None) + if self._pid_enabled and not self._pid_setpoints: + self._pid_enabled = False + self._last_pid_update = time.monotonic() + return dict(self._pid_setpoints) + + def apply_manual_axes_once(self, axes, source="HTTP"): + return self._dispatch_manual_axes(axes, source=source) + def calibrate_axes(self): """Capture initial axis values to use as offsets (fixes stuck axes).""" pygame.event.pump() @@ -332,26 +511,98 @@ def get_manipulator(self): "updated_at": self._manipulator_updated, } - def _reset_command(self): - """Reset all axes to neutral/zero.""" + def _record_output(self, manual_axes, output_axes, source): + with self._runtime_lock: + self._last_manual_command = dict(manual_axes) + self._last_output_command = dict(output_axes) + self._last_runtime_source = source + + def _send_axes_to_bitmask(self, axes): manip = self.get_manipulator()["setpoint_norm"] if self.bm: self.bm.set_from_axes( - surge=0, - sway=0, - heave=0, - roll=0, - pitch=0, - yaw=0, - light=self.light, # Keep light at current level + surge=axes.get("surge", 0), + sway=axes.get("sway", 0), + heave=axes.get("heave", 0), + roll=axes.get("roll", 0), + pitch=axes.get("pitch", 0), + yaw=axes.get("yaw", 0), + light=self.light, manip=manip, ) + def _send_pid_setpoints(self, setpoints): + client = self._setpoint_client + if not client or not setpoints: + return + try: + client.send_override(setpoints, replay_attempts=1, replay_delay=0.0) + with self._runtime_lock: + self._last_pid_error = None + except Exception as exc: # pylint: disable=broad-except + with self._runtime_lock: + self._last_pid_error = str(exc) + if hasattr(client, "set_error"): + client.set_error(str(exc)) + + def _dispatch_manual_axes(self, axes, source): + manual = _neutral_axes() + for axis in CONTROL_AXES: + try: + manual[axis] = _clamp(axes.get(axis, 0.0), -1.0, 1.0) + except (TypeError, ValueError): + manual[axis] = 0.0 + + now = time.monotonic() + setpoints_to_send = None + with self._runtime_lock: + if self._killed: + output = _neutral_axes() + self._last_manual_command = dict(manual) + self._last_output_command = dict(output) + self._last_runtime_source = "KILLED" + else: + output = dict(manual) + if self._pid_enabled: + dt = _clamp(now - self._last_pid_update, 0.0, 0.25) + self._last_pid_update = now + changed = False + for axis in ATTITUDE_AXES: + output[axis] = 0.0 + if axis not in self._pid_setpoints: + continue + delta = manual[axis] * self._pid_setpoint_rates[axis] * dt + if abs(delta) < 0.000001: + continue + self._pid_setpoints[axis] = _clamp_setpoint(axis, self._pid_setpoints[axis] + delta) + changed = True + if changed: + setpoints_to_send = dict(self._pid_setpoints) + self._last_manual_command = dict(manual) + self._last_output_command = dict(output) + self._last_runtime_source = source + + self._send_axes_to_bitmask(output) + if setpoints_to_send: + self._send_pid_setpoints(setpoints_to_send) + return dict(output) + + def _reset_command(self): + """Reset all axes to neutral/zero.""" + neutral = _neutral_axes() + self._record_output(neutral, neutral, "KILLED" if self.is_killed() else self._last_runtime_source) + self._send_axes_to_bitmask(neutral) + # --- Debug override API --- def set_debug_override(self, axes: dict): """Enable debug override with the given axis values.""" + with self._runtime_lock: + if self._killed: + self._reset_command() + return False with self._debug_lock: self._debug_override = dict(axes) + return True def clear_debug_override(self): """Disable debug override; return to physical controller.""" @@ -360,23 +611,26 @@ def clear_debug_override(self): self._reset_command() def update(self): + with self._runtime_lock: + killed = self._killed + if killed: + self._reset_command() + self._set_input_status( + { + "connected": self.joystick is not None, + "source": "killed", + "name": self.joystick.get_name() if self.joystick else None, + "buttons": [0.0] * self.VISUALIZER_BUTTON_COUNT, + } + ) + return + # --- Check for debug override first --- 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( - surge=override.get("surge", 0), - sway=override.get("sway", 0), - heave=override.get("heave", 0), - roll=override.get("roll", 0), - pitch=override.get("pitch", 0), - yaw=override.get("yaw", 0), - light=self.light, - manip=manip, - ) + self._dispatch_manual_axes(override, source="Override Controls") + # Debug sliders have priority over the physical controller. self._set_input_status( { "connected": self.joystick is not None, @@ -439,8 +693,6 @@ def update(self): 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 left_shoulder = self._read_button(pygame.CONTROLLER_BUTTON_LEFTSHOULDER, 9) @@ -468,11 +720,10 @@ def update(self): self._prev_dpad_down = dpad_down self._update_input_status(buttons) - # Send to ROV! - if self.bm: - self.bm.set_from_axes( - surge=surge, sway=sway, yaw=yaw, pitch=pitch, heave=heave, roll=roll, light=self.light, manip=manip - ) + self._dispatch_manual_axes( + {"surge": surge, "sway": sway, "heave": heave, "roll": roll, "pitch": pitch, "yaw": yaw}, + source="PS4", + ) def run_loop(self): """Blocking loop that polls controller at ~60 Hz.""" diff --git a/routes.py b/routes.py index 3709ed5..317fb50 100644 --- a/routes.py +++ b/routes.py @@ -2,6 +2,7 @@ import json import math import re +import subprocess import time from pathlib import Path from urllib.parse import urlparse @@ -42,7 +43,9 @@ def _save_pid_configs(configs): _DEFAULT_OFFSET = {"x": 0.0, "y": 0.0, "z": 0.0} ATTITUDE_LIMITS_DEG = {"roll": 180.0, "pitch": 90.0, "yaw": 180.0} CONTROL_AXES = ("surge", "sway", "heave", "roll", "pitch", "yaw") +TRANSLATIONAL_AXES = ("surge", "sway", "heave") ATTITUDE_AXES = ("roll", "pitch", "yaw") +DEFAULT_PID_SETPOINT_RATES = {axis: 90.0 for axis in ATTITUDE_AXES} DEFAULT_IP_CAMERA_IP = "10.77.0.4" @@ -69,6 +72,54 @@ def _zero_pid_gains(): return {axis: {"kp": 0.0, "ki": 0.0, "kd": 0.0} for axis in PID_AXES} +def _attitude_pid_gains(gains): + return {axis: gains.get(axis, {"kp": 0.0, "ki": 0.0, "kd": 0.0}) for axis in ATTITUDE_AXES} + + +def _mcu_pid_gains(gains): + packet = _zero_pid_gains() + for axis in ATTITUDE_AXES: + axis_gains = gains.get(axis, {}) if isinstance(gains, dict) else {} + cleaned = {} + for key in ("kp", "ki", "kd"): + try: + cleaned[key] = float(axis_gains.get(key, 0.0)) + except (AttributeError, TypeError, ValueError): + cleaned[key] = 0.0 + packet[axis] = { + "kp": cleaned["kp"], + "ki": cleaned["ki"], + "kd": cleaned["kd"], + } + return packet + + +def _clean_pid_rates(data): + rates = {} + for axis in ATTITUDE_AXES: + try: + value = float(data.get(axis, DEFAULT_PID_SETPOINT_RATES[axis])) + except (AttributeError, TypeError, ValueError): + value = DEFAULT_PID_SETPOINT_RATES[axis] + if not math.isfinite(value): + value = DEFAULT_PID_SETPOINT_RATES[axis] + rates[axis] = _clamp(value, 0.0, 90.0) + return rates + + +def _load_pid_rates(): + return _clean_pid_rates(config_handler.get_section("pid_setpoint_rates") or {}) + + +def _save_pid_rates(rates): + cleaned = _clean_pid_rates(rates) + config_handler.update_data({"pid_setpoint_rates": cleaned}) + ctrl = current_app.config.get("CONTROLLER") + if ctrl and hasattr(ctrl, "set_pid_rates"): + ctrl.set_pid_rates(cleaned) + return cleaned + + def _coerce_attitude_setpoints(data): axes = {} for axis in ATTITUDE_AXES: @@ -87,15 +138,80 @@ def _coerce_attitude_setpoints(data): return axes +def _imu_attitude_sanity(stats): + age_ms = stats.get("age_ms") + raw = stats.get("last_data") or {} + reasons = [] + numeric = {} + + if age_ms is None: + reasons.append("IMU data is missing") + elif age_ms > 2000: + reasons.append("IMU data is stale") + + for axis in ATTITUDE_AXES: + value = raw.get(axis) + try: + value = float(value) + except (TypeError, ValueError): + reasons.append(f"{axis} is missing or not numeric") + continue + if not math.isfinite(value): + reasons.append(f"{axis} is NaN or infinite") + continue + limit = ATTITUDE_LIMITS_DEG[axis] + if value < -limit or value > limit: + reasons.append(f"{axis} is outside -{limit:.0f}..{limit:.0f}") + numeric[axis] = value + + setpoints = _coerce_attitude_setpoints(numeric) + usable = len(setpoints) == len(ATTITUDE_AXES) + return { + "ok": usable and not reasons, + "usable": usable, + "reason": "; ".join(reasons), + "raw": raw, + "age_ms": age_ms, + "setpoints": setpoints, + } + + +def _send_active_pid_setpoints(ctrl, client): + setpoints = ctrl.get_pid_setpoints() if ctrl and hasattr(ctrl, "get_pid_setpoints") else {} + if not client: + return {} + client.clear_override() + if setpoints: + return client.send_override(setpoints, replay_attempts=5, replay_delay=0.1) + return client.get_state() + + +def _git_info(): + try: + branch = subprocess.check_output( + ["git", "rev-parse", "--abbrev-ref", "HEAD"], + cwd=PROJECT_ROOT, + stderr=subprocess.DEVNULL, + text=True, + timeout=1.0, + ).strip() + except Exception: + branch = "unknown" + return {"branch": branch} + + 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) + if hasattr(ctrl, "clear_debug_override"): + ctrl.clear_debug_override() + if hasattr(ctrl, "apply_manual_axes_once"): + ctrl.apply_manual_axes_once(neutral, source="HTTP") bm = current_app.config.get("BITMASK") - if bm: + if bm and not ctrl: bm.set_from_axes(**neutral, manip=manip) return neutral @@ -533,17 +649,64 @@ def get_command_status(): udp_rx, udp_err = resource.get_udp_counters() if resource else (0, 0) state = override.get_state() if override else {} controller_state = controller.get_input_status() if controller else {} + control_state = controller.get_control_state() if controller and hasattr(controller, "get_control_state") else {} return jsonify( { "ok": True, "uplink": uplink, "controller": controller_state, + "control_state": control_state, "udp_rx_count": udp_rx, "udp_rx_errors": udp_err, "override": state, } ) + @app.route("/api/control/state", methods=["GET"]) + def control_state(): + ctrl = current_app.config.get("CONTROLLER") + if not ctrl or not hasattr(ctrl, "get_control_state"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + return jsonify({"ok": True, "state": ctrl.get_control_state()}) + + @app.route("/api/control/killswitch", methods=["POST"]) + def control_killswitch(): + ctrl = current_app.config.get("CONTROLLER") + if not ctrl or not hasattr(ctrl, "kill"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + state = ctrl.kill() + zero_gains = _zero_pid_gains() + confirmed, attempts = send_pid_gains(zero_gains, timeout=0.5, max_retries=2) + client = current_app.config.get("SETPOINT_OVERRIDE") + if client: + try: + client.clear_override() + except Exception: + pass + return jsonify( + { + "ok": True, + "state": state, + "pid_gains_zeroed": confirmed is not None, + "pid_zero_attempts": attempts, + "pid_gains": _attitude_pid_gains(confirmed or zero_gains), + } + ) + + @app.route("/api/control/rearm", methods=["POST"]) + def control_rearm(): + ctrl = current_app.config.get("CONTROLLER") + if not ctrl or not hasattr(ctrl, "rearm"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + state = ctrl.rearm() + client = current_app.config.get("SETPOINT_OVERRIDE") + if client: + try: + client.clear_override() + except Exception: + pass + return jsonify({"ok": True, "state": state}) + @app.route("/api/control/telemetry", methods=["GET"]) def control_telemetry(): receiver = current_app.config.get("CONTROL_TELEM") @@ -578,6 +741,10 @@ def system_reset(): return jsonify({"ok": False, "error": str(exc)}), 503 return jsonify({"ok": True, "reset": result}) + @app.route("/api/system/git", methods=["GET"]) + def system_git(): + return jsonify({"ok": True, "git": _git_info()}) + @app.route("/api/setpoint/status", methods=["GET"]) def setpoint_status(): client = current_app.config.get("SETPOINT_OVERRIDE") @@ -593,43 +760,59 @@ def set_rov_command(): or normalized axes in [-1..1] via "axes": and optional "rate_hz" """ data = request.get_json(force=True, silent=True) or {} - bm = current_app.config["BITMASK"] + bm = current_app.config.get("BITMASK") + ctrl = current_app.config.get("CONTROLLER") + + if ctrl and hasattr(ctrl, "is_killed") and ctrl.is_killed(): + _neutralize_thruster_command() + return jsonify({"ok": False, "error": "Controls are killed", "state": ctrl.get_control_state()}), 423 # 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) + if ctrl and hasattr(ctrl, "apply_manual_axes_once"): + ctrl.apply_manual_axes_once(axes, source="HTTP") + elif bm: + bm.set_from_axes(**axes) # allow raw fields allowed = {"surge", "sway", "heave", "roll", "pitch", "yaw", "light", "manip"} raw = {k: int(v) for k, v in data.items() if k in allowed} if raw: - bm.set_command(**raw) + raw_axes = {axis: raw[axis] / 127.0 for axis in CONTROL_AXES if axis in raw} + if raw_axes and ctrl and hasattr(ctrl, "apply_manual_axes_once"): + ctrl.apply_manual_axes_once(raw_axes, source="HTTP") + elif raw_axes and bm: + bm.set_command(**{axis: raw[axis] for axis in raw_axes}) + non_axes = {key: value for key, value in raw.items() if key not in CONTROL_AXES} + if non_axes and bm: + bm.set_command(**non_axes) # optional live rate change - if "rate_hz" in data: + if bm and "rate_hz" in data: try: rate = float(data["rate_hz"]) bm.period = 1.0 / rate if rate > 0 else 0.0 except Exception: pass - return jsonify({"ok": True, "now": bm.get_command()}) + now = bm.get_command() if bm else {} + state = ctrl.get_control_state() if ctrl and hasattr(ctrl, "get_control_state") else {} + return jsonify({"ok": True, "now": now, "state": state}) @app.route("/api/rov/status", methods=["GET"]) def get_rov_status(): bm = current_app.config["BITMASK"] resource = current_app.config.get("RESOURCE") + ctrl = current_app.config.get("CONTROLLER") udp_rx, udp_err = resource.get_udp_counters() if resource else (0, 0) return jsonify( { "ok": True, "command": bm.get_command(), "uplink": bm.get_uplink_status(), + "control_state": ctrl.get_control_state() if ctrl and hasattr(ctrl, "get_control_state") else {}, "resource": { "udp_rx_count": udp_rx, "udp_rx_errors": udp_err, @@ -742,8 +925,12 @@ def debug_override(): """Set debug override axes as raw virtual joystick input via the bitmask command link.""" data = request.get_json(force=True, silent=True) or {} bm = current_app.config.get("BITMASK") - if not bm: + ctrl = current_app.config.get("CONTROLLER") + if not bm and not ctrl: return jsonify({"ok": False, "error": "Bitmask client unavailable"}), 503 + if ctrl and hasattr(ctrl, "is_killed") and ctrl.is_killed(): + _neutralize_thruster_command() + return jsonify({"ok": False, "error": "Controls are killed", "state": ctrl.get_control_state()}), 423 axes = {} for key in ("surge", "sway", "heave", "roll", "pitch", "yaw"): if key in data: @@ -752,14 +939,13 @@ def debug_override(): if not axes: return jsonify({"ok": False, "error": "No axes supplied"}), 400 - 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: + if ctrl and hasattr(ctrl, "set_debug_override"): ctrl.set_debug_override(axes) + elif bm: + bm.set_from_axes(**axes) - return jsonify({"ok": True, "override": axes}) + state = ctrl.get_control_state() if ctrl and hasattr(ctrl, "get_control_state") else {} + return jsonify({"ok": True, "override": axes, "state": state}) @app.route("/api/debug/attitude_setpoint", methods=["POST"]) def debug_attitude_setpoint(): @@ -793,92 +979,173 @@ def debug_clear(): if ctrl: ctrl.clear_debug_override() - # Zero out the bitmask axes (slider override path) - bm = current_app.config.get("BITMASK") - if bm: - 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") if client: try: - client.clear_override() + if ctrl and hasattr(ctrl, "is_pid_enabled") and ctrl.is_pid_enabled(): + _send_active_pid_setpoints(ctrl, client) + else: + client.clear_override() except Exception: pass - return jsonify({"ok": True}) + state = ctrl.get_control_state() if ctrl and hasattr(ctrl, "get_control_state") else {} + return jsonify({"ok": True, "state": state}) @app.route("/api/pid/start", methods=["POST"]) def start_pid_hold(): """Start PID tuning from the current attitude and neutral manual command axes.""" + data = request.get_json(force=True, silent=True) or {} + force = bool(data.get("force")) imu = current_app.config.get("IMU") client = current_app.config.get("SETPOINT_OVERRIDE") + ctrl = current_app.config.get("CONTROLLER") if not imu: return jsonify({"ok": False, "error": "IMU receiver not running"}), 503 if not client: return jsonify({"ok": False, "error": "Setpoint override client unavailable"}), 503 + if not ctrl or not hasattr(ctrl, "start_pid"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + if ctrl.is_killed(): + return jsonify({"ok": False, "error": "Controls are killed", "state": ctrl.get_control_state()}), 423 stats = imu.get_stats() - age_ms = stats.get("age_ms") - if age_ms is None or age_ms > 2000: - return jsonify({"ok": False, "error": "IMU data is stale; PID start was blocked"}), 503 - - attitude = stats.get("last_data") or {} - try: - attitude_setpoints = _coerce_attitude_setpoints({axis: float(attitude[axis]) for axis in ATTITUDE_AXES}) - except (KeyError, TypeError, ValueError): - return jsonify({"ok": False, "error": "Current attitude is incomplete"}), 503 - if len(attitude_setpoints) != len(ATTITUDE_AXES): - return jsonify({"ok": False, "error": "Current attitude is incomplete"}), 503 - - neutral = _neutralize_thruster_command() - setpoints = {**neutral, **attitude_setpoints} + sanity = _imu_attitude_sanity(stats) + if not sanity["usable"]: + return jsonify({"ok": False, "error": sanity["reason"] or "Current attitude is incomplete", "sanity": sanity}), 503 + if not sanity["ok"] and not force: + return jsonify({"ok": False, "error": sanity["reason"], "sanity": sanity, "force_supported": True}), 409 + + pending = ctrl.get_pid_setpoints() if hasattr(ctrl, "get_pid_setpoints") else {} + setpoints = {**sanity["setpoints"], **pending} + ctrl.start_pid(setpoints) try: client.clear_override() - state = client.send_override(setpoints, replay_attempts=5, replay_delay=0.1) + override_state = client.send_override(setpoints, replay_attempts=5, replay_delay=0.1) except Exception as exc: # pylint: disable=broad-except client.set_error(str(exc)) - return jsonify({"ok": False, "error": str(exc), "neutralized": True}), 503 + ctrl.stop_pid(clear=False) + return jsonify({"ok": False, "error": str(exc), "sanity": sanity}), 503 return jsonify( { "ok": True, "setpoints": setpoints, - "state": state, - "neutralized": True, + "state": ctrl.get_control_state(), + "override_state": override_state, + "sanity": sanity, + "forced": force and not sanity["ok"], "units": "deg", } ) @app.route("/api/pid/setpoints", methods=["POST"]) def set_pid_attitude_setpoints(): - """Send roll/pitch/yaw PID attitude setpoints in VN-100 degrees.""" + """Save roll/pitch/yaw PID attitude setpoints in VN-100 degrees.""" data = request.get_json(force=True, silent=True) or {} client = current_app.config.get("SETPOINT_OVERRIDE") - if not client: - return jsonify({"ok": False, "error": "Setpoint override client unavailable"}), 503 + ctrl = current_app.config.get("CONTROLLER") + if not ctrl or not hasattr(ctrl, "set_pid_setpoints"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + if ctrl.is_killed(): + return jsonify({"ok": False, "error": "Controls are killed", "state": ctrl.get_control_state()}), 423 axes = _coerce_attitude_setpoints(data) if not axes: return jsonify({"ok": False, "error": "No valid attitude setpoints supplied"}), 400 - try: - state = client.send_override(axes, replay_attempts=5, replay_delay=0.1) - except Exception as exc: # pylint: disable=broad-except - client.set_error(str(exc)) - return jsonify({"ok": False, "error": str(exc)}), 503 + setpoints = ctrl.set_pid_setpoints(axes) + pid_active = ctrl.is_pid_enabled() if hasattr(ctrl, "is_pid_enabled") else False + if pid_active: + if not client: + return jsonify({"ok": False, "error": "Setpoint override client unavailable"}), 503 + try: + state = client.send_override(setpoints, replay_attempts=5, replay_delay=0.1) + except Exception as exc: # pylint: disable=broad-except + client.set_error(str(exc)) + return jsonify({"ok": False, "error": str(exc)}), 503 + else: + state = client.get_state() if client and hasattr(client, "get_state") else {} return jsonify( { "ok": True, - "sent": axes, + "sent": setpoints, "limits": ATTITUDE_LIMITS_DEG, "state": state, + "control_state": ctrl.get_control_state(), + "pid_active": pid_active, "units": "deg", } ) + @app.route("/api/pid/setpoints/", methods=["DELETE"]) + def clear_pid_attitude_setpoint(axis): + """Clear one saved or live roll/pitch/yaw PID setpoint.""" + axis = axis.lower() + if axis not in ATTITUDE_AXES: + return jsonify({"ok": False, "error": "Invalid PID axis"}), 400 + ctrl = current_app.config.get("CONTROLLER") + client = current_app.config.get("SETPOINT_OVERRIDE") + if not ctrl or not hasattr(ctrl, "clear_pid_setpoint"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + remaining = ctrl.clear_pid_setpoint(axis) + pid_active = ctrl.is_pid_enabled() if hasattr(ctrl, "is_pid_enabled") else False + if pid_active: + if not client: + return jsonify({"ok": False, "error": "Setpoint override client unavailable"}), 503 + try: + state = _send_active_pid_setpoints(ctrl, client) + except Exception as exc: # pylint: disable=broad-except + client.set_error(str(exc)) + return jsonify({"ok": False, "error": str(exc)}), 503 + else: + state = client.get_state() if client and hasattr(client, "get_state") else {} + return jsonify( + { + "ok": True, + "cleared": axis, + "remaining": remaining, + "state": state, + "control_state": ctrl.get_control_state(), + "pid_active": pid_active, + } + ) + + @app.route("/api/pid/stop", methods=["POST"]) + def stop_pid_hold(): + """Stop PID and optionally clear attitude setpoints.""" + data = request.get_json(force=True, silent=True) or {} + clear = bool(data.get("clear", True)) + ctrl = current_app.config.get("CONTROLLER") + if not ctrl or not hasattr(ctrl, "stop_pid"): + return jsonify({"ok": False, "error": "Controller not available"}), 503 + state = ctrl.stop_pid(clear=clear) + client = current_app.config.get("SETPOINT_OVERRIDE") + if client: + try: + client.clear_override() + except Exception: + pass + return jsonify({"ok": True, "state": state}) + + @app.route("/api/pid/rates", methods=["GET", "POST"]) + def pid_setpoint_rates(): + """Get or update roll/pitch/yaw setpoint rates in deg/s.""" + if request.method == "GET": + rates = _load_pid_rates() + ctrl = current_app.config.get("CONTROLLER") + if ctrl and hasattr(ctrl, "set_pid_rates"): + ctrl.set_pid_rates(rates) + return jsonify({"ok": True, "rates": rates, "units": "deg/s"}) + + data = request.get_json(force=True, silent=True) or {} + rates = _save_pid_rates(data) + return jsonify({"ok": True, "rates": rates, "units": "deg/s"}) + @app.route("/api/pid/zero_all", methods=["POST"]) def zero_all_pid(): """Force neutral manual command axes and send zero PID gains to the MCU.""" + ctrl = current_app.config.get("CONTROLLER") + if ctrl and hasattr(ctrl, "stop_pid"): + ctrl.stop_pid() neutral = _neutralize_thruster_command() client = current_app.config.get("SETPOINT_OVERRIDE") if client: @@ -919,26 +1186,17 @@ def get_pid_gains(): gains = request_pid_gains(timeout=2.0) if gains is None: return jsonify({"ok": False, "error": "No response from MCU"}), 504 - return jsonify({"ok": True, "gains": gains}) + return jsonify({"ok": True, "gains": _attitude_pid_gains(gains), "raw_gains": gains}) @app.route("/api/pid/gains", methods=["POST"]) def set_pid_gains(): """Send PID gains to the MCU via UDP. Expects JSON: {axis: {kp, ki, kd}, ...}.""" data = request.get_json(force=True, silent=True) or {} - gains = {} - for axis in PID_AXES: - if axis in data and isinstance(data[axis], dict): - gains[axis] = { - "kp": float(data[axis].get("kp", 0.0)), - "ki": float(data[axis].get("ki", 0.0)), - "kd": float(data[axis].get("kd", 0.0)), - } - else: - gains[axis] = {"kp": 0.0, "ki": 0.0, "kd": 0.0} + gains = _mcu_pid_gains(data) confirmed, attempts = send_pid_gains(gains, timeout=1.0, max_retries=3) if confirmed is None: return jsonify({"ok": False, "error": "No response from MCU after %d attempts" % attempts}), 504 - return jsonify({"ok": True, "gains": confirmed, "attempts": attempts}) + return jsonify({"ok": True, "gains": _attitude_pid_gains(confirmed), "raw_gains": confirmed, "attempts": attempts}) # --- PID config save/load --- @app.route("/api/pid/configs", methods=["GET"]) @@ -958,7 +1216,7 @@ def save_pid_config(): if not isinstance(gains, dict): return jsonify({"ok": False, "error": "Missing gains data"}), 400 configs = _load_pid_configs() - configs[name] = gains + configs[name] = _mcu_pid_gains(gains) _save_pid_configs(configs) return jsonify({"ok": True, "name": name}) @@ -968,7 +1226,7 @@ def load_pid_config(name): configs = _load_pid_configs() if name not in configs: return jsonify({"ok": False, "error": "Config not found"}), 404 - return jsonify({"ok": True, "name": name, "gains": configs[name]}) + return jsonify({"ok": True, "name": name, "gains": _attitude_pid_gains(configs[name]), "raw_gains": configs[name]}) @app.route("/api/pid/configs/", methods=["DELETE"]) def delete_pid_config(name): diff --git a/static/css/pid_tuning.css b/static/css/pid_tuning.css index 01d0a4b..7ddeebc 100644 --- a/static/css/pid_tuning.css +++ b/static/css/pid_tuning.css @@ -14,19 +14,26 @@ position: sticky; top: 56px; z-index: 20; - display: flex; - justify-content: space-between; + display: grid; + grid-template-columns: minmax(0, 1fr) auto; align-items: center; gap: 0.75rem; padding: 0.75rem; - margin-bottom: 1rem; + margin-bottom: 0.75rem; background: rgba(18, 24, 31, 0.98); border: 1px solid #495057; border-radius: 8px; box-shadow: 0 8px 18px rgba(0, 0, 0, 0.25); } -.pid-action-status, +.pid-action-status { + display: grid; + grid-template-columns: minmax(210px, 1.35fr) minmax(76px, 0.45fr) minmax(130px, 0.85fr) minmax(92px, 0.55fr); + align-items: stretch; + gap: 0.5rem; + min-width: 0; +} + .pid-action-controls, .pid-button-row, .pid-chart-buttons, @@ -38,12 +45,102 @@ gap: 0.5rem; } -.pid-kicker { - color: #0dcaf0; +.pid-action-controls { + flex-wrap: nowrap; + justify-content: flex-end; +} + +.pid-status-card { + display: flex; + flex-direction: column; + justify-content: center; + gap: 0.2rem; + min-width: 0; + padding: 0.45rem 0.6rem; + background: rgba(255, 255, 255, 0.055); + border: 1px solid rgba(255, 255, 255, 0.12); + border-radius: 8px; +} + +.pid-status-card-wide { + min-width: 0; +} + +.pid-status-label { + color: #adb5bd; + font-size: 0.68rem; font-weight: 700; + line-height: 1; text-transform: uppercase; - font-size: 0.75rem; - letter-spacing: 0.08em; +} + +.pid-status-value { + color: #f8f9fa; + font-family: Consolas, Monaco, monospace; + font-size: 0.88rem; + font-weight: 700; + line-height: 1.2; + min-width: 0; + overflow: hidden; + text-overflow: ellipsis; + white-space: nowrap; +} + +.pid-setpoint-strip { + color: #0dcaf0; +} + +.pid-axis-readouts { + display: grid; + grid-template-columns: repeat(3, minmax(0, 1fr)); + gap: 0.75rem; + margin-bottom: 1rem; +} + +.pid-axis-readout { + min-width: 0; + padding: 0.75rem; + background: rgba(22, 29, 36, 0.96); + border: 1px solid rgba(13, 202, 240, 0.22); + border-radius: 8px; +} + +.pid-axis-readout-title { + color: #f8f9fa; + font-size: 0.9rem; + font-weight: 800; + margin-bottom: 0.55rem; +} + +.pid-axis-readout-grid { + display: grid; + grid-template-columns: repeat(3, minmax(0, 1fr)); + gap: 0.5rem; +} + +.pid-axis-readout-cell { + min-width: 0; + padding: 0.45rem 0.5rem; + background: rgba(255, 255, 255, 0.045); + border: 1px solid rgba(255, 255, 255, 0.09); + border-radius: 6px; +} + +.pid-readout-number { + display: block; + color: #f8f9fa; + font-family: Consolas, Monaco, monospace; + font-size: 1.02rem; + font-weight: 800; + line-height: 1.2; + margin-top: 0.25rem; + overflow: hidden; + text-overflow: ellipsis; + white-space: nowrap; +} + +.pid-readout-error { + color: #20c997; } .pid-compact-label { @@ -56,9 +153,14 @@ width: 96px; } -.pid-zero-btn { +.pid-kill-btn { + font-weight: 900; + letter-spacing: 0.04em; +} + +.pid-toggle-btn { + min-width: 132px; font-weight: 800; - letter-spacing: 0.03em; } .pid-layout-grid { @@ -127,7 +229,7 @@ .pid-setpoint-row { display: grid; - grid-template-columns: 58px minmax(0, 1fr) auto; + grid-template-columns: 58px minmax(0, 1fr) auto auto; gap: 0.5rem; align-items: center; } @@ -325,6 +427,20 @@ } @media (max-width: 1200px) { + .pid-action-bar { + display: flex; + align-items: stretch; + flex-direction: column; + } + + .pid-action-status { + grid-template-columns: repeat(3, minmax(0, 1fr)); + } + + .pid-action-controls { + flex-wrap: wrap; + } + .pid-layout-grid { grid-template-columns: 1fr; } @@ -346,6 +462,7 @@ .pid-action-bar, .pid-action-status, .pid-action-controls { + display: flex; flex-direction: column; } @@ -353,11 +470,16 @@ width: 100%; } + .pid-status-card { + width: 100%; + } + #pid-time-window { width: 100%; } .pid-attitude-grid, + .pid-axis-readouts, .pid-slider-grid { grid-template-columns: 1fr; } diff --git a/static/js/configuration.js b/static/js/configuration.js index 99cc446..1ed26cd 100644 --- a/static/js/configuration.js +++ b/static/js/configuration.js @@ -14,6 +14,47 @@ document.addEventListener("DOMContentLoaded", function () { const axesYaw = document.getElementById("axes-yaw"); const axesPitch = document.getElementById("axes-pitch"); const axesRoll = document.getElementById("axes-roll"); + const pidRateRoll = document.getElementById("pid-rate-roll"); + const pidRatePitch = document.getElementById("pid-rate-pitch"); + const pidRateYaw = document.getElementById("pid-rate-yaw"); + + fetch("/api/pid/rates") + .then((r) => r.json()) + .then((data) => { + if (!data.ok || !data.rates) return; + if (pidRateRoll) pidRateRoll.value = data.rates.roll; + if (pidRatePitch) pidRatePitch.value = data.rates.pitch; + if (pidRateYaw) pidRateYaw.value = data.rates.yaw; + }) + .catch(() => {}); + + const savePidRates = document.getElementById("btn-save-pid-rates"); + if (savePidRates) { + savePidRates.addEventListener("click", async function () { + try { + const res = await fetch("/api/pid/rates", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ + roll: parseFloat(pidRateRoll.value) || 0, + pitch: parseFloat(pidRatePitch.value) || 0, + yaw: parseFloat(pidRateYaw.value) || 0, + }), + }); + const data = await res.json(); + if (data.ok && data.rates) { + pidRateRoll.value = data.rates.roll; + pidRatePitch.value = data.rates.pitch; + pidRateYaw.value = data.rates.yaw; + setFeedback("pid-rates-feedback", "Rates saved", "text-success"); + } else { + setFeedback("pid-rates-feedback", "Failed to save rates", "text-danger"); + } + } catch (error) { + setFeedback("pid-rates-feedback", "Error: " + error.message, "text-danger"); + } + }); + } fetch("/api/imu/axes") .then((r) => r.json()) diff --git a/static/js/pid_tuning.js b/static/js/pid_tuning.js index bc81ced..6f9308f 100644 --- a/static/js/pid_tuning.js +++ b/static/js/pid_tuning.js @@ -12,10 +12,12 @@ let sendTimer = null; let latestImu = {}; let latestTelemetry = null; + let latestControlState = {}; const localSetpoints = { roll: NaN, pitch: NaN, yaw: NaN }; - const pageStatus = document.getElementById("pid-page-status"); - const linkStatus = document.getElementById("pid-link-status"); + const controlPathStatus = document.getElementById("control-path-status"); + const pidModeStatus = document.getElementById("pid-mode-status"); + const branchStatus = document.getElementById("pid-branch"); const imuAge = document.getElementById("pid-imu-age"); const pidStatus = document.getElementById("pid-status"); const setpointStatus = document.getElementById("setpoint-status"); @@ -39,8 +41,9 @@ return wrapped; } - function angleError(setpoint, position) { + function axisError(axis, setpoint, position) { if (!isFiniteNumber(setpoint) || !isFiniteNumber(position)) return NaN; + if (axis === "pitch") return setpoint - position; return normalizeAngle(setpoint - position); } @@ -50,10 +53,6 @@ el.className = "badge " + cls; } - function setPageStatus(text, cls) { - setBadge(pageStatus, text, cls || "bg-secondary"); - } - function setPidStatus(text, cls) { setBadge(pidStatus, text, cls || "bg-secondary"); } @@ -68,6 +67,73 @@ setpointFeedback.className = "pid-feedback " + (cls || ""); } + function controlPathLabel(path, killed) { + if (killed) return "Controls locked"; + if (path === "Override Controls") return "Override sliders"; + if (path === "PS4") return "PS4 Controller"; + return path || "PS4 Controller"; + } + + function setControlPathStatus(path, killed) { + if (!controlPathStatus) return; + controlPathStatus.textContent = controlPathLabel(path, killed); + controlPathStatus.className = "pid-status-value " + (killed ? "text-danger" : path === "Override Controls" ? "text-warning" : "text-info"); + } + + function syncLocalSetpoints(setpoints, options) { + const clearMissing = !options || options.clearMissing !== false; + ROT_AXES.forEach((axis) => { + const hasAxis = setpoints && Object.prototype.hasOwnProperty.call(setpoints, axis); + const value = hasAxis ? Number(setpoints[axis]) : NaN; + const el = document.getElementById("setpoint-" + axis); + if (Number.isFinite(value)) { + localSetpoints[axis] = value; + if (el && document.activeElement !== el) el.value = value.toFixed(1); + } else { + localSetpoints[axis] = NaN; + if (clearMissing && el && document.activeElement !== el) el.value = ""; + } + }); + } + + function setControlsDisabled(killed) { + document.querySelectorAll("#btn-toggle-pid, #btn-enable, #btn-send-setpoints, .js-clear-axis").forEach((el) => { + el.disabled = killed; + }); + const btnKill = document.getElementById("btn-killswitch"); + const btnRearm = document.getElementById("btn-rearm"); + if (btnKill) btnKill.disabled = killed; + if (btnRearm) btnRearm.disabled = !killed; + } + + function updatePidToggle(pidOn, killed) { + const btn = document.getElementById("btn-toggle-pid"); + if (!btn) return; + btn.textContent = pidOn ? "Stop PID" : "Start PID"; + btn.className = "btn pid-toggle-btn " + (pidOn ? "btn-success" : "btn-primary"); + btn.disabled = killed; + } + + function updateControlBanner(state) { + latestControlState = state || {}; + const killed = latestControlState.killed === true; + const pidOn = latestControlState.pid_enabled === true; + const path = latestControlState.control_path || "PS4"; + const setpoints = latestControlState.pid_setpoints || {}; + const hasSetpoints = ROT_AXES.some((axis) => Number.isFinite(Number(setpoints[axis]))); + syncLocalSetpoints(setpoints, { clearMissing: false }); + + setControlPathStatus(path, killed); + setBadge(pidModeStatus, pidOn ? "ON" : "OFF", pidOn ? "bg-success" : "bg-secondary"); + setSetpointStatus(pidOn ? "ACTIVE" : hasSetpoints ? "SAVED" : "IDLE", pidOn ? "bg-danger" : hasSetpoints ? "bg-info text-dark" : "bg-secondary"); + updatePidToggle(pidOn, killed); + setControlsDisabled(killed); + updateAxisReadouts(); + + const overrideBadge = document.getElementById("debug-status"); + setBadge(overrideBadge, latestControlState.override_active ? "ACTIVE" : "INACTIVE", latestControlState.override_active ? "bg-danger" : "bg-secondary"); + } + function clampSetpoint(axis, value) { const limit = Number(attitudeLimits[axis] || 180); let next = Number(value); @@ -78,17 +144,40 @@ function getTelemetrySetpoint(axis) { const fromTelemetry = latestTelemetry && latestTelemetry.setpoint ? Number(latestTelemetry.setpoint[axis]) : NaN; - if (Number.isFinite(fromTelemetry)) return fromTelemetry; - return localSetpoints[axis]; + const fromLocal = localSetpoints[axis]; + if (latestControlState.pid_enabled === true && Number.isFinite(fromTelemetry)) return fromTelemetry; + if (Number.isFinite(fromLocal)) return fromLocal; + return fromTelemetry; + } + + function updateAxisReadouts() { + ROT_AXES.forEach((axis) => { + const setpoint = getTelemetrySetpoint(axis); + const position = Number(latestImu[axis]); + const error = axisError(axis, setpoint, position); + const positionEl = document.getElementById("readout-" + axis + "-position"); + const setpointEl = document.getElementById("readout-" + axis + "-setpoint"); + const errorEl = document.getElementById("readout-" + axis + "-error"); + if (positionEl) positionEl.textContent = fmt(position, 2); + if (setpointEl) setpointEl.textContent = fmt(setpoint, 2); + if (errorEl) { + errorEl.textContent = fmt(error, 2); + errorEl.className = "pid-readout-number pid-readout-error"; + if (!isFiniteNumber(error)) errorEl.className = "pid-readout-number text-muted"; + else if (Math.abs(error) > 25) errorEl.className = "pid-readout-number text-danger"; + else if (Math.abs(error) > 10) errorEl.className = "pid-readout-number text-warning"; + } + }); } function updateTelemetryTable() { + updateAxisReadouts(); if (!telemetryBody) return; const frag = document.createDocumentFragment(); ROT_AXES.forEach((axis) => { const setpoint = getTelemetrySetpoint(axis); const position = Number(latestImu[axis]); - const error = angleError(setpoint, position); + const error = axisError(axis, setpoint, position); const tr = document.createElement("tr"); const tdAxis = document.createElement("td"); const tdSet = document.createElement("td"); @@ -140,6 +229,26 @@ updateTelemetryTable(); } + async function pollControlState() { + try { + const res = await fetch("/api/control/state", { cache: "no-store" }); + const data = await res.json(); + if (data.ok) updateControlBanner(data.state || {}); + } catch (err) { + console.debug("Control state polling failed:", err); + } + } + + async function loadGitBranch() { + try { + const res = await fetch("/api/system/git", { cache: "no-store" }); + const data = await res.json(); + if (branchStatus) branchStatus.textContent = data.ok && data.git ? data.git.branch : "--"; + } catch (_) { + if (branchStatus) branchStatus.textContent = "--"; + } + } + function getSliderValue(axis) { const slider = document.getElementById("slider-" + axis); return slider ? parseInt(slider.value, 10) / 100 : 0; @@ -175,11 +284,17 @@ async function sendOverride() { if (!overrideActive) return; try { - await fetch("/api/debug/override", { + const res = await fetch("/api/debug/override", { method: "POST", headers: { "Content-Type": "application/json" }, body: JSON.stringify(getAllSliderValues()), }); + const data = await res.json().catch(() => ({})); + if (!res.ok || data.ok === false) { + resetOverrideUi(); + setFeedback(data.error || "Override blocked.", "text-danger"); + if (data.state) updateControlBanner(data.state); + } } catch (err) { console.error("Failed to send override:", err); } @@ -221,11 +336,6 @@ } } - function activateNeutralOverride() { - AXES.forEach(resetSlider); - enableOverride(); - } - function readPidFields() { const gains = {}; AXES.forEach((axis) => { @@ -249,14 +359,6 @@ }); } - function zeroGainPayload() { - const zeros = {}; - AXES.forEach((axis) => { - zeros[axis] = { kp: 0, ki: 0, kd: 0 }; - }); - return zeros; - } - async function requestPidGains() { setPidStatus("REQUESTING", "bg-warning text-dark"); const btn = document.getElementById("btn-pid-request"); @@ -303,75 +405,47 @@ } } - async function zeroAllPidAndThrusters() { - activateNeutralOverride(); - fillPidFields(zeroGainPayload()); - setPidStatus("NEUTRALIZING", "bg-warning text-dark"); - setPageStatus("NEUTRALIZING", "bg-warning text-dark"); - document.querySelectorAll(".js-zero-all-pid").forEach((btn) => { - btn.disabled = true; - }); - try { - const res = await fetch("/api/pid/zero_all", { method: "POST" }); - const data = await res.json().catch(() => ({})); - if (data.gains) fillPidFields(data.gains); - if (res.ok && data.ok) { - setPidStatus("ZERO CONFIRMED", "bg-success"); - setPageStatus("NEUTRAL HOLD", "bg-danger"); - } else { - setPidStatus("NEUTRAL, PID NO REPLY", "bg-danger"); - setPageStatus("NEUTRAL, CHECK MCU", "bg-danger"); - } - } catch (err) { - setPidStatus("NEUTRAL, ERROR", "bg-danger"); - setPageStatus("NEUTRAL, ERROR", "bg-danger"); - console.error("Zero PID failed:", err); - } finally { - document.querySelectorAll(".js-zero-all-pid").forEach((btn) => { - btn.disabled = false; - }); - } - } - function fillSetpointFields(setpoints) { - ROT_AXES.forEach((axis) => { - const value = setpoints && Number(setpoints[axis]); - if (Number.isFinite(value)) { - localSetpoints[axis] = value; - const el = document.getElementById("setpoint-" + axis); - if (el) el.value = value.toFixed(1); - } - }); + syncLocalSetpoints(setpoints); } - async function startPid() { - activateNeutralOverride(); + async function startPid(force) { setSetpointStatus("STARTING", "bg-warning text-dark"); - setPageStatus("STARTING PID", "bg-warning text-dark"); - document.querySelectorAll(".js-start-pid").forEach((btn) => { - btn.disabled = true; - }); + const toggleBtn = document.getElementById("btn-toggle-pid"); + if (toggleBtn) toggleBtn.disabled = true; try { - const res = await fetch("/api/pid/start", { method: "POST" }); + const res = await fetch("/api/pid/start", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ force: force === true }), + }); const data = await res.json(); if (data.ok) { fillSetpointFields(data.setpoints || {}); + if (data.state) updateControlBanner(data.state); setSetpointStatus("ACTIVE", "bg-danger"); - setPageStatus("PID HOLD ACTIVE", "bg-success"); - setFeedback("Started from current IMU attitude with neutral manual command axes.", "text-success"); + setFeedback("Started from current IMU attitude.", "text-success"); + } else if (res.status === 409 && data.force_supported) { + const raw = data.sanity && data.sanity.raw ? JSON.stringify(data.sanity.raw) : "{}"; + const proceed = window.confirm( + "IMU sanity check failed.\n\nReason: " + + (data.error || "Unknown") + + "\nRaw: " + + raw + + "\n\nCancel is recommended. Press OK to continue anyway." + ); + if (proceed) await startPid(true); } else { setSetpointStatus("BLOCKED", "bg-danger"); - setPageStatus("START BLOCKED", "bg-danger"); setFeedback(data.error || "Start failed.", "text-danger"); + if (data.state) updateControlBanner(data.state); } } catch (err) { setSetpointStatus("ERROR", "bg-danger"); - setPageStatus("START ERROR", "bg-danger"); setFeedback("Error: " + err.message, "text-danger"); } finally { - document.querySelectorAll(".js-start-pid").forEach((btn) => { - btn.disabled = false; - }); + if (toggleBtn) toggleBtn.disabled = false; + setControlsDisabled(latestControlState.killed === true); } } @@ -405,11 +479,15 @@ const data = await res.json(); if (data.ok) { fillSetpointFields(data.sent || {}); - setSetpointStatus("ACTIVE", "bg-danger"); - setFeedback("Sent setpoints: " + Object.entries(data.sent || {}).map(([k, v]) => k + "=" + v.toFixed(1)).join(", "), "text-success"); + if (data.control_state) updateControlBanner(data.control_state); + const pidActive = data.pid_active === true || (data.control_state && data.control_state.pid_enabled === true); + const text = Object.entries(data.sent || {}).map(([k, v]) => k + "=" + v.toFixed(1)).join(", "); + setSetpointStatus(pidActive ? "ACTIVE" : "SAVED", pidActive ? "bg-danger" : "bg-info text-dark"); + setFeedback((pidActive ? "Updated active setpoints: " : "Saved setpoints: ") + text, "text-success"); } else { setSetpointStatus("ERROR", "bg-danger"); setFeedback(data.error || "Setpoint send failed.", "text-danger"); + if (data.state) updateControlBanner(data.state); } } catch (err) { setSetpointStatus("ERROR", "bg-danger"); @@ -417,16 +495,69 @@ } } - async function clearSetpoints() { + async function stopPid(clearSaved) { + const clear = clearSaved === true; try { - await fetch("/api/debug/clear", { method: "POST" }); - ROT_AXES.forEach((axis) => { - localSetpoints[axis] = NaN; + const res = await fetch("/api/pid/stop", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ clear }), }); + const data = await res.json().catch(() => ({})); + if (clear) syncLocalSetpoints({}); + else if (data.state && data.state.pid_setpoints) syncLocalSetpoints(data.state.pid_setpoints, { clearMissing: false }); resetOverrideUi(); - setSetpointStatus("IDLE", "bg-secondary"); - setPageStatus("READY", "bg-secondary"); - setFeedback("Setpoints cleared.", "text-light"); + if (data.state) updateControlBanner(data.state); + setSetpointStatus(clear ? "IDLE" : "SAVED", clear ? "bg-secondary" : "bg-info text-dark"); + setFeedback(clear ? "Setpoints cleared." : "PID stopped. Setpoints kept.", "text-light"); + } catch (err) { + setFeedback("Error: " + err.message, "text-danger"); + } + } + + function clearSetpoints() { + return stopPid(true); + } + + async function clearAxis(axis) { + try { + const res = await fetch("/api/pid/setpoints/" + encodeURIComponent(axis), { method: "DELETE" }); + const data = await res.json(); + if (data.ok) { + syncLocalSetpoints(data.remaining || {}); + if (data.control_state) updateControlBanner(data.control_state); + setFeedback(axis + " setpoint cleared.", "text-success"); + } else { + setFeedback(data.error || "Clear failed.", "text-danger"); + } + } catch (err) { + setFeedback("Error: " + err.message, "text-danger"); + } + } + + async function killControls() { + try { + const res = await fetch("/api/control/killswitch", { method: "POST" }); + const data = await res.json(); + AXES.forEach(resetSlider); + resetOverrideUi(); + syncLocalSetpoints({}); + if (data.state) updateControlBanner(data.state); + setFeedback(res.ok ? "Controls killed." : data.error || "Killswitch failed.", res.ok ? "text-danger" : "text-warning"); + } catch (err) { + setFeedback("Error: " + err.message, "text-danger"); + } + } + + async function rearmControls() { + try { + const res = await fetch("/api/control/rearm", { method: "POST" }); + const data = await res.json(); + AXES.forEach(resetSlider); + resetOverrideUi(); + syncLocalSetpoints({}); + if (data.state) updateControlBanner(data.state); + setFeedback(res.ok ? "Controls re-armed." : data.error || "Re-arm failed.", res.ok ? "text-success" : "text-danger"); } catch (err) { setFeedback("Error: " + err.message, "text-danger"); } @@ -436,21 +567,37 @@ try { const res = await fetch("/api/rov/status"); const data = await res.json(); + const control = data.control_state || latestControlState || {}; + const uplink = data.uplink || {}; + if (data.control_state) updateControlBanner(data.control_state); if (rovStatus) { rovStatus.textContent = JSON.stringify( - { command: data.command, uplink: data.uplink, resource: data.resource }, + { + control_path: controlPathLabel(control.control_path, control.killed === true), + pid_enabled: control.pid_enabled, + active_setpoints: control.pid_setpoints, + manual_command_before_pid: control.manual_command_before_pid, + pid_output: latestTelemetry && latestTelemetry.output ? latestTelemetry.output : {}, + final_topside_command: data.command, + raw_payload: uplink.last_packet_hex, + timestamp: uplink.last_send_timestamp, + sequence: uplink.sequence, + link: { + ack_age_ms: uplink.last_ack_age_ms, + watchdog_resends: uplink.watchdog_resends, + }, + telemetry: { + sequence: latestTelemetry ? latestTelemetry.sequence : null, + timestamp: latestTelemetry ? latestTelemetry.timestamp : null, + }, + resource: data.resource, + }, null, 2 ); } - const ackAge = data.uplink && isFiniteNumber(data.uplink.last_ack_age_ms) ? data.uplink.last_ack_age_ms : null; - if (ackAge == null) setBadge(linkStatus, "LINK IDLE", "bg-secondary"); - else if (ackAge < 1000) setBadge(linkStatus, "LINK LIVE", "bg-success"); - else if (ackAge < 3000) setBadge(linkStatus, "LINK STALE", "bg-warning text-dark"); - else setBadge(linkStatus, "LINK DEGRADED", "bg-danger"); } catch (err) { if (rovStatus) rovStatus.textContent = "Error fetching status"; - setBadge(linkStatus, "LINK ERROR", "bg-danger"); } } @@ -547,8 +694,13 @@ } function wireEvents() { - document.querySelectorAll(".js-start-pid").forEach((btn) => btn.addEventListener("click", startPid)); - document.querySelectorAll(".js-zero-all-pid").forEach((btn) => btn.addEventListener("click", zeroAllPidAndThrusters)); + const btnTogglePid = document.getElementById("btn-toggle-pid"); + if (btnTogglePid) { + btnTogglePid.addEventListener("click", () => { + if (latestControlState.pid_enabled === true) stopPid(false); + else startPid(); + }); + } const btnEnable = document.getElementById("btn-enable"); const btnDisable = document.getElementById("btn-disable"); @@ -579,6 +731,15 @@ if (btnSendSetpoints) btnSendSetpoints.addEventListener("click", sendSetpoints); if (btnClearSetpoints) btnClearSetpoints.addEventListener("click", clearSetpoints); + document.querySelectorAll(".js-clear-axis").forEach((btn) => { + btn.addEventListener("click", () => clearAxis(btn.dataset.axis)); + }); + + const btnKill = document.getElementById("btn-killswitch"); + const btnRearm = document.getElementById("btn-rearm"); + if (btnKill) btnKill.addEventListener("click", killControls); + if (btnRearm) btnRearm.addEventListener("click", rearmControls); + const btnPidRequest = document.getElementById("btn-pid-request"); const btnPidSend = document.getElementById("btn-pid-send"); if (btnPidRequest) btnPidRequest.addEventListener("click", requestPidGains); @@ -594,8 +755,11 @@ wireEvents(); refreshConfigList(); + loadGitBranch(); + pollControlState(); pollImuAndTelemetry(); pollRovStatus(); + setInterval(pollControlState, 500); setInterval(pollImuAndTelemetry, 200); setInterval(pollRovStatus, 500); })(); diff --git a/static/templates/config.html b/static/templates/config.html index 22ac0f2..ba60658 100644 --- a/static/templates/config.html +++ b/static/templates/config.html @@ -36,6 +36,31 @@
Input Source
+
+
+
+
+
PID Setpoint Rates
+
+ {% for axis in ["roll", "pitch", "yaw"] %} +
+ +
+ + deg/s +
+
+ {% endfor %} +
+ +
+
+
+
+
+
+
+
diff --git a/static/templates/pid_tuning.html b/static/templates/pid_tuning.html index 507fb32..928448a 100644 --- a/static/templates/pid_tuning.html +++ b/static/templates/pid_tuning.html @@ -15,17 +15,52 @@
- PID Tuning - READY - LINK IDLE - IMU -- ms +
+ Active control + PS4 Controller +
+
+ PID + OFF +
+
+ Branch + -- +
+
+ IMU + -- ms +
- - + + +
+
+ {% for axis, label in rotational_axes %} +
+
{{ label }}
+
+
+ Position + -- +
+
+ Setpoint + -- +
+
+ Error + -- +
+
+
+ {% endfor %} +
+
@@ -78,13 +113,13 @@
Angle Setpoints
deg
+
{% endfor %}
- - - + +
Waiting for input.
@@ -101,7 +136,6 @@
PID Values
-
@@ -120,22 +154,6 @@
PID Values
-
-
Surge, Sway, Heave
-
-
Axis
-
P
-
I
-
D
- {% for axis, label in translational_axes %} - - - - - {% endfor %} -
-
-
@@ -153,7 +171,12 @@
PID Values
Telemetry and Link
- NO TELEMETRY +
+ NO TELEMETRY + +
@@ -170,7 +193,9 @@
Telemetry and Link
- 
Loading...
+
+ 
Loading...
+
diff --git a/tests/test_controller.py b/tests/test_controller.py index f322f16..89b4cfc 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -90,9 +90,33 @@ def build_controller(controller=None, joystick=None): ctrl._debug_lock = threading.Lock() ctrl._input_status_lock = threading.Lock() ctrl._input_status = ctrl._empty_input_status() + ctrl._runtime_lock = threading.RLock() + ctrl._killed = False + ctrl._pid_enabled = False + ctrl._pid_setpoints = {} + ctrl._pid_setpoint_rates = dict(controller_module.DEFAULT_PID_SETPOINT_RATES) + ctrl._last_pid_update = 0.0 + ctrl._last_manual_command = {axis: 0.0 for axis in controller_module.CONTROL_AXES} + ctrl._last_output_command = {axis: 0.0 for axis in controller_module.CONTROL_AXES} + ctrl._last_runtime_source = "PS4" + ctrl._last_pid_error = None + ctrl._setpoint_client = None return ctrl +class FakeSetpointClient: + def __init__(self): + self.sent = [] + self.errors = [] + + def send_override(self, axes, replay_attempts=3, replay_delay=0.05): + self.sent.append(dict(axes)) + return {"active": True, "axes": dict(axes)} + + def set_error(self, message): + self.errors.append(message) + + def test_sdl_gamecontroller_mapping_normalizes_linux_playstation_layout(monkeypatch): monkeypatch.setattr(pygame.event, "get", lambda: []) sdl = FakeSdlController( @@ -245,3 +269,66 @@ def is_controller(index): assert ctrl.controller is None assert ctrl.joystick is joystick assert ctrl.get_input_status()["source"] == "raw_joystick" + + +def test_killswitch_zeroes_axes_and_blocks_manual_commands(): + ctrl = build_controller() + + state = ctrl.kill() + output = ctrl.apply_manual_axes_once({"surge": 1.0, "roll": 1.0, "yaw": -1.0}, source="HTTP") + + assert state["killed"] is True + assert state["pid_enabled"] is False + assert output == {axis: 0.0 for axis in controller_module.CONTROL_AXES} + assert ctrl.bm.calls[-1]["surge"] == 0 + assert ctrl.bm.calls[-1]["roll"] == 0 + assert ctrl.bm.calls[-1]["yaw"] == 0 + + +def test_rearm_returns_to_ps4_pid_off_neutral_state(): + ctrl = build_controller() + ctrl.kill() + + state = ctrl.rearm() + + assert state["killed"] is False + assert state["pid_enabled"] is False + assert state["control_path"] == "PS4" + assert state["pid_setpoints"] == {} + assert ctrl.bm.calls[-1]["surge"] == 0 + + +def test_pid_manual_input_updates_setpoints_and_blocks_rotational_thrust(monkeypatch): + ctrl = build_controller() + client = FakeSetpointClient() + ctrl.set_setpoint_client(client) + ctrl.set_pid_rates({"roll": 40, "pitch": 40, "yaw": 40}) + ctrl.start_pid({"roll": 170, "pitch": 89, "yaw": 179}) + ctrl._last_pid_update = 100.0 + monkeypatch.setattr(controller_module.time, "monotonic", lambda: 100.5) + + output = ctrl.apply_manual_axes_once( + {"surge": 0.5, "sway": -0.25, "heave": 0.1, "roll": 1, "pitch": 1, "yaw": 1}, + source="HTTP", + ) + + assert output["surge"] == pytest.approx(0.5) + assert output["sway"] == pytest.approx(-0.25) + assert output["heave"] == pytest.approx(0.1) + assert output["roll"] == 0.0 + assert output["pitch"] == 0.0 + assert output["yaw"] == 0.0 + assert ctrl.get_pid_setpoints() == {"roll": 180.0, "pitch": 90.0, "yaw": -171.0} + assert client.sent[-1] == {"roll": 180.0, "pitch": 90.0, "yaw": -171.0} + + +def test_pid_off_allows_direct_rotational_manual_control(): + ctrl = build_controller() + ctrl.start_pid({"roll": 0, "pitch": 0, "yaw": 0}) + ctrl.stop_pid() + + output = ctrl.apply_manual_axes_once({"roll": 0.4, "pitch": -0.3, "yaw": 0.2}, source="HTTP") + + assert output["roll"] == pytest.approx(0.4) + assert output["pitch"] == pytest.approx(-0.3) + assert output["yaw"] == pytest.approx(0.2) diff --git a/tests/test_pid_runtime_routes.py b/tests/test_pid_runtime_routes.py new file mode 100644 index 0000000..d3c5f82 --- /dev/null +++ b/tests/test_pid_runtime_routes.py @@ -0,0 +1,277 @@ +from flask import Flask + +import routes +from routes import register_routes + + +class FakeController: + def __init__(self): + self.killed = False + self.pid_enabled = False + self.setpoints = {} + self.rates = {"roll": 90.0, "pitch": 90.0, "yaw": 90.0} + + def get_control_state(self): + return { + "killed": self.killed, + "pid_enabled": self.pid_enabled, + "pid_setpoints": dict(self.setpoints), + "active_setpoints": dict(self.setpoints) if self.pid_enabled else {}, + "control_path": "KILLED" if self.killed else "PS4", + "override_active": False, + "manual_command_before_pid": {}, + "topside_command": {}, + } + + def get_input_status(self): + return {"connected": False, "source": "none", "name": None, "buttons": []} + + def get_manipulator(self): + return {"setpoint_norm": 0.0} + + def is_killed(self): + return self.killed + + def is_pid_enabled(self): + return self.pid_enabled + + def kill(self): + self.killed = True + self.pid_enabled = False + self.setpoints = {} + return self.get_control_state() + + def rearm(self): + self.killed = False + self.pid_enabled = False + self.setpoints = {} + return self.get_control_state() + + def set_debug_override(self, axes): + return not self.killed + + def clear_debug_override(self): + pass + + def apply_manual_axes_once(self, axes, source="HTTP"): + return {axis: 0.0 for axis in routes.CONTROL_AXES} if self.killed else dict(axes) + + def start_pid(self, setpoints): + self.pid_enabled = True + self.setpoints = dict(setpoints) + return dict(self.setpoints) + + def stop_pid(self, clear=True): + self.pid_enabled = False + if clear: + self.setpoints = {} + return self.get_control_state() + + def set_pid_setpoints(self, setpoints): + self.setpoints.update(setpoints) + return dict(self.setpoints) + + def clear_pid_setpoint(self, axis): + self.setpoints.pop(axis, None) + if self.pid_enabled and not self.setpoints: + self.pid_enabled = False + return dict(self.setpoints) + + def get_pid_setpoints(self): + return dict(self.setpoints) + + def set_pid_rates(self, rates): + self.rates.update(rates) + return dict(self.rates) + + +class FakeSetpointOverride: + def __init__(self): + self.sent = [] + self.clear_count = 0 + + def clear_override(self): + self.clear_count += 1 + return {"active": False, "axes": {}} + + def send_override(self, axes, replay_attempts=3, replay_delay=0.05): + self.sent.append(dict(axes)) + return {"active": True, "axes": dict(axes)} + + def get_state(self): + return {"active": bool(self.sent), "axes": self.sent[-1] if self.sent else {}} + + def set_error(self, message): + self.error = message + + +class FakeIMU: + def __init__(self, age_ms=10, data=None): + self.age_ms = age_ms + self.data = data or {"roll": 1.0, "pitch": 2.0, "yaw": 3.0} + + def get_stats(self): + return {"age_ms": self.age_ms, "last_data": dict(self.data)} + + +class FakeBitmask: + def __init__(self): + self.axes = {} + + def set_from_axes(self, **kwargs): + self.axes.update(kwargs) + + def get_command(self): + return dict(self.axes) + + def get_uplink_status(self): + return {"sequence": 1, "last_packet_hex": "00", "last_ack_age_ms": 5} + + +def make_client(ctrl=None, imu=None, override=None): + app = Flask(__name__) + app.config["CONTROLLER"] = ctrl or FakeController() + app.config["IMU"] = imu or FakeIMU() + app.config["SETPOINT_OVERRIDE"] = override or FakeSetpointOverride() + app.config["BITMASK"] = FakeBitmask() + register_routes(app) + return app.test_client(), app.config["CONTROLLER"], app.config["SETPOINT_OVERRIDE"] + + +def test_pid_start_returns_sanity_failure_then_allows_force(): + client, ctrl, override = make_client(imu=FakeIMU(age_ms=3000)) + + res = client.post("/api/pid/start", json={}) + assert res.status_code == 409 + assert res.get_json()["force_supported"] is True + + res = client.post("/api/pid/start", json={"force": True}) + assert res.status_code == 200 + data = res.get_json() + assert data["forced"] is True + assert ctrl.pid_enabled is True + assert data["setpoints"] == {"roll": 1.0, "pitch": 2.0, "yaw": 3.0} + assert override.sent[-1] == {"roll": 1.0, "pitch": 2.0, "yaw": 3.0} + + +def test_setpoints_save_without_starting_pid_or_sending_override(): + client, ctrl, override = make_client() + + res = client.post("/api/pid/setpoints", json={"roll": 45.0}) + + assert res.status_code == 200 + data = res.get_json() + assert data["pid_active"] is False + assert ctrl.pid_enabled is False + assert ctrl.setpoints == {"roll": 45.0} + assert override.sent == [] + + +def test_pid_start_uses_saved_setpoints_and_current_imu_for_missing_axes(): + client, ctrl, override = make_client() + client.post("/api/pid/setpoints", json={"roll": 45.0}) + + res = client.post("/api/pid/start", json={}) + + assert res.status_code == 200 + data = res.get_json() + assert ctrl.pid_enabled is True + assert data["setpoints"] == {"roll": 45.0, "pitch": 2.0, "yaw": 3.0} + assert override.sent[-1] == {"roll": 45.0, "pitch": 2.0, "yaw": 3.0} + + +def test_active_pid_setpoints_still_update_override(): + ctrl = FakeController() + ctrl.start_pid({"roll": 10.0}) + override = FakeSetpointOverride() + client, _ctrl, _override = make_client(ctrl=ctrl, override=override) + + res = client.post("/api/pid/setpoints", json={"yaw": -20.0}) + + assert res.status_code == 200 + data = res.get_json() + assert data["pid_active"] is True + assert ctrl.setpoints == {"roll": 10.0, "yaw": -20.0} + assert override.sent[-1] == {"roll": 10.0, "yaw": -20.0} + + +def test_stop_pid_can_keep_saved_setpoints(): + ctrl = FakeController() + ctrl.start_pid({"roll": 10.0, "pitch": 20.0}) + override = FakeSetpointOverride() + client, _ctrl, _override = make_client(ctrl=ctrl, override=override) + + res = client.post("/api/pid/stop", json={"clear": False}) + + assert res.status_code == 200 + assert ctrl.pid_enabled is False + assert ctrl.setpoints == {"roll": 10.0, "pitch": 20.0} + assert override.clear_count == 1 + + +def test_killswitch_zeroes_pid_gains_and_blocks_debug_override_until_rearm(monkeypatch): + captured = {} + + def fake_send_pid_gains(gains, timeout=1.0, max_retries=3): + captured["gains"] = gains + captured["timeout"] = timeout + captured["max_retries"] = max_retries + return gains, 1 + + monkeypatch.setattr(routes, "send_pid_gains", fake_send_pid_gains) + client, _ctrl, _override = make_client() + + res = client.post("/api/control/killswitch") + assert res.status_code == 200 + data = res.get_json() + assert data["state"]["killed"] is True + assert data["pid_gains_zeroed"] is True + assert captured["gains"] == {axis: {"kp": 0.0, "ki": 0.0, "kd": 0.0} for axis in routes.PID_AXES} + + res = client.post("/api/debug/override", json={"surge": 1}) + assert res.status_code == 423 + + res = client.post("/api/control/rearm") + assert res.status_code == 200 + assert res.get_json()["state"]["killed"] is False + + +def test_clear_pid_axis_clears_all_then_resends_remaining(): + ctrl = FakeController() + ctrl.start_pid({"roll": 10.0, "pitch": 20.0, "yaw": 30.0}) + override = FakeSetpointOverride() + client, _ctrl, _override = make_client(ctrl=ctrl, override=override) + + res = client.delete("/api/pid/setpoints/roll") + + assert res.status_code == 200 + data = res.get_json() + assert data["remaining"] == {"pitch": 20.0, "yaw": 30.0} + assert override.clear_count == 1 + assert override.sent[-1] == {"pitch": 20.0, "yaw": 30.0} + + +def test_pid_gains_force_translation_axes_to_zero(monkeypatch): + captured = {} + + def fake_send_pid_gains(gains, timeout=1.0, max_retries=3): + captured["gains"] = gains + return gains, 1 + + monkeypatch.setattr(routes, "send_pid_gains", fake_send_pid_gains) + client, _ctrl, _override = make_client() + + res = client.post( + "/api/pid/gains", + json={ + "surge": {"kp": 9, "ki": 9, "kd": 9}, + "roll": {"kp": 1.0, "ki": 2.0, "kd": 3.0}, + }, + ) + + assert res.status_code == 200 + assert captured["gains"]["surge"] == {"kp": 0.0, "ki": 0.0, "kd": 0.0} + assert captured["gains"]["sway"] == {"kp": 0.0, "ki": 0.0, "kd": 0.0} + assert captured["gains"]["heave"] == {"kp": 0.0, "ki": 0.0, "kd": 0.0} + assert captured["gains"]["roll"] == {"kp": 1.0, "ki": 2.0, "kd": 3.0} + assert set(res.get_json()["gains"].keys()) == {"roll", "pitch", "yaw"} From ef026dbae7505761940ff01e8db020377a61cd54 Mon Sep 17 00:00:00 2001 From: Tomas Aas-Hansen Date: Sun, 14 Jun 2026 18:07:20 +0200 Subject: [PATCH 2/3] No new features or fixes. Committing config settings and PID values. --- data/config.json | 10 +++--- data/data.json | 26 +++++++------- data/pid_configs.json | 82 +++++++++++++------------------------------ 3 files changed, 43 insertions(+), 75 deletions(-) diff --git a/data/config.json b/data/config.json index 1cb47b8..b9cb54a 100644 --- a/data/config.json +++ b/data/config.json @@ -5,9 +5,9 @@ "z": -50.0 }, "imu_axes": { - "yaw": "+yaw", - "pitch": "+pitch", - "roll": "+roll" + "yaw": "-yaw", + "pitch": "-pitch", + "roll": "-roll" }, "accel_axes": { "x": "+x", @@ -24,8 +24,8 @@ ] }, "pid_setpoint_rates": { - "roll": 90.0, - "pitch": 90.0, + "roll": 20.0, + "pitch": 20.0, "yaw": 90.0 } } \ No newline at end of file diff --git a/data/data.json b/data/data.json index 587e95e..892e4bb 100644 --- a/data/data.json +++ b/data/data.json @@ -1,14 +1,14 @@ { "imu": { - "yaw": -123.53, - "pitch": 1.98, - "roll": -174.45, - "yr": -0.01, - "pr": -0.0, - "rr": 0.14, - "ax": 0.001, - "ay": 0.001, - "az": 0.0 + "yaw": -54.67, + "pitch": -1.79, + "roll": -179.41, + "yr": -0.06, + "pr": 0.0, + "rr": -0.03, + "ax": -0.037, + "ay": 0.015, + "az": 0.022 }, "9dof": { "acceleration": { @@ -74,14 +74,14 @@ "dptSet": 0.0 }, "resources": { - "sequence": 3232, - "uptime_ms": 3241146, - "cpu_percent": 5, + "sequence": 5010, + "uptime_ms": 5023099, + "cpu_percent": 6, "heap_used_percent": 2, "heap_free_kb": 502, "heap_total_kb": 512, "thread_count": 20, - "udp_rx_count": 40626, + "udp_rx_count": 103352, "udp_rx_errors": 0 }, "control_telemetry": { diff --git a/data/pid_configs.json b/data/pid_configs.json index 652a556..5501599 100644 --- a/data/pid_configs.json +++ b/data/pid_configs.json @@ -1,37 +1,5 @@ { - "rollPitchRough": { - "surge": { - "kp": 0, - "ki": 0, - "kd": 0 - }, - "sway": { - "kp": 0, - "ki": 0, - "kd": 0 - }, - "heave": { - "kp": 0, - "ki": 0, - "kd": 0 - }, - "roll": { - "kp": 0.035, - "ki": 0, - "kd": 0.0015 - }, - "pitch": { - "kp": 0.05, - "ki": 0, - "kd": 0.001 - }, - "yaw": { - "kp": 0, - "ki": 0, - "kd": 0 - } - }, - "rollPitchRough2": { + "rollPitchYawRough": { "surge": { "kp": 0, "ki": 0, @@ -58,12 +26,12 @@ "kd": 0.001 }, "yaw": { - "kp": 0, + "kp": -0.05, "ki": 0, "kd": 0 } }, - "rollPitchYawRough": { + "rollPitchYawPrecise": { "surge": { "kp": 0, "ki": 0, @@ -82,49 +50,49 @@ "roll": { "kp": 0.03, "ki": 0, - "kd": 0.0015 + "kd": 0.003 }, "pitch": { "kp": 0.045, "ki": 0, - "kd": 0.001 + "kd": 0.006 }, "yaw": { - "kp": -0.05, + "kp": -0.15, "ki": 0, "kd": 0 } }, - "rollPitchYawPrecise": { + "manipulator-rough": { "surge": { - "kp": 0, - "ki": 0, - "kd": 0 + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 }, "sway": { - "kp": 0, - "ki": 0, - "kd": 0 + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 }, "heave": { - "kp": 0, - "ki": 0, - "kd": 0 + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 }, "roll": { - "kp": 0.03, - "ki": 0, - "kd": 0.003 + "kp": 0.012, + "ki": 0.0, + "kd": 0.0 }, "pitch": { - "kp": 0.045, - "ki": 0, - "kd": 0.006 + "kp": 0.012, + "ki": 0.0, + "kd": 0.0 }, "yaw": { - "kp": -0.15, - "ki": 0, - "kd": 0 + "kp": 0.008, + "ki": 0.0, + "kd": 0.0075 } } } \ No newline at end of file From 8522250297f7be232e9f7b2e75a9c952611a5dc0 Mon Sep 17 00:00:00 2001 From: Tomas Aas-Hansen Date: Sun, 14 Jun 2026 18:52:43 +0200 Subject: [PATCH 3/3] Codex added fixed Nucleo contact proof in Topside tab 'Connection' as an end-to-end solution on both Topside and zephyr branches. --- data/data.json | 28 +++--- lib/bitmask.py | 18 +++- lib/control_telemetry.py | 165 +++++++++++++++++++++++++------ lib/log_udp_receiver.py | 71 ++++++++++--- lib/resource_receiver.py | 11 +++ routes.py | 58 ++++++++++- static/js/connection.js | 71 +++++++++---- static/js/debug.js | 94 ------------------ static/js/pid_tuning.js | 61 +++++++++++- static/templates/connection.html | 38 ++++--- static/templates/debug.html | 41 +------- static/templates/pid_tuning.html | 7 +- tests/test_protocols.py | 47 +++++++++ 13 files changed, 473 insertions(+), 237 deletions(-) diff --git a/data/data.json b/data/data.json index 892e4bb..98b77d9 100644 --- a/data/data.json +++ b/data/data.json @@ -1,14 +1,14 @@ { "imu": { - "yaw": -54.67, - "pitch": -1.79, - "roll": -179.41, - "yr": -0.06, - "pr": 0.0, - "rr": -0.03, - "ax": -0.037, - "ay": 0.015, - "az": 0.022 + "yaw": -67.08, + "pitch": -1.1, + "roll": 179.3, + "yr": 0.18, + "pr": 0.15, + "rr": 0.3, + "ax": 0.085, + "ay": -0.004, + "az": -0.042 }, "9dof": { "acceleration": { @@ -74,14 +74,14 @@ "dptSet": 0.0 }, "resources": { - "sequence": 5010, - "uptime_ms": 5023099, - "cpu_percent": 6, + "sequence": 49, + "uptime_ms": 50908, + "cpu_percent": 5, "heap_used_percent": 2, - "heap_free_kb": 502, + "heap_free_kb": 501, "heap_total_kb": 512, "thread_count": 20, - "udp_rx_count": 103352, + "udp_rx_count": 900, "udp_rx_errors": 0 }, "control_telemetry": { diff --git a/lib/bitmask.py b/lib/bitmask.py index eea00d1..ed3c19e 100644 --- a/lib/bitmask.py +++ b/lib/bitmask.py @@ -77,6 +77,7 @@ def __init__(self, host=NUCLEO_HOST, port=NUCLEO_PORT, rate_hz=DEFAULT_RATE_HZ, self._last_ack_count = None self._watchdog_timeout = watchdog_timeout self._watchdog_resends = 0 + self._last_watchdog_resend_time = 0.0 self._last_command_snapshot: dict | None = None def start(self): @@ -85,7 +86,8 @@ def start(self): self._sender = UdpSender(self.host, self.port) self._stop.clear() with self._status_lock: - self._last_ack_time = time.monotonic() + self._last_ack_time = 0.0 + self._last_ack_count = None self._thread.start() self._watchdog_thread.start() @@ -117,6 +119,11 @@ def get_uplink_status(self) -> dict: now = time.monotonic() send_age = None if self._last_send_time == 0 else max(0.0, now - self._last_send_time) * 1000.0 ack_age = None if self._last_ack_time == 0 else max(0.0, now - self._last_ack_time) * 1000.0 + resend_age = ( + None + if self._last_watchdog_resend_time == 0 + else max(0.0, now - self._last_watchdog_resend_time) * 1000.0 + ) return { "sequence": self._seq, "last_send_age_ms": send_age, @@ -125,6 +132,7 @@ def get_uplink_status(self) -> dict: "last_ack_count": self._last_ack_count, "watchdog_timeout": self._watchdog_timeout, "watchdog_resends": self._watchdog_resends, + "last_watchdog_resend_age_ms": resend_age, "last_command": self._last_command_snapshot or {}, "last_packet_hex": self._last_packet.hex() if self._last_packet else None, } @@ -181,20 +189,24 @@ def _watchdog_loop(self): udp_rx_count, _errors = counters() now = time.monotonic() with self._status_lock: + if self._last_ack_count is None: + self._last_ack_count = udp_rx_count + continue if udp_rx_count != self._last_ack_count: self._last_ack_count = udp_rx_count self._last_ack_time = now continue # No new acks yet - if self._last_packet and (now - self._last_ack_time) > self._watchdog_timeout: + resend_due = (now - self._last_watchdog_resend_time) > self._watchdog_timeout + if self._last_packet and (now - self._last_ack_time) > self._watchdog_timeout and resend_due: sender = self._sender if sender: try: sender.send(self._last_packet) self._watchdog_resends += 1 + self._last_watchdog_resend_time = now except Exception: pass - self._last_ack_time = now # simple initializer diff --git a/lib/control_telemetry.py b/lib/control_telemetry.py index d68d293..e95942a 100644 --- a/lib/control_telemetry.py +++ b/lib/control_telemetry.py @@ -1,16 +1,9 @@ """Control loop telemetry receiver. Consumes the packed struct emitted by ``control_telemetry.c`` and exposes the -latest setpoint/output/error triplets for each axis so the debug UI can render -10 Hz charts. Packets are structured as:: - - sequence (u32 big-endian) - 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) +latest setpoint, measurement, output, error, gain, and input values for each +axis. The decoder accepts the older compact packet too, so Topside can still +show partial telemetry if the MCU has not been flashed yet. The CRC covers the bytes up to but excluding the CRC field. """ @@ -31,11 +24,20 @@ CONTROL_TELEM_PORT = 5005 AXES = ["surge", "sway", "heave", "roll", "pitch", "yaw"] -FLOAT_COUNT = len(AXES) * 3 -PACKET_SIZE = 4 + FLOAT_COUNT * 4 + struct.calcsize(" None: @@ -86,42 +92,145 @@ def get_history(self, limit: int = 120) -> List[dict]: hist_list = list(self._history) return hist_list[-limit:] + def get_stats(self) -> dict: + with self._lock: + latest = self._latest.copy() + last_ts = latest.get("timestamp") + age_ms = None if last_ts is None else max(0.0, (time.time() - last_ts) * 1000.0) + return { + "packet_count": self._packet_count, + "crc_errors": self._crc_errors, + "invalid_packets": self._invalid_packets, + "last_sequence": latest.get("sequence"), + "last_age_ms": age_ms, + "last_addr": list(self._last_addr) if self._last_addr else None, + "protocol_version": latest.get("protocol_version"), + } + # 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, NEW_PACKET_SIZE): + with self._lock: + self._invalid_packets += 1 + print( + "Control telemetry: invalid packet size from " + f"{addr}: {len(data)} bytes (expected {OLD_PACKET_SIZE} or {NEW_PACKET_SIZE})" + ) return body = data[:-4] crc = struct.unpack("!I", data[-4:])[0] calc = crc32_ieee(body) if calc != crc: + with self._lock: + self._crc_errors += 1 print(f"Control telemetry: CRC mismatch (calc=0x{calc:08X}, recv=0x{crc:08X})") return + if len(data) == NEW_PACKET_SIZE: + snapshot = self._decode_v2(body) + else: + snapshot = self._decode_v1(body) + snapshot["timestamp"] = time.time() + snapshot["source"] = {"host": addr[0], "port": addr[1]} + with self._lock: + self._packet_count += 1 + self._last_addr = addr + self._latest = snapshot + self._history.append(snapshot) + try: + self.data_handler.update_data({"control_telemetry": snapshot}) + except Exception as exc: + print(f"Control telemetry: failed to persist snapshot: {exc}") + if self._capture_enabled: + self._append_log(snapshot) + + def _decode_v1(self, body: bytes) -> dict: sequence = struct.unpack("!I", body[:4])[0] - float_end = 4 + FLOAT_COUNT * 4 - floats = struct.unpack("<" + "f" * FLOAT_COUNT, body[4:float_end]) + float_end = 4 + OLD_FLOAT_COUNT * 4 + floats = struct.unpack("<" + "f" * OLD_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(" dict: + sequence, mcu_uptime_ms, last_command_age_ms = struct.unpack("!III", body[:12]) + meta_start = 12 + meta_end = meta_start + NEW_META_SIZE + flags, override_mask, pid_active_mask, *rest = struct.unpack(NEW_META_FORMAT, body[meta_start:meta_end]) + pilot_values = rest[:6] + light = rest[6] + manipulator_command = rest[7] + float_start = meta_end + float_end = float_start + NEW_FLOAT_COUNT * 4 + floats = struct.unpack("<" + "f" * NEW_FLOAT_COUNT, body[float_start:float_end]) + + idx = 0 + setpoints = dict(zip(AXES, floats[idx : idx + 6])) + idx += 6 + measurements = dict(zip(AXES, floats[idx : idx + 6])) + idx += 6 + outputs = dict(zip(AXES, floats[idx : idx + 6])) + idx += 6 + errors = dict(zip(AXES, floats[idx : idx + 6])) + idx += 6 + gain_values = floats[idx : idx + 18] + gains = {} + for axis_index, axis in enumerate(AXES): + base = axis_index * 3 + gains[axis] = { + "kp": round(gain_values[base], 6), + "ki": round(gain_values[base + 1], 6), + "kd": round(gain_values[base + 2], 6), + } + + manip_deg, manip_pulse_us = struct.unpack(" None: try: diff --git a/lib/log_udp_receiver.py b/lib/log_udp_receiver.py index bccfbd7..a1a8707 100644 --- a/lib/log_udp_receiver.py +++ b/lib/log_udp_receiver.py @@ -21,7 +21,19 @@ LOG_PORT = 5006 LOG_DIR = logs_dir() LOG_FILE = log_path("zephyr.log") -SEVERITY_RE = re.compile(r"^\[(?P[IWRD])\]\s*") +LEGACY_SEVERITY_RE = re.compile(r"^\[(?P[IWERD])\]\s*") +ZEPHYR_SEVERITY_RE = re.compile(r"<(?Pinf|wrn|err|dbg)>") +LEVEL_MAP = { + "I": "I", + "W": "W", + "E": "E", + "R": "E", + "D": "D", + "inf": "I", + "wrn": "W", + "err": "E", + "dbg": "D", +} class LogStreamReceiver: @@ -32,6 +44,10 @@ def __init__(self, host: str = "0.0.0.0", port: int = LOG_PORT, max_entries: int self._listener: UdpListener | None = None self._buffer: List[dict] = [] self._lock = threading.Lock() + self._packet_count = 0 + self._decode_errors = 0 + self._last_ts = None + self._last_addr = None LOG_DIR.mkdir(parents=True, exist_ok=True) def start(self) -> None: @@ -52,27 +68,56 @@ def get_recent(self, limit: int = 100) -> List[dict]: with self._lock: return list(self._buffer[-limit:]) + def get_stats(self) -> dict: + with self._lock: + age_ms = None if self._last_ts is None else max(0.0, (time.time() - self._last_ts) * 1000.0) + return { + "packet_count": self._packet_count, + "decode_errors": self._decode_errors, + "last_age_ms": age_ms, + "last_addr": list(self._last_addr) if self._last_addr else None, + "log_file": str(LOG_FILE), + } + def _handle_packet(self, data: bytes, addr: tuple[str, int]): try: text = data.decode("utf-8", errors="replace").rstrip("\r\n") except Exception as exc: + with self._lock: + self._decode_errors += 1 print(f"Log stream: decode error from {addr}: {exc}") return - level = "I" - match = SEVERITY_RE.match(text) - if match: - level = match.group("level") - text = text[match.end() :] - entry = { - "ts": time.time(), - "level": level, - "message": text, - } + now = time.time() + entries = [] + for line in (part for part in text.splitlines() if part.strip()): + entries.append(self._build_entry(line, now)) + if not entries: + return with self._lock: - self._buffer.append(entry) + self._packet_count += 1 + self._last_ts = now + self._last_addr = addr + self._buffer.extend(entries) if len(self._buffer) > self.max_entries: self._buffer = self._buffer[-self.max_entries :] - self._append_log(entry) + for entry in entries: + self._append_log(entry) + + def _build_entry(self, text: str, now: float) -> dict: + level = "I" + legacy = LEGACY_SEVERITY_RE.match(text) + if legacy: + level = LEVEL_MAP.get(legacy.group("level"), "I") + text = text[legacy.end() :] + else: + zephyr = ZEPHYR_SEVERITY_RE.search(text) + if zephyr: + level = LEVEL_MAP.get(zephyr.group("level"), "I") + return { + "ts": now, + "level": level, + "message": text.strip(), + } def _append_log(self, entry: dict) -> None: try: diff --git a/lib/resource_receiver.py b/lib/resource_receiver.py index d748a52..394459d 100644 --- a/lib/resource_receiver.py +++ b/lib/resource_receiver.py @@ -64,6 +64,8 @@ def __init__(self, host=UDP_IP, port=UDP_PORT, data_handler=None): self._last_seq = None self._packets_lost = 0 self._last_data = {} + self._last_received_ts = None + self._last_addr = None self._last_diag_log = 0.0 LOG_DIR.mkdir(parents=True, exist_ok=True) @@ -89,12 +91,19 @@ def stop(self): def get_stats(self) -> dict: """Get receiver statistics.""" with self._lock: + age_ms = ( + None + if self._last_received_ts is None + else max(0.0, (time.time() - self._last_received_ts) * 1000.0) + ) return { "packet_count": self._packet_count, "crc_errors": self._crc_errors, "packets_lost": self._packets_lost, "last_seq": self._last_seq, "last_data": self._last_data.copy(), + "last_age_ms": age_ms, + "last_addr": list(self._last_addr) if self._last_addr else None, } def _process_packet(self, data: bytes, addr: tuple): @@ -160,6 +169,8 @@ def _process_packet(self, data: bytes, addr: tuple): self._last_seq = sequence self._last_data = telemetry.copy() + self._last_received_ts = time.time() + self._last_addr = addr # Update data.json with new resource values try: diff --git a/routes.py b/routes.py index 317fb50..2ecf7ca 100644 --- a/routes.py +++ b/routes.py @@ -200,6 +200,53 @@ def _git_info(): return {"branch": branch} +def _live_from_age(age_ms, max_age_ms): + return age_ms is not None and age_ms <= max_age_ms + + +def _connection_proof_payload(): + bm = current_app.config.get("BITMASK") + resource = current_app.config.get("RESOURCE") + imu = current_app.config.get("IMU") + + uplink = bm.get_uplink_status() if bm else {} + resource_stats = resource.get_stats() if resource and hasattr(resource, "get_stats") else {} + imu_stats = imu.get_stats() if imu and hasattr(imu, "get_stats") else {} + + proofs = [ + { + "name": "Command ACK", + "active": _live_from_age(uplink.get("last_ack_age_ms"), 2500), + "age_ms": uplink.get("last_ack_age_ms"), + "detail": "Nucleo UDP counter changed after Topside command packets", + }, + { + "name": "Resource telemetry", + "active": _live_from_age(resource_stats.get("last_age_ms"), 2500), + "age_ms": resource_stats.get("last_age_ms"), + "detail": "Nucleo resource packet on UDP 12346", + }, + { + "name": "IMU telemetry", + "active": _live_from_age(imu_stats.get("age_ms"), 1200), + "age_ms": imu_stats.get("age_ms"), + "detail": "Nucleo IMU packet on UDP 5002", + }, + ] + connected = any(proof["active"] for proof in proofs) + status = "live" if connected else "offline" + return { + "ok": True, + "connected": connected, + "status": status, + "generated_at": time.time(), + "proofs": proofs, + "uplink": uplink, + "resource": resource_stats, + "imu": imu_stats, + } + + def _neutralize_thruster_command(): """Force topside manual command output to neutral axes.""" neutral = _neutral_axis_values() @@ -662,6 +709,11 @@ def get_command_status(): } ) + @app.route("/api/connection/status", methods=["GET"]) + def connection_status(): + """Return live Nucleo contact proof from all receiver paths.""" + return jsonify(_connection_proof_payload()) + @app.route("/api/control/state", methods=["GET"]) def control_state(): ctrl = current_app.config.get("CONTROLLER") @@ -711,7 +763,8 @@ def control_rearm(): def control_telemetry(): receiver = current_app.config.get("CONTROL_TELEM") latest = receiver.get_latest() if receiver else data_handler.get_section("control_telemetry") - return jsonify({"ok": bool(latest), "telemetry": latest or {}}) + stats = receiver.get_stats() if receiver and hasattr(receiver, "get_stats") else {} + return jsonify({"ok": bool(latest), "telemetry": latest or {}, "stats": stats}) @app.route("/api/control/telemetry/history", methods=["GET"]) def control_telemetry_history(): @@ -728,7 +781,8 @@ def live_logs(): limit = max(1, min(500, limit)) log_stream = current_app.config.get("LOG_STREAM") entries = log_stream.get_recent(limit) if log_stream else [] - return jsonify({"ok": True, "logs": entries}) + stats = log_stream.get_stats() if log_stream and hasattr(log_stream, "get_stats") else {} + return jsonify({"ok": True, "logs": entries, "stats": stats}) @app.route("/api/system/reset", methods=["POST"]) def system_reset(): diff --git a/static/js/connection.js b/static/js/connection.js index c29a390..27b43e5 100644 --- a/static/js/connection.js +++ b/static/js/connection.js @@ -1,8 +1,9 @@ (function () { const badge = document.getElementById("nucleo-contact-badge"); const lastAck = document.getElementById("nucleo-last-ack"); - const udpRx = document.getElementById("nucleo-udp-rx"); - const udpErrors = document.getElementById("nucleo-udp-errors"); + const bestProof = document.getElementById("nucleo-best-proof"); + const imuAge = document.getElementById("nucleo-imu-age"); + const proofBody = document.getElementById("nucleo-proof-body"); const details = document.getElementById("nucleo-link-details"); const resetBtn = document.getElementById("btn-system-reset"); const resetStatus = document.getElementById("system-reset-status"); @@ -11,50 +12,80 @@ return value == null ? "--" : String(Math.round(value)); } - function setBadge(ackAge) { + function setBadge(connected, bestAgeMs) { if (!badge) return; - if (ackAge == null) { - badge.textContent = "NO ACK"; + if (!connected) { + badge.textContent = "NO LIVE PROOF"; badge.className = "badge bg-secondary"; - } else if (ackAge < 1000) { + } else if (bestAgeMs != null && bestAgeMs < 1000) { badge.textContent = "LIVE"; badge.className = "badge bg-success"; - } else if (ackAge < 3000) { + } else if (bestAgeMs != null && bestAgeMs < 3000) { badge.textContent = "STALE"; badge.className = "badge bg-warning text-dark"; } else { - badge.textContent = "DEGRADED"; - badge.className = "badge bg-danger"; + badge.textContent = "LIVE, SLOW"; + badge.className = "badge bg-warning text-dark"; } } + function renderProofs(proofs) { + if (!proofBody) return; + const frag = document.createDocumentFragment(); + (proofs || []).forEach((proof) => { + const tr = document.createElement("tr"); + const status = document.createElement("span"); + status.className = "badge " + (proof.active ? "bg-success" : "bg-secondary"); + status.textContent = proof.active ? "LIVE" : "STALE"; + [proof.name || "--", status, proof.age_ms == null ? "--" : fmt(proof.age_ms) + " ms", proof.detail || ""].forEach( + (value) => { + const td = document.createElement("td"); + if (value instanceof HTMLElement) td.appendChild(value); + else td.textContent = value; + tr.appendChild(td); + } + ); + frag.appendChild(tr); + }); + proofBody.innerHTML = ""; + proofBody.appendChild(frag); + } + + function minAge(proofs) { + const liveAges = (proofs || []) + .filter((proof) => proof.active && proof.age_ms != null) + .map((proof) => Number(proof.age_ms)) + .filter((value) => Number.isFinite(value)); + return liveAges.length ? Math.min(...liveAges) : null; + } + async function pollConnection() { try { - const res = await fetch("/api/command/status", { cache: "no-store" }); + const res = await fetch("/api/connection/status", { cache: "no-store" }); const data = await res.json(); if (!data.ok) return; const uplink = data.uplink || {}; - const ackAge = uplink.last_ack_age_ms; - setBadge(ackAge); - if (lastAck) lastAck.textContent = ackAge == null ? "--" : fmt(ackAge) + " ms"; - if (udpRx) udpRx.textContent = data.udp_rx_count == null ? "--" : String(data.udp_rx_count); - if (udpErrors) udpErrors.textContent = data.udp_rx_errors == null ? "--" : String(data.udp_rx_errors); + const imu = data.imu || {}; + const bestAgeMs = minAge(data.proofs); + setBadge(data.connected === true, bestAgeMs); + renderProofs(data.proofs); + if (bestProof) bestProof.textContent = bestAgeMs == null ? "--" : fmt(bestAgeMs) + " ms"; + if (lastAck) lastAck.textContent = uplink.last_ack_age_ms == null ? "--" : fmt(uplink.last_ack_age_ms) + " ms"; + if (imuAge) imuAge.textContent = imu.age_ms == null ? "--" : fmt(imu.age_ms) + " ms"; if (details) { details.textContent = JSON.stringify( { uplink, - resource: { - udp_rx_count: data.udp_rx_count, - udp_rx_errors: data.udp_rx_errors, - }, + resource: data.resource, + imu: data.imu, }, null, 2 ); } } catch (_) { - setBadge(null); + setBadge(false, null); if (details) details.textContent = "Connection status unavailable"; } } diff --git a/static/js/debug.js b/static/js/debug.js index a6767b4..f382fcc 100644 --- a/static/js/debug.js +++ b/static/js/debug.js @@ -11,9 +11,6 @@ const btnStopAll = document.getElementById("btn-stop-all"); const statusBadge = document.getElementById("debug-status"); const rovStatus = document.getElementById("rov-status"); - const telemetryBody = document.getElementById("control-telemetry-body"); - const telemetryAge = document.getElementById("control-telemetry-age"); - const logStreamEl = document.getElementById("log-stream"); function slider(axis) { return document.getElementById("slider-" + axis); @@ -120,93 +117,6 @@ } } - function fmtMs(value) { - if (value == null || Number.isNaN(value)) return "--"; - return value.toFixed(0); - } - - function fmtFloat(value) { - if (value == null || Number.isNaN(value)) return "NaN"; - return Number(value).toFixed(2); - } - - function renderTelemetryTable(snapshot) { - if (!telemetryBody) return; - const setpoint = (snapshot && snapshot.setpoint) || {}; - const output = (snapshot && snapshot.output) || {}; - const error = (snapshot && snapshot.error) || {}; - const frag = document.createDocumentFragment(); - AXES.forEach((axis) => { - const tr = document.createElement("tr"); - [axis.toUpperCase(), fmtFloat(setpoint[axis]), fmtFloat(output[axis]), fmtFloat(error[axis])].forEach((text) => { - const td = document.createElement("td"); - td.textContent = text; - tr.appendChild(td); - }); - frag.appendChild(tr); - }); - telemetryBody.innerHTML = ""; - telemetryBody.appendChild(frag); - } - - async function pollControlTelemetry() { - try { - const res = await fetch("/api/control/telemetry", { cache: "no-store" }); - const data = await res.json(); - if (!data.ok) return; - const snapshot = data.telemetry || {}; - renderTelemetryTable(snapshot); - if (telemetryAge) { - const ageMs = snapshot.timestamp ? Math.max(0, Date.now() - snapshot.timestamp * 1000) : null; - telemetryAge.textContent = fmtMs(ageMs); - } - } catch (error) { - console.error("control telemetry poll failed", error); - } - } - - function renderLogs(entries) { - if (!logStreamEl) return; - const frag = document.createDocumentFragment(); - entries.forEach((entry) => { - const row = document.createElement("div"); - row.className = "d-flex justify-content-between border-bottom border-secondary py-1"; - - const body = document.createElement("div"); - const badge = document.createElement("span"); - const level = entry.level || "I"; - const levelMap = { I: "bg-info text-dark", W: "bg-warning text-dark", R: "bg-danger", D: "bg-secondary" }; - badge.className = "badge me-2 " + (levelMap[level] || "bg-secondary"); - badge.textContent = level; - body.appendChild(badge); - - const msg = document.createElement("span"); - msg.textContent = entry.message || ""; - body.appendChild(msg); - - const ts = document.createElement("span"); - ts.className = "text-light-muted small ms-3"; - ts.textContent = (entry.ts ? new Date(entry.ts * 1000) : new Date()).toLocaleTimeString(); - - row.appendChild(body); - row.appendChild(ts); - frag.appendChild(row); - }); - logStreamEl.innerHTML = ""; - logStreamEl.appendChild(frag); - logStreamEl.scrollTop = logStreamEl.scrollHeight; - } - - async function pollLogs() { - try { - const res = await fetch("/api/logs/live?limit=50", { cache: "no-store" }); - const data = await res.json(); - if (data.ok) renderLogs(data.logs || []); - } catch (error) { - console.error("log stream poll failed", error); - } - } - if (btnEnable) btnEnable.addEventListener("click", enableOverride); if (btnDisable) btnDisable.addEventListener("click", stopAll); if (btnResetAll) btnResetAll.addEventListener("click", () => AXES.forEach(resetSlider)); @@ -222,8 +132,4 @@ pollStatus(); setInterval(pollStatus, 500); - pollControlTelemetry(); - setInterval(pollControlTelemetry, 500); - pollLogs(); - setInterval(pollLogs, 1500); })(); diff --git a/static/js/pid_tuning.js b/static/js/pid_tuning.js index 6f9308f..7975844 100644 --- a/static/js/pid_tuning.js +++ b/static/js/pid_tuning.js @@ -150,11 +150,49 @@ return fromTelemetry; } + function bitForAxis(axis) { + const index = AXES.indexOf(axis); + return index < 0 ? 0 : 1 << index; + } + + function getAxisMode(axis) { + if (!latestTelemetry) return "--"; + const flags = latestTelemetry.flags || {}; + if (flags.timeout) return "TIMEOUT"; + const bit = bitForAxis(axis); + if ((latestTelemetry.override_mask || 0) & bit) return "OVR"; + if ((latestTelemetry.pid_active_mask || 0) & bit) return "PID"; + return latestTelemetry.protocol_version === 1 ? "LEGACY" : "PASS"; + } + + function getAxisMeasurement(axis) { + const fromTelemetry = latestTelemetry && latestTelemetry.measurement ? Number(latestTelemetry.measurement[axis]) : NaN; + if (Number.isFinite(fromTelemetry)) return fromTelemetry; + return Number(latestImu[axis]); + } + + function getAxisError(axis, setpoint, position) { + const fromTelemetry = latestTelemetry && latestTelemetry.error ? Number(latestTelemetry.error[axis]) : NaN; + if (Number.isFinite(fromTelemetry)) return fromTelemetry; + return axisError(axis, setpoint, position); + } + + function getAxisOutput(axis) { + const value = latestTelemetry && latestTelemetry.output ? Number(latestTelemetry.output[axis]) : NaN; + return Number.isFinite(value) ? value : NaN; + } + + function getAxisGains(axis) { + const gains = latestTelemetry && latestTelemetry.gains ? latestTelemetry.gains[axis] : null; + if (!gains) return "--"; + return "P " + fmt(Number(gains.kp), 2) + " I " + fmt(Number(gains.ki), 2) + " D " + fmt(Number(gains.kd), 2); + } + function updateAxisReadouts() { ROT_AXES.forEach((axis) => { const setpoint = getTelemetrySetpoint(axis); - const position = Number(latestImu[axis]); - const error = axisError(axis, setpoint, position); + const position = getAxisMeasurement(axis); + const error = getAxisError(axis, setpoint, position); const positionEl = document.getElementById("readout-" + axis + "-position"); const setpointEl = document.getElementById("readout-" + axis + "-setpoint"); const errorEl = document.getElementById("readout-" + axis + "-error"); @@ -176,20 +214,28 @@ const frag = document.createDocumentFragment(); ROT_AXES.forEach((axis) => { const setpoint = getTelemetrySetpoint(axis); - const position = Number(latestImu[axis]); - const error = axisError(axis, setpoint, position); + const position = getAxisMeasurement(axis); + const error = getAxisError(axis, setpoint, position); + const output = getAxisOutput(axis); const tr = document.createElement("tr"); const tdAxis = document.createElement("td"); + const tdMode = document.createElement("td"); const tdSet = document.createElement("td"); const tdPos = document.createElement("td"); const tdErr = document.createElement("td"); + const tdOut = document.createElement("td"); + const tdGains = document.createElement("td"); tdAxis.textContent = axis.toUpperCase(); + tdMode.textContent = getAxisMode(axis); tdSet.textContent = fmt(setpoint, 2); tdPos.textContent = fmt(position, 2); tdErr.textContent = fmt(error, 2); + tdOut.textContent = fmt(output, 2); + tdGains.textContent = getAxisGains(axis); if (isFiniteNumber(error) && Math.abs(error) > 10) tdErr.className = "text-warning"; if (isFiniteNumber(error) && Math.abs(error) > 25) tdErr.className = "text-danger"; - tr.append(tdAxis, tdSet, tdPos, tdErr); + if (latestTelemetry && latestTelemetry.flags && latestTelemetry.flags.timeout) tr.className = "table-danger"; + tr.append(tdAxis, tdMode, tdSet, tdPos, tdErr, tdOut, tdGains); frag.appendChild(tr); }); telemetryBody.innerHTML = ""; @@ -200,6 +246,7 @@ if (!telemetryAge) return; const ageMs = snapshot && snapshot.timestamp ? Math.max(0, Date.now() - snapshot.timestamp * 1000) : null; if (ageMs == null) setBadge(telemetryAge, "NO TELEMETRY", "bg-secondary"); + else if (snapshot.flags && snapshot.flags.timeout) setBadge(telemetryAge, "NUCLEO TIMEOUT", "bg-danger"); else if (ageMs < 750) setBadge(telemetryAge, ageMs.toFixed(0) + " ms", "bg-success"); else if (ageMs < 2500) setBadge(telemetryAge, ageMs.toFixed(0) + " ms", "bg-warning text-dark"); else setBadge(telemetryAge, "STALE", "bg-danger"); @@ -577,7 +624,11 @@ pid_enabled: control.pid_enabled, active_setpoints: control.pid_setpoints, manual_command_before_pid: control.manual_command_before_pid, + mcu_flags: latestTelemetry && latestTelemetry.flags ? latestTelemetry.flags : {}, + mcu_command_age_ms: latestTelemetry ? latestTelemetry.last_command_age_ms : null, + mcu_measurement: latestTelemetry && latestTelemetry.measurement ? latestTelemetry.measurement : {}, pid_output: latestTelemetry && latestTelemetry.output ? latestTelemetry.output : {}, + pid_gains_mcu: latestTelemetry && latestTelemetry.gains ? latestTelemetry.gains : {}, final_topside_command: data.command, raw_payload: uplink.last_packet_hex, timestamp: uplink.last_send_timestamp, diff --git a/static/templates/connection.html b/static/templates/connection.html index a64d69f..230430e 100644 --- a/static/templates/connection.html +++ b/static/templates/connection.html @@ -4,7 +4,7 @@ {% block content %}
-
+
@@ -14,37 +14,43 @@
Nucleo Contact Proof
-
Last Ack
-
--
+
Best Proof Age
+
--
-
UDP RX
-
--
+
Command Ack
+
--
-
RX Errors
-
--
+
IMU Telemetry
+
--
+
+ + + + + + + + + + + + +
Proof SourceStatusAgeDetail
Waiting for status...
+ 
Loading...
-
-
-
-
ESC Proof
- UNAVAILABLE -

Real ESC telemetry is not available yet.

-
-
-
diff --git a/static/templates/debug.html b/static/templates/debug.html index e720d6b..cf71106 100644 --- a/static/templates/debug.html +++ b/static/templates/debug.html @@ -39,7 +39,7 @@
Debug Override
-
+
@@ -50,45 +50,6 @@
Topside Sends
-
-
-
-
-
Nucleo Control Telemetry
- Updated -- ms ago -
-
- - - - - - - - - - - - -
AxisSetpointOutputError
Waiting for packets...
-
-
-
-
-
- -
-
-
-
-
-
Zephyr Log Stream
- /api/logs/live -
-
-
-
-
diff --git a/static/templates/pid_tuning.html b/static/templates/pid_tuning.html index 928448a..bee0d8b 100644 --- a/static/templates/pid_tuning.html +++ b/static/templates/pid_tuning.html @@ -183,13 +183,16 @@
Telemetry and Link
Axis + Mode Setpoint - Position + MCU Measure Error + Output + Gains - Waiting for packets... + Waiting for packets...
diff --git a/tests/test_protocols.py b/tests/test_protocols.py index eda3e67..559f58e 100644 --- a/tests/test_protocols.py +++ b/tests/test_protocols.py @@ -107,6 +107,53 @@ def test_control_telemetry_includes_manipulator_fields(monkeypatch, tmp_path): assert latest["manipulator"] == {"deg": -12.5, "pulse_us": 1375} +def test_control_telemetry_v2_decodes_debug_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 = 9 + uptime_ms = 123456 + command_age_ms = 42 + flags = 0x06 + override_mask = 0b001000 + pid_active_mask = 0b111000 + pilot = [0, 1, -2, 64, -64, 127] + light = 80 + manip = -10 + setpoints = [float(i) for i in range(6)] + measurements = [value + 0.1 for value in setpoints] + outputs = [value / 10 for value in setpoints] + errors = [sp - meas for sp, meas in zip(setpoints, measurements)] + gains = [] + for idx in range(6): + gains.extend([idx + 0.1, idx + 0.2, idx + 0.3]) + + body = ( + struct.pack("!III", sequence, uptime_ms, command_age_ms) + + struct.pack(control_telem.NEW_META_FORMAT, flags, override_mask, pid_active_mask, *pilot, light, manip) + + struct.pack("<" + "f" * control_telem.NEW_FLOAT_COUNT, *(setpoints + measurements + outputs + errors + gains)) + + struct.pack("