Release v0.1.0

src/serial_sync/__init__.py

@@ -0,0 +1,172 @@
+from dataclasses import dataclass, field
+import struct
+import time
+from serial import Serial
+
+SYNC_PATTERN = 0xBEEF
+SYNC_BYTES = struct.pack("<H", SYNC_PATTERN)
+GOOD_SAMPLE_FRACTION = 0.25
+SYNC_INTERVAL_S = 0.05
+
+
+class SerialSync:
+    def __init__(self, serial: Serial) -> None:
+        self._serial = serial
+        self._scale: float | None = None
+        self._offset: float | None = None
+
+    def calibrate_sync(self, period: float) -> None:
+        request_packet = SyncPacket(
+            type=3,
+            r_t=0,
+            s_t=0,
+        ).pack()
+
+        print("Waiting 5 seconds before synchronization...")
+
+        time.sleep(5)
+
+        self._serial.reset_input_buffer()
+
+        samples: list[SyncSample] = []
+
+        start_ns: int = time.monotonic_ns()
+        duration_ns: int = int(period * 1_000_000_000)
+
+        while time.monotonic_ns() < start_ns + duration_ns:
+            tbw = time.monotonic_ns()
+            self._serial.write(request_packet)
+            self._serial.flush()
+            taw = time.monotonic_ns()
+
+            previous: bytes | None = None
+
+            while True:
+                byte = self._serial.read(1)
+
+                if not byte:
+                    raise TimeoutError("Timed out waiting for sync pattern")
+
+                if previous == b"\xef" and byte == b"\xbe":
+                    break
+
+                previous = byte
+
+            data = bytearray()
+            size = SyncPacket.SIZE - len(SYNC_BYTES)
+            while len(data) < size:
+                chunk = self._serial.read(size - len(data))
+
+                if not chunk:
+                    raise TimeoutError(
+                        f"Timed out reading packet: received {len(data)}/{size} bytes"
+                    )
+
+                data.extend(chunk)
+
+            payload = bytes(data)
+
+            tar = time.monotonic_ns()
+            reply = SyncPacket.unpack(SYNC_BYTES + payload)
+
+            t1 = (tbw + taw) // 2
+            t2 = reply.r_t * 1_000
+            t3 = reply.s_t * 1_000
+            t4 = tar
+
+            arduino_mid_ns = (t2 + t3) / 2
+            pc_mid_ns = (t1 + t4) / 2
+
+            arduino_processing_ns = t3 - t2
+
+            rtt_ns = (t4 - t1) - arduino_processing_ns
+
+            samples.append(SyncSample(arduino_mid_ns, pc_mid_ns, rtt_ns))
+            time.sleep(SYNC_INTERVAL_S)
+
+        if len(samples) < 2:
+            raise RuntimeError("Not enough valid synchronization samples")
+
+        sorted_samples = sorted(
+            samples,
+            key=lambda sample: sample.rtt_ns,
+        )
+
+        keep_count = max(
+            2,
+            int(len(sorted_samples) * GOOD_SAMPLE_FRACTION),
+        )
+
+        good_samples = sorted_samples[:keep_count]
+
+        xs = [sample.arduino_mid_ns for sample in good_samples]
+
+        ys = [sample.pc_mid_ns for sample in good_samples]
+
+        x_mean = sum(xs) / len(xs)
+        y_mean = sum(ys) / len(ys)
+
+        numerator = sum((x - x_mean) * (y - y_mean) for x, y in zip(xs, ys))
+
+        denominator = sum((x - x_mean) ** 2 for x in xs)
+
+        if denominator == 0:
+            raise ValueError("Cannot fit clock: all Arduino timestamps are equal")
+
+        self._scale = numerator / denominator
+        self._offset = y_mean - self._scale * x_mean
+
+    def convert_timestamp(self, arduino_ns: int) -> int:
+        if self._scale is None or self._offset is None:
+            raise RuntimeError("SerialSync has not been calibrated")
+
+        return round(self._scale * arduino_ns + self._offset)
+
+
+@dataclass
+class SyncPacket:
+    sync: int = field(default=0xBEEF, init=False)
+    type: int
+    r_t: int
+    s_t: int
+
+    FORMAT = "<HBII"
+    SIZE = struct.calcsize(FORMAT)
+
+    def pack(self) -> bytes:
+        return struct.pack(
+            self.FORMAT,
+            self.sync,
+            self.type,
+            self.r_t,
+            self.s_t,
+        )
+
+    @classmethod
+    def unpack(cls, data: bytes) -> "SyncPacket":
+        if len(data) != cls.SIZE:
+            raise ValueError(
+                f"Invalid SyncPacket size: expected {cls.SIZE}, got {len(data)}"
+            )
+
+        _, type_, r_t, s_t = struct.unpack(
+            cls.FORMAT,
+            data,
+        )
+
+        return cls(
+            type=type_,
+            r_t=r_t,
+            s_t=s_t,
+        )
+
+
+@dataclass
+class SyncSample:
+    arduino_mid_ns: float
+    pc_mid_ns: float
+    rtt_ns: int
+
+    @property
+    def observed_offset_ns(self) -> float:
+        return self.pc_mid_ns - self.arduino_mid_ns