172 lines
7.8 KiB
Python
172 lines
7.8 KiB
Python
"""Private lifecycle/RPC boundary to unmodified upstream VESC Tool C++.
|
|
|
|
No wire encoding or calibration algorithm lives here. The native process owns
|
|
one exact USB attachment and sends all commands through upstream Commands.
|
|
"""
|
|
import base64
|
|
from collections import deque
|
|
import hashlib
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
import select
|
|
import subprocess
|
|
import time
|
|
|
|
from .serial import check_attachment
|
|
|
|
|
|
class NativeLink:
|
|
def __init__(self, attachment):
|
|
self.attachment = attachment
|
|
self.process = None
|
|
self.buffer = b""
|
|
self.sequence = 0
|
|
self.hall_pending = False
|
|
self.history = deque(maxlen=32)
|
|
self.check()
|
|
root = Path("/usr/lib/mission-core-vesc/native")
|
|
config = Path("/run/mission-core-vesc/native") / hashlib.sha256(attachment.binding.encode()).hexdigest()[:24]
|
|
config.mkdir(parents=True, mode=0o700, exist_ok=True)
|
|
env = dict(os.environ, QT_QPA_PLATFORM="offscreen", XDG_CONFIG_HOME=str(config),
|
|
XDG_CACHE_HOME=str(config / "cache"), LD_LIBRARY_PATH=str(root / "lib"),
|
|
QT_PLUGIN_PATH=str(root / "plugins"))
|
|
# Preserve native startup diagnostics in the private runtime directory.
|
|
with (config / "engine.log").open("wb") as diagnostic:
|
|
self.process = subprocess.Popen([str(root / "bin/mission-core-vesc-engine"), "/dev/" + attachment.tty],
|
|
stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=diagnostic,
|
|
env=env, bufsize=0, close_fds=True)
|
|
try:
|
|
hello = self._receive(time.monotonic() + 6)
|
|
if hello.get("ready") is not True or not hello.get("engine", {}).get("connected"):
|
|
raise OSError("Native VESC Tool did not connect")
|
|
self.engine = hello["engine"]
|
|
self.check()
|
|
except BaseException:
|
|
self.close()
|
|
raise
|
|
|
|
@property
|
|
def alive(self):
|
|
return self.process is not None and self.process.poll() is None
|
|
|
|
def check(self):
|
|
check_attachment(self.attachment)
|
|
|
|
def _receive(self, deadline):
|
|
while b"\n" not in self.buffer:
|
|
if not self.alive or not select.select([self.process.stdout], [], [], max(0, deadline-time.monotonic()))[0]:
|
|
raise TimeoutError("Native VESC Tool response timed out")
|
|
data = os.read(self.process.stdout.fileno(), 65536)
|
|
if not data: raise OSError("Native VESC Tool exited")
|
|
self.buffer += data
|
|
if len(self.buffer) > 2 * 1024 * 1024: raise ValueError("Native response exceeds bound")
|
|
line, self.buffer = self.buffer.split(b"\n", 1)
|
|
return json.loads(line)
|
|
|
|
def rpc(self, method, timeout=2, **parameters):
|
|
self.sequence += 1
|
|
request = json.dumps({"id": self.sequence, "method": method, **parameters}, allow_nan=False).encode()+b"\n"
|
|
if len(request) > 65536: raise ValueError("Native request exceeds bound")
|
|
started = time.monotonic()
|
|
deadline = started + timeout
|
|
trace = {"method": method, "command": parameters.get("command"),
|
|
"timeout_ms": parameters.get("timeout_ms", timeout*1000),
|
|
"attachment": {"usb": self.attachment.usb, "address": self.attachment.address,
|
|
"tty": self.attachment.tty}}
|
|
try:
|
|
trace["stage"] = "attachment_before"
|
|
self.check()
|
|
trace["attachment_before_ms"] = (time.monotonic()-started)*1000
|
|
trace["stage"] = "request_write"
|
|
if not self.alive: raise OSError("Native VESC Tool is not running")
|
|
if not select.select([], [self.process.stdin], [], max(0, deadline-time.monotonic()))[1]:
|
|
raise TimeoutError("Native VESC Tool request timed out")
|
|
if os.write(self.process.stdin.fileno(), request) != len(request):
|
|
raise OSError("Native request write incomplete")
|
|
sent_at = time.monotonic()
|
|
trace["request_write_ms"] = (sent_at-started)*1000-trace["attachment_before_ms"]
|
|
trace["stage"] = "native_response"
|
|
response = self._receive(deadline)
|
|
received_at = time.monotonic()
|
|
trace["native_response_ms"] = (received_at-sent_at)*1000
|
|
if not isinstance(response, dict): raise ValueError("Invalid native response")
|
|
if response.get("id") != self.sequence: raise ValueError("Native response identity mismatch")
|
|
if response.get("ok") is not True:
|
|
if isinstance(response.get("diagnostics"), dict):
|
|
trace["transport"] = response["diagnostics"]
|
|
raise OSError(response.get("error", "Native operation failed"))
|
|
trace["stage"] = "attachment_after"
|
|
self.check()
|
|
trace["attachment_after_ms"] = (time.monotonic()-received_at)*1000
|
|
trace["stage"] = "complete"
|
|
result = response.get("result")
|
|
if not isinstance(result, dict): raise ValueError("Invalid native result")
|
|
trace.update(ok=True, elapsed_ms=(time.monotonic()-started)*1000)
|
|
self.last_rpc = trace
|
|
if hasattr(self, "history"): self.history.append(trace)
|
|
return result
|
|
except (OSError, ValueError, TimeoutError) as error:
|
|
# An unconfirmed reply is never silently retried on the same stream.
|
|
trace.update(ok=False, elapsed_ms=(time.monotonic()-started)*1000,
|
|
process_alive=self.alive, error=str(error))
|
|
try: self.check(); trace["attachment_present"] = True
|
|
except OSError: trace["attachment_present"] = False
|
|
self.last_rpc = trace
|
|
if hasattr(self, "history"): self.history.append(trace)
|
|
error.native_rpc = trace
|
|
error.native_history = list(getattr(self, "history", []))
|
|
self.close()
|
|
raise
|
|
|
|
def query(self, command, timeout=2):
|
|
result = self.rpc("query", timeout=timeout+0.1, command=command, timeout_ms=max(20, int(timeout*1000)))
|
|
return base64.b64decode(result["payload"], validate=True)
|
|
|
|
def test_command(self, action):
|
|
if action not in ("claim", "release"): raise ValueError("Unknown control action")
|
|
self.rpc("lease" if action == "claim" else "release", timeout=0.1)
|
|
|
|
def test_current(self, current_a):
|
|
self.rpc("current", timeout=0.1, current_a=current_a)
|
|
|
|
def test_speed(self, erpm):
|
|
self.rpc("rpm", timeout=0.1, erpm=erpm)
|
|
|
|
def set_temporary_limits(self, config):
|
|
from .temporary_limits import FIELDS
|
|
self.rpc("limits", timeout=2.2, parameters={k: config[k] for k in FIELDS})
|
|
|
|
def configuration(self):
|
|
return self.rpc("configuration", timeout=5)
|
|
|
|
def detect_hall(self):
|
|
if self.hall_pending: raise ValueError("Hall measurement already pending")
|
|
self.hall_pending = True
|
|
self.rpc("hall_start", timeout=0.2, current_a=5)
|
|
|
|
def calibrate_foc(self, max_power_loss_w):
|
|
self.rpc("foc_start", timeout=0.2, max_power_loss_w=max_power_loss_w)
|
|
|
|
def procedure_result(self):
|
|
return self.rpc("procedure_result", timeout=0.2)
|
|
|
|
@property
|
|
def hall_result(self):
|
|
if not self.hall_pending: return None
|
|
state = self.rpc("procedure_result", timeout=0.1)
|
|
result = state.get("result", {})
|
|
if state.get("running") or not result.get("completed"): return None
|
|
return base64.b64decode(result["payload"], validate=True)
|
|
|
|
def close(self):
|
|
process, self.process = self.process, None
|
|
if process is None: return
|
|
if process.poll() is None:
|
|
process.terminate()
|
|
try: process.wait(timeout=1)
|
|
except subprocess.TimeoutExpired:
|
|
process.kill(); process.wait(timeout=1)
|
|
for stream in (process.stdin, process.stdout):
|
|
if stream: stream.close()
|