344 lines
19 KiB
Python
344 lines
19 KiB
Python
"""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")
|