Adds the driver half of the v2 controller link as extra 0x4C message types: DATA with fragment reassembly, de-duplication and acks, a state-version digest with jittered resync requests, a bench echo, and a BEAT frame handled by BeatSync (accepts one of the two copies, free-runs on the last period when a beat is missed). Presets.beat() steps the selected pattern without a message. Optional settings: espnow_loss, bench, beat_pin, usb_log. The working tree also carries the files already running on the bench device, which were ahead of origin/p2p: board.py (per-chip LED pin and CPU speed), the reversed device_config flag in controller_messages.py, and the apply_cpu_freq call in main.py. Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
157 lines
4.6 KiB
Python
157 lines
4.6 KiB
Python
#!/usr/bin/env python3
|
|
"""LinkRx: reassembly, dedupe, acks, commit and digest resync."""
|
|
import struct
|
|
from espnow_rx import LinkRx, MSG_DATA, MSG_ACK, MSG_DIGEST, MSG_RESYNC, F_ACK, F_COMMIT
|
|
from beat_sync import BeatSync
|
|
|
|
M = b"\x01\x02\x03\x04\x05\x06"
|
|
|
|
|
|
def data(seq, payload, idx=0, cnt=1, flags=F_ACK, ver=0):
|
|
return struct.pack("<BBBHHBB", 0x4C, MSG_DATA, flags, seq, ver, idx, cnt) + payload
|
|
|
|
|
|
def make(jitter=0):
|
|
got, sent = [], []
|
|
link = LinkRx(lambda mac, m: got.append(m), BeatSync(), lambda mac, f: sent.append((mac, f)), jitter)
|
|
return link, got, sent
|
|
|
|
|
|
def feed(link, frame, now=0, mac=M):
|
|
link.on_packet(mac, frame, now)
|
|
link.poll(now)
|
|
|
|
|
|
def test_single():
|
|
link, got, sent = make()
|
|
feed(link, data(1, b"hello"))
|
|
assert got == [b"hello"]
|
|
_, t, _, seq, cnt, mask = struct.unpack("<BBBHBI", sent[0][1])
|
|
assert (t, seq, cnt, mask) == (MSG_ACK, 1, 1, 1)
|
|
|
|
|
|
def test_duplicate_acks_again_but_delivers_once():
|
|
link, got, sent = make()
|
|
feed(link, data(1, b"x"))
|
|
feed(link, data(1, b"x"), now=60)
|
|
assert got == [b"x"] and len(sent) == 2 and link.stats["dups"] == 1
|
|
|
|
|
|
def test_fragments_out_of_order_and_partial_mask():
|
|
link, got, sent = make()
|
|
feed(link, data(7, b"CC", 2, 3))
|
|
assert got == [] and struct.unpack("<BBBHBI", sent[-1][1])[5] == 0b100
|
|
feed(link, data(7, b"AA", 0, 3))
|
|
assert struct.unpack("<BBBHBI", sent[-1][1])[5] == 0b101
|
|
feed(link, data(7, b"BB", 1, 3))
|
|
assert got == [b"AABBCC"] and struct.unpack("<BBBHBI", sent[-1][1])[5] == 0b111
|
|
|
|
|
|
def test_stale_fragments_dropped():
|
|
link, got, _ = make()
|
|
feed(link, data(3, b"A", 0, 2), now=0)
|
|
feed(link, data(9, b"x"), now=4000) # triggers cleanup
|
|
feed(link, data(3, b"B", 1, 2), now=4001)
|
|
assert got == [b"x"]
|
|
|
|
|
|
def test_no_ack_when_not_requested():
|
|
link, got, sent = make()
|
|
feed(link, data(2, b"b", flags=0))
|
|
assert got == [b"b"] and sent == []
|
|
|
|
|
|
def test_commit_sets_version_only_when_complete():
|
|
link, got, _ = make()
|
|
feed(link, data(1, b"A", 0, 2, F_ACK | F_COMMIT, ver=5))
|
|
assert link.ver == 0
|
|
feed(link, data(1, b"B", 1, 2, F_ACK | F_COMMIT, ver=5))
|
|
assert link.ver == 5
|
|
feed(link, data(2, b"p", flags=0, ver=6)) # no commit flag: version unchanged
|
|
assert link.ver == 5
|
|
|
|
|
|
def digest(ver):
|
|
return struct.pack("<BBH", 0x4C, MSG_DIGEST, ver)
|
|
|
|
|
|
def test_digest_match_no_resync():
|
|
link, _, sent = make()
|
|
feed(link, data(1, b"a", flags=F_COMMIT, ver=4))
|
|
feed(link, digest(4), now=100)
|
|
link.poll(5000)
|
|
assert not [s for s in sent if s[1][1] == MSG_RESYNC]
|
|
|
|
|
|
def test_digest_mismatch_requests_resync_after_jitter_and_rate_limits():
|
|
link, _, sent = make(jitter=300)
|
|
feed(link, digest(9), now=1000)
|
|
link.poll(1100)
|
|
assert sent == [] # jitter not elapsed
|
|
link.poll(1300)
|
|
assert sent[-1] == (M, struct.pack("<BBH", 0x4C, MSG_RESYNC, 0))
|
|
feed(link, digest(9), now=2000)
|
|
link.poll(2400)
|
|
assert len(sent) == 1 # within the 3 s gap
|
|
feed(link, digest(9), now=4500)
|
|
link.poll(4900)
|
|
assert len(sent) == 2
|
|
|
|
|
|
def test_resync_cancelled_when_state_arrives():
|
|
link, _, sent = make(jitter=500)
|
|
feed(link, digest(9), now=0)
|
|
feed(link, data(1, b"s", flags=F_COMMIT, ver=9), now=100)
|
|
link.poll(600)
|
|
assert sent == []
|
|
|
|
|
|
def test_digest_zero_ignored():
|
|
link, _, sent = make()
|
|
feed(link, digest(0), now=0)
|
|
link.poll(5000)
|
|
assert sent == []
|
|
|
|
|
|
def test_legacy_and_beat_frames():
|
|
link, got, _ = make()
|
|
feed(link, b"\x4c\x03\x02\xff\x00\x00\x00") # plain CMD passes straight to the handler
|
|
feed(link, b'{"v":"1"}')
|
|
assert len(got) == 2
|
|
link.on_packet(M, struct.pack("<BBBHHB", 0x4C, 0x25, 0, 1, 500, 0), 0)
|
|
assert link.beat.poll(0) is True
|
|
|
|
|
|
def test_inbox_overflow_counted():
|
|
link, _, _ = make()
|
|
for i in range(20):
|
|
link.on_packet(M, data(i, b"x"), 0)
|
|
assert link.stats["dropped"] == 4
|
|
|
|
|
|
def test_loss_drops_frames_and_can_spare_beats():
|
|
seq = iter([5, 90, 5, 90])
|
|
link, got, _ = make()
|
|
link.rand = lambda: next(seq)
|
|
link.loss_pct = 50
|
|
feed(link, data(1, b"a")) # 5 < 50: lost
|
|
feed(link, data(2, b"b")) # 90: kept
|
|
assert got == [b"b"] and link.stats["sim_lost"] == 1
|
|
link.loss_beats = False
|
|
link.on_packet(M, struct.pack("<BBBHHB", 0x4C, 0x25, 0, 1, 500, 0), 0) # beats exempt, rand not consulted
|
|
assert link.beat.poll(0) is True
|
|
|
|
|
|
def test_echo_goes_to_master():
|
|
link, _, sent = make()
|
|
feed(link, data(1, b"a"))
|
|
link.echo(0x1234)
|
|
assert sent[-1] == (M, struct.pack("<BBH", 0x4C, 0x24, 0x1234))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
for name, fn in list(globals().items()):
|
|
if name.startswith("test_"):
|
|
fn()
|
|
print("ok", name)
|