stsL/argo
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects 1 Releases Wiki Activity

Build Argo V2 control mappings and telemetry dashboard

Browse Source 
Add configurable Xbox motion and action bindings, frame-aware six-axis MAVLink controls, sensor logging, camera viewing and recording, and a faster startup path. Synchronize both app copies and document companion hardware setup and verification limits.
This commit is contained in:
PitresAli
2026-10-22 20:22:05 +02:00
parent 5e91c28f1e
commit 8ccd7689f7
38 changed files with 4295 additions and 1897 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SUB Control/web_ui.py
+192 -12
View File
@@ -8,8 +8,10 @@ import time
import base64
import shutil
import http.server
import socketserver
import urllib.parse
import csv
import uuid
import math
from pathlib import Path
import webview
@@ -17,6 +19,7 @@ from serial.tools import list_ports
from app import Axes, MOTOR_FUNCTIONS, MavlinkWorker, motor_number_for_function
from vehicle_state import VehicleState
from rov_v2 import Telemetry, clean_profile, clean_station, save_json, number
class ControlApi:
@@ -31,9 +34,28 @@ class ControlApi:
self.battery_last_current = None
self.battery_last_current_at = None
self.battery_session_consumed_mah = 0.0
self.battery_fields_at = {}
self.demo_mode = False
self.lock = threading.Lock()
self._poll_lock = threading.Lock()
self.telemetry = Telemetry()
self.station_path = Path(__file__).resolve().parent / "station_settings.json"
self.profiles_path = self.station_path.with_name("controller_profiles.json")
self.station_settings = clean_station({})
self.controller_profiles = {}
try:
self.station_settings = clean_station(json.loads(self.station_path.read_text(encoding="utf-8")))
except (OSError, ValueError, TypeError):
pass
try:
self.controller_profiles = {str(key)[:256]: clean_profile(value) for key, value in
json.loads(self.profiles_path.read_text(encoding="utf-8")).items()}
except (OSError, ValueError, TypeError, AttributeError):
pass
self._recording_lock = threading.Lock()
self._recording = None
self._sensor_file = None
self._sensor_writer = None
self.state = {
"connected": False,
"link": "DISCONNECTED",
@@ -47,6 +69,9 @@ class ControlApi:
"heavy_quality": None,
"log_path": None,
"motor_test_active": False,
"capabilities": {"frame": None, "firmware": None},
"accessories": {},
"sensor_recording": False,
"servos": {str(i): {"function": None, "direction": None, "output_us": None} for i in range(1, 17)},
}
self.servo_functions: dict[int, int] = {}
@@ -62,6 +87,8 @@ class ControlApi:
self.motor_settings = self._clean_motor_settings(saved_motor_settings)
except (OSError, ValueError, TypeError):
pass
if self.station_settings["frame"] != 4:
self.motor_settings["independent"] = False
self.worker.set_motor_scaling(
self.motor_settings["independent"],
{int(i): entry["scale"] for i, entry in self.motor_settings["motors"].items()},
@@ -88,6 +115,7 @@ class ControlApi:
"baud": 57600, "control_hz": 20,
"ping_hz": 1, "system_hz": 1, "battery_hz": 1, "servo_hz": 2,
"rc_hz": 2, "esc_hz": 1,
"attitude_hz": 2, "depth_hz": 2, "imu_hz": 1, "pressure_hz": 1,
}
try:
self.link_config.update(json.loads(self.link_path.read_text(encoding="utf-8")))
@@ -100,6 +128,8 @@ class ControlApi:
"battery_config": self.battery_config,
"link_config": self.link_config,
"motor_settings": self.motor_settings,
"station_settings": self.station_settings,
"controller_profiles": self.controller_profiles,
"state": self._snapshot(),
}
@@ -257,7 +287,12 @@ class ControlApi:
print(f"[web_ui] connect() called: port={port!r}, baud={baud!r}, rate={rate!r}")
if not port:
raise ValueError("Select a USB serial port")
if str(port).lower() in ("demo", "tcp:127.0.0.1:5760", "udpin:127.0.0.1:14550"):
self.telemetry = Telemetry()
self.state["capabilities"] = {"frame": None, "firmware": None}
self.state["accessories"] = {}
self.state["battery"] = None
self.battery_fields_at = {}
if str(port).lower() == "demo":
self.demo_mode = True
self.servo_functions = {1: 33, 2: 34, 3: 35}
self.motor_directions = {1: 1, 2: 1, 3: 1}
@@ -307,6 +342,7 @@ class ControlApi:
raise TimeoutError(f"No MAVLink heartbeat received from {port} within 8 seconds")
def disconnect(self):
self.sensor_record(False)
self.worker.stop_motor_test()
with self.lock:
self.state["motor_test_active"] = False
@@ -358,7 +394,8 @@ class ControlApi:
self.state["status"] = "Motor test released; waiting for flight-controller stop"
return True
def set_axes(self, forward, turn, vertical, sequence=None):
def set_axes(self, forward, turn, vertical, sequence=None, lateral=0, pitch=0, roll=0):
values = [number(value, -1, 1) for value in (forward, lateral, vertical, turn, pitch, roll)]
if self.demo_mode:
with self.lock:
self.state["status"] = (
@@ -368,13 +405,95 @@ class ControlApi:
return True
with self.lock:
allowed = self.vehicle_state.snapshot()["controls_allowed"]
self.worker.set_axes(Axes(
max(-1.0, min(1.0, float(forward))), 0.0,
max(-1.0, min(1.0, float(vertical))),
max(-1.0, min(1.0, float(turn))),
) if allowed else Axes(), sequence)
self.worker.set_axes(Axes(*values) if allowed else Axes(), sequence)
return True
def _require_settings(self):
if self.state.get("connected") and (self.state.get("armed") or
(not self.demo_mode and not self.vehicle_state.snapshot()["armed_known"])):
raise RuntimeError("Disengage first")
def save_controller_profile(self, device, profile):
with self.lock:
self._require_settings()
clean = clean_profile(profile)
updated = {**self.controller_profiles, str(device)[:256]: clean}
save_json(self.profiles_path, updated)
self.controller_profiles = updated
self.worker.set_axes(Axes())
return clean
def save_station_settings(self, config):
with self.lock:
self._require_settings()
clean = clean_station(config)
save_json(self.station_path, clean)
self.station_settings = clean
if clean["frame"] != 4 and (self.demo_mode or not self.state.get("connected")):
self.motor_settings["independent"] = False
self.worker.set_motor_scaling(False, {int(key): item["scale"] for key, item in self.motor_settings["motors"].items()})
# Frame selection describes the preview; actual FRAME_TYPE is read
# from the connected autopilot and never silently overwritten.
return clean
def set_accessory(self, name, enabled):
if name not in ("lights", "camera") or not isinstance(enabled, bool):
raise ValueError("Invalid accessory")
relay = self.station_settings[name + "_relay"]
if relay < 0:
raise RuntimeError("Set relay first")
if self.demo_mode:
self.state["accessories"][name] = {"requested": enabled, "accepted": True, "demo": True}
return True
self.worker.accessory(name, relay, enabled)
with self.lock:
self.state["accessories"][name] = {"requested": enabled, "accepted": None, "at": time.monotonic()}
return True
def sensor_record(self, enabled):
with self.lock:
if enabled and self._sensor_file is None:
folder = Path(__file__).resolve().parent / "logs"
folder.mkdir(exist_ok=True)
self._sensor_file = (folder / f"sensors_{time.time_ns()}.csv").open("w", newline="", encoding="utf-8")
self._sensor_writer = csv.writer(self._sensor_file)
self._sensor_writer.writerow(("monotonic_s", "group", "field", "value"))
elif not enabled and self._sensor_file is not None:
self._sensor_file.close()
self._sensor_file = self._sensor_writer = None
self.state["sensor_recording"] = bool(enabled)
return self._sensor_file.name if self._sensor_file else None
def recording_begin(self):
with self._recording_lock:
if self._recording:
raise RuntimeError("Already recording")
folder = Path(__file__).resolve().parent / "runtime" / "recordings"
folder.mkdir(parents=True, exist_ok=True)
path = folder / f"rov_{time.time_ns()}.webm"
token = uuid.uuid4().hex
self._recording = (token, path.open("wb"), path)
return token
def recording_chunk(self, token, data):
if not isinstance(data, str) or len(data) > 12 * 1024 * 1024:
raise ValueError("Invalid video chunk")
chunk = base64.b64decode(data, validate=True)
with self._recording_lock:
if not self._recording or token != self._recording[0]:
raise ValueError("Recording expired")
self._recording[1].write(chunk)
return True
def recording_end(self, token):
with self._recording_lock:
if not self._recording or token != self._recording[0]:
return None
_, handle, path = self._recording
handle.close()
self._recording = None
return str(path)
def stop_controls(self, sequence=None):
"""Invalidate queued input without claiming that the vehicle disarmed."""
with self.lock:
@@ -412,6 +531,10 @@ class ControlApi:
def save_motor_settings(self, settings, persist=True):
clean = self._clean_motor_settings(settings)
if clean["independent"] != self.motor_settings["independent"]:
self._require_settings()
if clean["independent"] and self.worker.frame_type not in (None, 4):
raise ValueError("Independent scales require SimpleROV 3")
if persist:
self.motor_settings_path.write_text(json.dumps(clean, indent=2) + "\n", encoding="utf-8")
self.motor_settings = clean
@@ -516,6 +639,10 @@ class ControlApi:
"SERVO_OUTPUT_RAW": self.link_config["servo_hz"],
"RC_CHANNELS": self.link_config["rc_hz"],
"ESC_TELEMETRY_1_TO_4": self.link_config["esc_hz"],
"ATTITUDE": self.link_config["attitude_hz"],
"GLOBAL_POSITION_INT": self.link_config["depth_hz"],
"SCALED_IMU": self.link_config["imu_hz"],
"SCALED_PRESSURE2": self.link_config["pressure_hz"],
})
def save_link_settings(self, config):
@@ -527,7 +654,7 @@ class ControlApi:
raise ValueError("Control rate must be 10 or 20 Hz")
if clean["ping_hz"] not in {1, 2, 5, 10}:
raise ValueError("Link test rate must be 1, 2, 5 or 10 Hz")
if any(clean[key] not in allowed for key in ("system_hz", "battery_hz", "servo_hz", "rc_hz", "esc_hz")):
if any(clean[key] not in allowed for key in ("system_hz", "battery_hz", "servo_hz", "rc_hz", "esc_hz", "attitude_hz", "depth_hz", "imu_hz", "pressure_hz")):
raise ValueError("Telemetry rates must be Off, 1, 2, 5 or 10 Hz")
self.link_path.write_text(json.dumps(clean, indent=2) + "\n", encoding="utf-8")
self.link_config = clean
@@ -577,6 +704,31 @@ class ControlApi:
with self.lock:
snap = json.loads(json.dumps(self.state))
snap["motor_settings"] = self.motor_settings
snap["telemetry"] = self.telemetry.snapshot(bool(snap.get("connected")))
if snap.get("battery") and not self.demo_mode:
now = time.monotonic()
snap["battery"]["current_stale"] = now - self.battery_fields_at.get("current_a", 0) > 5
if snap["battery"]["current_stale"]:
snap["battery"]["current_a"] = None
for field in ("voltage", "per_cell", "consumed_mah", "remaining_percent"):
key = "voltage" if field == "per_cell" else field
if now - self.battery_fields_at.get(key, 0) > 5:
snap["battery"][field] = None
battery = snap["battery"]
if battery.get("remaining_percent") is not None:
battery["percent"] = battery["remaining_percent"]
elif battery.get("per_cell") is not None:
low, high = self.battery_config["empty_v_per_cell"], self.battery_config["full_v_per_cell"]
battery["percent"] = max(0, min(100, (battery["per_cell"] - low) * 100 / (high - low)))
battery["charge_source"] = "Voltage estimate"
else:
battery["percent"] = None
capacity = self.battery_config["capacity_mah"]
battery["remaining_mah"] = (max(0, capacity - battery["consumed_mah"]) if battery.get("consumed_mah") is not None else
round(capacity * battery["remaining_percent"] / 100) if battery.get("remaining_percent") is not None else None)
for accessory in snap["accessories"].values():
if accessory.get("accepted") is None and time.monotonic() - accessory.get("at", 0) > 3:
accessory["error"] = "No ACK"
snap["app_state"] = self._derive_app_state()
if not self.demo_mode:
snap.update(self.vehicle_state.snapshot())
@@ -608,6 +760,19 @@ class ControlApi:
self.state["status"] = f"Command {event[1]} ACK: {event[2]}"
elif kind == "battery":
self._consume_battery(event[1])
elif kind == "capabilities":
self.state["capabilities"].update(event[1])
if event[1].get("frame", 4) != 4:
self.motor_settings["independent"] = False
self.worker.set_motor_scaling(False, {int(key): item["scale"] for key, item in self.motor_settings["motors"].items()})
elif kind == "telemetry":
self.telemetry.update(event[1], event[2], event[3])
if self._sensor_writer:
self._sensor_writer.writerows((event[3], event[1], key, value) for key, value in event[2].items())
self._sensor_file.flush()
elif kind == "accessory_ack":
self.state["accessories"][event[1]] = {"requested": event[2], "accepted": event[3] == 0,
"error": None if event[3] == 0 else "Rejected"}
elif kind == "servo_output":
for servo, output_us in event[1].items():
if servo in self.state["servos"]:
@@ -655,6 +820,14 @@ class ControlApi:
previous = self.state.get("battery") or {}
battery = dict(previous)
battery["cells"] = self.battery_config["cells"]
for field in ("voltage", "current_a", "consumed_mah", "remaining_percent"):
if field in data:
value = data[field]
if isinstance(value, (float, int)) and math.isfinite(value) and value >= 0:
self.battery_fields_at[field] = now
else:
battery[field] = None
self.battery_fields_at.pop(field, None)
voltage = data.get("voltage")
if voltage is not None and float(voltage) > 0:
battery["voltage"] = float(voltage)
@@ -670,6 +843,9 @@ class ControlApi:
self.battery_last_current = current
self.battery_last_current_at = now
battery["current_a"] = current
elif "current_a" in data:
self.battery_last_current = None
self.battery_last_current_at = None
consumed = data.get("consumed_mah")
if consumed is not None and int(consumed) >= 0:
battery["consumed_mah"] = int(consumed)
@@ -677,6 +853,7 @@ class ControlApi:
elif self.battery_last_current is not None:
battery["consumed_mah"] = round(self.battery_session_consumed_mah)
battery["consumed_source"] = "Session estimate"
self.battery_fields_at["consumed_mah"] = now
remaining = data.get("remaining_percent")
if remaining is not None and 0 <= int(remaining) <= 100:
battery["remaining_percent"] = int(remaining)
@@ -711,6 +888,9 @@ class ControlApi:
}
def close(self):
self.sensor_record(False)
if self._recording:
self.recording_end(self._recording[0])
self.worker.disconnect()
@@ -785,7 +965,7 @@ class AppHTTPHandler(http.server.SimpleHTTPRequestHandler):
def start_local_server(directory: Path) -> int:
handler = lambda *args, **kwargs: AppHTTPHandler(*args, directory=str(directory), **kwargs)
server = socketserver.TCPServer(("127.0.0.1", 0), handler)
server = http.server.ThreadingHTTPServer(("127.0.0.1", 0), handler)
port = server.server_address[1]
threading.Thread(target=server.serve_forever, daemon=True).start()
return port
@@ -804,7 +984,7 @@ def main():
app_url = f"http://127.0.0.1:{port}/web/index.html?v={int(time.time())}"
win_w, win_h, min_dims = _get_window_size()
window = webview.create_window(
"SUB Control", url=app_url, js_api=api,
"Argo V2", url=app_url, js_api=api,
width=win_w, height=win_h, min_size=min_dims,
background_color="#111719", frameless=True, easy_drag=False,
shadow=True,
@@ -815,7 +995,7 @@ def main():
window.events.restored += lambda: setattr(api, "maximized", False)
try:
icon_path = project_root / "assets" / "sub-control.ico"
webview.start(debug=True, icon=str(icon_path) if icon_path.is_file() else None)
webview.start(debug=os.environ.get("SUB_CONTROL_DEBUG") == "1", icon=str(icon_path) if icon_path.is_file() else None)
finally:
api.close()