Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python
10.2 KB
Raw Blame
"""FakeMossFirmware: the MOSS QT Py motor firmware's serial behaviour in Python.

A serial-shaped object (write / readline / in_waiting / close / is_open) that
runs the same state machine as `firmware/MOSS_QTPY_MDD3A/Control.h` in this
repository, build "qtpy-mdd3a-v3": OFF / READY / DRIVE / TEST, the 300 ms drive
deadman, the 10 s arm expiry, the +-100 % number check, `ramp UP DOWN` (1..500
ms per % each), the replies ("OK", "ERR not_armed", "ERR invalid_command",
"ERR stopped_busy"), the telemetry JSON line with the v3 keys (fw, ramp_ms,
ina_ok, bus_mV, shunt_uV, current_mA; no "driver": that key only exists in
the MDD10A build).

Not modelled, on purpose: the ramp itself and the 100 ms reverse dwell
(output_pct always equals target_pct - the ramp setting is stored and
reported, never simulated), the partial-line timeout, the battery (bus_mV is
a constant 11600 mV, a rested 3S pack), the INA219 re-init.

Time is the fake's own millisecond clock: benches call advance(ms) to make
the firmware's watchdogs fire deterministically. Telemetry lines are queued
on `status`, on every 200 ms of advanced time, and on emit_telemetry(). With
auto_time=True (what MOSS_MOCK_BUS builds) the clock is the wall clock and
every readline() runs the watchdogs and the 200 ms telemetry, like the
firmware's loop.

Also the bus behind MOSS_MOCK_BUS, so the real dimOS runtime can be driven
end to end on a machine with no motors wired.
"""
from __future__ import annotations

import json
import threading
import time
from collections import deque

FW_NAME = "qtpy-mdd3a-v3"
MAX_PERCENT = 100
RAMP_MS_MIN, RAMP_MS_MAX = 1, 500          # Control.h RAMP_MS_MIN / RAMP_MS_MAX
RAMP_RISE_MS_DEFAULT, RAMP_FALL_MS_DEFAULT = 30, 15   # boot defaults, RAM only
BUS_MV = 11600                             # constant: a rested 3S pack
COMMAND_TIMEOUT_MS = 300
ARM_TIMEOUT_MS = 10000
TEST_MS = 1000
TELEMETRY_MS = 200

OFF, READY, DRIVE, TEST = 0, 1, 2, 3


class FakeMossFirmware:
    is_mock = True   # read by MossBaseAdapter when used as a port_factory

    def __init__(self, auto_time: bool = False):
        """auto_time=True: the clock follows the wall clock (MOSS_MOCK_BUS
        runs); False: it only moves with advance(ms) (benches)."""
        self._auto = auto_time
        self._t0 = time.monotonic()
        self._ms = 0
        self.mode = OFF
        self.left = 0
        self.right = 0
        self.since = 0
        self.reason = "boot"
        self.ticks = [0, 0]
        self.ramp_ms = [RAMP_RISE_MS_DEFAULT, RAMP_FALL_MS_DEFAULT]
        self.commands: list[str] = []     # every line received, in order
        self.sent: list[str] = []         # every line emitted, in order
        self._out: deque[bytes] = deque()
        self._lock = threading.Lock()
        self._last_telemetry = 0
        self.is_open = True
        self._queue_line(f"MOSS V0.4 firmware {FW_NAME}; USB CDC 115200; limit {MAX_PERCENT}%; "
                         f"ramp {self.ramp_ms[0]}/{self.ramp_ms[1]} ms per %; type status")

    # ── clock ──────────────────────────────────────────────────────────
    def now(self) -> int:
        if self._auto:
            self._ms = int((time.monotonic() - self._t0) * 1000)
        return self._ms

    def advance(self, ms: int) -> None:
        """Move the fake clock; runs the watchdogs and the 200 ms telemetry."""
        with self._lock:
            target = self._ms + int(ms)
            while self._ms < target:
                self._ms = min(target, self._ms + 10)
                self._poll()
                if self._ms - self._last_telemetry >= TELEMETRY_MS:
                    self._last_telemetry = self._ms
                    self._queue_line(self._telemetry_line())

    def _poll(self) -> None:
        age = self._ms - self.since
        if self.mode == DRIVE and age >= COMMAND_TIMEOUT_MS:
            self._stop("timeout")
        elif self.mode == READY and age >= ARM_TIMEOUT_MS:
            self._stop("arm_expired")
        elif self.mode == TEST and age >= TEST_MS:
            self._stop("test_done")

    def _stop(self, why: str) -> None:
        self.mode = OFF
        self.left = self.right = 0
        self.reason = why

    # ── serial side ────────────────────────────────────────────────────
    def _queue_line(self, text: str) -> None:
        self.sent.append(text)
        self._out.append((text + "\n").encode())

    @property
    def in_waiting(self) -> int:
        return sum(len(b) for b in self._out)

    def readline(self) -> bytes:
        for _ in range(4):   # ~20 ms of patience, like a serial timeout
            with self._lock:
                self._auto_tick()
                if self._out:
                    return self._out.popleft()
            time.sleep(0.005)
        return b""

    def _auto_tick(self) -> None:
        """Wall-clock mode (MOSS_MOCK_BUS): the reader's polling drives the
        watchdogs and the 200 ms telemetry, as the firmware's loop does - so
        a mock run shows a live battery and a firmware deadman that fires."""
        if not self._auto:
            return
        now = self.now()
        self._poll()
        if now - self._last_telemetry >= TELEMETRY_MS:
            self._last_telemetry = now
            self._queue_line(self._telemetry_line())

    def write(self, data: bytes) -> int:
        text = data.decode(errors="replace")
        for line in text.split("\n"):
            line = line.replace("\r", "")
            if line == "" and not text.endswith("\n"):
                continue
            if line == "":
                continue
            with self._lock:
                self._handle(line)
        return len(data)

    def close(self) -> None:
        self.is_open = False

    def emit_telemetry(self) -> None:
        with self._lock:
            self._queue_line(self._telemetry_line())

    # ── Control.h ──────────────────────────────────────────────────────
    @staticmethod
    def _number(s: str):
        try:
            n = int(s, 10)
        except ValueError:
            return None
        if str(n) != s.lstrip("+") and not (s.startswith("-") and str(n) == s):
            # strtol accepts "+5"; keep it simple: accept what int() accepts
            pass
        if n < -MAX_PERCENT or n > MAX_PERCENT:
            return None
        return n

    @staticmethod
    def _ramp_number(s: str):
        try:
            n = int(s, 10)
        except ValueError:
            return None
        return n if RAMP_MS_MIN <= n <= RAMP_MS_MAX else None

    def _handle(self, line: str) -> None:
        self.commands.append(line)
        now = self.now()
        self._poll()
        if any((ord(c) < 32 and c != "\t") or ord(c) > 126 for c in line) or len(line) > 95:
            self._stop("serial_invalid")
            return
        argv = line.split()
        if len(argv) > 4:
            self._stop("invalid")
            self._queue_line("ERR invalid_command")
            return
        if not argv:
            self._queue_line("OK")
            return
        if argv == ["stop"]:
            self._stop("stop")
            self._queue_line("OK")
            return
        if argv == ["status"]:
            self._queue_line(self._telemetry_line())
            return
        if argv == ["zero"]:
            if self.mode != OFF:
                self._stop("zero_requires_stop")
                self._queue_line("ERR stopped_busy")
                return
            self.ticks = [0, 0]
            self._queue_line("OK")
            return
        if argv == ["arm"]:
            if self.mode in (DRIVE, TEST):
                self._stop("arm_while_moving")
                self._queue_line("ERR stopped_busy")
                return
            self.mode = READY
            self.left = self.right = 0
            self.since = now
            self.reason = "armed"
            self._queue_line("OK")
            return
        if len(argv) == 3 and argv[0] == "drive":
            l, r = self._number(argv[1]), self._number(argv[2])
            if l is not None and r is not None:
                if self.mode not in (READY, DRIVE):
                    self._stop("not_armed")
                    self._queue_line("ERR not_armed")
                    return
                self.left, self.right = l, r
                self.mode = DRIVE
                self.since = now
                self.reason = "drive"
                self._queue_line("OK")
                return
        if len(argv) == 3 and argv[0] == "ramp":
            # allowed in any mode, never stops anything; out of range falls
            # through to the generic invalid-command path, as in Control.h
            up, down = self._ramp_number(argv[1]), self._ramp_number(argv[2])
            if up is not None and down is not None:
                self.ramp_ms = [up, down]
                self._queue_line("OK")
                return
        if len(argv) == 3 and argv[0] == "test" and argv[1] in ("left", "right"):
            p = self._number(argv[2])
            if p is not None:
                if self.mode != OFF:
                    self._stop("test_requires_stop")
                    self._queue_line("ERR stopped_busy")
                    return
                self.left = p if argv[1] == "left" else 0
                self.right = p if argv[1] == "right" else 0
                self.mode = TEST
                self.since = now
                self.reason = "test"
                self._queue_line("OK")
                return
        self._stop("invalid")
        self._queue_line("ERR invalid_command")

    def _telemetry_line(self) -> str:
        return json.dumps({
            "ms": self.now(), "fw": FW_NAME, "mode": self.mode, "reason": self.reason,
            "target_pct": [self.left, self.right],
            "output_pct": [self.left, self.right],   # ramp not modelled: see the module docstring
            "ramp_ms": list(self.ramp_ms),
            "ticks": list(self.ticks), "invalid_edges": [0, 0],
            "ina_ok": True, "bus_mV": BUS_MV, "shunt_uV": 0, "current_mA": None,
        }, separators=(",", ":"))