MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
"""Minimal RPLIDAR C1 reader over pyserial: the standard SLAMTEC scan, no third-party lib.
The C1 answers the classic SLAMTEC protocol: SCAN (A5 20) -> 7-byte descriptor
(A5 5A 05 00 00 40 81) -> endless 5-byte samples. rplidar-roboticia 0.9.5
rejects the C1's descriptor ("Descriptor length mismatch"), while the raw
protocol below works as is. Measured on the bench: ~3.8 k samples/s at
460800 baud, 10 Hz scans.
Sample (5 bytes): b0: S (bit0), !S (bit1), quality (bits 2-7)
b1: C (bit0, always 1), angle_q6 low 7 bits
b2: angle_q6 high 8 bits -> angle_deg = angle_q6 / 64
b3-b4: distance_q2 (u16 LE) -> dist_mm = distance_q2 / 4
"""
from __future__ import annotations
import time
from collections.abc import Iterator
import serial
SCAN = b"\xa5\x20"
STOP = b"\xa5\x25"
RESET = b"\xa5\x40"
GET_HEALTH = b"\xa5\x52"
GET_INFO = b"\xa5\x50"
SCAN_DESCRIPTOR = b"\xa5\x5a\x05\x00\x00\x40\x81"
SPINUP_GRACE_S = 10.0 # motor spin-up after a STOP before the first sample
class C1Scanner:
def __init__(self, port: str, baudrate: int = 460800, timeout: float = 2.0):
self.port, self.baudrate, self.timeout = port, baudrate, timeout
self._ser: serial.Serial | None = None
# ── lifecycle ──────────────────────────────────────────────────────
def open(self) -> None:
self._ser = serial.Serial(self.port, self.baudrate, timeout=self.timeout)
# The modem lines matter on the C1's CP2102 bridge: the C1 answers
# with exactly ONE of DTR/RTS asserted and stays mute with both low or
# both high. The cp210x driver keeps the last state across opens, so a
# process killed at the wrong moment leaves the mute combination in
# place and every later open inherits it (a power cycle does not
# clear it). DTR low + RTS high is the known-good state; set BOTH,
# on every open.
self._ser.dtr = False
self._ser.rts = True
self.stop()
self._wake()
def _wake(self) -> None:
"""Wake a C1 that stays numb after a hot unplug: its link can answer
nothing until it receives a flood of requests. Probe GET_INFO; on
silence, toggle RTS (the rising edge resets the C1), then send bursts
of GET_INFO for up to 6 s.
6 s: a numb C1 has always woken within that; waiting longer only
delays the verdict."""
try:
self.info()
return
except Exception: # noqa: BLE001 - numb: go wake it up
pass
self._ser.rts = False
time.sleep(0.3)
self._ser.rts = True # rising edge: the C1 reboots (banner) and listens again
time.sleep(2.5)
self._ser.reset_input_buffer()
try:
self.info()
return
except Exception: # noqa: BLE001 - still nothing: the flood, then
pass
deadline = time.monotonic() + 6.0
while time.monotonic() < deadline:
burst_until = time.monotonic() + 1.0
while time.monotonic() < burst_until:
self._ser.write(bytes([0xA5, 0x50]) * 50)
time.sleep(0.1)
self._ser.reset_input_buffer()
try:
self.info()
return
except Exception: # noqa: BLE001 - still numb: keep insisting
pass
raise RuntimeError("lidar silent even after the wake-up burst")
def close(self) -> None:
if self._ser is not None:
try:
self.stop()
finally:
self._ser.close()
self._ser = None
def __enter__(self) -> C1Scanner:
self.open()
return self
def __exit__(self, *exc: object) -> None:
self.close()
def stop(self) -> None:
assert self._ser is not None
self._ser.write(STOP)
time.sleep(0.05)
self._ser.reset_input_buffer()
def reset(self) -> None:
assert self._ser is not None
self._ser.write(RESET)
time.sleep(2.0)
self._ser.reset_input_buffer()
# ── queries ────────────────────────────────────────────────────────
def _query(self, cmd: bytes, payload_len: int) -> bytes:
assert self._ser is not None
self._ser.reset_input_buffer()
self._ser.write(cmd)
head = self._ser.read(7)
if len(head) != 7 or head[:2] != b"\xa5\x5a":
raise RuntimeError(f"no descriptor for {cmd.hex()}: {head.hex()}")
return self._ser.read(payload_len)
def health(self) -> tuple[str, int]:
p = self._query(GET_HEALTH, 3)
status = {0: "Good", 1: "Warning", 2: "Error"}.get(p[0], str(p[0]))
return status, p[1] | (p[2] << 8)
def info(self) -> dict[str, object]:
p = self._query(GET_INFO, 20)
return {"model": p[0], "firmware": (p[2], p[1]), "hardware": p[3],
"serial": p[4:20].hex().upper()}
# ── scanning ───────────────────────────────────────────────────────
def samples(self) -> Iterator[tuple[bool, int, float, float]]:
"""Yield (new_turn, quality, angle_deg, dist_m) forever; dist 0.0 = no return."""
assert self._ser is not None
s = self._ser
s.reset_input_buffer()
s.write(SCAN)
head = s.read(7)
if head != SCAN_DESCRIPTOR:
raise RuntimeError(f"unexpected scan descriptor: {head.hex()}")
buf = b""
# After a STOP the C1 spins its motor down; on SCAN it answers the
# descriptor at once but streams samples only once the motor is back
# up - a few seconds. Give the first sample that grace, then the
# normal timeout applies.
first_deadline = time.monotonic() + SPINUP_GRACE_S
while True:
chunk = s.read(max(5, s.in_waiting or 0))
if not chunk:
if not buf and time.monotonic() < first_deadline:
continue
raise RuntimeError("scan stream timed out")
buf += chunk
i = 0
while len(buf) - i >= 5:
b0, b1 = buf[i], buf[i + 1]
start, nstart, check = b0 & 1, (b0 >> 1) & 1, b1 & 1
if start == nstart or check != 1: # lost sync: slide one byte
i += 1
continue
quality = b0 >> 2
angle = ((b1 >> 1) | (buf[i + 2] << 7)) / 64.0
dist = (buf[i + 3] | (buf[i + 4] << 8)) / 4.0 / 1000.0
yield bool(start), quality, angle, dist
i += 5
buf = buf[i:]
def scans(self, min_len: int = 30) -> Iterator[list[tuple[int, float, float]]]:
"""Yield one list of (quality, angle_deg, dist_m) per revolution."""
current: list[tuple[int, float, float]] = []
for new_turn, q, ang, d in self.samples():
if new_turn and len(current) >= min_len: # the S flag marks a revolution
yield current
current = []
current.append((q, ang, d))
class C1Lidar:
"""Drop-in for the subset of the rplidar-roboticia API that rplidar_c1.py
uses: opens the port in __init__ (so a missing device raises right there),
get_info/get_health, iter_scans() yielding (quality, angle_deg, distance_MM)
per revolution, stop/stop_motor/disconnect. The C1 stops its motor on STOP,
so stop_motor is a no-op kept for interface parity."""
def __init__(self, port: str, baudrate: int = 460800, timeout: float = 2.0,
logger: object | None = None):
self._s = C1Scanner(port, baudrate=baudrate, timeout=timeout)
self._s.open()
def get_info(self) -> dict[str, object]:
return self._s.info()
def get_health(self) -> tuple[str, int]:
return self._s.health()
def iter_scans(self, min_len: int = 30) -> Iterator[list[tuple[int, float, float]]]:
for scan in self._s.scans(min_len=min_len):
yield [(q, ang, d * 1000.0) for (q, ang, d) in scan]
def stop(self) -> None:
self._s.stop()
def stop_motor(self) -> None:
return None
def disconnect(self) -> None:
self._s.close()