fix gamepad layout and refuse a pad whose axes disagree

An Xbox One S pad over Bluetooth numbers its controls differently from the
indices this node shipped with: the right stick is on axes 2 and 3, the
triggers on 4 and 5, and the buttons carry gaps at 2 and 5. Against the old
layout an untouched pad reported a resting trigger as full right-stick
deflection, which asked for 30 deg/s of pitch while lying on the desk, held
the lease for ever so GAMEPAD_IDLE_RELEASE_S never fired, and moved a real
arm into a controller error. X toggled freedrive and LB was the E-STOP, so
the resulting latch could not be released from the pad at all.

Layout now carries the swept indices. Each pad is also measured the first
time it is seen at rest: a stick must rest near centre and a trigger near
the bottom of its travel, and a pad that does not fit reads as absent and is
named in the log rather than driving an arm from a numbering nobody checked.
The measured rest becomes each axis's zero, so a worn stick's tilt is
subtracted rather than deadzoned away and a trigger that reads 0 until it
first moves stops reading as half pressed.

Rest is never re-measured while the pad is in use, because doing that under
a held stick would record the deflection as centre and the arm would move
when the operator let go. The probe drives from the same calibration, so
what it prints is still what the arm would get, and it shows the mismatch.

Tests name their controls through axes_for and buttons_for instead of
numbering them; the positional lists are why a wrong index passed a green
suite.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
2026-09-16 18:06:06 -03:00
co-authored by Claude Opus 5
parent 7c1c33f5b1
commit d0f884c7e2
8 changed files with 514 additions and 84 deletions
+75 -3
View File
@@ -12,11 +12,19 @@ unplugged pad. So silence past `JOY_TTL_S` reads as *absent*, which is the same
answer, because a reading nobody is refreshing is not evidence of anything.
`dora-joy` publishes on every tick whether or not the pad moved, so silence
means one thing only.
The other thing this seam does is :class:`Calibration`: no two pads rest in
quite the same place, and one that rests somewhere the layout does not expect
is a pad this node must not drive an arm from. Every reading the loop sees has
been through it, so the rest of the node only ever handles a pad that behaves.
"""
import time
from dataclasses import dataclass, field
from dora_teleop import mapping
from dora_teleop.log import log
@dataclass(frozen=True)
class Sample:
@@ -52,17 +60,81 @@ def decode(event: dict) -> Sample:
)
class Calibration:
"""Where the pad in front of us rests, and whether the layout fits it.
Captured from the first reading of a pad that is sitting still, and kept
until that pad goes away. It is not re-taken while the pad is in use, which
would be the one way to get this badly wrong: an operator holding a stick
when it happened would have that deflection recorded as centre, and the arm
would then move whenever they let go.
A pad whose resting axes do not fit the layout is reported **absent** rather
than corrected. The numbering cannot be guessed from one reading — a trigger
resting at -1.0 and a stick held hard over look identical — and guessing
wrong drives a real arm. `dora-teleop-probe` shows what the pad reports, and
`Layout` in :mod:`dora_teleop.mapping` is where a pad that disagrees is
written down.
"""
def __init__(self, layout: mapping.Layout = mapping.LAYOUT):
self.layout = layout
self.mismatch: str | None = None
self._zeros: list[float] | None = None
@property
def known(self) -> bool:
"""Whether a pad has been measured at rest and may be driven from."""
return self._zeros is not None
def forget(self) -> None:
"""Drop what was learned, so the next pad to appear is measured afresh."""
self._zeros = None
self.mismatch = None
def apply(self, sample: Sample) -> Sample:
"""Return the reading as the layout's ideal pad would have sent it.
An absent pad passes straight through and takes the calibration with
it. A pad that does not fit the layout comes back absent, which every
path in this node already treats as a reason to stop.
"""
if not sample.present:
self.forget()
return sample
if self._zeros is None or len(self._zeros) != len(sample.axes):
try:
self._zeros = mapping.rest_zeros(sample.axes, self.layout)
except ValueError as wrong:
# Logged once per pad: this runs on every reading, and a pad
# that does not fit the layout does not fit it fifty times a
# second.
if self.mismatch is None:
log(
f"ignoring the gamepad: {wrong}. Run dora-teleop-probe and check Layout in mapping.py"
)
self.mismatch = str(wrong)
return ABSENT
self.mismatch = None
return Sample(
axes=mapping.canonical(sample.axes, self._zeros, self.layout),
buttons=sample.buttons,
present=True,
)
class JoyInput:
"""The latest reading, and how long ago it arrived."""
def __init__(self, ttl_s: float):
def __init__(self, ttl_s: float, layout: mapping.Layout = mapping.LAYOUT):
self.ttl_s = ttl_s
self.calibration = Calibration(layout)
self._sample = ABSENT
self._seen_at = 0.0
def accept(self, sample: Sample, moment: float) -> None:
"""Take one reading off the wire."""
self._sample = sample
"""Take one reading off the wire, calibrated to the pad that sent it."""
self._sample = self.calibration.apply(sample)
self._seen_at = moment
def read(self, moment: float) -> Sample: