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

Save pre-V2 motor controls and UI checkpoint

Browse Source
This commit is contained in:
PitresAli
2026-10-22 19:31:28 +02:00
parent a4d8f1b7af
commit 5e91c28f1e
15 changed files with 1041 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SUB Control/web_ui.py
+164 -19
View File
@@ -28,6 +28,9 @@ class ControlApi:
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.demo_mode = False
self.lock = threading.Lock()
self._poll_lock = threading.Lock()
@@ -43,10 +46,26 @@ class ControlApi:
"link_quality": None,
"heavy_quality": None,
"log_path": None,
"servos": {str(i): {"function": None, "direction": None} for i in range(1, 17)},
"motor_test_active": 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
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"
@@ -67,7 +86,7 @@ class ControlApi:
pass
self.link_config = {
"baud": 57600, "control_hz": 20,
"ping_hz": 1, "system_hz": 1, "servo_hz": 2,
"ping_hz": 1, "system_hz": 1, "battery_hz": 1, "servo_hz": 2,
"rc_hz": 2, "esc_hz": 1,
}
try:
@@ -78,11 +97,10 @@ class ControlApi:
def initial_state(self):
return {
"ports": self.refresh_ports(),
"battery_config": self.battery_config,
"link_config": self.link_config,
"motor_settings": self.motor_settings,
"state": self._snapshot(),
"model_info": self.get_saved_model_info(),
}
def refresh_ports(self):
@@ -259,8 +277,12 @@ class ControlApi:
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()
@@ -285,6 +307,9 @@ class ControlApi:
raise TimeoutError(f"No MAVLink heartbeat received from {port} within 8 seconds")
def disconnect(self):
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:
@@ -305,6 +330,34 @@ class ControlApi:
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):
if self.demo_mode:
with self.lock:
@@ -330,10 +383,44 @@ class ControlApi:
return True
def set_gain(self, value):
value = max(5, min(50, int(value)))
self.worker.gain = value / 100.0
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 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"):
@@ -425,6 +512,7 @@ class ControlApi:
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"],
@@ -439,7 +527,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", "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")):
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
@@ -488,6 +576,7 @@ class ControlApi:
def _snapshot(self):
with self.lock:
snap = json.loads(json.dumps(self.state))
snap["motor_settings"] = self.motor_settings
snap["app_state"] = self._derive_app_state()
if not self.demo_mode:
snap.update(self.vehicle_state.snapshot())
@@ -503,6 +592,9 @@ class ControlApi:
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),
@@ -515,16 +607,19 @@ class ControlApi:
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.battery_received_at = time.monotonic()
voltage = float(event[1])
cells = self.battery_config["cells"]
per_cell = voltage / cells
low, high = self.battery_config["empty_v_per_cell"], self.battery_config["full_v_per_cell"]
percent = max(0, min(100, (per_cell - low) * 100 / (high - low)))
self.state["battery"] = {
"voltage": voltage, "per_cell": per_cell, "percent": percent,
"cells": cells, "low": percent <= self.battery_config["warning_percent"],
}
self._consume_battery(event[1])
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":
@@ -551,9 +646,59 @@ class ControlApi:
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)
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"]
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
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"
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)
@@ -562,6 +707,7 @@ class ControlApi:
"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):
@@ -653,7 +799,6 @@ def main():
from desktop_shortcut import ensure_desktop_shortcut
ensure_desktop_shortcut()
api = ControlApi()
api._migrate_legacy_model()
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())}"