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

moss_pi: drive MOSS from a Raspberry Pi 5 through the ESP32-S3 firmware, no dimOS

metrox859aab0f1faa
diff --git a/docs/software.md b/docs/software.md
index fe1b948..4676e44 100644
--- a/docs/software.md
+++ b/docs/software.md
@@ -7,3 +7,28 @@ What carries over: the idea of a gamepad module publishing a Twist behind a dead
What is written for MOSS: `moss_dimos/teleop_logic.py` (pure: two stick axes only, forward and yaw, no lateral input exists; deadzone; deadman with a brake window of zeros; every value validated) and `moss_dimos/diffdrive.py` (differential-drive kinematics, scaling instead of clipping, invalid configs and non-finite commands refused). `moss_dimos/gamepad.py` is the thin dimOS wrapper around them. The motor layer on the microcontroller side exists: [`firmware/`](../firmware/README.md), an ESP32-S3 commissioning firmware (newline-terminated serial commands over USB-UART at 115200 baud: `arm`, `drive LEFT RIGHT`, `test left|right PERCENT`, `stop`, `status`, `zero`; disarmed at boot; a 300 ms command watchdog stops and disarms; 25 % commissioning limit; JSON telemetry every 200 ms with raw encoder ticks and battery voltage). Its command set is the Jetson–ESP32 contract for now. The Jetson-side driver that speaks it, speed control and odometry are not written yet.
Everything in `moss_dimos/` is cold-tested only until the base runs.
+
+## Driving from a Raspberry Pi 5 (`moss_pi/`, no dimOS)
+
+`moss_pi/` drives MOSS from a Raspberry Pi 5 through the same ESP32-S3 firmware and its serial commands, without dimOS. It reuses the pure pad logic in `moss_dimos/teleop_logic.py` (forward and yaw only, deadzone, brake window) and needs only the Python standard library plus pyserial, and pygame for a gamepad.
+
+- `serial_bridge.py` writes `status`, `arm`, `stop`, `zero` and `drive LEFT RIGHT` (integers, -25..+25) and parses the JSON telemetry. It refuses to drive before `arm` and sends `stop` when 300 ms pass without a drive command. It also disarms on any `ERR` reply and reopens a lost port every second, sending `stop` first. `test` is left out: it is a bench command.
+- `teleop.py`, keyboard or gamepad. Nothing moves until you press arm with the deadman released. Hold the deadman to drive. On release it sends `drive 0 0` for 0.5 s, then `stop`, which disarms, so each new drive needs a new arm. The stick is mapped open loop: `max_track_speed_mps` (0.6 by default) becomes 25 % PWM duty. The speed MOSS actually reaches at 25 % has not been measured.
+- `telemetry_log.py` appends each telemetry line to a JSONL file as `{"wall_time_s": ..., "telemetry": {...}}`. On its own it only listens.
+
+On the Pi, from a clone of this repository:
+
+```
+python3 -m venv .venv && . .venv/bin/activate
+pip install -e '.[pi,test]'
+python -m pytest tests # cold tests, no robot needed
+sudo usermod -aG dialout "$USER" # once, then log in again
+ls /dev/serial/by-id/ # find the ESP32 USB-UART adapter
+python -m moss_pi.telemetry_log --port /dev/ttyUSB0 --out telemetry.jsonl
+python -m moss_pi.teleop --port /dev/ttyUSB0 --keyboard # r arm, hold w/s/a/d, space stop, q quit
+python -m moss_pi.teleop --port /dev/ttyUSB0 --gamepad --log drive.jsonl # A arm, hold L1, R1 boost
+```
+
+Use the DevKitC port wired to the USB-to-UART bridge (see [`firmware/`](../firmware/README.md)). It shows up as `/dev/ttyUSB0` or `/dev/ttyACM0`, depending on the bridge chip. A terminal reports key presses but not key releases, so with the keyboard a key counts as held for 0.6 s after its last repeat, and the rover brakes later than with a pad. Put the rover on blocks for the first run.
+
+State: cold-tested only, against a fake serial port that replays the 20 September 2026 bench telemetry and against a Linux pseudo-terminal through pyserial. Nothing in `moss_pi/` has run on a Pi or on the rover yet.
diff --git a/moss_pi/__init__.py b/moss_pi/__init__.py
new file mode 100644
index 0000000..905c95d
--- /dev/null
+++ b/moss_pi/__init__.py
@@ -0,0 +1,5 @@
+"""moss_pi - drive MOSS from a Raspberry Pi through the ESP32-S3 firmware, without dimOS.
+
+Only the standard library is needed to import this package; pyserial (and
+pygame for a gamepad) are imported where a real port or pad is opened.
+"""
diff --git a/moss_pi/serial_bridge.py b/moss_pi/serial_bridge.py
new file mode 100644
index 0000000..769981c
--- /dev/null
+++ b/moss_pi/serial_bridge.py
@@ -0,0 +1,354 @@
+"""Serial bridge to the MOSS ESP32-S3 firmware (see firmware/README.md).
+
+The firmware contract, restated so this file can be read alone:
+- 115200 baud, newline-terminated commands: `status`, `stop`, `zero`, `arm`,
+ `drive LEFT RIGHT` (integers, -25..+25), `test left|right PERCENT`;
+- replies are `OK` or `ERR <reason>`; every ERR also stops and disarms;
+- `status` and a timer produce one JSON telemetry line about every 200 ms;
+- the firmware itself stops and disarms 300 ms after the last drive command.
+
+What this module adds on the Pi side:
+- pure `format_*` functions that build the exact command lines;
+- `parse_line`, which turns a firmware line into a Telemetry, a Reply or an Info;
+- `SerialBridge`, which writes commands, reads lines, reopens a lost port and
+ runs a host-side deadman: no drive request for 300 ms means one `stop`.
+
+`test` is left out on purpose: it moves one motor without arming and belongs to
+bench commissioning, not to driving.
+
+Time and the port are injected (`clock`, `open_port`) so every behaviour here
+is testable without a robot or a serial device.
+"""
+from __future__ import annotations
+
+import json
+import threading
+import time
+from dataclasses import dataclass
+from typing import Callable, Protocol
+
+BAUD = 115200
+MAX_PERCENT = 25 # firmware MAX_PERCENT (commissioning limit)
+DEADMAN_S = 0.300 # same value as the firmware COMMAND_TIMEOUT_MS
+ARM_TIMEOUT_S = 10.0 # firmware ARM_TIMEOUT_MS: an arm with no drive expires
+RECONNECT_S = 1.0 # delay between two attempts to reopen a lost port
+MODES = {0: "off", 1: "ready", 2: "drive", 3: "test"}
+
+
+# --------------------------------------------------------------------------- commands
+
+def format_drive(left: int, right: int) -> bytes:
+ """(left, right) duty in percent -> the exact line the firmware expects, e.g. b'drive 10 10\\n'.
+
+ Only integers in [-MAX_PERCENT, MAX_PERCENT] are accepted: the firmware
+ would answer anything else with `ERR invalid_command` and a stop, so the
+ Pi refuses it before it reaches the wire.
+ """
+ for name, v in (("left", left), ("right", right)):
+ if not isinstance(v, int) or isinstance(v, bool):
+ raise ValueError(f"{name} must be an int percent, got {v!r}")
+ if not -MAX_PERCENT <= v <= MAX_PERCENT:
+ raise ValueError(f"{name} must be in [-{MAX_PERCENT}, {MAX_PERCENT}], got {v}")
+ return f"drive {left} {right}\n".encode("ascii")
+
+
+def format_simple(command: str) -> bytes:
+ """One of the argument-free commands -> its line."""
+ if command not in ("status", "stop", "zero", "arm"):
+ raise ValueError(f"unknown command {command!r}")
+ return f"{command}\n".encode("ascii")
+
+
+# --------------------------------------------------------------------------- telemetry
+
+@dataclass(frozen=True)
+class Telemetry:
+ """One firmware telemetry line. Units are the firmware's; properties convert."""
+ ms: int # firmware uptime, milliseconds
+ mode: int # 0 off, 1 ready, 2 drive, 3 test
+ reason: str # why the firmware is in this mode
+ target_pct: tuple[int, int] # commanded PWM duty, percent
+ output_pct: tuple[int, int] # ramped PWM duty actually applied, percent
+ ticks: tuple[int, int] # raw quadrature x4 encoder counts
+ invalid_edges: tuple[int, int]
+ ina_ok: bool
+ bus_mV: int | None # battery bus voltage, None when the INA219 is not read
+ shunt_uV: int | None
+ current_mA: float | None # None in the reference firmware (no validated shunt)
+ raw: dict # the decoded JSON object, untouched
+
+ @property
+ def mode_name(self) -> str:
+ return MODES.get(self.mode, f"unknown({self.mode})")
+
+ @property
+ def bus_V(self) -> float | None:
+ return None if self.bus_mV is None else self.bus_mV / 1000.0
+
+ @property
+ def uptime_s(self) -> float:
+ return self.ms / 1000.0
+
+
+@dataclass(frozen=True)
+class Reply:
+ """`OK` or `ERR <reason>`."""
+ ok: bool
+ error: str | None = None
+
+
+@dataclass(frozen=True)
+class Info:
+ """Anything else: the boot banner, a garbled or truncated line."""
+ text: str
+
+
+def _pair(obj: dict, key: str) -> tuple[int, int]:
+ v = obj[key]
+ if not (isinstance(v, list) and len(v) == 2 and all(isinstance(x, int) and not isinstance(x, bool) for x in v)):
+ raise ValueError(key)
+ return v[0], v[1]
+
+
+def _opt_int(obj: dict, key: str) -> int | None:
+ v = obj[key]
+ if v is not None and (not isinstance(v, int) or isinstance(v, bool)):
+ raise ValueError(key)
+ return v
+
+
+def parse_line(line: str) -> Telemetry | Reply | Info:
+ """A firmware line (without its newline) -> Telemetry, Reply or Info. Never raises."""
+ text = line.strip()
+ if text == "OK":
+ return Reply(ok=True)
+ if text.startswith("ERR"):
+ return Reply(ok=False, error=text[3:].strip() or "unknown")
+ if text.startswith("{"):
+ try:
+ obj = json.loads(text)
+ current = obj["current_mA"]
+ if current is not None and (not isinstance(current, (int, float)) or isinstance(current, bool)):
+ raise ValueError("current_mA")
+ if not isinstance(obj["ms"], int) or not isinstance(obj["mode"], int) or not isinstance(obj["reason"], str):
+ raise ValueError("header")
+ return Telemetry(
+ ms=obj["ms"], mode=obj["mode"], reason=obj["reason"],
+ target_pct=_pair(obj, "target_pct"), output_pct=_pair(obj, "output_pct"),
+ ticks=_pair(obj, "ticks"), invalid_edges=_pair(obj, "invalid_edges"),
+ ina_ok=obj["ina_ok"] is True, bus_mV=_opt_int(obj, "bus_mV"),
+ shunt_uV=_opt_int(obj, "shunt_uV"),
+ current_mA=None if current is None else float(current), raw=obj,
+ )
+ except (ValueError, KeyError, TypeError):
+ pass
+ return Info(text)
+
+
+# --------------------------------------------------------------------------- bridge
+
+class Port(Protocol):
+ """The subset of pyserial.Serial the bridge uses."""
+ def write(self, data: bytes) -> int | None: ...
+ def read(self, size: int = 1) -> bytes: ...
+ def close(self) -> None: ...
+
+
+def open_serial(device: str) -> Port:
+ """Open the real port: 115200 8N1, non-blocking reads.
+
+ DTR and RTS are held low before opening. On the ESP32-S3-DevKitC-1 these
+ lines drive EN and IO0 through the auto-program circuit; asserting them can
+ reset the board or stop it in the bootloader. (Hypothesis for the Pi: this
+ is the usual behaviour of that circuit, not yet checked on MOSS.)
+ """
+ import serial # optional dependency: pip install 'moss-dimos[pi]'
+
+ s = serial.Serial()
+ s.port, s.baudrate, s.timeout, s.write_timeout = device, BAUD, 0, 0.1
+ s.dtr = False
+ s.rts = False
+ s.open()
+ return s
+
+
+Listener = Callable[[str, "Telemetry | Reply | Info"], None]
+
+
+class SerialBridge:
+ """Writes commands, reads firmware lines, stops on silence, reopens a lost port.
+
+ Call `tick()` often (the teleop loop does, or `start()` runs it in a
+ thread). All methods are safe to call from several threads.
+
+ Local safety rules, on top of the firmware's own:
+ - `drive()` is refused unless `arm()` was sent since the last stop, error
+ or reconnect, and less than ARM_TIMEOUT_S ago if no drive followed it;
+ - the deadman: `DEADMAN_S` after the last `drive()` call, one `stop` is
+ written and the bridge disarms;
+ - a failed read or write closes the port and disarms; the port is reopened
+ every `RECONNECT_S`, and the first line written after reopening is `stop`.
+ """
+
+ def __init__(self, open_port: Callable[[], Port], clock: Callable[[], float] = time.monotonic,
+ deadman_s: float = DEADMAN_S, reconnect_s: float = RECONNECT_S) -> None:
+ self._open_port = open_port
+ self._clock = clock
+ self.deadman_s = deadman_s
+ self.reconnect_s = reconnect_s
+ self._port: Port | None = None
+ self._next_open = 0.0
+ self._buf = b""
+ self._lock = threading.RLock()
+ self._armed = False
+ self._last_drive: float | None = None
+ self._armed_at: float | None = None
+ self._listeners: list[Listener] = []
+ self.last_telemetry: Telemetry | None = None
+ self.last_reply: Reply | None = None
+ self._thread: threading.Thread | None = None
+ self._stop_thread = threading.Event()
+
+ # -- state
+ @property
+ def connected(self) -> bool:
+ return self._port is not None
+
+ @property
+ def armed(self) -> bool:
+ return self._armed
+
+ def add_listener(self, fn: Listener) -> None:
+ """fn(line, parsed) is called for every line read from the firmware."""
+ self._listeners.append(fn)
+
+ # -- commands
+ def status(self) -> bool:
+ return self._write(format_simple("status"))
+
+ def zero(self) -> bool:
+ return self._write(format_simple("zero"))
+
+ def arm(self) -> bool:
+ with self._lock:
+ ok = self._write(format_simple("arm"))
+ self._armed = ok
+ self._armed_at = self._clock() if ok else None
+ self._last_drive = None
+ return ok
+
+ def stop(self) -> bool:
+ with self._lock:
+ self._armed = False
+ self._last_drive = None
+ return self._write(format_simple("stop"))
+
+ def drive(self, left: int, right: int) -> bool:
+ """Send `drive LEFT RIGHT` and feed the deadman. Returns False if nothing was sent."""
+ line = format_drive(left, right) # validate before touching any state
+ with self._lock:
+ if not self._armed:
+ return False
+ if not self._write(line):
+ return False
+ self._last_drive = self._clock()
+ return True
+
+ # -- loop
+ def tick(self) -> None:
+ """Reopen the port if due, read and dispatch lines, apply the deadman."""
+ with self._lock:
+ now = self._clock()
+ if self._port is None and now >= self._next_open:
+ self._try_open(now)
+ self._read()
+ if self._last_drive is not None and now - self._last_drive >= self.deadman_s:
+ self.stop()
+ elif (self._armed and self._last_drive is None and self._armed_at is not None
+ and now - self._armed_at >= ARM_TIMEOUT_S):
+ self._armed = False # the firmware has disarmed itself; nothing to send
+
+ def start(self, period_s: float = 0.01) -> None:
+ """Run tick() in a background thread until close()."""
+ def run() -> None:
+ while not self._stop_thread.is_set():
+ self.tick()
+ time.sleep(period_s)
+ self._stop_thread.clear()
+ self._thread = threading.Thread(target=run, name="moss-serial-bridge", daemon=True)
+ self._thread.start()
+
+ def close(self) -> None:
+ """Send a last `stop` if the port is open, then close it."""
+ self._stop_thread.set()
+ if self._thread:
+ self._thread.join(timeout=1.0)
+ with self._lock:
+ if self._port is not None:
+ self.stop()
+ self._drop_port()
+
+ # -- internals
+ def _try_open(self, now: float) -> None:
+ try:
+ self._port = self._open_port()
+ except Exception: # device absent, permission, busy: retry later
+ self._port = None
+ self._next_open = now + self.reconnect_s
+ return
+ self._buf = b""
+ self.stop() # a fresh link starts from a known, stopped state
+
+ def _drop_port(self) -> None:
+ port, self._port = self._port, None
+ self._armed = False
+ self._last_drive = None
+ self._buf = b""
+ self._next_open = self._clock() + self.reconnect_s
+ if port is not None:
+ try:
+ port.close()
+ except Exception:
+ pass
+
+ def _write(self, data: bytes) -> bool:
+ with self._lock:
+ if self._port is None:
+ self._armed = False
+ return False
+ try:
+ self._port.write(data)
+ return True
+ except Exception:
+ self._drop_port()
+ return False
+
+ def _read(self) -> None:
+ if self._port is None:
+ return
+ try:
+ chunk = self._port.read(4096)
+ except Exception:
+ self._drop_port()
+ return
+ if not chunk:
+ return
+ self._buf += chunk
+ *lines, self._buf = self._buf.split(b"\n")
+ if len(self._buf) > 4096: # no newline for far too long: garbage, drop it
+ self._buf = b""
+ for raw in lines:
+ text = raw.decode("ascii", errors="replace").rstrip("\r")
+ if not text:
+ continue
+ parsed = parse_line(text)
+ if isinstance(parsed, Telemetry):
+ self.last_telemetry = parsed
+ elif isinstance(parsed, Reply):
+ self.last_reply = parsed
+ if not parsed.ok:
+ # every ERR stops and disarms the firmware: mirror it
+ self._armed = False
+ self._last_drive = None
+ for fn in self._listeners:
+ fn(text, parsed)
diff --git a/moss_pi/telemetry_log.py b/moss_pi/telemetry_log.py
new file mode 100644
index 0000000..6af7e71
--- /dev/null
+++ b/moss_pi/telemetry_log.py
@@ -0,0 +1,71 @@
+"""Append every firmware telemetry line to a JSONL file, with a wall-clock timestamp.
+
+One JSON object per line:
+
+ {"wall_time_s": 1790000000.123, "telemetry": {"ms": 305602, "mode": 2, ...}}
+
+`wall_time_s` is the Pi's clock (Unix seconds, UTC) when the line was read;
+`telemetry` is the firmware object exactly as decoded. OK/ERR replies and other
+lines are not written. Each record is flushed, so a power cut loses at most the
+line being written.
+
+Run alone, it only listens: it sends `stop` on connect (see SerialBridge) and
+nothing else, so it cannot move a motor.
+
+ python -m moss_pi.telemetry_log --port /dev/ttyUSB0 --out moss_telemetry.jsonl
+"""
+from __future__ import annotations
+
+import argparse
+import json
+import time
+from pathlib import Path
+from typing import Callable, TextIO
+
+from .serial_bridge import Info, Reply, SerialBridge, Telemetry, open_serial
+
+
+class TelemetryLog:
+ """Listener for SerialBridge: `bridge.add_listener(log.on_line)`."""
+
+ def __init__(self, path: str | Path, wall_clock: Callable[[], float] = time.time) -> None:
+ self.path = Path(path)
+ self._clock = wall_clock
+ self._fh: TextIO = self.path.open("a", encoding="utf-8")
+ self.count = 0
+
+ def on_line(self, line: str, parsed: Telemetry | Reply | Info) -> None:
+ if not isinstance(parsed, Telemetry):
+ return
+ record = {"wall_time_s": self._clock(), "telemetry": parsed.raw}
+ self._fh.write(json.dumps(record, separators=(",", ":")) + "\n")
+ self._fh.flush()
+ self.count += 1
+
+ def close(self) -> None:
+ self._fh.close()
+
+
+def main(argv: list[str] | None = None) -> None:
+ ap = argparse.ArgumentParser(description="Log MOSS firmware telemetry to JSONL (listen only).")
+ ap.add_argument("--port", required=True, help="serial device, e.g. /dev/ttyUSB0")
+ ap.add_argument("--out", required=True, help="JSONL file, appended to")
+ args = ap.parse_args(argv)
+
+ log = TelemetryLog(args.out)
+ bridge = SerialBridge(lambda: open_serial(args.port))
+ bridge.add_listener(log.on_line)
+ try:
+ while True:
+ bridge.tick()
+ time.sleep(0.01)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ bridge.close()
+ log.close()
+ print(f"{log.count} telemetry lines written to {args.out}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/moss_pi/teleop.py b/moss_pi/teleop.py
new file mode 100644
index 0000000..3f21f8e
--- /dev/null
+++ b/moss_pi/teleop.py
@@ -0,0 +1,246 @@
+"""Keyboard or gamepad teleop for MOSS on a Raspberry Pi, through SerialBridge.
+
+Reuses `moss_dimos.teleop_logic` (pure, no dimOS import): two inputs only,
+forward and yaw; deadzone; deadman with a brake window.
+
+Sequence, identical for keyboard and pad:
+1. nothing moves until the arm key/button is pressed with the deadman released;
+2. while the deadman is held, `drive L R` is sent every loop (default 50 Hz);
+3. on release, `drive 0 0` for the brake window (default 0.5 s), then `stop`;
+4. `stop` disarms the firmware, so each new drive needs a new arm.
+The bridge adds its own deadman (300 ms without drive -> stop), and the
+firmware its own (300 ms without drive -> stop and disarm).
+
+Duty mapping: track speeds from `twist_to_tracks` are scaled so that
+`max_track_speed_mps` maps to 25 % PWM duty. This is open loop: the firmware
+has no speed control, and the speed MOSS actually reaches at 25 % duty is not
+measured. The metres per second here are a scale, not a promise.
+
+ python -m moss_pi.teleop --port /dev/ttyUSB0 --keyboard
+ python -m moss_pi.teleop --port /dev/ttyUSB0 --gamepad --log drive.jsonl
+"""
+from __future__ import annotations
+
+import argparse
+import math
+import os
+import sys
+import time
+
+from moss_dimos.diffdrive import twist_to_tracks
+from moss_dimos.teleop_logic import DeadmanBrake, TeleopConfig, axes_to_twist
+
+from .serial_bridge import MAX_PERCENT, SerialBridge, open_serial
+
+# Standard dual-stick pad, pygame numbering (same as moss_dimos/gamepad.py).
+AXIS_LEFT_Y = 1
+AXIS_RIGHT_X = 2
+BUTTON_ARM = 0 # A / cross
+BUTTON_DEADMAN = 4 # L1
+BUTTON_BOOST = 5 # R1
+
+
+def tracks_to_percent(left_mps: float, right_mps: float, max_track_speed_mps: float,
+ max_pct: int = MAX_PERCENT) -> tuple[int, int]:
+ """Track speeds in m/s -> integer duty percent for `drive`, max speed -> max_pct.
+
+ Rounds half away from zero and clamps to [-max_pct, max_pct].
+ """
+ def one(v: float) -> int:
+ if not math.isfinite(v):
+ raise ValueError(f"track speed must be finite, got {v!r}")
+ p = v / max_track_speed_mps * max_pct
+ p = math.copysign(math.floor(abs(p) + 0.5), p)
+ return int(max(-max_pct, min(max_pct, p)))
+ return one(left_mps), one(right_mps)
+
+
+class TeleopSession:
+ """Turns one sample of the inputs into at most one bridge call. Pure apart from the bridge."""
+
+ def __init__(self, bridge: SerialBridge, cfg: TeleopConfig | None = None) -> None:
+ self.bridge = bridge
+ self.cfg = cfg or TeleopConfig()
+ self._brake = DeadmanBrake(self.cfg.brake_s)
+ self._arm_prev = False
+ self._driving = False # a drive was sent since the last stop
+
+ def step(self, now_s: float, arm_pressed: bool, deadman_held: bool,
+ left_y: float = 0.0, right_x: float = 0.0, boost: bool = False) -> str:
+ """Returns what was done: 'arm', 'drive L R', 'brake', 'stop', 'not_armed' or 'idle'."""
+ arm_edge = arm_pressed and not self._arm_prev
+ self._arm_prev = arm_pressed
+ if arm_edge and not deadman_held and not self._driving:
+ self.bridge.arm()
+ return "arm"
+
+ state = self._brake.tick(deadman_held, now_s)
+ if state == "command":
+ if not self.bridge.armed:
+ return "not_armed"
+ vx, wz = axes_to_twist(left_y, right_x, self.cfg, boost_held=boost)
+ left, right = tracks_to_percent(*twist_to_tracks(vx, wz, self.cfg.drive),
+ self.cfg.drive.max_track_speed_mps)
+ if self.bridge.drive(left, right):
+ self._driving = True
+ return f"drive {left} {right}"
+ return "not_armed"
+ if state == "brake" and self._driving:
+ self.bridge.drive(0, 0)
+ return "brake"
+ if state == "silent" and self._driving:
+ self._driving = False
+ self.bridge.stop()
+ return "stop"
+ return "idle"
+
+ def emergency_stop(self) -> None:
+ self._driving = False
+ self.bridge.stop()
+
+
+class KeyboardAxes:
+ """Terminal keys -> (arm, deadman, left_y, right_x).
+
+ A terminal reports key presses, not releases: a direction key counts as held
+ for `hold_s` after its last repeat. `hold_s` must cover the keyboard's
+ auto-repeat delay (often 0.5 s), so a keyboard brakes later than a pad.
+
+ w/s forward/back, a/d turn left/right, r arm, space stop now, q quit.
+ """
+
+ def __init__(self, speed: float = 0.5, hold_s: float = 0.6) -> None:
+ if not 0 < speed <= 1:
+ raise ValueError("speed must be in (0, 1]")
+ self.speed, self.hold_s = speed, hold_s
+ self._seen: dict[str, float] = {}
+ self._arm = False
+ self.stop_requested = False
+ self.quit_requested = False
+
+ def feed(self, key: str, now_s: float) -> None:
+ k = key.lower()
+ if k in "wasd" and len(k) == 1:
+ self._seen[k] = now_s
+ elif k == "r":
+ self._arm = True
+ elif k == " ":
+ self._seen.clear()
+ self.stop_requested = True
+ elif k == "q":
+ self._seen.clear()
+ self.quit_requested = True
+
+ def sample(self, now_s: float) -> tuple[bool, bool, float, float]:
+ live = {k for k, t in self._seen.items() if now_s - t <= self.hold_s}
+ arm, self._arm = self._arm, False
+ # Stick convention of teleop_logic: forward is left_y < 0, right turn is right_x > 0.
+ ly = self.speed * ((("s" in live) - ("w" in live)))
+ rx = self.speed * ((("d" in live) - ("a" in live)))
+ return arm, bool(live), float(ly), float(rx)
+
+
+def _run_keyboard(session: TeleopSession, bridge: SerialBridge, speed: float) -> None:
+ import select
+ import termios
+ import tty
+
+ keys = KeyboardAxes(speed=speed)
+ fd = sys.stdin.fileno()
+ saved = termios.tcgetattr(fd)
+ period = 1.0 / session.cfg.rate_hz
+ print("r arm | hold w/s/a/d to drive | space stop | q quit", end="\r\n")
+ try:
+ tty.setcbreak(fd)
+ while not keys.quit_requested:
+ while select.select([sys.stdin], [], [], 0)[0]:
+ keys.feed(os.read(fd, 1).decode(errors="ignore"), time.monotonic())
+ if keys.stop_requested:
+ keys.stop_requested = False
+ session.emergency_stop()
+ arm, held, ly, rx = keys.sample(time.monotonic())
+ _report(session.step(time.monotonic(), arm, held, ly, rx))
+ bridge.tick()
+ time.sleep(period)
+ finally:
+ termios.tcsetattr(fd, termios.TCSADRAIN, saved)
+
+
+def _run_gamepad(session: TeleopSession, bridge: SerialBridge) -> None:
+ os.environ.setdefault("SDL_VIDEODRIVER", "dummy")
+ os.environ.setdefault("SDL_AUDIODRIVER", "dummy")
+ os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")
+ import pygame # optional dependency: pip install 'moss-dimos[pi]'
+
+ pygame.init()
+ pygame.joystick.init()
+ period = 1.0 / session.cfg.rate_hz
+ pad = None
+ print("A/cross arm | hold L1 + sticks to drive | R1 boost | Ctrl-C quit")
+ while True:
+ if pad is None:
+ pygame.joystick.quit(); pygame.joystick.init()
+ if pygame.joystick.get_count() == 0:
+ bridge.tick(); time.sleep(0.5); continue
+ pad = pygame.joystick.Joystick(0); pad.init()
+ print(f"gamepad connected: {pad.get_name()}")
+ pygame.event.pump()
+ try:
+ arm = bool(pad.get_button(BUTTON_ARM))
+ held = bool(pad.get_button(BUTTON_DEADMAN))
+ boost = bool(pad.get_button(BUTTON_BOOST))
+ ly, rx = pad.get_axis(AXIS_LEFT_Y), pad.get_axis(AXIS_RIGHT_X)
+ except pygame.error:
+ print("gamepad lost: stop")
+ session.emergency_stop()
+ pad = None
+ continue
+ _report(session.step(time.monotonic(), arm, held, ly, rx, boost))
+ bridge.tick()
+ time.sleep(period)
+
+
+_last_report = ""
+
+
+def _report(action: str) -> None:
+ global _last_report
+ kind = action.split()[0]
+ if kind != _last_report: # one line per change, not one per loop
+ print(action, end="\r\n")
+ _last_report = kind
+
+
+def main(argv: list[str] | None = None) -> None:
+ ap = argparse.ArgumentParser(description="Drive MOSS from a Pi through the ESP32-S3 firmware.")
+ ap.add_argument("--port", required=True, help="serial device, e.g. /dev/ttyUSB0")
+ mode = ap.add_mutually_exclusive_group(required=True)
+ mode.add_argument("--keyboard", action="store_true")
+ mode.add_argument("--gamepad", action="store_true")
+ ap.add_argument("--key-speed", type=float, default=0.5, help="keyboard stick deflection, 0..1 (default 0.5)")
+ ap.add_argument("--log", help="also append telemetry to this JSONL file")
+ args = ap.parse_args(argv)
+
+ bridge = SerialBridge(lambda: open_serial(args.port))
+ log = None
+ if args.log:
+ from .telemetry_log import TelemetryLog
+ log = TelemetryLog(args.log)
+ bridge.add_listener(log.on_line)
+ bridge.add_listener(lambda line, parsed: print(line, end="\r\n") if line.startswith("ERR") else None)
+ session = TeleopSession(bridge)
+ try:
+ if args.keyboard:
+ _run_keyboard(session, bridge, args.key_speed)
+ else:
+ _run_gamepad(session, bridge)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ bridge.close() # sends a last stop
+ if log:
+ log.close()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/pyproject.toml b/pyproject.toml
index 5f5aa75..f096d01 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -9,7 +9,8 @@ dependencies = ["numpy"]
[project.optional-dependencies]
gamepad = ["pygame>=2.6"]
+pi = ["pyserial>=3.5", "pygame>=2.6"]
test = ["pytest"]
[tool.setuptools]
-packages = ["moss_dimos"]
+packages = ["moss_dimos", "moss_pi"]
diff --git a/tests/fake_serial.py b/tests/fake_serial.py
new file mode 100644
index 0000000..08b7a2e
--- /dev/null
+++ b/tests/fake_serial.py
@@ -0,0 +1,87 @@
+"""A fake serial port and a fake clock for cold tests of moss_pi (no pyserial, no robot).
+
+FakeSerial replays firmware lines queued with `feed_line` / `feed_recording`
+and keeps every byte the bridge writes in `written`. `fail_next_io()` makes
+the next read or write raise, like an unplugged USB adapter.
+"""
+from __future__ import annotations
+
+from pathlib import Path
+
+FIXTURES = Path(__file__).parent / "fixtures"
+# 52 telemetry lines of the 20 September 2026 bench test at the 25 % limit
+# (hardware/bench/20260920/ina_bypassed_NOT_motor_current.json), rewritten in
+# the firmware's exact snprintf format; host-side fields dropped.
+RECORDING = FIXTURES / "firmware_telemetry_20260920.txt"
+
+
+class FakeClock:
+ def __init__(self, t: float = 0.0) -> None:
+ self.t = t
+
+ def __call__(self) -> float:
+ return self.t
+
+
+class FakeSerial:
+ def __init__(self) -> None:
+ self.rx = b""
+ self.written: list[bytes] = []
+ self.closed = False
+ self._fail = False
+
+ # firmware -> Pi
+ def feed_line(self, text: str) -> None:
+ self.rx += text.encode("ascii") + b"\n"
+
+ def feed_bytes(self, data: bytes) -> None:
+ self.rx += data
+
+ def feed_recording(self, path: Path = RECORDING) -> list[str]:
+ lines = path.read_text().splitlines()
+ for line in lines:
+ self.feed_line(line)
+ return lines
+
+ def fail_next_io(self) -> None:
+ self._fail = True
+
+ # pyserial subset
+ def read(self, size: int = 1) -> bytes:
+ self._check()
+ out, self.rx = self.rx[:size], self.rx[size:]
+ return out
+
+ def write(self, data: bytes) -> int:
+ self._check()
+ self.written.append(bytes(data))
+ return len(data)
+
+ def close(self) -> None:
+ self.closed = True
+
+ def _check(self) -> None:
+ if self.closed:
+ raise OSError("port closed")
+ if self._fail:
+ self._fail = False
+ raise OSError("device disconnected")
+
+
+class PortFactory:
+ """open_port callable for SerialBridge: hands out FakeSerial objects, or fails while `absent`."""
+
+ def __init__(self) -> None:
+ self.ports: list[FakeSerial] = []
+ self.absent = False
+
+ def __call__(self) -> FakeSerial:
+ if self.absent:
+ raise OSError("no such device")
+ p = FakeSerial()
+ self.ports.append(p)
+ return p
+
+ @property
+ def port(self) -> FakeSerial:
+ return self.ports[-1]
diff --git a/tests/fixtures/firmware_telemetry_20260920.txt b/tests/fixtures/firmware_telemetry_20260920.txt
new file mode 100644
index 0000000..8165573
--- /dev/null
+++ b/tests/fixtures/firmware_telemetry_20260920.txt
@@ -0,0 +1,52 @@
+{"ms":304202,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
+{"ms":304402,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-460,"current_mA":null}
+{"ms":304602,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
+{"ms":304802,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11948,"shunt_uV":-500,"current_mA":null}
+{"ms":305002,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-420,"current_mA":null}
+{"ms":305202,"mode":1,"reason":"armed","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
+{"ms":305402,"mode":1,"reason":"armed","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-460,"current_mA":null}
+{"ms":305602,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[20,0],"ticks":[-60,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11916,"shunt_uV":-700,"current_mA":null}
+{"ms":305802,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-330,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11920,"shunt_uV":-700,"current_mA":null}
+{"ms":306001,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-651,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11924,"shunt_uV":-720,"current_mA":null}
+{"ms":306202,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-979,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11916,"shunt_uV":-680,"current_mA":null}
+{"ms":306402,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-1306,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11920,"shunt_uV":-760,"current_mA":null}
+{"ms":306602,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-1634,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11924,"shunt_uV":-660,"current_mA":null}
+{"ms":306802,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-1962,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11920,"shunt_uV":-740,"current_mA":null}
+{"ms":307002,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-2288,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11924,"shunt_uV":-660,"current_mA":null}
+{"ms":307202,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-2616,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11924,"shunt_uV":-680,"current_mA":null}
+{"ms":307402,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[25,0],"ticks":[-2944,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11920,"shunt_uV":-700,"current_mA":null}
+{"ms":307602,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
+{"ms":307802,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-350,"current_mA":null}
+{"ms":308002,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
+{"ms":308202,"mode":1,"reason":"armed","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11948,"shunt_uV":-410,"current_mA":null}
+{"ms":308402,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,4],"ticks":[-3022,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11932,"shunt_uV":-610,"current_mA":null}
+{"ms":308602,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,24],"ticks":[-3022,-109],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11916,"shunt_uV":-850,"current_mA":null}
+{"ms":308802,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-412],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11924,"shunt_uV":-760,"current_mA":null}
+{"ms":309002,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-741],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11928,"shunt_uV":-760,"current_mA":null}
+{"ms":309202,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-1072],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11920,"shunt_uV":-850,"current_mA":null}
+{"ms":309402,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-1404],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11920,"shunt_uV":-720,"current_mA":null}
+{"ms":309601,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-1736],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11924,"shunt_uV":-700,"current_mA":null}
+{"ms":309802,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-2070],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11928,"shunt_uV":-660,"current_mA":null}
+{"ms":310002,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-2403],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11916,"shunt_uV":-720,"current_mA":null}
+{"ms":310202,"mode":2,"reason":"drive","target_pct":[0,25],"output_pct":[0,25],"ticks":[-3022,-2736],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11916,"shunt_uV":-740,"current_mA":null}
+{"ms":310402,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,-3047],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11940,"shunt_uV":-410,"current_mA":null}
+{"ms":310602,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,-3073],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11940,"shunt_uV":-310,"current_mA":null}
+{"ms":310802,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,-3073],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
+{"ms":311002,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,-3073],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11940,"shunt_uV":-390,"current_mA":null}
+{"ms":311202,"mode":1,"reason":"armed","target_pct":[0,0],"output_pct":[0,0],"ticks":[-3022,-3073],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-420,"current_mA":null}
+{"ms":311402,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[9,9],"ticks":[-3028,-3079],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11908,"shunt_uV":-630,"current_mA":null}
+{"ms":311602,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-3198,-3253],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11896,"shunt_uV":-1030,"current_mA":null}
+{"ms":311802,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-3514,-3574],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11896,"shunt_uV":-850,"current_mA":null}
+{"ms":312002,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-3839,-3905],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11892,"shunt_uV":-1000,"current_mA":null}
+{"ms":312202,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-4164,-4238],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11896,"shunt_uV":-1070,"current_mA":null}
+{"ms":312402,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-4489,-4570],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11896,"shunt_uV":-1030,"current_mA":null}
+{"ms":312602,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-4814,-4902],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11888,"shunt_uV":-960,"current_mA":null}
+{"ms":312802,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-5139,-5233],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11904,"shunt_uV":-1090,"current_mA":null}
+{"ms":313002,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-5465,-5565],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11896,"shunt_uV":-850,"current_mA":null}
+{"ms":313202,"mode":2,"reason":"drive","target_pct":[25,25],"output_pct":[25,25],"ticks":[-5791,-5897],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11892,"shunt_uV":-1050,"current_mA":null}
+{"ms":313402,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-6044,-6157],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11940,"shunt_uV":-370,"current_mA":null}
+{"ms":313602,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-6049,-6163],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11940,"shunt_uV":-410,"current_mA":null}
+{"ms":313802,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-6049,-6163],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-460,"current_mA":null}
+{"ms":314002,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-6049,-6163],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11940,"shunt_uV":-410,"current_mA":null}
+{"ms":314202,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-6049,-6163],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-420,"current_mA":null}
+{"ms":314402,"mode":0,"reason":"stop","target_pct":[0,0],"output_pct":[0,0],"ticks":[-6049,-6163],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11944,"shunt_uV":-390,"current_mA":null}
diff --git a/tests/test_pi_teleop_cold.py b/tests/test_pi_teleop_cold.py
new file mode 100644
index 0000000..6fb88d5
--- /dev/null
+++ b/tests/test_pi_teleop_cold.py
@@ -0,0 +1,155 @@
+"""Cold tests of moss_pi.teleop: arm step, deadman, brake on release, duty mapping, keyboard hold."""
+import math
+import subprocess
+import sys
+
+import pytest
+
+from fake_serial import FakeClock, PortFactory
+from moss_dimos.diffdrive import DiffDriveConfig
+from moss_dimos.teleop_logic import TeleopConfig
+from moss_pi.serial_bridge import SerialBridge
+from moss_pi.teleop import KeyboardAxes, TeleopSession, tracks_to_percent
+
+CFG = TeleopConfig(drive=DiffDriveConfig(track_width_m=0.30, max_track_speed_mps=0.6),
+ max_yaw_rps=2.0, deadzone=0.08, brake_s=0.5, rate_hz=50.0)
+
+
+def make():
+ clock, ports = FakeClock(), PortFactory()
+ bridge = SerialBridge(ports, clock=clock)
+ bridge.tick()
+ ports.port.written.clear()
+ return TeleopSession(bridge, CFG), bridge, ports, clock
+
+
+def run(session, bridge, clock, t0, t1, **inputs):
+ """Step session and bridge every 20 ms over [t0, t1)."""
+ actions = []
+ for i in range(round((t1 - t0) / 0.02)):
+ clock.t = t0 + i * 0.02
+ actions.append(session.step(clock.t, **inputs))
+ bridge.tick()
+ return actions
+
+
+# ---------------------------------------------------------------- duty mapping
+
+def test_full_speed_maps_to_25_percent_and_0_24_mps_to_10():
+ assert tracks_to_percent(0.6, 0.6, 0.6) == (25, 25)
+ assert tracks_to_percent(0.24, -0.24, 0.6) == (10, -10)
+
+
+def test_rounding_is_half_away_from_zero_and_clamped():
+ assert tracks_to_percent(0.012, -0.012, 0.6) == (1, -1) # 0.5 % -> 1
+ assert tracks_to_percent(9.0, -9.0, 0.6) == (25, -25)
+ with pytest.raises(ValueError):
+ tracks_to_percent(math.nan, 0.0, 0.6)
+
+
+# ---------------------------------------------------------------- session
+
+def test_deadman_without_arm_sends_no_drive():
+ session, bridge, ports, clock = make()
+ actions = run(session, bridge, clock, 0.0, 1.0, arm_pressed=False, deadman_held=True, left_y=-1.0)
+ assert set(actions) == {"not_armed"}
+ assert not any(w.startswith(b"drive") for w in ports.port.written)
+
+
+def test_arm_is_refused_while_the_deadman_is_held():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=True, left_y=-1.0)
+ assert b"arm\n" not in ports.port.written
+
+
+def test_full_forward_after_arm_is_drive_25_25():
+ session, bridge, ports, clock = make()
+ assert session.step(0.0, arm_pressed=True, deadman_held=False) == "arm"
+ assert session.step(0.02, arm_pressed=False, deadman_held=True, left_y=-1.0) == "drive 25 25"
+ assert ports.port.written == [b"arm\n", b"drive 25 25\n"]
+
+
+def test_half_forward_after_deadzone_is_drive_13_13():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=False)
+ # (0.54 - 0.08) / 0.92 = 0.5 of 0.6 m/s = 0.3 m/s -> 12.5 % -> 13
+ assert session.step(0.02, arm_pressed=False, deadman_held=True, left_y=-0.54) == "drive 13 13"
+
+
+def test_right_stick_right_turns_right_left_track_forward():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=False)
+ # 2 rad/s * 0.15 m = 0.3 m/s per track -> 12.5 % -> 13, left forward, right back
+ assert session.step(0.02, arm_pressed=False, deadman_held=True, right_x=1.0) == "drive 13 -13"
+
+
+def test_release_brakes_with_zeros_for_0_5_s_then_stops_once():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=False)
+ run(session, bridge, clock, 0.02, 1.02, arm_pressed=False, deadman_held=True, left_y=-1.0)
+ ports.port.written.clear()
+ actions = run(session, bridge, clock, 1.02, 2.02, arm_pressed=False, deadman_held=False)
+ assert actions[0] == "brake"
+ brake = [w for w in ports.port.written if w == b"drive 0 0\n"]
+ assert 24 <= len(brake) <= 26 # 0.5 s at 50 Hz
+ assert ports.port.written[len(brake)] == b"stop\n"
+ assert ports.port.written.count(b"stop\n") == 1 # the bridge deadman adds none
+ assert not any(w.startswith(b"drive") and w != b"drive 0 0\n" for w in ports.port.written)
+ assert not bridge.armed
+
+
+def test_new_drive_after_release_needs_a_new_arm():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=False)
+ run(session, bridge, clock, 0.02, 0.2, arm_pressed=False, deadman_held=True, left_y=-1.0)
+ run(session, bridge, clock, 0.2, 1.0, arm_pressed=False, deadman_held=False)
+ assert run(session, bridge, clock, 1.0, 1.1, arm_pressed=False, deadman_held=True, left_y=-1.0)[0] == "not_armed"
+
+
+def test_a_stalled_loop_is_caught_by_the_bridge_deadman():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=False)
+ session.step(0.02, arm_pressed=False, deadman_held=True, left_y=-1.0)
+ ports.port.written.clear()
+ clock.t = 0.32 # the teleop loop hangs; only the bridge thread ticks
+ bridge.tick()
+ assert ports.port.written == [b"stop\n"]
+
+
+def test_emergency_stop_writes_stop_and_disarms():
+ session, bridge, ports, clock = make()
+ session.step(0.0, arm_pressed=True, deadman_held=False)
+ session.step(0.02, arm_pressed=False, deadman_held=True, left_y=-1.0)
+ session.emergency_stop()
+ assert ports.port.written[-1] == b"stop\n" and not bridge.armed
+
+
+# ---------------------------------------------------------------- keyboard
+
+def test_keyboard_w_is_forward_at_the_chosen_deflection_and_expires():
+ keys = KeyboardAxes(speed=0.5, hold_s=0.6)
+ keys.feed("w", 0.0)
+ assert keys.sample(0.1) == (False, True, -0.5, 0.0)
+ assert keys.sample(0.61) == (False, False, 0.0, 0.0)
+
+
+def test_keyboard_d_turns_right_and_r_arms_once():
+ keys = KeyboardAxes(speed=0.5)
+ keys.feed("d", 0.0); keys.feed("r", 0.0)
+ assert keys.sample(0.0) == (True, True, 0.0, 0.5)
+ assert keys.sample(0.02)[0] is False
+
+
+def test_keyboard_space_clears_everything_and_requests_stop():
+ keys = KeyboardAxes()
+ keys.feed("w", 0.0); keys.feed(" ", 0.1)
+ assert keys.stop_requested and keys.sample(0.1)[1] is False
+
+
+# ---------------------------------------------------------------- no dimOS
+
+def test_moss_pi_imports_without_dimos_pyserial_or_pygame():
+ code = ("import sys, moss_pi.serial_bridge, moss_pi.teleop, moss_pi.telemetry_log; "
+ "bad = [m for m in ('dimos', 'serial', 'pygame', 'numpy') if m in sys.modules]; "
+ "assert not bad, bad")
+ subprocess.run([sys.executable, "-c", code], check=True)
diff --git a/tests/test_serial_bridge_cold.py b/tests/test_serial_bridge_cold.py
new file mode 100644
index 0000000..c0fb2ea
--- /dev/null
+++ b/tests/test_serial_bridge_cold.py
@@ -0,0 +1,232 @@
+"""Cold tests of moss_pi.serial_bridge: exact command bytes, telemetry parsing, deadman, reconnect."""
+import pytest
+
+from fake_serial import FakeClock, PortFactory
+from moss_pi.serial_bridge import (Info, Reply, SerialBridge, Telemetry, format_drive, format_simple,
+ parse_line)
+
+
+def make():
+ clock, ports = FakeClock(), PortFactory()
+ bridge = SerialBridge(ports, clock=clock)
+ bridge.tick() # opens the port, writes the initial stop
+ ports.port.written.clear()
+ return bridge, ports, clock
+
+
+# ---------------------------------------------------------------- command lines
+
+def test_drive_plus_10_plus_10_is_the_exact_firmware_line():
+ assert format_drive(10, 10) == b"drive 10 10\n"
+
+
+def test_drive_limits_and_signs_are_written_as_the_firmware_parses_them():
+ assert format_drive(-25, 25) == b"drive -25 25\n"
+ assert format_drive(0, 0) == b"drive 0 0\n"
+
+
[email protected]("left,right", [(26, 0), (0, -26), (10.0, 10), (True, 0), ("10", 10), (None, 0)])
+def test_out_of_contract_drive_is_refused_before_the_wire(left, right):
+ with pytest.raises(ValueError):
+ format_drive(left, right)
+
+
+def test_simple_commands_and_nothing_else():
+ assert [format_simple(c) for c in ("status", "stop", "zero", "arm")] == [b"status\n", b"stop\n", b"zero\n", b"arm\n"]
+ for bad in ("test left 10", "drive", "reboot", "STOP"):
+ with pytest.raises(ValueError):
+ format_simple(bad)
+
+
+# ---------------------------------------------------------------- roundtrip through the fake port
+
+def test_arm_then_drive_10_10_reaches_the_port_byte_for_byte():
+ bridge, ports, _ = make()
+ assert bridge.arm() and bridge.drive(10, 10)
+ assert ports.port.written == [b"arm\n", b"drive 10 10\n"]
+
+
+def test_drive_without_arm_sends_nothing():
+ bridge, ports, _ = make()
+ assert bridge.drive(10, 10) is False
+ assert ports.port.written == []
+
+
+def test_first_line_on_a_new_link_is_stop():
+ clock, ports = FakeClock(), PortFactory()
+ bridge = SerialBridge(ports, clock=clock)
+ bridge.tick()
+ assert ports.port.written == [b"stop\n"] and not bridge.armed
+
+
+# ---------------------------------------------------------------- deadman
+
+def test_300_ms_of_silence_after_a_drive_produces_one_stop():
+ bridge, ports, clock = make()
+ bridge.arm(); bridge.drive(10, 10)
+ ports.port.written.clear()
+ clock.t = 0.299
+ bridge.tick()
+ assert ports.port.written == [] # 299 ms: still driving
+ clock.t = 0.300
+ bridge.tick()
+ assert ports.port.written == [b"stop\n"] # 300 ms: stop
+ assert not bridge.armed
+ clock.t = 5.0
+ bridge.tick()
+ assert ports.port.written == [b"stop\n"] # once, not a flood
+
+
+def test_drive_every_20_ms_keeps_the_deadman_quiet_for_2_s():
+ bridge, ports, clock = make()
+ bridge.arm()
+ for i in range(100):
+ clock.t = i * 0.020
+ bridge.drive(5, 5)
+ bridge.tick()
+ assert b"stop\n" not in ports.port.written
+ assert ports.port.written.count(b"drive 5 5\n") == 100
+
+
+def test_after_a_deadman_stop_drive_needs_a_new_arm():
+ bridge, ports, clock = make()
+ bridge.arm(); bridge.drive(10, 10)
+ clock.t = 0.5; bridge.tick()
+ assert bridge.drive(10, 10) is False
+ assert bridge.arm() and bridge.drive(10, 10)
+
+
+def test_arm_expires_after_10_s_without_drive_like_the_firmware():
+ bridge, ports, clock = make()
+ bridge.arm()
+ clock.t = 9.99; bridge.tick(); assert bridge.armed
+ clock.t = 10.0; bridge.tick(); assert not bridge.armed
+ assert bridge.drive(10, 10) is False
+
+
+def test_any_err_reply_disarms_the_bridge_like_the_firmware():
+ bridge, ports, clock = make()
+ bridge.arm(); bridge.drive(10, 10)
+ ports.port.feed_line("ERR invalid_command")
+ bridge.tick()
+ assert not bridge.armed and bridge.last_reply == Reply(ok=False, error="invalid_command")
+ assert bridge.drive(10, 10) is False
+
+
+# ---------------------------------------------------------------- reconnect
+
+def test_lost_port_disarms_and_reopens_with_a_stop():
+ bridge, ports, clock = make()
+ bridge.arm(); bridge.drive(10, 10)
+ ports.port.fail_next_io()
+ assert bridge.drive(10, 10) is False # write fails: port dropped
+ assert not bridge.connected and not bridge.armed and ports.port.closed
+ ports.absent = True
+ clock.t = 1.0; bridge.tick()
+ assert not bridge.connected # device still gone: retry later
+ ports.absent = False
+ clock.t = 1.5; bridge.tick()
+ assert not bridge.connected # 1 s back-off after the failed attempt
+ clock.t = 2.0; bridge.tick()
+ assert bridge.connected and len(ports.ports) == 2
+ assert ports.port.written == [b"stop\n"]
+ assert bridge.drive(10, 10) is False # re-arm required
+
+
+def test_read_error_drops_the_port():
+ bridge, ports, clock = make()
+ ports.port.fail_next_io()
+ bridge.tick()
+ assert not bridge.connected
+
+
+def test_close_sends_a_last_stop():
+ bridge, ports, _ = make()
+ first = ports.port
+ bridge.close()
+ assert first.written == [b"stop\n"] and first.closed
+
+
+# ---------------------------------------------------------------- telemetry
+
+def test_recorded_line_parses_to_known_values_in_physical_units():
+ t = parse_line('{"ms":305602,"mode":2,"reason":"drive","target_pct":[25,0],"output_pct":[20,0],'
+ '"ticks":[-60,0],"invalid_edges":[0,0],"ina_ok":true,"bus_mV":11916,"shunt_uV":-700,"current_mA":null}')
+ assert isinstance(t, Telemetry)
+ assert t.mode_name == "drive" and t.reason == "drive"
+ assert t.target_pct == (25, 0) and t.output_pct == (20, 0) and t.ticks == (-60, 0)
+ assert t.bus_V == 11.916 and t.uptime_s == 305.602
+ assert t.current_mA is None
+
+
+def test_replies_and_other_lines():
+ assert parse_line("OK") == Reply(ok=True)
+ assert parse_line("ERR not_armed") == Reply(ok=False, error="not_armed")
+ assert parse_line("MOSS V0.3 bench firmware; USB-UART 115200; limit 25%; type status") == \
+ Info("MOSS V0.3 bench firmware; USB-UART 115200; limit 25%; type status")
+
+
[email protected]("line", ['{"ms":1', '{"ms":1,"mode":0}', '{"ms":"1","mode":0,"reason":"x","target_pct":[0,0],'
+ '"output_pct":[0,0],"ticks":[0,0],"invalid_edges":[0,0],"ina_ok":false,'
+ '"bus_mV":null,"shunt_uV":null,"current_mA":null}', "{\x00}"])
+def test_truncated_or_malformed_json_is_info_never_telemetry(line):
+ assert isinstance(parse_line(line), Info)
+
+
+def test_recorded_session_replays_in_order_through_the_bridge():
+ bridge, ports, _ = make()
+ lines = ports.port.feed_recording()
+ seen = []
+ bridge.add_listener(lambda text, parsed: seen.append(parsed))
+ while ports.port.rx:
+ bridge.tick()
+ assert len(seen) == len(lines) == 52 and all(isinstance(p, Telemetry) for p in seen)
+ assert [p.mode_name for p in seen[4:8]] == ["off", "ready", "ready", "drive"]
+ last = seen[-1]
+ assert last.mode_name == "off" and last.ticks == (-6049, -6163) and last.bus_V == 11.944
+
+
+def test_a_line_split_across_reads_is_reassembled():
+ bridge, ports, _ = make()
+ seen = []
+ bridge.add_listener(lambda text, parsed: seen.append(parsed))
+ ports.port.feed_bytes(b"O")
+ bridge.tick()
+ ports.port.feed_bytes(b"K\r\nERR inv")
+ bridge.tick()
+ ports.port.feed_bytes(b"alid_command\n")
+ bridge.tick()
+ assert seen == [Reply(ok=True), Reply(ok=False, error="invalid_command")]
+
+
+def test_real_pyserial_on_a_pseudo_terminal_reads_telemetry_and_writes_exact_bytes():
+ """The one place pyserial itself runs: a Linux pty stands in for the USB-UART adapter."""
+ pytest.importorskip("serial")
+ import os
+ import time
+
+ from moss_pi.serial_bridge import open_serial
+
+ if not hasattr(os, "openpty"):
+ pytest.skip("no pty on this platform")
+ master, slave = os.openpty()
+ try:
+ bridge = SerialBridge(lambda: open_serial(os.ttyname(slave)))
+ bridge.tick()
+ assert bridge.connected
+ os.write(master, b'{"ms":1,"mode":0,"reason":"boot","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],'
+ b'"invalid_edges":[0,0],"ina_ok":true,"bus_mV":12000,"shunt_uV":0,"current_mA":null}\n')
+ deadline = time.monotonic() + 2.0
+ while bridge.last_telemetry is None and time.monotonic() < deadline:
+ bridge.tick(); time.sleep(0.01)
+ assert bridge.last_telemetry is not None and bridge.last_telemetry.bus_V == 12.0
+ bridge.arm(); bridge.drive(10, 10)
+ time.sleep(0.05)
+ assert os.read(master, 1024) == b"stop\narm\ndrive 10 10\n"
+ bridge.close()
+ finally:
+ os.close(master)
+ try:
+ os.close(slave)
+ except OSError:
+ pass
diff --git a/tests/test_telemetry_log_cold.py b/tests/test_telemetry_log_cold.py
new file mode 100644
index 0000000..66a56c4
--- /dev/null
+++ b/tests/test_telemetry_log_cold.py
@@ -0,0 +1,43 @@
+"""Cold tests of moss_pi.telemetry_log: every telemetry line, and only those, with a wall-clock time."""
+import json
+
+from fake_serial import FakeClock, PortFactory
+from moss_pi.serial_bridge import SerialBridge
+from moss_pi.telemetry_log import TelemetryLog
+
+
+def test_recorded_session_is_logged_line_for_line_with_wall_time(tmp_path):
+ wall = FakeClock(1790000000.0)
+ log = TelemetryLog(tmp_path / "t.jsonl", wall_clock=wall)
+ bridge = SerialBridge(PortFactory(), clock=FakeClock())
+ bridge.add_listener(log.on_line)
+ bridge.tick()
+ port = bridge._port
+ port.feed_line("MOSS V0.3 bench firmware; USB-UART 115200; limit 25%; type status")
+ port.feed_line("OK")
+ lines = port.feed_recording()
+ port.feed_line("ERR invalid_command")
+ while port.rx:
+ wall.t += 0.2
+ bridge.tick()
+ log.close()
+
+ records = [json.loads(r) for r in (tmp_path / "t.jsonl").read_text().splitlines()]
+ assert len(records) == 52 == log.count
+ assert [r["telemetry"] for r in records] == [json.loads(line) for line in lines]
+ assert records[0]["wall_time_s"] >= 1790000000.0
+ assert all(b["wall_time_s"] >= a["wall_time_s"] for a, b in zip(records, records[1:]))
+
+
+def test_log_appends_and_does_not_truncate(tmp_path):
+ path = tmp_path / "t.jsonl"
+ path.write_text('{"earlier":true}\n')
+ log = TelemetryLog(path, wall_clock=lambda: 1.5)
+ from moss_pi.serial_bridge import parse_line
+ line = ('{"ms":1,"mode":0,"reason":"boot","target_pct":[0,0],"output_pct":[0,0],"ticks":[0,0],'
+ '"invalid_edges":[0,0],"ina_ok":false,"bus_mV":null,"shunt_uV":null,"current_mA":null}')
+ log.on_line(line, parse_line(line))
+ log.close()
+ rows = path.read_text().splitlines()
+ assert rows[0] == '{"earlier":true}'
+ assert json.loads(rows[1]) == {"wall_time_s": 1.5, "telemetry": json.loads(line)}