Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
404 changes: 404 additions & 0 deletions examples/o30_classic_can.py

Large diffs are not rendered by default.

207 changes: 207 additions & 0 deletions examples/o6_can_mode.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,207 @@
"""Standalone O6 HOP CAN mode switch; no motion and no default Flash writes."""

from __future__ import annotations

import argparse
import math
import subprocess
from time import monotonic

import can


def exchange(bus, request_id, data, *, can_fd, timeout=0.5, response_fd=None):
bus.send(
can.Message(
arbitration_id=request_id,
data=data,
is_extended_id=False,
is_fd=can_fd,
check=True,
),
timeout=timeout,
)
deadline = monotonic() + timeout
while (remaining := deadline - monotonic()) > 0:
reply = bus.recv(timeout=remaining)
if reply is None:
break
if (
reply.is_error_frame
or reply.is_remote_frame
or reply.is_extended_id
or reply.arbitration_id != request_id | 0x400
or (response_fd is not None and reply.is_fd != response_fd)
):
continue
payload = bytes(reply.data)
if len(payload) < 3:
continue
control, _start, size = payload[:3]
if control & 0x7F == 0x4F:
raise RuntimeError(f"Device communication error: {payload.hex()}")
if payload[:3] != data[:3]:
continue
if len(payload) < 3 + size:
raise RuntimeError("Truncated HOP response")
return payload[3 : 3 + size], reply.is_fd
raise TimeoutError(f"No matching reply for {data.hex()}")


def inspect(bus, request_id, can_fd, side, timeout):
product, wire = exchange(
bus, request_id, bytes.fromhex("410005"), can_fd=can_fd, timeout=timeout
)
model = product.split(b"\x00", 1)[0].decode("ascii")
if model.upper() not in {"O6", "LHO6"}:
raise RuntimeError(f"Expected O6, received {model!r}; device not changed")
reported_side, _ = exchange(
bus,
request_id,
bytes.fromhex("41F105"),
can_fd=can_fd,
timeout=timeout,
response_fd=wire,
)
if reported_side.rstrip(b"\x00").decode("ascii").lower() != side:
raise RuntimeError("Device side mismatch; device not changed")
configuration, _ = exchange(
bus,
request_id,
bytes.fromhex("B50604"),
can_fd=can_fd,
timeout=timeout,
response_fd=wire,
)
mode, brs, nominal, data = configuration
if (
mode not in (1, 2)
or brs not in (0, 1)
or not 1 <= nominal <= 8
or not 1 <= data <= 13
):
raise RuntimeError(f"Invalid device configuration: {configuration.hex()}")
print(
f"O6 {side}: reply={'canfd' if wire else 'can'}, mode={mode}, BRS={brs}",
flush=True,
)
return configuration, wire


def configure_host(channel, fd, bitrate, dbitrate):
subprocess.run(
["sudo", "-n", "ip", "link", "set", "dev", channel, "down"], check=True
)
command = [
"sudo",
"-n",
"ip",
"link",
"set",
"dev",
channel,
"type",
"can",
"bitrate",
str(bitrate),
]
if fd:
command += ["dbitrate", str(dbitrate), "fd", "on"]
else:
command += ["fd", "off"]
subprocess.run(command, check=True)
subprocess.run(
["sudo", "-n", "ip", "link", "set", "dev", channel, "up"], check=True
)


def switch(bus, request_id, source_fd, target_fd, side, *, timeout=0.5, save=False):
configuration, wire = inspect(bus, request_id, source_fd, side, timeout)
if wire != source_fd:
raise RuntimeError("Actual reply format differs from --from-mode")
target = 2 if target_fd else 1
if not target_fd and configuration[1]:
raise RuntimeError("BRS must be disabled before switching to classic CAN")
if configuration[0] != target or wire != target_fd:
bus.send(
can.Message(
arbitration_id=request_id,
data=bytes((0x35, 6, 1, target)),
is_extended_id=False,
is_fd=source_fd,
check=True,
),
timeout=timeout,
)
print(
"Mode write sent once; an absent reply does not imply no change", flush=True
)
updated, actual_fd = inspect(bus, request_id, target_fd, side, timeout)
if updated[0] != target or actual_fd != target_fd:
raise RuntimeError("Target mode or active reply format was not verified")
if save:
result, _ = exchange(
bus,
request_id,
bytes.fromhex("351E0101"),
can_fd=target_fd,
timeout=timeout,
response_fd=target_fd,
)
if result != b"\x00":
raise RuntimeError(
"Save completion not confirmed; do not repeat automatically"
)
print(
"Flash save completion confirmed; power-cycle persistence not tested",
flush=True,
)
print("Target configuration and active wire format verified", flush=True)


def main(argv=None):
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--channel", default="can0")
parser.add_argument("--request-id", type=lambda value: int(value, 0), default=1)
parser.add_argument("--side", choices=("left", "right"), required=True)
parser.add_argument("--from-mode", choices=("can", "canfd"), required=True)
parser.add_argument("--mode", choices=("can", "canfd"))
parser.add_argument(
"--save",
action="store_true",
help="Explicitly save the verified target to Flash",
)
parser.add_argument("--bitrate", type=int, default=1_000_000)
parser.add_argument("--dbitrate", type=int, default=5_000_000)
parser.add_argument("--timeout", type=float, default=0.5)
args = parser.parse_args(argv)
if not 0 <= args.request_id <= 0x3FE:
parser.error("request ID must be within 0..0x3FE")
if not 0 < args.bitrate <= 1_000_000 or not 0 < args.dbitrate <= 8_000_000:
parser.error("invalid bitrates")
if not math.isfinite(args.timeout) or args.timeout <= 0:
parser.error("timeout must be finite and positive")
if args.save and args.mode is None:
parser.error("--save requires --mode")
configure_host(args.channel, True, args.bitrate, args.dbitrate)
with can.Bus(interface="socketcan", channel=args.channel, fd=True) as bus:
if args.mode is None:
inspect(
bus, args.request_id, args.from_mode == "canfd", args.side, args.timeout
)
else:
switch(
bus,
args.request_id,
args.from_mode == "canfd",
args.mode == "canfd",
args.side,
timeout=args.timeout,
save=args.save,
)
if args.mode is not None:
configure_host(args.channel, args.mode == "canfd", args.bitrate, args.dbitrate)


if __name__ == "__main__":
main()
4 changes: 4 additions & 0 deletions src/linkerbot_hand/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
AckSupport,
AngleFeedbackPolicy,
AngleSample,
CANConfiguration,
ChannelMetrics,
ClosedError,
CommandFamily,
Expand Down Expand Up @@ -54,6 +55,7 @@
PositionKdSample,
PositionKpSample,
ProtocolError,
ProtocolKind,
Quantization,
QueueFullError,
RawNumber,
Expand Down Expand Up @@ -138,6 +140,7 @@
"AmbiguousHandError",
"AngleFeedbackPolicy",
"AngleSample",
"CANConfiguration",
"ChannelMetrics",
"ClosedError",
"CommandCapability",
Expand Down Expand Up @@ -217,6 +220,7 @@
"PositionKdSample",
"PositionKpSample",
"ProtocolError",
"ProtocolKind",
"PythonCANDiscoveryConfig",
"Quantization",
"QueueFullError",
Expand Down
4 changes: 4 additions & 0 deletions src/linkerbot_hand/api/__init__.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
"""Stable public contracts for the Hand SDK architecture."""

from .can_configuration import CANConfiguration
from .commands import (
AckSupport,
CommandFamily,
Expand All @@ -16,6 +17,7 @@
DataOrigin,
HandSide,
JointMotionType,
ProtocolKind,
Quantization,
ReadMode,
SampleProjection,
Expand Down Expand Up @@ -95,6 +97,7 @@
"AckSupport",
"AngleFeedbackPolicy",
"AngleSample",
"CANConfiguration",
"ChannelMetrics",
"ClosedError",
"CommandFamily",
Expand Down Expand Up @@ -145,6 +148,7 @@
"PositionKdSample",
"PositionKpSample",
"ProtocolError",
"ProtocolKind",
"Quantization",
"QueueFullError",
"RawNumber",
Expand Down
22 changes: 21 additions & 1 deletion src/linkerbot_hand/api/_device_core.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
from .commands import CommandHandle
from .config import DeviceConfig, PollingPlan
from .device_info import DeviceInfo
from .enums import ProtocolKind, TransportKind
from .errors import (
ClosedError,
LifecycleError,
Expand Down Expand Up @@ -83,6 +84,7 @@ def __init__(
model: str,
transport: object,
side: str | None = None,
protocol: ProtocolKind | str | None = None,
config: DeviceConfig | None = None,
clock: object | None = None,
protocol_backend: object | None = None,
Expand All @@ -99,16 +101,34 @@ def __init__(
if runtime_factory is not None and not callable(runtime_factory):
raise ValidationError("runtime_factory must be callable or None")

capabilities = get_declared_capabilities(model, side=side)
self._transport = transport
self._config = DeviceConfig() if config is None else config
capabilities = get_declared_capabilities(model, side=side, protocol=protocol)
if self._config.hop_transport is TransportKind.CAN:
if (
capabilities.model not in {"O6", "O30"}
or capabilities.protocol is not ProtocolKind.HOP
):
raise ValidationError(
"classic HOP CAN requires O6 or O30 with protocol='hop'"
)
capabilities = get_declared_capabilities(
model, side=side, protocol=protocol, transport_kind=TransportKind.CAN
)
if self._config.o30_request_id is not None:
if capabilities.model != "O30":
raise ValidationError("o30_request_id is only valid for O30")
if protocol_backend is not None:
raise ValidationError(
"use o30_request_id or protocol_backend, not both"
)
if self._config.hop_request_id is not None or self._config.hop_brs:
if capabilities.protocol is not ProtocolKind.HOP:
raise ValidationError("HOP configuration requires protocol='hop'")
if protocol_backend is not None:
raise ValidationError(
"use HOP configuration or protocol_backend, not both"
)
self._clock = clock
self._protocol_backend = protocol_backend
self._runtime_factory = (
Expand Down
7 changes: 7 additions & 0 deletions src/linkerbot_hand/api/_runtime_contract.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
DeviceCapabilities,
TelemetrySemantic,
)
from .can_configuration import CANConfiguration
from .commands import CommandHandle
from .config import DeviceConfig, PollingPlan
from .device_info import DeviceInfo
Expand Down Expand Up @@ -37,6 +38,12 @@ def read(self, semantic: TelemetrySemantic, timeout: float) -> object: ...

def read_device_info(self, timeout: float) -> DeviceInfo: ...

def read_can_configuration(self, timeout: float) -> CANConfiguration: ...

def write_can_configuration(
self, sub_index: int, payload: bytes, timeout: float
) -> None: ...

def operate(self, operation: str) -> CommandHandle: ...

def recover(self, timeout: float) -> None: ...
Expand Down
Loading
Loading