79 lines
3.5 KiB
Python
79 lines
3.5 KiB
Python
#!/usr/bin/env python3
|
|
"""Type-2 G20-protocol matrix queries only. No driver/ROS/serial/motor writes."""
|
|
import argparse
|
|
import json
|
|
import subprocess
|
|
import time
|
|
from pathlib import Path
|
|
import can
|
|
|
|
NAMES = ['thumb', 'index', 'middle', 'ring', 'little']
|
|
ROW_IDS = {i * 16: i for i in range(12)}
|
|
|
|
|
|
def query(bus, command, payload=(), timeout=0.15):
|
|
# Drain old traffic so incomplete responses never reuse cached matrices.
|
|
deadline = time.monotonic() + 0.05
|
|
while bus.recv(0) is not None:
|
|
if time.monotonic() > deadline:
|
|
raise RuntimeError('Unexpected continuous CAN traffic; stop other CAN clients')
|
|
bus.send(can.Message(arbitration_id=0x28, is_extended_id=False,
|
|
data=[command, *payload]), timeout=0.2)
|
|
frames = []
|
|
end = time.monotonic() + timeout
|
|
while time.monotonic() < end:
|
|
msg = bus.recv(max(0, end-time.monotonic()))
|
|
if msg is None:
|
|
break
|
|
if msg.is_error_frame:
|
|
raise RuntimeError('CAN error frame')
|
|
if (msg.is_rx and not msg.is_extended_id and msg.arbitration_id == 0x28
|
|
and msg.data and msg.data[0] == command):
|
|
frames.append(list(msg.data))
|
|
return frames
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument('--duration', type=float, default=25)
|
|
args = parser.parse_args()
|
|
if not 1 <= args.duration <= 120:
|
|
parser.error('duration must be 1..120 seconds')
|
|
processes = subprocess.check_output(['ps', '-eo', 'args='], text=True)
|
|
if any('linker_hand_sdk' in line and 'python' in line for line in processes.splitlines()):
|
|
raise SystemExit('Stop linker_hand_sdk first. This test requires no motion controller.')
|
|
records = []
|
|
print('READ ONLY: type-2 touch raw intensity, NOT calibrated N or grams.', flush=True)
|
|
with can.Bus(channel='can0', interface='socketcan', receive_own_messages=False) as bus:
|
|
frames = query(bus, 0xB0)
|
|
if not any(len(f) >= 2 and f[1] == 2 for f in frames):
|
|
raise RuntimeError(f'Expected touch type 2, got {frames}')
|
|
start = time.monotonic()
|
|
try:
|
|
while time.monotonic()-start < args.duration:
|
|
sample = {'elapsed': round(time.monotonic()-start, 2), 'fingers': {}}
|
|
for i, name in enumerate(NAMES):
|
|
frames = query(bus, 0xB1+i, [0xC6], timeout=0.08)
|
|
rows = {ROW_IDS[f[1]]: f[2:] for f in frames
|
|
if len(f) == 8 and f[1] in ROW_IDS}
|
|
complete = len(rows) == 12
|
|
values = [v for row in rows.values() for v in row]
|
|
sample['fingers'][name] = {
|
|
'complete': complete, 'rows': len(rows),
|
|
'max': max(values) if complete else None,
|
|
'sum_raw': sum(values) if complete else None,
|
|
'matrix': [rows.get(r) for r in range(12)],
|
|
}
|
|
records.append(sample)
|
|
print(' '.join(f"{name}:max={v['max']} rows={v['rows']}/12"
|
|
for name,v in sample['fingers'].items()), flush=True)
|
|
finally:
|
|
destination = Path(__file__).resolve().parents[1]/'diagnostics'/f'touch-readonly-{time.time_ns()}.json'
|
|
destination.parent.mkdir(exist_ok=True)
|
|
destination.write_text(json.dumps(records, indent=2))
|
|
print(f'Saved: {destination}', flush=True)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|