feat(fleet): preserve operator VESC integration before final driver merge

2026-09-25 16:40:46 +03:00
parent dad11b47d7
commit 71a648fec8
128 changed files with 22390 additions and 59 deletions
@@ -0,0 +1,52 @@
+"""Speed acquisition and measured hold time; never infers motion from a command."""
+from .protocol import SPEED_LIMITS
+
+
+class SpeedHold:
+    def __init__(self, erpm, duration, started):
+        self.erpm, self.duration, self.started = erpm, duration, started
+        self.previous = started
+        self.tachometer = None
+        self.motion_at = None
+        self.stable_at = None
+        self.hold_started = None
+        self.lost_at = None
+        self.rotation_s = 0.0
+        self.phase = "accelerating"
+        self.previous_good = False
+
+    def update(self, now, value):
+        delta = now - self.previous
+        self.previous = now
+        if self.tachometer is not None and value["tachometer"] != self.tachometer:
+            self.motion_at = now
+        self.tachometer = value["tachometer"]
+        good = (abs(value["erpm"] - self.erpm) <= self.erpm * SPEED_LIMITS["speed_tolerance"]
+                and self.motion_at is not None and now - self.motion_at <= 0.25)
+        error = None
+        if self.hold_started is None:
+            if good:
+                if self.stable_at is None: self.stable_at = now
+                if now - self.stable_at >= SPEED_LIMITS["settle_s"]:
+                    self.hold_started = now
+                    self.phase = "holding"
+            else:
+                self.stable_at = None
+            if self.hold_started is None and now - self.started >= SPEED_LIMITS["startup_timeout_s"]:
+                error = "Мотор не вышел на заданную скорость за 15 секунд. Отсчёт вращения не начался."
+        else:
+            if good:
+                # Both endpoints must be observed in band. Do not count a gap,
+                # USB delay, stationary tachometer or an unobserved last interval.
+                if self.previous_good and delta <= 0.25: self.rotation_s += delta
+                self.lost_at = None
+            else:
+                if self.lost_at is None: self.lost_at = now
+                if now - self.lost_at >= SPEED_LIMITS["lost_speed_timeout_s"]:
+                    error = "Мотор перестал удерживать заданную скорость. Проверка завершена досрочно."
+        self.previous_good = good
+        if now - self.started > SPEED_LIMITS["startup_timeout_s"] + self.duration + 5:
+            error = "Истёк общий срок проверки; заданное время вращения не набрано."
+        done = self.rotation_s >= self.duration
+        setpoint = min(self.erpm, max(1, (now - self.started) * SPEED_LIMITS["ramp_erpm_per_s"]))
+        return setpoint, done, error