SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(vesc): integrate native calibration diagnostics and configuration archives

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-09-25 16:37:56 +03:00
parent 24bbaefb00
commit 45fb14b206
85 changed files with 17968 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugins/vesc/runtime/remote_control.py
+343
View File
@@ -0,0 +1,343 @@
"""Single Node-owned, volatile keyboard control session and live read projection.
Configuration/calibration keeps the same exclusive hardware owner. Browser input
is a short lease, not a queue. Native Tool still owns all serial commands.
"""
import copy
import math
import re
import threading
import time
import uuid
from concurrent.futures import ThreadPoolExecutor
from datetime import datetime, timedelta, timezone
from .group_test import batch
from .protocol import ppm, values
from .drive_profile import LAYOUTS, slot_label
class ControlEnded(ValueError):
"""A terminal input lease is normal stop; cleanup still verifies release."""
class InputLease:
def __init__(self, clock=time.monotonic):
self.clock = clock
self.id = None
self.sequence = -1
self.until = 0
self.demand = (0., 0.)
self.retired = set()
def accept(self, command):
if not isinstance(command, dict) or set(command) != {"id", "sequence", "ttl_ms", "left", "right", "settings"}:
raise ValueError("Invalid control envelope")
if (not re.fullmatch(r"[0-9a-f]{32}", str(command["id"]))
or type(command["sequence"]) is not int or not 0 <= command["sequence"] < 2**53
or any(type(command[k]) not in (int,float) or not math.isfinite(command[k]) for k in ("left","right","ttl_ms"))
or max(abs(command["left"]), abs(command["right"])) > 1
or not 0 < command["ttl_ms"] <= 400):
raise ValueError("Invalid control bounds")
s = command["settings"]
if (not isinstance(s, dict) or set(s) != {"standstill_confirmed","current_a","max_erpm"}
or s["standstill_confirmed"] is not True
or type(s["current_a"]) not in (int,float) or not .5 <= s["current_a"] <= 30
or type(s["max_erpm"]) not in (int,float) or not 300 <= s["max_erpm"] <= 3000):
raise ValueError("Invalid control settings")
if command["id"] in self.retired:
return False
if self.id is not None and (self.id != command["id"] or self.clock() >= self.until):
self.stop()
return False
if self.id == command["id"] and command["sequence"] <= self.sequence:
return False # Repeated frames cannot extend a lease.
self.id, self.sequence = command["id"], command["sequence"]
self.until = self.clock()+command["ttl_ms"]/1000
self.demand = (command["left"], command["right"])
return True
def stop(self):
if self.id:
self.retired.add(self.id)
# Bound tombstones; Core never reuses a cryptographic session id.
if len(self.retired)>1024:
raise ValueError("Restart control service after excessive sessions")
self.until = 0
self.demand = (0.,0.)
def live(self):
return self.id is not None and self.id not in self.retired and self.clock() < self.until
class RemoteControl:
def __init__(self, service):
self.service = service
self.lock = threading.RLock()
self.lease = InputLease()
self.instance = uuid.uuid4().hex
self.relay = None
self.primed = False
self.thread = None
self.watch_until = 0
self.state = "observing"
self.message = None
self.readings = {}
self.release_confirmed = None
def feed(self, body):
if (set(body) != {"watch","command","relay_id"} or type(body["watch"]) is not bool
or not re.fullmatch(r"[0-9a-f]{32}", str(body["relay_id"]))):
raise ValueError("Invalid relay")
with self.lock:
if self.relay != body["relay_id"]:
self.lease.stop()
self.relay = body["relay_id"]
self.primed = False
if body["watch"]:
self.watch_until = time.monotonic()+2
command = body["command"]
if command is None:
self.primed = True
self.lease.stop()
elif not self.primed:
self.lease.retired.add(command.get("id"))
else:
running = self.thread is not None and self.thread.is_alive()
if not running and command.get("id") != self.lease.id:
self.lease.id = None
self.lease.sequence = -1
accepted = self.lease.accept(command)
if accepted and not running:
self.state, self.message = "preparing", None
self.thread = threading.Thread(target=self._prepare, args=(copy.deepcopy(command),), daemon=True,
name="vesc-remote-control")
self.thread.start()
return self.snapshot()
def snapshot(self):
with self.lock:
readings = [{**copy.deepcopy(v), "age_ms": int((time.monotonic()-v["sampled_at"])*1000)} for v in self.readings.values()]
for v in readings: v.pop("sampled_at", None)
return {"supported": True, "instance": self.instance, "state": self.state,
"session_id": self.lease.id, "message": self.message, "devices": readings,
"release_confirmed": self.release_confirmed,
"profile": copy.deepcopy(self.service.drive.value)}
def observe(self):
with self.lock:
# Let an in-flight read finish, but do not start another one while
# the control worker is waiting to become the exclusive owner.
if self.thread is not None and self.thread.is_alive():
return
if time.monotonic() >= self.watch_until or not self.service.operation_lock.acquire(False):
return
try:
with self.service.lock: devices = [d for d in self.service.devices.values() if d.link and d.readable]
# Reading is explicit window interest, never discovery-port reset.
for d in devices:
if not d.lock.acquire(False): continue
try:
self._publish(d, values(d.link.query(4, timeout=.06)), ppm(d.link.query(31, timeout=.06)))
except (OSError, ValueError, TimeoutError):
pass # Old values retain age; a read failure is never zero.
finally: d.lock.release()
finally: self.service.operation_lock.release()
def _publish(self, device, value, receiver):
profile = self.service.drive.value
slot = next((k for k,b in profile["bindings"].items() if b["device_id"]==device.id), None)
with self.lock:
self.readings[device.id] = {"id": device.id,"uuid":device.identity["uuid"],
"slot":slot,"label":slot_label(profile["layout"],slot) if slot else "VESC "+device.identity["uuid"][:6].upper(),
"values":value,"receiver":receiver,"sampled_at":time.monotonic()}
def _prepare(self, envelope):
owner = self.service.motor
acquired = False
try:
# Observation also owns this lock. A nonblocking attempt made
# arming depend on which thread happened to read first. Wait only
# briefly, with a live input lease, never enqueue a future drive.
deadline = time.monotonic() + .5
while not acquired:
self.ensure_live()
remaining = deadline - time.monotonic()
if remaining <= 0:
raise ValueError("Другая операция с VESC ещё выполняется.")
acquired = self.service.operation_lock.acquire(timeout=min(.02, remaining))
self.ensure_live()
with self.service.lock: devices = list(self.service.devices.values())
profile = self.service.drive.value
slots = LAYOUTS.get(profile["layout"], ())
if not slots or set(profile["bindings"]) != set(slots):
raise ValueError("Назначьте все моторы в настройках борта.")
if not devices or any(not d.link for d in devices):
raise ValueError("Один из VESC недоступен.")
now = datetime.now(timezone.utc)
settings = envelope["settings"]
command = {"action_id":"vesc.drive.run","operation_id":"op_"+uuid.uuid4().hex,
"requested_at":now.isoformat(),"deadline_at":(now+timedelta(seconds=300)).isoformat(),
"parameters":{"current_a":settings["current_a"],"duration_s":30,"erpm":settings["max_erpm"],
"standstill_confirmed":True,"profile_revision":profile["revision"],
"device_ids":[profile["bindings"][s]["device_id"] for s in slots]}}
owner.run(command, devices, devices[0], remote=self)
except ControlEnded:
with self.lock:
self.state = "fault" if self.message or self.release_confirmed is False else "stopped"
except (OSError, ValueError, TimeoutError, KeyError) as error:
with self.lock:
self.state = "fault"
self.message = str(error) if self.message is None else self.message + " " + str(error)
finally:
with self.lock: self.lease.stop()
if acquired: self.service.operation_lock.release()
def ensure_live(self):
with self.lock:
if not self.lease.live(): raise ControlEnded("Управление остановлено: команда больше не подтверждается.")
def drive(self, owner, command, devices, moving, originals, unchanged):
from .motor_test import check_values
limit = command["parameters"]["current_a"]
maximum = command["parameters"]["erpm"]
profile = copy.deepcopy(self.service.drive.value)
sides = {b["device_id"]:0 if slot.startswith("left.") else 1 for slot,b in profile["bindings"].items()}
configs, minimum_speeds, claimed, neutral_since = {}, {}, False, None
previous, speeds = time.monotonic(), {d.id:0. for d in moving}
last_sign, undriven_since = {}, None
self.release_confirmed = None
owner.active, owner.mode = True, "remote"
receiver = False
zero_seen = False
stalls = {}
with ThreadPoolExecutor(max_workers=min(16,len(devices)),thread_name_prefix="vesc-remote") as pool:
try:
for d in moving:
self.ensure_live()
configs[d.id] = owner.limits.apply(d, originals[d.id]["motor"], limit)
minimum = configs[d.id]["s_pid_min_erpm"]
if not math.isfinite(minimum) or minimum < 0:
raise ValueError("Некорректный минимальный порог регулятора VESC.")
# Native Tool serializes setRpm as an integer. Round up so
# the first command actually reaches the firmware threshold.
minimum_speeds[d.id] = max(1, math.ceil(minimum))
if maximum < minimum_speeds[d.id]:
raise ValueError(f"Минимальная скорость регулятора этого VESC: {minimum_speeds[d.id]} ERPM.")
self.state = "ready"
while True:
cycle = time.monotonic()
if owner.stop.is_set(): break
if not receiver: self.ensure_live()
unchanged()
if self.service.drive.value != profile: raise ValueError("Назначения моторов изменились.")
def read(d): return values(d.link.query(4,timeout=.06)),ppm(d.link.query(31,timeout=.06))
readouts = batch(pool, devices, read)
for d in devices: self._publish(d,*readouts[d.id])
active = any(owner.receiver_active(d.id,readouts[d.id][1]["level"]) for d in devices)
if active:
receiver = True
self.state = "receiver"
owner.state("rc")
with self.lock: self.lease.stop()
if receiver:
# Hold zero locally through the first gesture. Neutral
# releases PPM, so only a subsequent gesture drives it.
stopped = all(abs(v[0]["motor_current_a"])<=1 and abs(v[0]["duty"])<.01 for v in readouts.values())
neutral_since = (neutral_since or cycle) if not active and stopped else None
if neutral_since is not None and cycle-neutral_since>=.5: break
demand=(0.,0.)
else:
with self.lock: demand=self.lease.demand
if demand==(0.,0.): zero_seen=True
if not zero_seen: demand=(0.,0.)
now=time.monotonic()
dt=min(.15,now-previous);previous=now
targets = {}
for d in moving:
value=readouts[d.id][0]
check_values(value,moving=True,current_a=limit,motor=configs[d.id])
target=demand[sides[d.id]]*maximum
minimum = minimum_speeds[d.id]
# A small analogue request must never be rounded UP to
# a faster requested speed. Release below the operable
# range; firmware would otherwise enter zero-duty mode.
if abs(target) < minimum: target=0
targets[d.id] = target
signs = {key: 1 if target>0 else -1 if target<0 else 0 for key,target in targets.items()}
quiet = all(abs(readouts[d.id][0]["erpm"])<300
and abs(readouts[d.id][0]["motor_current_a"])<=1
and abs(readouts[d.id][0]["duty"])<.01 for d in moving)
# Count only observed neutral while ALL previous outputs
# were released. A long operator pause already satisfies it.
if quiet and not any(speeds.values()):
if undriven_since is None: undriven_since = now
else: undriven_since = None
reversing = any(sign and last_sign.get(key,sign)!=sign for key,sign in signs.items())
if reversing and (undriven_since is None or now-undriven_since<.5):
# One shared barrier: after a turn, the side keeping its
# direction must not drive while the other waits to reverse.
targets = dict.fromkeys(targets, 0.)
else:
last_sign.update({key:sign for key,sign in signs.items() if sign})
for d in moving:
value=readouts[d.id][0]
target=targets[d.id]
minimum=minimum_speeds[d.id]
# Zero is immediate release, never an RPM hold/brake.
if target==0:
speeds[d.id]=0
else:
step=600*dt
speeds[d.id]+=max(-step,min(step,target-speeds[d.id]))
# FW 5.02 disables its speed PID below s_pid_min_erpm.
# Ramping from zero spent 900/600 = 1.5 s sending
# ineffective commands. Enter the configured range
# immediately, then keep the existing ramp above it.
if abs(speeds[d.id]) < minimum:
speeds[d.id] = math.copysign(minimum, target)
if abs(value["motor_current_a"])>5:
at,tacho=stalls.setdefault(d.id,(now,value["tachometer"]))
if abs(value["erpm"])>=60 and abs(value["tachometer"]-tacho)>=3: stalls[d.id]=(now,value["tachometer"])
elif now-at>=2: raise ValueError("Мотор не движется при токе выше 5 А.")
else: stalls.pop(d.id,None)
if now-cycle>.12: raise ValueError("Связь с VESC слишком медленная.")
if not receiver: self.ensure_live()
claimed=True
batch(pool,devices,lambda d:d.link.test_command("claim"))
if time.monotonic()-cycle>.16: raise ValueError("Связь с VESC слишком медленная.")
if not receiver: self.ensure_live()
def send(d):
if not receiver: self.ensure_live()
if owner.stop.is_set() or receiver or abs(speeds.get(d.id,0))<1: d.link.test_command("release")
else: d.link.test_speed(speeds[d.id])
batch(pool,devices,send)
if not receiver: self.state="driving" if any(speeds.values()) else "ready"
owner.sleep(max(0,.1-(time.monotonic()-cycle)))
finally:
self.state="stopping"
def release(d):
try: d.link.test_command("release")
except (OSError,ValueError,TimeoutError): pass
if claimed: batch(pool,devices,release)
if claimed:
owner.sleep(.3)
def released(d):
try:
value = values(d.link.query(4, timeout=.1))
return all(math.isfinite(value[k]) and abs(value[k]) <= 1
for k in ("motor_current_a", "input_current_a")) and abs(value["duty"]) < .01
except (OSError, ValueError, TimeoutError): return False
self.release_confirmed = all(batch(pool, devices, released).values())
if not self.release_confirmed:
self.message = "Снятие тока не подтверждено. Проверьте фактическое состояние моторов."
owner.state("rc")
for d in moving:
try: owner.limits.restore(d)
except (OSError,ValueError,TimeoutError):
notice="Восстановление пределов ожидает нейтрали."
self.message=(self.message+" " if self.message else "")+notice
owner.state("rc")
owner.active,owner.mode=False,None
self.state="fault" if self.release_confirmed is False else "receiver" if receiver else "stopped"
if not receiver and not owner.latched: owner.state("ready")