"""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")