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

2026-09-25 16:37:56 +03:00
parent 24bbaefb00
commit 45fb14b206
85 changed files with 17968 additions and 28 deletions
@@ -0,0 +1,135 @@
+"""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}