fix(piper): bool calibration flag and robust can setup/test fallback

这个提交包含在:
Elvin
2026-03-06 16:56:25 +08:00
父节点 5975798aef
当前提交 300f5a386d
共有 7 个文件被更改,包括 107 次插入 和 53 次删除

查看文件

@ -93,8 +93,8 @@ def check_interface_status(interface: str) -> tuple[bool, str, bool]:
return False, "ip command not found", False
def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> bool:
"""Configure a CAN interface."""
def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> tuple[bool, str]:
"""Configure a CAN interface and return (success, error_message)."""
try:
subprocess.run(["sudo", "ip", "link", "set", interface, "down"], check=False, capture_output=True) # nosec B607
@ -104,20 +104,42 @@ def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: boo
result = subprocess.run(cmd, capture_output=True, text=True) # nosec B607
if result.returncode != 0:
print(f" ✗ Failed to configure: {result.stderr}")
return False
err = (result.stderr or result.stdout or "unknown error").strip()
return False, f"configure failed: {err}"
result = subprocess.run( # nosec B607
["sudo", "ip", "link", "set", interface, "up"], capture_output=True, text=True
)
if result.returncode != 0:
print(f" ✗ Failed to bring up: {result.stderr}")
return False
err = (result.stderr or result.stdout or "unknown error").strip()
return False, f"bring-up failed: {err}"
return True
return True, ""
except Exception as e:
print(f" ✗ Error: {e}")
return False
return False, f"exception: {e}"
def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]:
"""Try primary mode first, then fallback mode. Returns (success, used_fd, summary)."""
attempts = [cfg.use_fd, not cfg.use_fd]
labels = {True: "CAN FD", False: "CAN 2.0"}
attempt_msgs: list[str] = []
for idx, use_fd in enumerate(attempts, start=1):
print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}")
ok, err = setup_interface(interface, cfg.bitrate, cfg.data_bitrate, use_fd)
if ok:
is_up, status, is_fd_iface = check_interface_status(interface)
if is_up:
print(f" ✓ {interface}: {status} (using {labels[use_fd]})")
return True, use_fd, labels[use_fd]
err = f"interface not up after setup ({status})"
print(f" ✗ {err}")
attempt_msgs.append(f"{labels[use_fd]} -> {err}")
return False, None, " | ".join(attempt_msgs)
def test_motor(bus, motor_id: int, timeout: float, use_fd: bool):
@ -162,7 +184,7 @@ def test_interface(cfg: CANSetupConfig, interface: str):
"""Test all motors on a CAN interface."""
import can
is_up, status, _ = check_interface_status(interface)
is_up, status, is_fd_iface = check_interface_status(interface)
print(f"\n{interface}: {status}")
if not is_up:
@ -170,8 +192,13 @@ def test_interface(cfg: CANSetupConfig, interface: str):
return {}
try:
# Avoid EINVAL on non-FD interfaces.
effective_use_fd = cfg.use_fd and is_fd_iface
if cfg.use_fd and not is_fd_iface:
print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.")
kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate}
if cfg.use_fd:
if effective_use_fd:
kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True})
bus = can.interface.Bus(**kwargs)
except Exception as e:
@ -180,12 +207,15 @@ def test_interface(cfg: CANSetupConfig, interface: str):
results = {}
try:
while bus.recv(timeout=0.01):
pass
# Drain stale frames briefly, but never block indefinitely on a busy bus.
drain_deadline = time.time() + 0.2
while time.time() < drain_deadline:
if bus.recv(timeout=0.01) is None:
break
for motor_id in cfg.motor_ids:
motor_name = MOTOR_NAMES.get(motor_id, f"motor_0x{motor_id:02X}")
responses, error = test_motor(bus, motor_id, cfg.timeout, cfg.use_fd)
responses, error = test_motor(bus, motor_id, cfg.timeout, effective_use_fd)
if error:
print(f" Motor 0x{motor_id:02X} ({motor_name}): ✗ {error}")
@ -213,7 +243,7 @@ def speed_test(cfg: CANSetupConfig, interface: str):
"""Test communication speed with motors."""
import can
is_up, status, _ = check_interface_status(interface)
is_up, status, is_fd_iface = check_interface_status(interface)
if not is_up:
print(f"{interface}: {status} - skipping")
return
@ -221,8 +251,13 @@ def speed_test(cfg: CANSetupConfig, interface: str):
print(f"\n{interface}: Running speed test ({cfg.speed_iterations} iterations)...")
try:
# Avoid EINVAL on non-FD interfaces.
effective_use_fd = cfg.use_fd and is_fd_iface
if cfg.use_fd and not is_fd_iface:
print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.")
kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate}
if cfg.use_fd:
if effective_use_fd:
kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True})
bus = can.interface.Bus(**kwargs)
except Exception as e:
@ -269,29 +304,58 @@ def speed_test(cfg: CANSetupConfig, interface: str):
print(" ✗ No successful responses")
def run_setup(cfg: CANSetupConfig):
def run_setup(cfg: CANSetupConfig) -> bool:
"""Setup CAN interfaces."""
print("=" * 50)
print("CAN Interface Setup")
print("=" * 50)
print(f"Mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}")
print(f"Primary mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}")
print(f"Fallback mode: {'CAN 2.0' if cfg.use_fd else 'CAN FD'}")
print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps")
if cfg.use_fd:
print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps")
print()
interfaces = cfg.get_interfaces()
success_count = 0
failed: dict[str, str] = {}
mode_map: dict[str, bool] = {}
for interface in interfaces:
print(f"Configuring {interface}...")
if setup_interface(interface, cfg.bitrate, cfg.data_bitrate, cfg.use_fd):
is_up, status, _ = check_interface_status(interface)
print(f" ✓ {interface}: {status}")
ok, used_fd, summary = setup_interface_with_fallback(interface, cfg)
if ok:
success_count += 1
mode_map[interface] = bool(used_fd)
else:
print(f" ✗ {interface}: Failed")
failed[interface] = summary
print(f" ✗ {interface}: setup failed ({summary})")
print("\nSetup complete!")
print("\nNext: Test motors with:")
print()
print("=" * 50)
print("Setup summary")
print("=" * 50)
print(f"Interfaces configured: {success_count}/{len(interfaces)}")
if mode_map:
print("Active mode:")
for interface in interfaces:
if interface in mode_map:
print(f" - {interface}: {'CAN FD' if mode_map[interface] else 'CAN 2.0'}")
if failed:
print("Failed interfaces:")
for interface in interfaces:
if interface in failed:
print(f" - {interface}: {failed[interface]}")
print()
print("✗ Setup failed")
return False
print()
print("✓ Setup succeeded")
print()
print("Next: Test motors with:")
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}")
return True
def run_test(cfg: CANSetupConfig):
@ -341,7 +405,9 @@ def setup_can(cfg: CANSetupConfig):
sys.exit(1)
if cfg.mode == "setup":
run_setup(cfg)
ok = run_setup(cfg)
if not ok:
sys.exit(2)
elif cfg.mode == "test":
run_test(cfg)
elif cfg.mode == "speed":