Add optional CAN FD support

damiao_motor/core/controller.py

@@ -116,6 +116,7 @@ def __init__(
         self,
         channel: str = "can0",
         bustype: str = "socketcan",
+        fd: bool = False,
         bitrate: Optional[int] = None,
         **kwargs: Any,
     ) -> None:
@@ -149,6 +150,7 @@ def __init__(
         bus_kwargs: Dict[str, Any] = {
             "channel": channel,
             "interface": bustype,
+            "fd": fd,
             **kwargs,
         }
         if bitrate is not None:
@@ -162,6 +164,7 @@ def __init__(
         self._polling_thread: Optional[threading.Thread] = None
         self._polling_active = False
         self._polling_lock = threading.Lock()
+        self._fd = fd
 
     # -----------------------
     # Motor management
@@ -192,6 +195,7 @@ def add_motor(
             feedback_id=feedback_id,
             bus=self.bus,
             motor_type=motor_type,
+            fd=self._fd,
             **kwargs,
         )
         self.motors[motor_id] = motor

damiao_motor/core/motor.py

@@ -279,6 +279,7 @@ def __init__(
         motor_id: int,
         feedback_id: int,
         bus: can.Bus,
+        fd: bool,
         *,
         motor_type: str,
         p_min: Optional[float] = None,
@@ -323,6 +324,7 @@ def __init__(
         self.register_request_time_lock = threading.Lock()
         self.register_reply_time: Dict[int, float] = {}
         self.register_reply_time_lock = threading.Lock()
+        self._fd = fd
 
     def _resolve_limits(self, motor_type: str) -> Dict[str, float]:
         """Resolve limits from motor_type preset. Returns a dict of the 6 P/V/T limit values."""
@@ -439,7 +441,10 @@ def send_raw(self, data: bytes, arbitration_id: int | None = None) -> None:
 
         try:
             msg = can.Message(
-                arbitration_id=arbitration_id, data=data, is_extended_id=False
+                arbitration_id=arbitration_id,
+                data=data,
+                is_extended_id=False,
+                is_fd=self._fd,
             )
             self.bus.send(msg)
         except OSError as e:
@@ -927,6 +932,10 @@ def write_register(self, rid: int, value: float | int) -> None:
                 f"Unknown data_type: {reg_info.data_type} for register {rid}"
             )
 
+        # Clear stale cache before sending write command.
+        with self.registers_lock:
+            self.registers.pop(rid, None)
+
         # Send write command
         self._send_register_cmd(0x55, rid, data_bytes)