feat(espnow): reliable link and beat trigger on the 0x4C protocol
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>
This commit is contained in:
1 parent
3286c4002d
commit
20d1af7177
8 files changed
+642
-11
No files matched your search
@@ -0,0 +1,156 @@
|
||||
#!/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)
|
||||
Reference in new issue
Block a user