Build Argo V2 control mappings and telemetry dashboard
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:
@@ -0,0 +1,132 @@
|
||||
"""Validated V2 settings and telemetry, independent of the desktop renderer."""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import time
|
||||
from pathlib import Path
|
||||
from urllib.parse import urlsplit
|
||||
|
||||
AXES = ("forward", "lateral", "vertical", "turn", "pitch", "roll")
|
||||
FRAMES = {
|
||||
0: ("BlueROV1", 6, AXES),
|
||||
1: ("Vectored", 6, tuple(axis for axis in AXES if axis != "pitch")),
|
||||
2: ("Vectored 6DOF", 8, AXES),
|
||||
3: ("Vectored 90", 8, AXES),
|
||||
4: ("SimpleROV 3", 3, ("forward", "vertical", "turn")),
|
||||
5: ("SimpleROV 4", 4, ("forward", "vertical", "turn", "roll")),
|
||||
6: ("SimpleROV 5", 5, ("forward", "lateral", "vertical", "turn", "roll")),
|
||||
}
|
||||
INPUT = re.compile(r"^(off|button:(?:[0-9]|[12][0-9]|3[01])|axis:(?:[0-9]|1[0-5]):[+-])$")
|
||||
|
||||
|
||||
def number(value, minimum, maximum):
|
||||
result = float(value)
|
||||
if not math.isfinite(result) or not minimum <= result <= maximum:
|
||||
raise ValueError(f"Value must be between {minimum} and {maximum}")
|
||||
return result
|
||||
|
||||
|
||||
def clean_profile(config):
|
||||
if not isinstance(config, dict) or config.get("version") != 2:
|
||||
raise ValueError("Invalid controller profile")
|
||||
if not isinstance(config.get("bindings", {}), dict) or not isinstance(config.get("actions", {}), dict):
|
||||
raise ValueError("Invalid controller bindings")
|
||||
bindings = {}
|
||||
for axis in AXES:
|
||||
item = config.get("bindings", {}).get(axis, {})
|
||||
if not isinstance(item, dict):
|
||||
raise ValueError("Invalid motion binding")
|
||||
negative, positive = item.get("negative", "off"), item.get("positive", "off")
|
||||
if not isinstance(negative, str) or not INPUT.fullmatch(negative):
|
||||
raise ValueError("Invalid negative input")
|
||||
if not isinstance(positive, str) or not INPUT.fullmatch(positive):
|
||||
raise ValueError("Invalid positive input")
|
||||
if negative == positive and positive != "off":
|
||||
raise ValueError("Choose different inputs for each direction")
|
||||
bindings[axis] = {"negative": negative, "positive": positive,
|
||||
"scale": number(item.get("scale", 1), 0, 1)}
|
||||
actions = {}
|
||||
for action in ("lights", "stream", "record"):
|
||||
source = config.get("actions", {}).get(action, "off")
|
||||
if source != "off" and (not isinstance(source, str) or not re.fullmatch(r"button:(?:[0-9]|[12][0-9]|3[01])", source)):
|
||||
raise ValueError("Actions require a button")
|
||||
actions[action] = source
|
||||
return {"version": 2, "preset": str(config.get("preset", "custom"))[:24],
|
||||
"deadZone": number(config.get("deadZone", .08), 0, .3),
|
||||
"expo": number(config.get("expo", .15), 0, 1),
|
||||
"bindings": bindings, "actions": actions}
|
||||
|
||||
|
||||
def clean_station(config):
|
||||
if not isinstance(config, dict):
|
||||
raise ValueError("Invalid station settings")
|
||||
frame_value = number(config.get("frame", 4), 0, 6)
|
||||
if not frame_value.is_integer():
|
||||
raise ValueError("Invalid frame")
|
||||
frame = int(frame_value)
|
||||
if frame not in FRAMES:
|
||||
raise ValueError("Choose an ArduSub frame")
|
||||
mode = config.get("video_mode", "whep")
|
||||
if mode not in ("whep", "video", "mjpeg"):
|
||||
raise ValueError("Unsupported stream type")
|
||||
url = str(config.get("video_url", "")).strip()
|
||||
if url:
|
||||
parsed = urlsplit(url)
|
||||
if parsed.scheme not in ("http", "https") or not parsed.hostname or parsed.username or parsed.password:
|
||||
raise ValueError("Use an HTTP stream URL without embedded credentials")
|
||||
relays = {}
|
||||
for name in ("lights_relay", "camera_relay"):
|
||||
value = number(config.get(name, -1), -1, 15)
|
||||
if not value.is_integer():
|
||||
raise ValueError("Invalid relay")
|
||||
relays[name] = int(value)
|
||||
if relays["lights_relay"] >= 0 and relays["lights_relay"] == relays["camera_relay"]:
|
||||
raise ValueError("Lights and camera need separate relays")
|
||||
return {"frame": frame, "video_mode": mode, "video_url": url[:2048], **relays}
|
||||
|
||||
|
||||
def save_json(path: Path, data):
|
||||
temporary = path.with_suffix(path.suffix + ".tmp")
|
||||
temporary.write_text(json.dumps(data, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
||||
temporary.replace(path)
|
||||
|
||||
|
||||
class Telemetry:
|
||||
"""Timestamp every group; missing or stale measurements stay unavailable."""
|
||||
def __init__(self, clock=time.monotonic):
|
||||
self.clock = clock
|
||||
self.groups = {}
|
||||
|
||||
def update(self, group, values, received_at=None):
|
||||
clean = {key: value if isinstance(value, (int, float)) and math.isfinite(value) else None
|
||||
for key, value in values.items()}
|
||||
self.groups[group] = {"values": clean, "at": self.clock() if received_at is None else received_at}
|
||||
|
||||
def snapshot(self, connected=True):
|
||||
now = self.clock()
|
||||
return {name: {"values": item["values"], "age_s": max(0, now - item["at"]),
|
||||
"stale": not connected or now - item["at"] > 3}
|
||||
for name, item in self.groups.items()}
|
||||
|
||||
|
||||
def decode_telemetry(msg):
|
||||
kind = msg.get_type()
|
||||
if kind == "ATTITUDE":
|
||||
return "attitude", {"roll": math.degrees(msg.roll), "pitch": math.degrees(msg.pitch),
|
||||
"heading": math.degrees(msg.yaw) % 360,
|
||||
"roll_rate": math.degrees(msg.rollspeed),
|
||||
"pitch_rate": math.degrees(msg.pitchspeed),
|
||||
"yaw_rate": math.degrees(msg.yawspeed)}
|
||||
if kind == "GLOBAL_POSITION_INT":
|
||||
# ArduSub reports height relative to its surface/home origin, positive up.
|
||||
return "depth", {"depth_m": -msg.relative_alt / 1000.0, "vertical_ms": msg.vz / 100.0}
|
||||
if kind in ("SCALED_IMU", "SCALED_IMU2", "SCALED_IMU3"):
|
||||
return "imu", {"ax": msg.xacc * .00980665, "ay": msg.yacc * .00980665,
|
||||
"az": msg.zacc * .00980665, "gx": msg.xgyro / 1000.0,
|
||||
"gy": msg.ygyro / 1000.0, "gz": msg.zgyro / 1000.0,
|
||||
"mx": msg.xmag / 1000.0, "my": msg.ymag / 1000.0, "mz": msg.zmag / 1000.0}
|
||||
if kind == "SCALED_PRESSURE2":
|
||||
return "pressure", {"pressure_hpa": msg.press_abs, "temperature_c": msg.temperature / 100.0}
|
||||
return None
|
||||
Reference in New Issue
Block a user