Archived
Proof of concept
This commit is contained in:
11 files changed
+690
No files matched your search
@@ -0,0 +1 @@
|
||||
3.13
|
||||
Whitespace-only changes.
@@ -0,0 +1,19 @@
|
||||
[project]
|
||||
name = "clock-sync-software"
|
||||
version = "0.1.0"
|
||||
description = "Add your description here"
|
||||
readme = "README.md"
|
||||
authors = [
|
||||
{ name = "Hector van der Aa", email = "hector@h3cx.dev" }
|
||||
]
|
||||
requires-python = ">=3.13"
|
||||
dependencies = [
|
||||
"pyserial>=3.5",
|
||||
]
|
||||
|
||||
[project.scripts]
|
||||
clock-sync-software = "clock_sync_software:main"
|
||||
|
||||
[build-system]
|
||||
requires = ["uv_build>=0.12.13,<0.13.0"]
|
||||
build-backend = "uv_build"
|
||||
@@ -0,0 +1,497 @@
|
||||
import struct
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
|
||||
from serial import Serial
|
||||
from serial.tools import list_ports
|
||||
|
||||
|
||||
SYNC_PATTERN = 0xBEEF
|
||||
SYNC_BYTES = struct.pack("<H", SYNC_PATTERN)
|
||||
|
||||
BAUDRATE = 115200
|
||||
SERIAL_TIMEOUT = 1.0
|
||||
|
||||
CALIBRATION_DURATION_S = 30.0
|
||||
VALIDATION_DURATION_S = 60.0
|
||||
|
||||
SYNC_INTERVAL_S = 0.05
|
||||
|
||||
# Keep the lowest-RTT 25% of calibration samples.
|
||||
GOOD_SAMPLE_FRACTION = 0.25
|
||||
|
||||
|
||||
@dataclass
|
||||
class SyncPacket:
|
||||
sync: int
|
||||
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)}"
|
||||
)
|
||||
|
||||
sync, type_, r_t, s_t = struct.unpack(
|
||||
cls.FORMAT,
|
||||
data,
|
||||
)
|
||||
|
||||
return cls(
|
||||
sync=sync,
|
||||
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
|
||||
|
||||
|
||||
def fit_clock(
|
||||
samples: list[SyncSample],
|
||||
) -> tuple[float, float]:
|
||||
"""
|
||||
Fit:
|
||||
|
||||
pc_ns = scale * arduino_ns + offset_ns
|
||||
|
||||
Since both sides are in nanoseconds, the ideal scale is 1.0.
|
||||
"""
|
||||
|
||||
if len(samples) < 2:
|
||||
raise ValueError("At least two samples are required")
|
||||
|
||||
xs = [sample.arduino_mid_ns for sample in samples]
|
||||
|
||||
ys = [sample.pc_mid_ns for sample in 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")
|
||||
|
||||
scale = numerator / denominator
|
||||
offset_ns = y_mean - scale * x_mean
|
||||
|
||||
return scale, offset_ns
|
||||
|
||||
|
||||
def arduino_ns_to_monotonic_ns(
|
||||
arduino_ns: float,
|
||||
scale: float,
|
||||
offset_ns: float,
|
||||
) -> int:
|
||||
return round(scale * arduino_ns + offset_ns)
|
||||
|
||||
|
||||
def arduino_us_to_monotonic_ns(
|
||||
arduino_us: int,
|
||||
scale: float,
|
||||
offset_ns: float,
|
||||
) -> int:
|
||||
return arduino_ns_to_monotonic_ns(
|
||||
arduino_us * 1_000,
|
||||
scale,
|
||||
offset_ns,
|
||||
)
|
||||
|
||||
|
||||
def start_sync(ser: Serial) -> None:
|
||||
previous: bytes | None = None
|
||||
|
||||
while True:
|
||||
byte = ser.read(1)
|
||||
|
||||
if not byte:
|
||||
raise TimeoutError("Timed out waiting for sync pattern")
|
||||
|
||||
if previous == b"\xef" and byte == b"\xbe":
|
||||
return
|
||||
|
||||
previous = byte
|
||||
|
||||
|
||||
def read_exactly(
|
||||
ser: Serial,
|
||||
size: int,
|
||||
) -> bytes:
|
||||
data = bytearray()
|
||||
|
||||
while len(data) < size:
|
||||
chunk = ser.read(size - len(data))
|
||||
|
||||
if not chunk:
|
||||
raise TimeoutError(
|
||||
f"Timed out reading packet: received {len(data)}/{size} bytes"
|
||||
)
|
||||
|
||||
data.extend(chunk)
|
||||
|
||||
return bytes(data)
|
||||
|
||||
|
||||
def choose_serial_port() -> str:
|
||||
ports = list(list_ports.comports())
|
||||
|
||||
if not ports:
|
||||
raise RuntimeError("No serial ports found")
|
||||
|
||||
for index, port in enumerate(ports):
|
||||
print(f"{index}: {port.device} {port.description}")
|
||||
|
||||
while True:
|
||||
try:
|
||||
choice = int(input("Choose a port to connect: "))
|
||||
|
||||
if 0 <= choice < len(ports):
|
||||
selected_port = ports[choice].device
|
||||
|
||||
print(f"Selected port: {selected_port}")
|
||||
|
||||
return selected_port
|
||||
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
print("Unknown port. Try again.")
|
||||
|
||||
|
||||
def get_sync_sample(
|
||||
ser: Serial,
|
||||
request_packet: bytes,
|
||||
) -> SyncSample:
|
||||
#
|
||||
# Bracket the serial write.
|
||||
#
|
||||
tbw = time.monotonic_ns()
|
||||
|
||||
ser.write(request_packet)
|
||||
ser.flush()
|
||||
|
||||
taw = time.monotonic_ns()
|
||||
|
||||
#
|
||||
# Wait for response packet.
|
||||
#
|
||||
start_sync(ser)
|
||||
|
||||
payload = read_exactly(
|
||||
ser,
|
||||
SyncPacket.SIZE - len(SYNC_BYTES),
|
||||
)
|
||||
|
||||
tar = time.monotonic_ns()
|
||||
|
||||
reply = SyncPacket.unpack(SYNC_BYTES + payload)
|
||||
|
||||
#
|
||||
# PC timestamps.
|
||||
#
|
||||
# Estimate request-transmit time as midpoint
|
||||
# around write()/flush().
|
||||
#
|
||||
t1 = (tbw + taw) // 2
|
||||
|
||||
#
|
||||
# Arduino receive/transmit timestamps.
|
||||
# Convert micros() -> ns.
|
||||
#
|
||||
t2 = reply.r_t * 1_000
|
||||
t3 = reply.s_t * 1_000
|
||||
|
||||
#
|
||||
# PC receive timestamp.
|
||||
#
|
||||
t4 = tar
|
||||
|
||||
#
|
||||
# Midpoints give us an approximate
|
||||
# Arduino<->PC correspondence.
|
||||
#
|
||||
arduino_mid_ns = (t2 + t3) / 2
|
||||
|
||||
pc_mid_ns = (t1 + t4) / 2
|
||||
|
||||
#
|
||||
# Corrected transport RTT:
|
||||
#
|
||||
# total PC round trip
|
||||
# minus Arduino processing time.
|
||||
#
|
||||
arduino_processing_ns = t3 - t2
|
||||
|
||||
rtt_ns = (t4 - t1) - arduino_processing_ns
|
||||
|
||||
return SyncSample(
|
||||
arduino_mid_ns=arduino_mid_ns,
|
||||
pc_mid_ns=pc_mid_ns,
|
||||
rtt_ns=rtt_ns,
|
||||
)
|
||||
|
||||
|
||||
def calibrate(
|
||||
ser: Serial,
|
||||
request_packet: bytes,
|
||||
) -> tuple[
|
||||
float,
|
||||
float,
|
||||
list[SyncSample],
|
||||
]:
|
||||
print()
|
||||
print(f"Calibrating clocks for {CALIBRATION_DURATION_S:.0f} seconds...")
|
||||
|
||||
samples: list[SyncSample] = []
|
||||
|
||||
start_ns = time.monotonic_ns()
|
||||
duration_ns = int(CALIBRATION_DURATION_S * 1_000_000_000)
|
||||
|
||||
while time.monotonic_ns() - start_ns < duration_ns:
|
||||
try:
|
||||
sample = get_sync_sample(
|
||||
ser,
|
||||
request_packet,
|
||||
)
|
||||
|
||||
samples.append(sample)
|
||||
|
||||
elapsed_s = (time.monotonic_ns() - start_ns) / 1_000_000_000
|
||||
|
||||
offset_ns = sample.pc_mid_ns - sample.arduino_mid_ns
|
||||
|
||||
print(
|
||||
f"\r"
|
||||
f"{elapsed_s:5.1f}/"
|
||||
f"{CALIBRATION_DURATION_S:.0f} s"
|
||||
f" | offset: "
|
||||
f"{offset_ns / 1_000_000:+.3f} ms"
|
||||
f" | RTT: "
|
||||
f"{sample.rtt_ns / 1_000_000:.3f} ms",
|
||||
end="",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
except TimeoutError as exc:
|
||||
print(f"\nSync timeout: {exc}")
|
||||
|
||||
time.sleep(SYNC_INTERVAL_S)
|
||||
|
||||
print()
|
||||
|
||||
if len(samples) < 2:
|
||||
raise RuntimeError("Not enough valid synchronization samples")
|
||||
|
||||
#
|
||||
# Select lowest-latency 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]
|
||||
|
||||
scale, offset_ns = fit_clock(good_samples)
|
||||
|
||||
return (
|
||||
scale,
|
||||
offset_ns,
|
||||
good_samples,
|
||||
)
|
||||
|
||||
|
||||
def validate(
|
||||
ser: Serial,
|
||||
request_packet: bytes,
|
||||
scale: float,
|
||||
offset_ns: float,
|
||||
) -> None:
|
||||
print()
|
||||
print(f"Running {VALIDATION_DURATION_S:.0f}-second validation test...")
|
||||
|
||||
errors_ns: list[float] = []
|
||||
rtts_ns: list[int] = []
|
||||
|
||||
start_ns = time.monotonic_ns()
|
||||
duration_ns = int(VALIDATION_DURATION_S * 1_000_000_000)
|
||||
|
||||
while time.monotonic_ns() - start_ns < duration_ns:
|
||||
try:
|
||||
sample = get_sync_sample(
|
||||
ser,
|
||||
request_packet,
|
||||
)
|
||||
|
||||
predicted_pc_ns = scale * sample.arduino_mid_ns + offset_ns
|
||||
|
||||
error_ns = sample.pc_mid_ns - predicted_pc_ns
|
||||
|
||||
errors_ns.append(error_ns)
|
||||
|
||||
rtts_ns.append(sample.rtt_ns)
|
||||
|
||||
elapsed_s = (time.monotonic_ns() - start_ns) / 1_000_000_000
|
||||
|
||||
print(
|
||||
f"\r"
|
||||
f"{elapsed_s:5.1f}/"
|
||||
f"{VALIDATION_DURATION_S:.0f} s"
|
||||
f" | error: "
|
||||
f"{error_ns / 1_000_000:+.3f} ms"
|
||||
f" | RTT: "
|
||||
f"{sample.rtt_ns / 1_000_000:.3f} ms",
|
||||
end="",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
except TimeoutError as exc:
|
||||
print(f"\nSync timeout: {exc}")
|
||||
|
||||
time.sleep(SYNC_INTERVAL_S)
|
||||
|
||||
print()
|
||||
|
||||
if not errors_ns:
|
||||
raise RuntimeError("No valid validation samples")
|
||||
|
||||
absolute_errors_ns = [abs(error) for error in errors_ns]
|
||||
|
||||
max_abs_error_ns = max(absolute_errors_ns)
|
||||
|
||||
mean_abs_error_ns = sum(absolute_errors_ns) / len(absolute_errors_ns)
|
||||
|
||||
mean_signed_error_ns = sum(errors_ns) / len(errors_ns)
|
||||
|
||||
rms_error_ns = (sum(error * error for error in errors_ns) / len(errors_ns)) ** 0.5
|
||||
|
||||
min_error_ns = min(errors_ns)
|
||||
max_error_ns = max(errors_ns)
|
||||
|
||||
print()
|
||||
print("Validation results")
|
||||
print("------------------")
|
||||
print(f"Samples: {len(errors_ns)}")
|
||||
print()
|
||||
print(f"Maximum abs error: {max_abs_error_ns / 1_000_000:.6f} ms")
|
||||
print(f"Mean abs error: {mean_abs_error_ns / 1_000_000:.6f} ms")
|
||||
print(f"RMS error: {rms_error_ns / 1_000_000:.6f} ms")
|
||||
print(f"Mean signed error: {mean_signed_error_ns / 1_000_000:+.6f} ms")
|
||||
print(f"Minimum error: {min_error_ns / 1_000_000:+.6f} ms")
|
||||
print(f"Maximum error: {max_error_ns / 1_000_000:+.6f} ms")
|
||||
|
||||
print()
|
||||
print(f"Best validation RTT: {min(rtts_ns) / 1_000_000:.6f} ms")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
try:
|
||||
selected_port = choose_serial_port()
|
||||
except KeyboardInterrupt:
|
||||
print()
|
||||
return
|
||||
|
||||
request_packet = SyncPacket(
|
||||
sync=SYNC_PATTERN,
|
||||
type=3,
|
||||
r_t=0,
|
||||
s_t=0,
|
||||
).pack()
|
||||
|
||||
with Serial(
|
||||
selected_port,
|
||||
baudrate=BAUDRATE,
|
||||
timeout=SERIAL_TIMEOUT,
|
||||
) as ser:
|
||||
print("Waiting 5 seconds before synchronization...")
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
#
|
||||
# Remove startup noise / stale bytes.
|
||||
#
|
||||
ser.reset_input_buffer()
|
||||
|
||||
scale, offset_ns, good_samples = calibrate(
|
||||
ser,
|
||||
request_packet,
|
||||
)
|
||||
|
||||
drift_ppm = (scale - 1.0) * 1_000_000
|
||||
|
||||
best_rtt_ns = min(sample.rtt_ns for sample in good_samples)
|
||||
|
||||
worst_good_rtt_ns = max(sample.rtt_ns for sample in good_samples)
|
||||
|
||||
print()
|
||||
print("Clock synchronization complete")
|
||||
print("--------------------------------")
|
||||
print(f"Calibration samples used: {len(good_samples)}")
|
||||
|
||||
print(f"Best RTT: {best_rtt_ns / 1_000_000:.6f} ms")
|
||||
|
||||
print(f"Calibration RTT cutoff: {worst_good_rtt_ns / 1_000_000:.6f} ms")
|
||||
|
||||
print()
|
||||
print(f"Scale: {scale:.12f}")
|
||||
|
||||
print(f"Drift: {drift_ppm:+.3f} ppm")
|
||||
|
||||
print(f"Offset: {offset_ns:.0f} ns")
|
||||
|
||||
print(f"Offset: {offset_ns / 1_000_000:.6f} ms")
|
||||
|
||||
print()
|
||||
print("Clock equation:")
|
||||
print(f"PC_ns = {scale:.12f} * Arduino_ns {offset_ns:+.0f}")
|
||||
|
||||
print()
|
||||
print("For an Arduino micros() timestamp:")
|
||||
|
||||
print(f"PC_ns = {scale:.12f} * (Arduino_us * 1000) {offset_ns:+.0f}")
|
||||
|
||||
#
|
||||
# Now test the model without changing it.
|
||||
#
|
||||
validate(
|
||||
ser,
|
||||
request_packet,
|
||||
scale,
|
||||
offset_ns,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Generated
+23
@@ -0,0 +1,23 @@
|
||||
version = 1
|
||||
revision = 3
|
||||
requires-python = ">=3.13"
|
||||
|
||||
[[package]]
|
||||
name = "clock-sync-software"
|
||||
version = "0.1.0"
|
||||
source = { editable = "." }
|
||||
dependencies = [
|
||||
{ name = "pyserial" },
|
||||
]
|
||||
|
||||
[package.metadata]
|
||||
requires-dist = [{ name = "pyserial", specifier = ">=3.5" }]
|
||||
|
||||
[[package]]
|
||||
name = "pyserial"
|
||||
version = "3.5"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/1e/7d/ae3f0a63f41e4d2f6cb66a5b57197850f919f59e558159a4dd3a818f5082/pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb", size = 159125, upload-time = "2020-11-23T03:59:15.045Z" }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/07/bc/587a445451b253b285629263eb51c2d8e9bcea4fc97826266d186f96f558/pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0", size = 90585, upload-time = "2020-11-23T03:59:13.41Z" },
|
||||
]
|
||||
Reference in new issue
Block a user