diff --git a/docs/control/can_control_architecture.md b/docs/control/can_control_architecture.md
new file mode 100644
index 0000000..c5f1723
--- /dev/null
+++ b/docs/control/can_control_architecture.md
@@ -0,0 +1,51 @@
+# CAN Control Architecture
+
+## End-to-End Network Diagram
+This defines the physical and logical boundaries of the Waybionic 6-DOF arm control network.
+
+```text
+[ Doctor Controller / Network ]
+ | (Ethernet/Wi-Fi)
+ v
+[ Robot Computer (Host) ] ---(USB3/GigE)---> [ Cameras ]
+ |-- ROS 2 High-Level
+ |-- ros2_control Hardware Interface
+ |-- SocketCAN Abstraction
+ | (CAN-FD)
+ v
+[ Logical CAN Channel (vcan0 / can0) ]
+ |--> [ Joint 1 Node ]
+ |--> [ Joint 2 Node ]
+ |--> [ Joint 3 Node ]
+ |--> [ Joint 4 Node ]
+ |--> [ Joint 5 Node ]
+ |--> [ Joint 6 Node ]
+
+* Note: Power, E-Stop, Motor Enable, and Hardware Safety loops operate on a completely separate hardware layer from the CAN bus.
+```
+
+## Responsibilities
+* **Host (Robot PC):** Computes kinematics, trajectories, and safety limits. Sends high-level position/velocity targets to the bus. Decodes joint feedback and publishes `sensor_msgs/msg/JointState`. Monitors CAN heartbeat/health and publishes to `/diagnostics`. **Does not generate individual step pulses.**
+* **Joint Nodes (Drives):** Close the local motor control loops (PID). Convert target pos/vel into actual motor currents/steps. Broadcast current position, velocity, and health/heartbeat back to the CAN bus.
+
+## Protocol Evaluation: `ros2_canopen` vs. Direct SocketCAN
+**1. `ros2_canopen` (CiA 402)**
+* **Pros:** Highly standardized. Plug-and-play if we purchase off-the-shelf (COTS) smart actuators that natively run the CANopen CiA 402 motion profile.
+* **Cons:** Massive overhead. The CANopen state machine is complex, and the SDO/PDO mapping can be rigid and difficult to debug.
+
+**2. Direct SocketCAN (Custom Protocol)**
+* **Pros:** Extremely low overhead. Allows us to fully utilize CAN-FD's 64-byte payload to pack pos/vel/health into single frames.
+* **Cons:** Requires us to define our own frame IDs and data packing.
+
+**Recommendation & Decision:**
+We will proceed with **Direct SocketCAN** wrapped in a clean, hardware-independent abstraction layer.
+* *If Electrical designs custom joint-controller PCBs:* We have the lightweight protocol we need.
+* *If Mechanical chooses COTS CANopen motors:* Our abstraction layer allows us to seamlessly swap the transport backend to `ros2_canopen` later without rewriting the core `ros2_control` logic.
+*(Provisional 6-node IDs and data layouts will be used until hardware is finalized).*
+
+## Useful Websites
+- https://www.csselectronics.com/pages/can-fd-flexible-data-rate-intro
+- https://docs.kernel.org/networking/can.html
+- https://github.com/linux-can/socketcand
+- https://github.com/ros-industrial/ros2_canopen
+- https://docs.openarm.dev/api-reference/can/
\ No newline at end of file
diff --git a/scripts/setup_vcan.sh b/scripts/setup_vcan.sh
new file mode 100755
index 0000000..056c5d7
--- /dev/null
+++ b/scripts/setup_vcan.sh
@@ -0,0 +1,16 @@
+#!/bin/bash
+set -e
+
+echo "=== Setting up Virtual CAN interface (vcan0) ==="
+
+# Load the virtual CAN kernel module
+sudo modprobe vcan
+
+# Create the vcan0 link (ignore error if it already exists)
+sudo ip link add dev vcan0 type vcan 2>/dev/null || true
+
+# Bring the interface up
+sudo ip link set up vcan0
+
+echo "✅ vcan0 is up and running!"
+echo "You can monitor traffic by running: candump vcan0"
diff --git a/waybionic_bringup/launch/can_demo.launch.py b/waybionic_bringup/launch/can_demo.launch.py
new file mode 100644
index 0000000..3973491
--- /dev/null
+++ b/waybionic_bringup/launch/can_demo.launch.py
@@ -0,0 +1,71 @@
+# Copyright 2026 Waybionic
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ can_interface_arg = DeclareLaunchArgument(
+ 'can_interface', default_value='vcan0',
+ description='SocketCAN interface; falls back to udp_multicast if unavailable')
+
+ simulate_faults_arg = DeclareLaunchArgument(
+ 'simulate_faults', default_value='true',
+ description='Inject the provisional joint 4 / joint 6 fault scenarios')
+
+ start_mock_drives_arg = DeclareLaunchArgument(
+ 'start_mock_drives', default_value='true',
+ description='Set false when driving real hardware on the bus')
+
+ transport_arg = DeclareLaunchArgument(
+ 'transport', default_value='socketcan',
+ description='Transport: socketcan (default) or udp_multicast.'
+ )
+
+ can_host_node = Node(
+ package='waybionic_control',
+ executable='can_host',
+ name='can_host',
+ output='screen',
+ parameters=[
+ {'can_interface': LaunchConfiguration('can_interface')},
+ {'transport': LaunchConfiguration('transport')},
+ ]
+ )
+
+ mock_drives_node = Node(
+ package='waybionic_control',
+ executable='mock_drives',
+ name='mock_drives',
+ output='screen',
+ condition=IfCondition(LaunchConfiguration('start_mock_drives')),
+ parameters=[
+ {'can_interface': LaunchConfiguration('can_interface')},
+ {'simulate_faults': LaunchConfiguration('simulate_faults')},
+ {'transport': LaunchConfiguration('transport')},
+ ]
+ )
+
+ return LaunchDescription([
+ can_interface_arg,
+ simulate_faults_arg,
+ start_mock_drives_arg,
+ transport_arg,
+ can_host_node,
+ mock_drives_node,
+ ])
diff --git a/waybionic_bringup/launch/ground_station.launch.py b/waybionic_bringup/launch/ground_station.launch.py
index 2183b8b..b9b3fab 100644
--- a/waybionic_bringup/launch/ground_station.launch.py
+++ b/waybionic_bringup/launch/ground_station.launch.py
@@ -28,7 +28,6 @@ def generate_launch_description():
default_rviz_config_path = os.path.join(
waybionic_bringup_dir, 'rviz', 'waybionic_unified.rviz')
- # --- Declare Launch Arguments ---
model_arg = DeclareLaunchArgument(
'model', default_value=default_model_path,
description='Absolute path to robot urdf')
@@ -63,7 +62,6 @@ def generate_launch_description():
file_check = OpaqueFunction(function=check_files_exist)
- # --- Nodes ---
robot_description_content = {
'robot_description': Command(['xacro ', LaunchConfiguration('model')])
}
@@ -81,7 +79,6 @@ def generate_launch_description():
condition=IfCondition(LaunchConfiguration('use_joint_state_publisher_gui'))
)
- # Pass the correct arguments to the temporary publisher
temp_diag_pub_node = Node(
package='waybionic_rviz_plugins', executable='temporary_diagnostics_publisher.py',
name='temp_diag_pub',
@@ -93,7 +90,6 @@ def generate_launch_description():
]
)
- # Pass the mock toggles into the RViz node parameters
rviz_node = Node(
package='rviz2', executable='rviz2', name='rviz2', output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
diff --git a/waybionic_bringup/package.xml b/waybionic_bringup/package.xml
index f262ccb..d35d763 100644
--- a/waybionic_bringup/package.xml
+++ b/waybionic_bringup/package.xml
@@ -13,6 +13,7 @@
xacro
waybionic_description
waybionic_rviz_plugins
+ waybionic_control
launch
launch_ros
robot_state_publisher
diff --git a/waybionic_control/package.xml b/waybionic_control/package.xml
new file mode 100644
index 0000000..c5914b3
--- /dev/null
+++ b/waybionic_control/package.xml
@@ -0,0 +1,27 @@
+
+
+
+ waybionic_control
+ 0.0.0
+
+ CAN-FD host and mock drive nodes for WayBionic ground station.
+ Bridges ROS 2 JointState commands to a provisional CAN-FD protocol over vcan0 with udp_multicast fallback
+ Publishes joint feedback and per-joint health diagnostics.
+
+ Harold Kim
+ Apache-2.0
+
+ rclpy
+ sensor_msgs
+ diagnostic_msgs
+ python3-can
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/waybionic_control/resource/waybionic_control b/waybionic_control/resource/waybionic_control
new file mode 100644
index 0000000..e69de29
diff --git a/waybionic_control/setup.cfg b/waybionic_control/setup.cfg
new file mode 100644
index 0000000..2678702
--- /dev/null
+++ b/waybionic_control/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/waybionic_control
+[install]
+install_scripts=$base/lib/waybionic_control
diff --git a/waybionic_control/setup.py b/waybionic_control/setup.py
new file mode 100644
index 0000000..c15da1c
--- /dev/null
+++ b/waybionic_control/setup.py
@@ -0,0 +1,31 @@
+from setuptools import find_packages, setup
+
+package_name = 'waybionic_control'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='hoodu',
+ maintainer_email='harold.kim@ucalgary.ca',
+ description='CAN-FD host and mock drive nodes for WayBionic ground station',
+ license='Apache-2.0',
+ extras_require={
+ 'test': [
+ 'pytest',
+ ],
+ },
+ entry_points={
+ 'console_scripts': [
+ 'mock_drives = waybionic_control.node.mock_drives:main',
+ 'can_host = waybionic_control.node.can_host:main'
+ ],
+ },
+)
diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py
new file mode 100644
index 0000000..891da9f
--- /dev/null
+++ b/waybionic_control/test/test_can_control.py
@@ -0,0 +1,325 @@
+# Copyright 2026 Waybionic
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import struct
+import time
+import unittest
+from unittest.mock import MagicMock, patch
+
+import can
+from diagnostic_msgs.msg import DiagnosticStatus
+import rclpy
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import JointState
+
+from waybionic_control.node.can_host import CanHostNode
+from waybionic_control.node.mock_drives import MockDrivesNode
+from waybionic_control.protocol import codec
+
+
+class TestCanControlLogic(unittest.TestCase):
+
+ @classmethod
+ def setUpClass(cls):
+ rclpy.init()
+
+ @classmethod
+ def tearDownClass(cls):
+ rclpy.shutdown()
+
+ def setUp(self):
+ self.bus_patcher = patch('can.interface.Bus')
+ self.mock_bus = self.bus_patcher.start()
+ self.node = CanHostNode()
+
+ def tearDown(self):
+ self.node.destroy_node()
+ self.bus_patcher.stop()
+
+ def test_codec_packing(self):
+ cmd_data = codec.encode_target_command(1.5, -0.5)
+ self.assertEqual(len(cmd_data), 8)
+ pos, vel = codec.decode_target_command(cmd_data)
+ self.assertAlmostEqual(pos, 1.5, places=4)
+ self.assertAlmostEqual(vel, -0.5, places=4)
+
+ state_data = codec.encode_joint_state(3.14, 0.0, 1, 0xAA)
+ self.assertEqual(len(state_data), 10)
+ p, v, h, f = codec.decode_joint_state(state_data)
+ self.assertEqual(f, 0xAA)
+
+ def test_incomplete_command_rejected(self):
+ js = JointState()
+ js.name = ['joint_1']
+ js.position = []
+
+ self.node.command_callback(js)
+ self.node.bus.send.assert_not_called()
+
+ msg_with_pos = JointState()
+ msg_with_pos.name = ['joint_1']
+ msg_with_pos.position = [1.5]
+ self.node.command_callback(msg_with_pos)
+ self.node.bus.send.assert_called_once()
+
+ def test_invalid_mappings(self):
+ msg = can.Message(
+ arbitration_id=0x999, data=b'\x00\x00', is_extended_id=False)
+ self.node.bus.recv.side_effect = [msg, None]
+
+ last_seen = dict(self.node.last_seen)
+
+ try:
+ self.node.read_bus()
+ except Exception as e:
+ self.fail(f'Node crashed on invalid mapping: {e}')
+ self.assertEqual(self.node.last_seen, last_seen)
+
+ def test_short_payloads(self):
+ with self.assertRaises(ValueError):
+ codec.decode_target_command(b'\x00' * 7)
+
+ with self.assertRaises(ValueError):
+ codec.decode_joint_state(b'\x00' * 9)
+
+ def test_non_finite_values_rejected(self):
+ with self.assertRaises(ValueError):
+ codec.encode_target_command(float('nan'), 0.0)
+ with self.assertRaises(ValueError):
+ codec.encode_joint_state(0.0, float('inf'), 1, 0)
+
+ bad_cmd = struct.pack('= len(msg.position):
+ self.get_logger().warning(f'Rejecting command {name}: missing position')
+ continue
+
+ try:
+ joint_id = int(name.split('_')[1])
+ if 1 <= joint_id <= 6:
+ target_pos = msg.position[i]
+ target_vel = msg.velocity[i] if i < len(msg.velocity) else 0.0
+
+ data = codec.encode_target_command(target_pos, target_vel)
+ can_msg = can.Message(
+ arbitration_id=codec.CMD_BASE_ID + joint_id,
+ data=data,
+ is_extended_id=False,
+ is_fd=True
+ )
+ self.bus.send(can_msg)
+ except (ValueError, IndexError, can.CanError) as e:
+ self.get_logger().error(f'Command error: {e}')
+
+ def read_bus(self):
+ if self.bus is None:
+ return
+
+ # Process max 100 messages per tick to prevent infinite blocking
+ for _ in range(100):
+ msg = self.bus.recv(0.0)
+ if msg is None:
+ break
+
+ if codec.STATE_BASE_ID + 1 <= msg.arbitration_id <= codec.STATE_BASE_ID + 6:
+ joint_id = msg.arbitration_id - codec.STATE_BASE_ID
+ try:
+ pos, vel, health, fault = codec.decode_joint_state(msg.data)
+ self.last_seen[joint_id] = time.time()
+ self.faults[joint_id] = fault
+ self.healths[joint_id] = health
+ self.publish_joint_state(joint_id, pos, vel)
+ except ValueError as e:
+ self.get_logger().warning(f'Ignored bad state: {e}')
+
+ def publish_joint_state(self, joint_id, pos, vel):
+ js = JointState()
+ js.header.stamp = self.get_clock().now().to_msg()
+ js.name = [f'joint_{joint_id}']
+ js.position = [pos]
+ js.velocity = [vel]
+ self.joint_pub.publish(js)
+
+ def publish_diagnostics(self):
+ diag_array = DiagnosticArray()
+ diag_array.header.stamp = self.get_clock().now().to_msg()
+ current_time = time.time()
+
+ if self.bus is None:
+ bus_stat = DiagnosticStatus(
+ name='can.bus: Link Status',
+ level=DiagnosticStatus.ERROR,
+ message=f'DOWN ({self.transport} init failed: {self.bus_error})'
+ )
+ else:
+ bus_stat = DiagnosticStatus(
+ name='can.bus: Link Status',
+ level=DiagnosticStatus.OK,
+ message=f'ACTIVE ({self.transport})'
+ )
+ bus_stat.values.append(
+ KeyValue(key='transport', value=str(self.transport)))
+ diag_array.status.append(bus_stat)
+
+ cmd_stat = DiagnosticStatus(name='can.bus: Command Age')
+ cmd_age = current_time - self.last_cmd_time
+ if self.last_cmd_time == 0.0:
+ cmd_stat.level = DiagnosticStatus.WARN
+ cmd_stat.message = 'NO COMMANDS RECEIVED YET'
+ elif cmd_age > 1.0:
+ cmd_stat.level = DiagnosticStatus.WARN
+ cmd_stat.message = f'STALE COMMANDS ({cmd_age:.1f}s ago)'
+ else:
+ cmd_stat.level = DiagnosticStatus.OK
+ cmd_stat.message = f'ACTIVE ({cmd_age:.1f}s ago)'
+ diag_array.status.append(cmd_stat)
+
+ for joint_id in range(1, 7):
+ status = DiagnosticStatus()
+ status.name = f'can.bus: Joint {joint_id} Health'
+ status.hardware_id = f'joint_{joint_id}'
+
+ status.values.append(KeyValue(key='fault_code', value=hex(self.faults[joint_id])))
+ status.values.append(KeyValue(key='health', value=str(self.healths[joint_id])))
+
+ if current_time - self.last_seen[joint_id] > 0.5:
+ status.level = DiagnosticStatus.ERROR
+ status.message = 'STALE (No heartbeat)'
+ elif self.faults[joint_id] != 0:
+ status.level = DiagnosticStatus.ERROR
+ status.message = f'HARDWARE FAULT (Code: {hex(self.faults[joint_id])})'
+ elif self.healths[joint_id] == 0:
+ status.level = DiagnosticStatus.ERROR
+ status.message = 'UNHEALTHY (health=0, no fault code)'
+ else:
+ status.level = DiagnosticStatus.OK
+ status.message = 'OK'
+
+ diag_array.status.append(status)
+
+ self.diag_pub.publish(diag_array)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = CanHostNode()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py
new file mode 100644
index 0000000..10ac092
--- /dev/null
+++ b/waybionic_control/waybionic_control/node/mock_drives.py
@@ -0,0 +1,137 @@
+# Copyright 2026 Waybionic
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import can
+import rclpy
+from rclpy.node import Node
+
+from waybionic_control.protocol import codec
+
+
+class MockDrivesNode(Node):
+ """Node simulating 6 CAN-based joint controllers."""
+
+ def __init__(self, **kwargs):
+ """Initialize the MockDrivesNode and connect to the configured transport."""
+ super().__init__('mock_drives', **kwargs)
+ self.declare_parameter('simulate_faults', True)
+ self.declare_parameter('can_interface', 'vcan0')
+ self.declare_parameter('transport', 'socketcan')
+
+ can_interface = self.get_parameter('can_interface').value
+ self.transport = self.get_parameter('transport').value
+ self.bus = None
+
+ if self.transport == 'socketcan':
+ try:
+ self.bus = can.interface.Bus(
+ bustype='socketcan', channel=can_interface, fd=True)
+ self.get_logger().info(f'SocketCAN active on {can_interface}')
+ except (can.CanError, OSError) as e:
+ self.get_logger().error(
+ f'SocketCAN init failed on {can_interface}: {e}. Node is '
+ 'degraded; no frames will be published.')
+ elif self.transport == 'udp_multicast':
+ try:
+ self.bus = can.interface.Bus(
+ bustype='udp_multicast', channel='224.0.0.1', fd=True)
+ self.get_logger().warning(
+ 'udp_multicast transport selected. This is NOT a physical '
+ 'CAN link and must not be used for hardware validation.')
+ except (can.CanError, OSError) as e:
+ self.get_logger().error(f'udp_multicast init failed: {e}')
+ else:
+ self.get_logger().error(
+ f'Unknown transport {self.transport!r}; '
+ "expected 'socketcan' or 'udp_multicast'.")
+
+ self.positions = {i: 0.0 for i in range(1, 7)}
+ self.velocities = {i: 0.0 for i in range(1, 7)}
+ self.targets = {i: 0.0 for i in range(1, 7)}
+
+ self.timer = self.create_timer(0.1, self.timer_callback)
+ self.count = 0
+ self.get_logger().info('Mock drives started. Broadcasting 6 joints at 10Hz.')
+
+ def timer_callback(self):
+ if self.bus is None:
+ return
+
+ simulate_faults = self.get_parameter('simulate_faults').value
+
+ # Process max 100 messages per tick to prevent infinite blocking
+ for _ in range(100):
+ msg = self.bus.recv(0.0)
+ if msg is None:
+ break
+ if codec.CMD_BASE_ID + 1 <= msg.arbitration_id <= codec.CMD_BASE_ID + 6:
+ joint_id = msg.arbitration_id - codec.CMD_BASE_ID
+ try:
+ target_pos, target_vel = codec.decode_target_command(msg.data)
+ self.targets[joint_id] = target_pos
+ except ValueError as e:
+ self.get_logger().warning(f'Ignored bad command: {e}')
+
+ for joint_id in range(1, 7):
+ if simulate_faults and joint_id == 6 and 30 < self.count <= 70:
+ continue
+
+ diff = self.targets[joint_id] - self.positions[joint_id]
+ self.velocities[joint_id] = diff * 2.0
+ self.positions[joint_id] += diff * 0.5
+
+ health_status = 1
+ fault_code = 0
+
+ # Provisional fault scenarios, pending Electrical confirmation.
+ if simulate_faults:
+ if joint_id == 4 and 50 < self.count <= 90:
+ health_status = 0
+ fault_code = 0xAA
+ elif joint_id == 5 and 60 < self.count <= 100:
+ health_status = 0
+ fault_code = 0
+
+ try:
+ data = codec.encode_joint_state(
+ self.positions[joint_id],
+ self.velocities[joint_id],
+ health_status,
+ fault_code
+ )
+ msg = can.Message(
+ arbitration_id=codec.STATE_BASE_ID + joint_id,
+ data=data,
+ is_extended_id=False,
+ is_fd=True
+ )
+ self.bus.send(msg)
+ except ValueError as e:
+ self.get_logger().warning(f'Skipped bad state for joint {joint_id}: {e}')
+ except can.CanError as e:
+ self.get_logger().error(f'CAN error: {e}')
+
+ self.count += 1
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = MockDrivesNode()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/waybionic_control/waybionic_control/protocol/__init__.py b/waybionic_control/waybionic_control/protocol/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/waybionic_control/waybionic_control/protocol/codec.py b/waybionic_control/waybionic_control/protocol/codec.py
new file mode 100644
index 0000000..01010d9
--- /dev/null
+++ b/waybionic_control/waybionic_control/protocol/codec.py
@@ -0,0 +1,109 @@
+# Copyright 2026 Waybionic
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Protocol codec for encoding and decoding provisional Waybionic CAN-FD frames."""
+
+import math
+import struct
+
+# Provisional CAN IDs
+STATE_BASE_ID = 0x100
+CMD_BASE_ID = 0x200
+
+# Maximum magnitude representable in IEEE-754 binary32 (' FLOAT32_MAX:
+ raise ValueError(
+ f'{label} ({value!r}) exceeds float32 wire range +/-{FLOAT32_MAX:g}')
+
+
+def encode_target_command(position, velocity):
+ """
+ Encode a target position and velocity into a CAN command frame.
+
+ :param position: Target position.
+ :param velocity: Target velocity.
+ :return: 8-byte packed payload.
+ :raises ValueError: If a value is NaN, Inf, or outside float32 range.
+ """
+ _validate_float32(position, 'Command position')
+ _validate_float32(velocity, 'Command velocity')
+ try:
+ return struct.pack('= 8:
+ pos, vel = struct.unpack('= 10:
+ pos, vel, health, fault = struct.unpack('