136 lines
8.5 KiB
Python
136 lines
8.5 KiB
Python
"""One local, bounded speed test for the complete owner-assigned drive profile."""
|
|
from concurrent.futures import ThreadPoolExecutor
|
|
|
|
from .drive_profile import LAYOUTS
|
|
from .protocol import ppm, values, TEST_LIMITS
|
|
from .speed_hold import SpeedHold
|
|
|
|
|
|
def targets(service, params, devices, selected):
|
|
profile = service.drive.value
|
|
bindings = profile["bindings"]
|
|
slots = LAYOUTS.get(profile["layout"], ())
|
|
if (not slots or params["profile_revision"] != profile["revision"]
|
|
or set(bindings) != set(slots)):
|
|
raise ValueError("Профиль изменился или не все моторы назначены.")
|
|
ids = [bindings[slot]["device_id"] for slot in slots]
|
|
if len(set(ids)) != len(ids) or set(params["device_ids"]) != set(ids) or selected.id not in ids:
|
|
raise ValueError("Состав проверяемого привода не совпадает с профилем.")
|
|
available = {device.id: device for device in devices}
|
|
if any(identifier not in available for identifier in ids):
|
|
raise ValueError("Один из назначенных моторов отключён.")
|
|
if any(available[b["device_id"]].identity["uuid"] != b["uuid"] for b in bindings.values()):
|
|
raise ValueError("Идентичность назначенного VESC изменилась.")
|
|
return [available[identifier] for identifier in ids]
|
|
|
|
|
|
def batch(pool, devices, function):
|
|
# Join every submitted call before propagating an error: no late worker may
|
|
# issue torque after the caller has already released the other controllers.
|
|
futures = [(device.id, pool.submit(function, device)) for device in devices]
|
|
result, errors = {}, []
|
|
for identifier, future in futures:
|
|
try: result[identifier] = future.result()
|
|
except (OSError, ValueError, TimeoutError) as error:
|
|
error.device_id = identifier
|
|
errors.append(error)
|
|
if errors: raise errors[0]
|
|
return result
|
|
|
|
|
|
def run(owner, command, devices, moving, originals, backups, unchanged, preflight):
|
|
from .motor_test import Rejected, check_values
|
|
params = command["parameters"]
|
|
duration, erpm, current_a = (params[key] for key in ("duration_s", "erpm", "current_a"))
|
|
ids = {device.id for device in moving}
|
|
motors, samples, after, cleanup = {}, [], {}, {}
|
|
restored, failure, rotation, previous_good = True, None, 0.0, False
|
|
outcome, claimed, started, previous = "duration", False, owner.monotonic(), owner.monotonic()
|
|
stalls = {}
|
|
owner.state("testing")
|
|
owner.active, owner.mode = True, "group_speed"
|
|
with ThreadPoolExecutor(max_workers=min(16, len(devices)), thread_name_prefix="vesc-drive") as pool:
|
|
try:
|
|
for device in moving:
|
|
motors[device.id] = owner.limits.apply(device, originals[device.id]["motor"], current_a)
|
|
started = previous = owner.monotonic()
|
|
holds = {device.id: SpeedHold(erpm, duration, started) for device in moving}
|
|
while owner.monotonic() - started < duration + 20:
|
|
cycle = owner.monotonic()
|
|
if owner.stop.is_set(): outcome = "stopped"; break
|
|
unchanged()
|
|
def read(device):
|
|
level = ppm(device.link.query(31, timeout=.06))["level"]
|
|
value = values(device.link.query(4, timeout=.06)) if device.id in ids else None
|
|
return level, value
|
|
observed = batch(pool, devices, read)
|
|
if any(owner.receiver_active(identifier, level) for identifier, (level, _) in observed.items()):
|
|
owner.state("rc")
|
|
raise Rejected("Приёмник передаёт команду. Общая проверка остановлена; управление за пультом.")
|
|
now = owner.monotonic()
|
|
readings = {identifier: observed[identifier][1] for identifier in ids}
|
|
setpoint = None
|
|
for identifier, value in readings.items():
|
|
check_values(value, moving=True, current_a=current_a, motor=motors[identifier])
|
|
setpoint, _, error = holds[identifier].update(now, value)
|
|
if error: raise Rejected(error)
|
|
if abs(value["motor_current_a"]) > TEST_LIMITS["stall_current_a"]:
|
|
at, tacho = stalls.setdefault(identifier, (now, value["tachometer"]))
|
|
if abs(value["erpm"]) >= 60 and abs(value["tachometer"] - tacho) >= 3:
|
|
stalls[identifier] = (now, value["tachometer"])
|
|
elif now - at >= TEST_LIMITS["stall_timeout_s"]:
|
|
raise Rejected("Один из моторов не движется при токе выше 5 А. Общая проверка остановлена.")
|
|
else: stalls.pop(identifier, None)
|
|
good = all(h.hold_started is not None and h.previous_good for h in holds.values())
|
|
delta = now - previous
|
|
if good and previous_good and delta <= .25: rotation += delta
|
|
previous, previous_good = now, good
|
|
sample = {"at": now-started, "devices": readings, "rotation_s": rotation,
|
|
"phase": "holding" if good else "accelerating", "commanded_erpm": None}
|
|
samples.append(sample)
|
|
if rotation >= duration: break
|
|
if owner.monotonic()-cycle > .12: raise Rejected("Связь слишком медленная для общей проверки.")
|
|
claimed = True
|
|
batch(pool, devices, lambda d: d.link.test_command("claim"))
|
|
if owner.stop.is_set(): outcome = "stopped"; break
|
|
if owner.monotonic()-cycle > .16: raise Rejected("Связь слишком медленная для общей проверки.")
|
|
def send(device):
|
|
if owner.stop.is_set(): return
|
|
if device.id in ids: device.link.test_speed(setpoint)
|
|
else: device.link.test_command("release")
|
|
sent_at = owner.monotonic()
|
|
batch(pool, devices, send)
|
|
sample.update(commanded_erpm=setpoint, command_batch_s=owner.monotonic()-sent_at)
|
|
owner.sleep(max(0, .1-(owner.monotonic()-cycle)))
|
|
if outcome == "duration" and rotation < duration:
|
|
outcome = "Общий срок проверки истёк; заданное время совместного вращения не набрано."
|
|
except (OSError, ValueError, TimeoutError) as error:
|
|
failure = {"type": type(error).__name__, "message": str(error), "elapsed_s": owner.monotonic()-started}
|
|
failure["device_id"] = getattr(error, "device_id", None)
|
|
failure["native_rpc"] = getattr(error, "native_rpc", None)
|
|
failure["native_history"] = getattr(error, "native_history", [])
|
|
outcome = str(error) if isinstance(error, Rejected) else "Ответ одного из VESC не получен вовремя. Общая проверка остановлена."
|
|
finally:
|
|
def release(device):
|
|
try:
|
|
device.link.test_command("release")
|
|
return "zero_current_sent"
|
|
except (OSError, ValueError, TimeoutError): return "unconfirmed"
|
|
if claimed: cleanup = batch(pool, devices, release)
|
|
owner.sleep(.3)
|
|
for device in devices:
|
|
try: after[device.id] = values(device.link.query(4, timeout=.2))
|
|
except (OSError, ValueError, TimeoutError): after[device.id] = None
|
|
confirmed = all(v is not None and abs(v["motor_current_a"]) <= 1 for v in after.values())
|
|
for device in moving:
|
|
try: owner.limits.restore(device)
|
|
except (OSError, ValueError, TimeoutError): restored = False
|
|
if not confirmed or not restored: owner.state("rc")
|
|
elif not owner.latched: owner.state("ready")
|
|
owner.active, owner.mode = False, None
|
|
return {"observed_at": owner.utc(), "device_ids": [d.id for d in moving], "mode": "group_speed",
|
|
"preflight": preflight, "profile_revision": params["profile_revision"], "current_a": current_a, "erpm_target": erpm,
|
|
"duration_limit_s": duration, "rotation_s": rotation, "outcome": outcome, "failure": failure,
|
|
"samples": samples, "release": cleanup, "release_confirmed": confirmed,
|
|
"limits_restored": restored, "after": after, "backups": backups}
|