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.
133 lines
6.1 KiB
Python
133 lines
6.1 KiB
Python
"""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
|