PitresAli
8ccd7689f7
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.
1006 lines
46 KiB
Python
1006 lines
46 KiB
Python
from __future__ import annotations
|
|
|
|
import json
|
|
import os
|
|
import queue
|
|
import threading
|
|
import time
|
|
import base64
|
|
import shutil
|
|
import http.server
|
|
import urllib.parse
|
|
import csv
|
|
import uuid
|
|
import math
|
|
from pathlib import Path
|
|
|
|
import webview
|
|
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:
|
|
"""Small, local-only bridge between the CSS UI and the MAVLink worker."""
|
|
|
|
def __init__(self):
|
|
self.events: queue.Queue = queue.Queue()
|
|
self.worker = MavlinkWorker(self.events)
|
|
self.worker.require_input_lease = True
|
|
self.vehicle_state = VehicleState()
|
|
self.battery_received_at = None
|
|
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",
|
|
"vehicle": "No heartbeat",
|
|
"armed": False,
|
|
"mode": None,
|
|
"app_state": "disconnected",
|
|
"status": "Ready. Select a USB port to begin.",
|
|
"battery": None,
|
|
"link_quality": None,
|
|
"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] = {}
|
|
self.motor_directions: dict[int, int] = {}
|
|
self.motor_settings_path = Path(__file__).resolve().parent / "motor_settings.json"
|
|
self.motor_settings = {
|
|
"independent": False,
|
|
"customized": False,
|
|
"motors": {str(i): {"name": f"Motor {i}", "scale": 20} for i in range(1, 13)},
|
|
}
|
|
try:
|
|
saved_motor_settings = json.loads(self.motor_settings_path.read_text(encoding="utf-8"))
|
|
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()},
|
|
)
|
|
self.battery_path = Path(__file__).resolve().parent / "battery_settings.json"
|
|
self.link_path = Path(__file__).resolve().parent / "link_settings.json"
|
|
self.model_path = Path(__file__).resolve().parent / "model_settings.json"
|
|
self.model_cache = Path(__file__).resolve().parent / "runtime" / "vehicle_model.stl"
|
|
self.model_cache_name = Path(__file__).resolve().parent / "runtime" / "vehicle_model_name.txt"
|
|
# Leading underscore keeps the native WebView object out of js_api
|
|
# serialization. Only the small public methods below are exposed.
|
|
self._window = None
|
|
self.maximized = False
|
|
self.battery_config = {
|
|
"cells": 4, "capacity_mah": 5500, "c_rating": 35,
|
|
"full_v_per_cell": 4.2, "empty_v_per_cell": 3.3,
|
|
"warning_percent": 20,
|
|
}
|
|
try:
|
|
self.battery_config.update(json.loads(self.battery_path.read_text(encoding="utf-8")))
|
|
except (OSError, ValueError, TypeError):
|
|
pass
|
|
self.link_config = {
|
|
"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")))
|
|
except (OSError, ValueError, TypeError):
|
|
pass
|
|
self._apply_link_config()
|
|
|
|
def initial_state(self):
|
|
return {
|
|
"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(),
|
|
}
|
|
|
|
def refresh_ports(self):
|
|
ports = []
|
|
# Add physical and virtual serial COM ports
|
|
for p in list_ports.comports():
|
|
desc = p.description or ""
|
|
if desc and desc != "n/a" and desc != p.device:
|
|
label = f"{p.device} ({desc})"
|
|
else:
|
|
label = p.device
|
|
ports.append({"device": p.device, "label": label, "type": "serial"})
|
|
|
|
# Built-in UI testing: no external process, port, or MAVLink socket.
|
|
ports.insert(0, {"device": "demo", "label": "Demo Mode (no hardware)", "type": "demo"})
|
|
return ports
|
|
|
|
def minimize_window(self):
|
|
if self._window:
|
|
self._window.minimize()
|
|
return True
|
|
|
|
def toggle_maximize_window(self):
|
|
if not self._window:
|
|
return False
|
|
if self.maximized:
|
|
self._window.restore()
|
|
else:
|
|
self._window.maximize()
|
|
self.maximized = not self.maximized
|
|
return self.maximized
|
|
|
|
def close_window(self):
|
|
if self._window:
|
|
self._window.destroy()
|
|
return True
|
|
|
|
def choose_model(self):
|
|
selected = None
|
|
# 1. Native Windows topmost open file dialog (instant, reliable)
|
|
try:
|
|
import tkinter as tk
|
|
from tkinter import filedialog
|
|
root = tk.Tk()
|
|
root.withdraw()
|
|
root.attributes("-topmost", True)
|
|
file_path = filedialog.askopenfilename(
|
|
title="Select 3D STL Vehicle Model",
|
|
initialdir=str(Path.home() / "Downloads"),
|
|
filetypes=[("STL 3D Meshes (*.stl)", "*.stl"), ("All files (*.*)", "*.*")],
|
|
)
|
|
root.destroy()
|
|
if file_path:
|
|
selected = (file_path,)
|
|
except Exception:
|
|
pass
|
|
|
|
# 2. Fallback to pywebview create_file_dialog
|
|
if not selected and self._window:
|
|
try:
|
|
selected = self._window.create_file_dialog(
|
|
webview.FileDialog.OPEN,
|
|
directory=str(Path.home()),
|
|
allow_multiple=False,
|
|
file_types=("STL mesh (*.stl)", "All files (*.*)"),
|
|
)
|
|
except Exception:
|
|
selected = None
|
|
|
|
if not selected:
|
|
return None
|
|
|
|
path = Path(selected[0])
|
|
if not path.is_file() or not path.suffix.lower().endswith(".stl"):
|
|
raise ValueError("Choose an STL mesh")
|
|
if path.stat().st_size > 30 * 1024 * 1024:
|
|
raise ValueError("The STL is larger than 30 MB; export a lighter mesh")
|
|
content = path.read_bytes()
|
|
self.model_cache.parent.mkdir(exist_ok=True)
|
|
self.model_cache.write_bytes(content)
|
|
self.model_cache_name.write_text(path.name, encoding="utf-8")
|
|
try:
|
|
self.model_path.write_text(json.dumps({"path": str(path)}, indent=2) + "\n", encoding="utf-8")
|
|
except Exception:
|
|
pass
|
|
return {"name": path.name, "data": base64.b64encode(content).decode("ascii")}
|
|
|
|
def save_model(self, name, data):
|
|
safe_name = Path(str(name)).name
|
|
if not safe_name.lower().endswith(".stl"):
|
|
raise ValueError("Choose an STL mesh")
|
|
try:
|
|
content = base64.b64decode(data, validate=True)
|
|
except (ValueError, TypeError) as exc:
|
|
raise ValueError("Invalid STL data") from exc
|
|
if not content or len(content) > 30 * 1024 * 1024:
|
|
raise ValueError("STL must be between 1 byte and 30 MB")
|
|
self.model_cache.parent.mkdir(exist_ok=True)
|
|
self.model_cache.write_bytes(content)
|
|
self.model_cache_name.write_text(safe_name, encoding="utf-8")
|
|
return True
|
|
|
|
def get_saved_model_info(self):
|
|
self._migrate_legacy_model()
|
|
try:
|
|
if self.model_cache.is_file() and self.model_cache.stat().st_size <= 30 * 1024 * 1024:
|
|
name = self.model_cache_name.read_text(encoding="utf-8").strip() or "Main Assembly V1 (2).stl"
|
|
return {"name": name, "size": self.model_cache.stat().st_size}
|
|
except (OSError, ValueError):
|
|
pass
|
|
return None
|
|
|
|
def read_saved_model_chunk(self, offset, size=262144):
|
|
offset, size = int(offset), min(int(size), 524288)
|
|
if offset < 0 or size < 1 or not self.model_cache.is_file():
|
|
raise ValueError("Saved model is unavailable")
|
|
with self.model_cache.open("rb") as stream:
|
|
stream.seek(offset)
|
|
content = stream.read(size)
|
|
return {"offset": offset, "data": base64.b64encode(content).decode("ascii"), "eof": offset + len(content) >= self.model_cache.stat().st_size}
|
|
|
|
def _migrate_legacy_model(self):
|
|
if not self.model_cache.is_file():
|
|
default_asset = Path(__file__).resolve().parent / "assets" / "Main Assembly V1 (2).stl"
|
|
if default_asset.is_file():
|
|
try:
|
|
self.model_cache.parent.mkdir(exist_ok=True)
|
|
shutil.copyfile(default_asset, self.model_cache)
|
|
self.model_cache_name.write_text(default_asset.name, encoding="utf-8")
|
|
return
|
|
except Exception:
|
|
pass
|
|
try:
|
|
if self.model_path.is_file():
|
|
path = Path(json.loads(self.model_path.read_text(encoding="utf-8"))["path"])
|
|
if path.is_file() and path.suffix.lower() == ".stl" and path.stat().st_size <= 30 * 1024 * 1024:
|
|
self.model_cache.parent.mkdir(exist_ok=True)
|
|
shutil.copyfile(path, self.model_cache)
|
|
self.model_cache_name.write_text(path.name, encoding="utf-8")
|
|
except Exception:
|
|
pass
|
|
|
|
def saved_model(self):
|
|
self._migrate_legacy_model()
|
|
try:
|
|
if self.model_cache.is_file() and self.model_cache.stat().st_size <= 30 * 1024 * 1024:
|
|
name = self.model_cache_name.read_text(encoding="utf-8").strip() or "Main Assembly V1 (2).stl"
|
|
return {"name": name, "data": base64.b64encode(self.model_cache.read_bytes()).decode("ascii")}
|
|
except (OSError, ValueError):
|
|
pass
|
|
return None
|
|
|
|
def connect(self, port, baud=57600, rate=20):
|
|
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")
|
|
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}
|
|
with self.lock:
|
|
self.state.update(
|
|
connected=True, link="LINK OK", vehicle="DEMO • DISARMED",
|
|
armed=False, mode="MANUAL", status="Demo mode ready — no hardware connected.",
|
|
link_quality={"grade": "excellent", "latency_ms": 12.0,
|
|
"loss_percent": 0.0, "samples": 20,
|
|
"corrupt_frames_20s": 0, "heartbeat_age_s": 0.1},
|
|
battery={"voltage": 15.8, "per_cell": 3.95, "percent": 72,
|
|
"cells": 4, "low": False},
|
|
)
|
|
for servo in range(1, 17):
|
|
function = self.servo_functions.get(servo, 0)
|
|
motor = motor_number_for_function(function)
|
|
self.state["servos"][str(servo)] = {
|
|
"function": function, "label": MOTOR_FUNCTIONS.get(function, "Unassigned"),
|
|
"motor": motor, "direction": self.motor_directions.get(motor) if motor else None,
|
|
"output_us": None,
|
|
}
|
|
return self._snapshot()
|
|
self.battery_last_current = None
|
|
self.battery_last_current_at = None
|
|
self.battery_session_consumed_mah = 0.0
|
|
self.worker.rate_hz = int(rate)
|
|
with self.lock:
|
|
self.vehicle_state.disconnect()
|
|
self.state.update(
|
|
connected=True, link="CONNECTING",
|
|
status=f"Opening {port} at {baud} baud…",
|
|
)
|
|
self.worker.connect(str(port), int(baud))
|
|
print(f"[web_ui] connect() dispatched to worker thread OK")
|
|
# Return only once the worker has either seen a heartbeat or reported a
|
|
# connection failure. This makes the button independent of the regular
|
|
# UI polling loop and prevents an indefinite visual "Connecting" state.
|
|
deadline = time.monotonic() + 8.0
|
|
while time.monotonic() < deadline:
|
|
snapshot = self.poll()
|
|
if snapshot["app_state"] != "connecting":
|
|
return snapshot
|
|
time.sleep(0.05)
|
|
self.worker.disconnect()
|
|
time.sleep(0.1)
|
|
self.poll()
|
|
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
|
|
if self.worker.heavy_quality_test is not None:
|
|
self.worker.cancel_heavy_quality_test()
|
|
if self.demo_mode:
|
|
self.demo_mode = False
|
|
with self.lock:
|
|
self.state.update(connected=False, link="DISCONNECTED", vehicle="No heartbeat",
|
|
armed=False, mode=None, battery=None, link_quality=None,
|
|
status="Demo mode disconnected")
|
|
return True
|
|
self.worker.disconnect()
|
|
return True
|
|
|
|
def start_heavy_quality_test(self):
|
|
if self.demo_mode:
|
|
raise RuntimeError("The heavy link test requires a real flight-controller connection")
|
|
return self.worker.start_heavy_quality_test()
|
|
|
|
def cancel_heavy_quality_test(self):
|
|
return self.worker.cancel_heavy_quality_test()
|
|
|
|
def start_motor_test(self, motor, throttle_percent):
|
|
motor, throttle_percent = int(motor), int(throttle_percent)
|
|
if self.demo_mode:
|
|
raise RuntimeError("Motor tests require a real flight-controller connection")
|
|
with self.lock:
|
|
if not self.state.get("connected") or not self.state.get("armed"):
|
|
raise RuntimeError("Connect and arm the vehicle before motor testing")
|
|
assignment = next((item for item in self.state["servos"].values() if item.get("motor") == motor), None)
|
|
if assignment is None:
|
|
raise ValueError(f"Motor {motor} is not assigned to a servo output")
|
|
if self.state.get("motor_test_active"):
|
|
raise RuntimeError("A motor test is already active")
|
|
effective_percent = round(throttle_percent * self.worker.motor_output_scale(motor))
|
|
self.state["motor_test_active"] = True
|
|
self.state["status"] = f"Motor {motor} test active at {effective_percent}% output"
|
|
try:
|
|
return self.worker.start_motor_test(motor, effective_percent)
|
|
except Exception:
|
|
with self.lock:
|
|
self.state["motor_test_active"] = False
|
|
raise
|
|
|
|
def stop_motor_test(self):
|
|
self.worker.stop_motor_test()
|
|
with self.lock:
|
|
self.state["status"] = "Motor test released; waiting for flight-controller stop"
|
|
return True
|
|
|
|
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"] = (
|
|
f"Demo controls • surge {float(forward):+.2f} • "
|
|
f"yaw {float(turn):+.2f} • heave {float(vertical):+.2f}"
|
|
)
|
|
return True
|
|
with self.lock:
|
|
allowed = self.vehicle_state.snapshot()["controls_allowed"]
|
|
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:
|
|
self.worker.set_axes(Axes(), sequence)
|
|
self.worker._send_control(force_neutral=True)
|
|
return True
|
|
|
|
def set_gain(self, value):
|
|
value = max(0, min(100, int(value)))
|
|
if self.motor_settings["independent"]:
|
|
raise RuntimeError("Use per-motor scales while independent scaling is enabled")
|
|
with self.worker.lock:
|
|
self.worker.gain = value / 100.0
|
|
return value
|
|
|
|
@staticmethod
|
|
def _clean_motor_settings(settings):
|
|
if not isinstance(settings, dict):
|
|
raise ValueError("Motor settings must be an object")
|
|
requested = settings.get("motors", {})
|
|
if not isinstance(requested, dict):
|
|
raise ValueError("Motor settings are invalid")
|
|
motors = {}
|
|
for motor in range(1, 13):
|
|
entry = requested.get(str(motor), {})
|
|
if not isinstance(entry, dict):
|
|
entry = {}
|
|
name = str(entry.get("name", f"Motor {motor}")).strip()[:24] or f"Motor {motor}"
|
|
try:
|
|
scale = max(0, min(100, int(entry.get("scale", 20))))
|
|
except (TypeError, ValueError):
|
|
scale = 20
|
|
motors[str(motor)] = {"name": name, "scale": scale}
|
|
return {"independent": bool(settings.get("independent", False)), "customized": bool(settings.get("customized", False)), "motors": motors}
|
|
|
|
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
|
|
self.worker.set_motor_scaling(
|
|
clean["independent"],
|
|
{int(i): entry["scale"] for i, entry in clean["motors"].items()},
|
|
)
|
|
return clean
|
|
|
|
def command(self, name, sequence=None, expires_at=None):
|
|
if self.demo_mode:
|
|
if name not in ("manual", "arm", "disarm"):
|
|
raise ValueError(f"Unknown command: {name}")
|
|
armed = name == "arm" if name != "manual" else self.state.get("armed", False)
|
|
with self.lock:
|
|
self.state.update(armed=armed, mode="MANUAL",
|
|
vehicle=f"DEMO • {'ARMED' if armed else 'DISARMED'}",
|
|
status=f"Demo: {'Manual mode' if name == 'manual' else name.capitalize()} confirmed")
|
|
return True
|
|
if not self.worker.connection or not self.worker.target_system:
|
|
if name == "manual":
|
|
raise RuntimeError("Connect to vehicle to enable Manual mode")
|
|
elif name == "arm":
|
|
raise RuntimeError("Connect to vehicle before arming")
|
|
elif name == "disarm":
|
|
raise RuntimeError("Connect to vehicle before disarming")
|
|
raise RuntimeError("No autopilot heartbeat")
|
|
if name in ("arm", "disarm"):
|
|
self.poll()
|
|
with self.lock:
|
|
if name == "arm":
|
|
if expires_at is not None and time.time() * 1000 > expires_at:
|
|
raise RuntimeError("Engage request expired before it could be sent")
|
|
if sequence is not None and sequence <= self.worker.last_input_sequence:
|
|
raise RuntimeError("Engage canceled by a newer input reset")
|
|
self.vehicle_state.request(name)
|
|
self.worker.set_axes(Axes(), sequence)
|
|
try:
|
|
self.worker.command_arm(name == "arm")
|
|
except Exception as exc:
|
|
self.vehicle_state.fail(f"{name.capitalize()} send failed: {exc}")
|
|
raise
|
|
return self._snapshot()
|
|
if name == "manual":
|
|
self.worker.log("OPERATOR", "clicked SET MANUAL")
|
|
self.worker.set_manual_mode()
|
|
else:
|
|
raise ValueError(f"Unknown command: {name}")
|
|
return True
|
|
|
|
def set_servo_function(self, servo, motor):
|
|
servo, motor = int(servo), int(motor)
|
|
if not 1 <= servo <= 16 or not 0 <= motor <= 12:
|
|
raise ValueError("Invalid servo or motor")
|
|
function = 0 if motor == 0 else (32 + motor if motor <= 8 else 73 + motor)
|
|
if self.demo_mode:
|
|
self.servo_functions[servo] = function
|
|
self._update_servo(servo)
|
|
return True
|
|
self.worker.set_parameter(f"SERVO{servo}_FUNCTION", function)
|
|
return True
|
|
|
|
def reverse_motor(self, servo):
|
|
servo = int(servo)
|
|
motor = motor_number_for_function(self.servo_functions.get(servo))
|
|
if motor is None:
|
|
raise ValueError(f"SERVO{servo} is not assigned to a motor")
|
|
current = self.motor_directions.get(motor)
|
|
if current is None:
|
|
raise ValueError(f"MOT_{motor}_DIRECTION has not been read yet")
|
|
if self.demo_mode:
|
|
self.motor_directions[motor] = -1 if current == 1 else 1
|
|
self._update_servo(servo)
|
|
return True
|
|
self.worker.set_parameter(f"MOT_{motor}_DIRECTION", -1 if current == 1 else 1)
|
|
return True
|
|
|
|
def save_battery(self, config):
|
|
clean = {
|
|
"cells": int(config["cells"]),
|
|
"capacity_mah": int(config["capacity_mah"]),
|
|
"c_rating": float(config["c_rating"]),
|
|
"full_v_per_cell": float(config["full_v_per_cell"]),
|
|
"empty_v_per_cell": float(config["empty_v_per_cell"]),
|
|
"warning_percent": int(config["warning_percent"]),
|
|
}
|
|
if clean["cells"] < 1 or clean["capacity_mah"] <= 0 or clean["c_rating"] <= 0:
|
|
raise ValueError("Battery cells, capacity and C rating must be positive")
|
|
if not clean["full_v_per_cell"] > clean["empty_v_per_cell"] > 0:
|
|
raise ValueError("Full voltage per cell must be greater than empty voltage")
|
|
if not 0 <= clean["warning_percent"] <= 100:
|
|
raise ValueError("Warning percentage must be between 0 and 100")
|
|
self.battery_path.write_text(json.dumps(clean, indent=2) + "\n", encoding="utf-8")
|
|
self.battery_config = clean
|
|
return clean
|
|
|
|
def _apply_link_config(self):
|
|
self.worker.rate_hz = self.link_config["control_hz"]
|
|
self.worker.set_link_rates(self.link_config["ping_hz"], {
|
|
"SYS_STATUS": self.link_config["system_hz"],
|
|
"BATTERY_STATUS": self.link_config["battery_hz"],
|
|
"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):
|
|
allowed = {0, 1, 2, 5, 10}
|
|
clean = {key: int(config.get(key, value)) for key, value in self.link_config.items()}
|
|
if clean["baud"] not in {9600, 19200, 38400, 57600, 115200}:
|
|
raise ValueError("Choose a supported baud rate")
|
|
if clean["control_hz"] not in {10, 20}:
|
|
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", "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
|
|
self._apply_link_config()
|
|
return clean
|
|
|
|
def open_logs(self):
|
|
path = Path(__file__).resolve().parent / "logs"
|
|
path.mkdir(exist_ok=True)
|
|
os.startfile(path)
|
|
return True
|
|
|
|
def read_console(self, limit=150):
|
|
"""Return a bounded, read-only view of the local session console."""
|
|
return self.worker.read_console(limit)
|
|
|
|
def poll(self):
|
|
# Bound a poll even if telemetry arrives continuously.
|
|
with self._poll_lock:
|
|
for _ in range(500):
|
|
try:
|
|
event = self.events.get_nowait()
|
|
except queue.Empty:
|
|
break
|
|
self._consume(event)
|
|
if self.worker.target_system and time.monotonic() - self.worker.last_heartbeat > 3:
|
|
with self.lock:
|
|
self.state["link"] = (
|
|
"DATA OK / HEARTBEAT DELAYED"
|
|
if time.monotonic() - self.worker.last_rx < 2 else "ALL LINK DATA LOST"
|
|
)
|
|
return self._snapshot()
|
|
|
|
def _derive_app_state(self) -> str:
|
|
if not self.state.get("connected"):
|
|
return "disconnected"
|
|
link = self.state.get("link", "")
|
|
if link == "CONNECTING":
|
|
return "connecting"
|
|
if link in ("DATA OK / HEARTBEAT DELAYED", "ALL LINK DATA LOST"):
|
|
return "link_degraded"
|
|
if self.state.get("armed"):
|
|
return "armed"
|
|
return "connected_disarmed"
|
|
|
|
def _snapshot(self):
|
|
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())
|
|
if snap["connected"] and snap["app_state"] != "connecting" and not snap["armed_known"]:
|
|
snap["app_state"] = "link_degraded"
|
|
snap["battery_stale"] = (self.battery_received_at is None
|
|
or time.monotonic() - self.battery_received_at > 5)
|
|
return snap
|
|
|
|
def _consume(self, event):
|
|
kind = event[0]
|
|
with self.lock:
|
|
if kind == "heartbeat":
|
|
_, system, autopilot, mode, armed = event[:5]
|
|
self.vehicle_state.heartbeat(armed, event[5] if len(event) > 5 else None)
|
|
if not armed and self.state.get("motor_test_active"):
|
|
self.state["motor_test_active"] = False
|
|
self.state["status"] = "Flight controller reports disarmed after motor test"
|
|
self.state.update(
|
|
connected=True, link="LINK OK", vehicle=f"{mode} • {'ARMED' if armed else 'DISARMED'}",
|
|
armed=bool(armed),
|
|
mode=str(mode).upper(),
|
|
)
|
|
elif kind in ("status", "statustext"):
|
|
self.state["status"] = event[1]
|
|
elif kind == "ack":
|
|
if event[1] == 400:
|
|
self.vehicle_state.ack(event[2], event[3] if len(event) > 3 else None)
|
|
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"]:
|
|
self.state["servos"][servo]["output_us"] = output_us
|
|
elif kind == "motor_test_ack":
|
|
self.state["status"] = "Motor test accepted by flight controller"
|
|
elif kind == "motor_test_rejected":
|
|
self.state["motor_test_active"] = False
|
|
self.state["status"] = f"Motor test rejected: {event[1]}"
|
|
elif kind == "motor_test_stopped":
|
|
self.state["motor_test_active"] = False
|
|
self.state["status"] = f"Motor test ended: {event[1]}"
|
|
elif kind == "link_quality":
|
|
self.state["link_quality"] = event[1]
|
|
elif kind == "heavy_quality":
|
|
self.state["heavy_quality"] = event[1]
|
|
elif kind == "servo_param":
|
|
_, param_id, value = event
|
|
servo = int(param_id[5:param_id.index("_")])
|
|
self.servo_functions[servo] = int(value)
|
|
self._update_servo(servo)
|
|
elif kind == "motor_direction":
|
|
_, motor, value = event
|
|
self.motor_directions[int(motor)] = int(value)
|
|
for servo in range(1, 17):
|
|
self._update_servo(servo)
|
|
elif kind == "param_saved":
|
|
self.state["status"] = f"Confirmed: {event[1]}={event[2]}"
|
|
elif kind == "log_path":
|
|
self.state["log_path"] = event[1]
|
|
elif kind == "error":
|
|
self.state["status"] = event[1]
|
|
self.state["last_error"] = event[1]
|
|
elif kind == "disconnected":
|
|
self.vehicle_state.disconnect()
|
|
last_err = self.state.get("last_error")
|
|
status_msg = last_err if last_err else "Disconnected"
|
|
self.state.update(connected=False, link="DISCONNECTED", vehicle="No heartbeat",
|
|
mode=None, link_quality=None, status=status_msg, motor_test_active=False)
|
|
self.state["last_error"] = None
|
|
|
|
def _consume_battery(self, data):
|
|
self.battery_received_at = time.monotonic()
|
|
now = self.battery_received_at
|
|
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)
|
|
battery["per_cell"] = float(voltage) / self.battery_config["cells"]
|
|
current = data.get("current_a")
|
|
if current is not None and float(current) >= 0:
|
|
current = float(current)
|
|
if self.battery_last_current is not None and self.battery_last_current_at is not None:
|
|
elapsed = now - self.battery_last_current_at
|
|
if 0 < elapsed <= 10:
|
|
average = (current + self.battery_last_current) / 2
|
|
self.battery_session_consumed_mah += average * 1000 * elapsed / 3600
|
|
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)
|
|
battery["consumed_source"] = "Flight controller"
|
|
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)
|
|
battery["charge_source"] = "Flight controller"
|
|
capacity = self.battery_config["capacity_mah"]
|
|
if battery.get("consumed_mah") is not None:
|
|
battery["remaining_mah"] = max(0, capacity - battery["consumed_mah"])
|
|
elif battery.get("remaining_percent") is not None:
|
|
battery["remaining_mah"] = round(capacity * battery["remaining_percent"] / 100)
|
|
if battery.get("remaining_percent") is not None:
|
|
percent = battery["remaining_percent"]
|
|
elif battery.get("per_cell") is not None:
|
|
low = self.battery_config["empty_v_per_cell"]
|
|
high = self.battery_config["full_v_per_cell"]
|
|
percent = max(0, min(100, (battery["per_cell"] - low) * 100 / (high - low)))
|
|
battery["charge_source"] = "Voltage estimate"
|
|
else:
|
|
percent = None
|
|
battery["percent"] = percent
|
|
battery["low"] = percent is not None and percent <= self.battery_config["warning_percent"]
|
|
self.state["battery"] = battery
|
|
|
|
def _update_servo(self, servo):
|
|
function = self.servo_functions.get(servo)
|
|
motor = motor_number_for_function(function)
|
|
self.state["servos"][str(servo)] = {
|
|
"function": function,
|
|
"label": MOTOR_FUNCTIONS.get(function, "Unassigned") if function is not None else "Waiting…",
|
|
"motor": motor,
|
|
"direction": self.motor_directions.get(motor) if motor else None,
|
|
"output_us": (self.state["servos"].get(str(servo)) or {}).get("output_us"),
|
|
}
|
|
|
|
def close(self):
|
|
self.sensor_record(False)
|
|
if self._recording:
|
|
self.recording_end(self._recording[0])
|
|
self.worker.disconnect()
|
|
|
|
|
|
def _apply_dwm_styling(window):
|
|
try:
|
|
hwnd = None
|
|
if hasattr(window, "native") and window.native:
|
|
hwnd = int(window.native.Handle.ToInt64())
|
|
if hwnd:
|
|
import ctypes
|
|
user32 = ctypes.windll.user32
|
|
dwmapi = ctypes.windll.dwmapi
|
|
corner_pref = ctypes.c_int(2) # DWMWCP_ROUND
|
|
dwmapi.DwmSetWindowAttribute(
|
|
ctypes.c_void_p(hwnd), 33, ctypes.byref(corner_pref), ctypes.sizeof(corner_pref)
|
|
)
|
|
dark_mode = ctypes.c_int(1) # DWMWA_USE_IMMERSIVE_DARK_MODE
|
|
dwmapi.DwmSetWindowAttribute(
|
|
ctypes.c_void_p(hwnd), 20, ctypes.byref(dark_mode), ctypes.sizeof(dark_mode)
|
|
)
|
|
user32.SetForegroundWindow(ctypes.c_void_p(hwnd))
|
|
except Exception:
|
|
pass
|
|
|
|
|
|
def _get_window_size() -> tuple[int, int, tuple[int, int]]:
|
|
target_w, target_h = 1100, 650
|
|
min_w, min_h = 960, 580
|
|
try:
|
|
import ctypes
|
|
user32 = ctypes.windll.user32
|
|
sw = user32.GetSystemMetrics(0)
|
|
sh = user32.GetSystemMetrics(1)
|
|
if sw > 0 and sh > 0:
|
|
target_w = min(target_w, max(min_w, int(sw * 0.92)))
|
|
target_h = min(target_h, max(min_h, int(sh * 0.90)))
|
|
min_w = min(min_w, target_w)
|
|
min_h = min(min_h, target_h)
|
|
except Exception:
|
|
pass
|
|
return target_w, target_h, (min_w, min_h)
|
|
|
|
|
|
class AppHTTPHandler(http.server.SimpleHTTPRequestHandler):
|
|
def log_message(self, format, *args):
|
|
pass # Suppress HTTP request logs in console
|
|
|
|
def end_headers(self):
|
|
self.send_header("Cache-Control", "no-cache, no-store, must-revalidate")
|
|
self.send_header("Pragma", "no-cache")
|
|
self.send_header("Expires", "0")
|
|
super().end_headers()
|
|
|
|
def do_GET(self):
|
|
unquoted = urllib.parse.unquote(self.path)
|
|
if self.path in ("/model", "/model.stl", "/runtime/vehicle_model.stl") or "Main Assembly" in unquoted or "vehicle_model" in unquoted:
|
|
project_root = Path(__file__).resolve().parent
|
|
model_file = project_root / "assets" / "Main Assembly V1 (2).stl"
|
|
if not model_file.is_file():
|
|
model_file = project_root / "runtime" / "vehicle_model.stl"
|
|
if model_file.is_file():
|
|
data = model_file.read_bytes()
|
|
self.send_response(200)
|
|
self.send_header("Content-Type", "model/stl")
|
|
self.send_header("Content-Length", str(len(data)))
|
|
self.send_header("Access-Control-Allow-Origin", "*")
|
|
self.end_headers()
|
|
self.wfile.write(data)
|
|
return
|
|
return super().do_GET()
|
|
|
|
|
|
def start_local_server(directory: Path) -> int:
|
|
handler = lambda *args, **kwargs: AppHTTPHandler(*args, directory=str(directory), **kwargs)
|
|
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
|
|
|
|
|
|
def main():
|
|
if os.name == "nt":
|
|
# Keep the running window separate from generic Python taskbar groups.
|
|
import ctypes
|
|
ctypes.windll.shell32.SetCurrentProcessExplicitAppUserModelID("SUBControl.Desktop")
|
|
from desktop_shortcut import ensure_desktop_shortcut
|
|
ensure_desktop_shortcut()
|
|
api = ControlApi()
|
|
project_root = Path(__file__).resolve().parent
|
|
port = start_local_server(project_root)
|
|
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(
|
|
"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,
|
|
)
|
|
api._window = window
|
|
window.events.shown += lambda: _apply_dwm_styling(window)
|
|
window.events.maximized += lambda: setattr(api, "maximized", True)
|
|
window.events.restored += lambda: setattr(api, "maximized", False)
|
|
try:
|
|
icon_path = project_root / "assets" / "sub-control.ico"
|
|
webview.start(debug=os.environ.get("SUB_CONTROL_DEBUG") == "1", icon=str(icon_path) if icon_path.is_file() else None)
|
|
finally:
|
|
api.close()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|