diff --git a/CMakeLists.txt b/CMakeLists.txt
index 078ff7e..09795fc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,11 +40,11 @@ target_include_directories(imu
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
 )
 
-target_link_libraries(imu PUBLIC ${PUBLIC_DEPENDENCIES} hipnuc_imu imu_protocol)
+target_link_libraries(imu PUBLIC ${PUBLIC_DEPENDENCIES} hipnuc_imu mct7123_imu imu_protocol)
 
 install(DIRECTORY include/ DESTINATION include)
 
-install(TARGETS imu hipnuc_imu imu_protocol
+install(TARGETS imu hipnuc_imu mct7123_imu imu_protocol
   EXPORT roboparty_imu-targets
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
@@ -79,7 +79,9 @@ install(FILES
   DESTINATION lib/cmake/roboparty_imu
 )
 
-pybind11_add_module(imu_py src/pybind_module.cpp)
+# Release 构建已经统一使用 -O3。禁用 pybind11 自动附加的 LTO，避免在
+# 内存较小的 ARM 设备上串行执行 LTRANS，并保持 colcon/CMake 输出无警告。
+pybind11_add_module(imu_py NO_EXTRAS src/pybind_module.cpp)
 target_include_directories(imu_py
   PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..1ae0901
--- /dev/null
+++ b/README.md
@@ -0,0 +1,332 @@
+# roboparty_imu
+
+RoboParty 双足机器人 IMU 驱动库，提供 C++ 接口和 Python 绑定，支持
+MCT7123（MD7123）与 HiPNUC 系列设备。
+
+## 1. 功能概览
+
+| 设备 | 通信接口 | 数据帧 | 校验 |
+| --- | --- | --- | --- |
+| MCT7123（MD7123） | 串口、CAN FD、CANable2 SLCAN-FD | 串口为 69 字节封装帧；CAN FD 为 64 字节载荷 | CRC16-CCITT（多项式 `0x1021`） |
+| HiPNUC（HI226、HI229 等） | 串口、经典 CAN（J1939） | `5A A5` 开头的变长帧或 J1939 帧 | 串口使用 CRC16；J1939 无额外应用层校验 |
+
+主要能力：
+
+- 同一条 CAN 总线同时接入经典 CAN 和 CAN FD 设备。
+- 自动区分并路由 MCT7123 原始数据、姿态数据和配置响应。
+- 支持多个驱动订阅同一 CAN 路由，并安全管理回调生命周期。
+- 提供角速度、加速度、磁场、四元数、欧拉角、温度和帧计数器等数据。
+- 提供命令行测试工具，可明确选择串口、SocketCAN 或 CANable2。
+
+## 2. 构建
+
+### 使用 ROS 2 和 colcon
+
+以 ROS 2 Humble 和 Ubuntu 为例，先安装构建依赖：
+
+```bash
+sudo apt-get update
+sudo apt-get install \
+  build-essential \
+  cmake \
+  ccache \
+  python3-colcon-common-extensions \
+  python3-dev \
+  pybind11-dev \
+  libfmt-dev \
+  libspdlog-dev \
+  ros-humble-ament-cmake
+```
+
+使用其他 ROS 2 发行版时，将 `ros-humble-ament-cmake` 中的 `humble`
+替换为对应的发行版名称。
+
+推荐将仓库放在 ROS 2 工作区的 `src/` 目录下：
+
+```text
+imu_ws/
+└── src/
+    └── imu/
+```
+
+在工作区根目录执行：
+
+```bash
+cd ~/imu_ws
+
+# 按实际安装的 ROS 2 发行版修改 humble
+source /opt/ros/humble/setup.bash
+
+# 只构建本包
+colcon build --symlink-install --packages-select roboparty_imu
+
+# 加载本工作区
+source install/setup.bash
+```
+
+如果已经加载 ROS 2 环境，也可以确认包是否被正确识别：
+
+```bash
+colcon list
+```
+
+输出中应包含：
+
+```text
+roboparty_imu    src/imu    (ros.ament_cmake)
+```
+
+修改代码后重新执行 `colcon build`，并在新终端中重新加载
+`install/setup.bash`。
+
+### 使用独立 CMake
+
+不使用 ROS 2 时，需要提前安装支持 C++17 的编译器、CMake 3.12、
+Python 3 开发头文件、pybind11、fmt、spdlog 和 ccache。
+
+在仓库根目录执行：
+
+```bash
+cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
+cmake --build build -j"$(nproc)"
+```
+
+独立构建完成后，Python 模块位于 `build/` 目录。测试脚本默认从该目录加载
+`imu_py`，也可以使用 `--build-dir` 指定其他构建目录。
+
+## 3. 测试工具
+
+测试脚本位于 `scripts/test_imu.py`。
+
+### 默认通信参数
+
+| 型号 | 原生串口 | CAN 仲裁速率 | CAN FD 数据速率 | 测试脚本默认节点编号 |
+| --- | --- | --- | --- | --- |
+| MCT7123 | `921600 bit/s` | `500 kbit/s` | `2 Mbit/s` | `1` |
+| HiPNUC | `115200 bit/s` | `500 kbit/s` | 不适用 | `8` |
+
+SocketCAN 接口需要在运行测试脚本前配置并拉起，脚本不会自动修改系统网络接口。
+节点编号是设备参数，可能与表中的脚本默认值不同。测试脚本不会修改设备节点
+编号；编号不一致时，仅通过 `--id` 传入设备当前编号。
+
+MCT7123（以下使用 `can1`，避免与示例中的 HiPNUC `can0` 冲突）：
+
+```bash
+# 500 kbit/s 仲裁段，2 Mbit/s 数据段，启用 CAN FD
+sudo ip link set can1 down
+sudo ip link set can1 type can bitrate 500000 dbitrate 2000000 fd on
+sudo ip link set can1 up
+```
+
+HiPNUC：
+
+```bash
+# 500 kbit/s 经典 CAN，不启用 CAN FD
+sudo ip link set can0 down
+sudo ip link set can0 type can bitrate 500000
+sudo ip link set can0 up
+```
+
+### 指定通信接口
+
+必须同时指定型号和通信接口：
+
+```bash
+# 原生串口
+python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 -d 5
+
+# SocketCAN
+python3 scripts/test_imu.py MCT7123 --can can1 -d 5
+python3 scripts/test_imu.py HIPNUC --can can0 --id 8 -d 5  # 对应 PGN 未启用时，默认显示项会全为 0
+
+# CANable2 SLCAN-FD（目前仅支持 MCT7123；仅在该设备节点存在时执行）
+python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM0 -d 5
+```
+
+`--slcan` 模式直接通过 Python 读取 CANable2，需要额外安装
+[`python-can`](https://python-can.readthedocs.io/)：
+
+```bash
+python3 -m pip install --user --no-warn-script-location python-can
+```
+
+CANable2 默认使用 `500 kbit/s` 仲裁速率和 `2 Mbit/s` 数据速率。脚本不会自动
+选择 SLCAN 设备，需要显式传入 `--slcan`。
+
+### 配置 HiPNUC 数据输出
+
+测试工具的默认界面显示欧拉角和温度。如果设备只启用了加速度、角速度和
+四元数 PGN，默认界面会全部显示为 `0`，但这不表示 CAN 通信或解析失败。
+使用 `--all` 可以查看当前已经启用的数据：
+
+```bash
+python3 scripts/test_imu.py HIPNUC --can can0 --id 8 --all -d 5
+```
+
+下面的命令以节点 ID `8` 为例，开启俯仰/横滚、航向角和温度输出：
+
+```bash
+cansend can0 0CEF0808#3D0106000A000000  # 寄存器 0x013D：俯仰/横滚，10 ms（100 Hz）
+cansend can0 0CEF0808#410106000A000000  # 寄存器 0x0141：航向角，10 ms（100 Hz）
+cansend can0 0CEF0808#4301060064000000  # 寄存器 0x0143：温度，100 ms（10 Hz）
+cansend can0 0CEF0808#0000060000000000  # 寄存器 0x0000，值 0：保存全部参数到 Flash
+cansend can0 0CEF0808#00000600FF000000  # 寄存器 0x0000，值 0xFF：复位设备
+```
+
+配置载荷格式为
+`[寄存器低字节][寄存器高字节][命令 0x06][保留][32 位小端值]`。
+例如 `3D 01 06 00 0A 00 00 00` 表示向寄存器 `0x013D` 写入周期
+`10 ms`。
+
+以上命令需要安装 `can-utils`：
+
+```bash
+sudo apt-get install can-utils
+```
+
+配置成功后，总线上应出现以下 J1939 报文：
+
+| CAN ID | PGN | 数据 | 周期 |
+| --- | --- | --- | --- |
+| `0x0CFF3D08` | `0xFF3D` | 俯仰角、横滚角 | 10 ms |
+| `0x0CFF4108` | `0xFF41` | 航向角 | 10 ms |
+| `0x0CFF4308` | `0xFF43` | 温度 | 100 ms |
+
+配置帧 `0x0CEF0808` 中，目标地址和源地址均为 `8`。设备使用其他节点编号时，
+需要相应修改配置帧 CAN ID；执行这些命令不会修改节点编号本身。航向角显示
+`360°` 与 `0°` 表示同一方向，属于正常的角度回绕。
+
+### 常用操作
+
+```bash
+# 列出检测到的原生串口、SLCAN 串口和 SocketCAN 接口
+python3 scripts/test_imu.py --list
+
+# 显示全部传感器数据
+python3 scripts/test_imu.py HIPNUC --can can0 --id 8 -a -d 5
+
+# 运行 5 秒，只输出读取速率
+python3 scripts/test_imu.py MCT7123 --can can1 -d 5 -q
+
+# 每 0.2 秒打印一次
+python3 scripts/test_imu.py MCT7123 --can can1 -i 0.2 -d 5
+```
+
+默认输出仅包含欧拉角和温度：
+
+```text
+  Time         R        P        Y   Temp
+   (s)       (°)      (°)      (°)   (°C)
+─────────────────────────────────────────
+  0.00     -0.17    -2.07   -11.47   36.2
+  1.00     -0.16    -2.07   -11.47   36.2
+```
+
+当前脚本末尾显示的 `reads/s` 是 getter 轮询速率，不是传感器实际输出帧率。
+判断传感器帧率应统计 CAN 报文、Cycle 计数器或时间戳变化。
+
+### 参数
+
+| 参数 | 说明 |
+| --- | --- |
+| `TYPE` | IMU 型号：`MCT7123` 或 `HIPNUC`，必须与通信接口一起指定 |
+| `--serial DEV` | 使用指定的 IMU 原生串口 |
+| `--can IFACE` | 使用指定的 SocketCAN 接口 |
+| `--slcan DEV` | 使用指定的 CANable2 SLCAN-FD 串口 |
+| `--id N` | 覆盖默认 CAN 节点编号；MCT7123 默认为 `1`，HiPNUC 默认为 `8` |
+| `--list` | 列出检测到的串口设备 |
+| `-d, --duration SEC` | 运行时长；`0` 表示持续运行 |
+| `-i, --interval SEC` | 输出间隔，默认为 `0.5` 秒 |
+| `-a, --all` | 显示全部传感器数据 |
+| `-q, --quiet` | 仅输出 getter 轮询速率 |
+| `-b, --build-dir DIR` | 指定包含 `imu_py` 的构建目录 |
+
+## 4. Python API
+
+通过 `IMUDriver.create_imu()` 创建驱动：
+
+```python
+import imu_py
+
+imu = imu_py.IMUDriver.create_imu(
+    imu_id=1,
+    interface_type="serial",   # "serial"、"can" 或 "canfd"
+    interface="/dev/ttyUSB0",  # 串口路径或 SocketCAN 接口名
+    imu_type="MCT7123",        # "MCT7123" 或 "HIPNUC"
+    baudrate=921600,           # 串口必填；CAN 和 CAN FD 模式忽略
+)
+
+angular_velocity = imu.get_ang_vel()
+linear_acceleration = imu.get_lin_acc()
+euler = imu.get_euler()
+cycle = imu.get_cycle()
+```
+
+公共读取接口：
+
+| 方法 | 返回值 | 单位或说明 |
+| --- | --- | --- |
+| `get_imu_id()` | `int` | CAN 节点编号 |
+| `get_ang_vel()` | `[x, y, z]` | `rad/s` |
+| `get_lin_acc()` | `[x, y, z]` | `m/s²` |
+| `get_mag()` | `[x, y, z]` | `µT`，九轴模式下有效 |
+| `get_quat()` | `[w, x, y, z]` | 姿态四元数 |
+| `get_euler()` | `[roll, pitch, yaw]` | 度 |
+| `get_timestamp()` | `int` | 协议时间戳换算为微秒；设备未输出时间戳时为 `0` |
+| `get_temperature()` | `float` | 摄氏度 |
+| `get_cycle()` | `int` | MCT7123 的 `0`–`255` 帧计数器；HiPNUC 固定返回 `0` |
+
+可通过 `help(imu_py.IMUDriver)` 查看 Python 行内文档。
+
+## 5. C++ API
+
+```cpp
+#include "imu_driver.hpp"
+
+auto imu = IMUDriver::create_imu(
+    1, "serial", "/dev/ttyUSB0", "MCT7123", 921600);
+
+std::vector<float> angular_velocity = imu->get_ang_vel();
+std::vector<float> linear_acceleration = imu->get_lin_acc();
+std::vector<float> magnetic_field = imu->get_mag();
+std::vector<float> quaternion = imu->get_quat();
+std::vector<float> euler = imu->get_euler();
+uint64_t timestamp = imu->get_timestamp();
+float temperature = imu->get_temperature();
+uint8_t cycle = imu->get_cycle();
+```
+
+## 6. CAN 与 CAN FD
+
+下面示例使用两个 SocketCAN 接口，和前文 OPI5 测试接线一致。设备也可以
+接在同一条总线上，但必须使用相同仲裁速率，且主机控制器和总线都支持
+CAN FD。驱动按照帧类型和长度分别路由数据。
+
+```python
+# HiPNUC 经典 CAN；J1939 源地址通常为 8
+hipnuc = imu_py.IMUDriver.create_imu(
+    8, "can", "can0", "HIPNUC")
+
+# MCT7123 CAN FD；当前示例连接在 can1
+mct7123 = imu_py.IMUDriver.create_imu(
+    1, "canfd", "can1", "MCT7123")
+```
+
+MCT7123 使用以下 CAN 标识：
+
+| CAN 标识 | 类型 | 频率 |
+| --- | --- | --- |
+| `0x181` | IMU 原始数据：陀螺仪、加速度计、磁力计 | 取决于设备输出配置 |
+| `0x182` | 姿态数据：欧拉角、四元数 | 取决于设备输出配置 |
+| `0x183` | 配置交互 | 事件触发 |
+
+## 7. MCT7123 坐标系
+
+MCT7123 默认使用坐标系 9：
+
+- 标签 `+Uz` 对应 X 轴正方向。
+- 标签 `-Uy` 对应 Y 轴正方向。
+- 标签 `+Ux` 对应 Z 轴正方向。
+
+机器人安装方向为正面朝右、接插件朝天。静止时 `AccZ` 约为
+`+9.81 m/s²`，表示 Z 轴朝上时测得的重力反作用力。
diff --git a/include/imu_driver.hpp b/include/imu_driver.hpp
index 2b0e341..dbf3d7f 100644
--- a/include/imu_driver.hpp
+++ b/include/imu_driver.hpp
@@ -22,6 +22,8 @@
 #include <string>
 #include <vector>
 
+#include "imu_sensor_data.h"
+
 class IMUDriver {
    public:
 
@@ -40,17 +42,16 @@ class IMUDriver {
                                                 const std::string& imu_type, const int baudrate=0);
 
     virtual uint16_t get_imu_id() { return imu_id_; }
-    virtual std::vector<float> get_ang_vel() { return ang_vel_; }
-    virtual std::vector<float> get_quat() { return quat_; }
-    virtual std::vector<float> get_lin_acc() { return lin_acc_; }
-    virtual float get_temperature() { return temperature_; }
+    virtual std::vector<float> get_ang_vel() { return {0.f, 0.f, 0.f}; }
+    virtual std::vector<float> get_quat() { return {0.f, 0.f, 0.f, 0.f}; }
+    virtual std::vector<float> get_lin_acc() { return {0.f, 0.f, 0.f}; }
+    virtual std::vector<float> get_mag() { return {0.f, 0.f, 0.f}; }
+    virtual std::vector<float> get_euler() { return {0.f, 0.f, 0.f}; }
+    virtual uint64_t get_timestamp() { return 0; }
+    virtual float get_temperature() { return 0.f; }
+    virtual uint8_t get_cycle() { return 0; }
 
    protected:
     std::shared_ptr<spdlog::logger> logger_;
-    uint16_t imu_id_;
-
-    std::vector<float> quat_{0.f, 0.f, 0.f, 0.f};       // w, x, y, z
-    std::vector<float> ang_vel_{0.f, 0.f, 0.f};         // x, y, z
-    std::vector<float> lin_acc_{0.f, 0.f, 0.f};         // x, y, z
-    float temperature_{0.f}; // temperature
+    uint16_t imu_id_{0};
 };
\ No newline at end of file
diff --git a/include/imu_sensor_data.h b/include/imu_sensor_data.h
new file mode 100644
index 0000000..22bd52b
--- /dev/null
+++ b/include/imu_sensor_data.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-3.0 */
+/* Copyright (C) 2026 Luo1imasi */
+
+/**
+ * @file imu_sensor_data.h
+ * @brief Unified IMU sensor data struct — C/C++ compatible.
+ *        Single buffer all transports (serial, CAN, CANFD) write to.
+ */
+
+#ifndef IMU_SENSOR_DATA_H
+#define IMU_SENSOR_DATA_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    uint8_t  node_id;
+    uint64_t hw_ts_us;
+    float    acc_x, acc_y, acc_z;
+    float    gyr_x, gyr_y, gyr_z;
+    float    mag_x, mag_y, mag_z;
+    float    quat_w, quat_x, quat_y, quat_z;
+    float    roll, pitch, imu_yaw;
+    float    incli_x, incli_y;
+    float    temperature;
+    float    pressure;
+    uint8_t  utc_year, utc_month, utc_day;
+    uint8_t  hours, minutes, seconds;
+    uint16_t milliseconds;
+    uint32_t timestamp_ms;
+    double   ins_lat, ins_lon, ins_msl;
+    float    undulation, diff_age_s;
+    float    ins_vel_e, ins_vel_n, ins_vel_u, ins_speed;
+    uint8_t  solq_pos, solq_heading, nv_pos, nv_heading, ins_status;
+    uint8_t  running_status[6];
+    uint8_t  cycle;
+} imu_sensor_data_t;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* IMU_SENSOR_DATA_H */
diff --git a/init_imu.sh b/init_imu.sh
index e4ada0f..470eae7 100755
--- a/init_imu.sh
+++ b/init_imu.sh
@@ -24,6 +24,19 @@
 #     2. 需要 root 权限（内部使用 sudo）
 #     3. 需要安装 can-utils: sudo apt-get install can-utils
 #     4. 执行完成后，按提示将主机 CAN 接口手动切换回 1Mbps
+#
+# ============================================================================
+# J1939 配置协议说明
+# ============================================================================
+# 配置帧 CAN ID = 0x0CEF[DA][SA]  (DA=IMU节点ID, SA=主机地址)
+# 默认 DA=0x08, SA=0x08 → 0x0CEF0808
+#
+# 数据载荷 8 字节格式:
+#   [addr低8][addr高8][CMD][保留][val低8][val+8][val+16][val+24]
+#   CMD: 0x06=写, 0x03=读
+#
+# 周期值 (uint32 LE, ms): 0=关闭, 5/10/20/50/100/200/500/1000 推荐
+#                         0x00008000 = SYNC_IN 触发模式
 
 CAN_IF=${1:-"can_imu"}
 DEFAULT_BITRATE=500000
@@ -52,43 +65,50 @@ fi
 
 echo "[1/4] 配置IMU参数..."
 
+# ── PGN 发送周期配置 ──────────────────────────────────────────────
+# PGN 0xFF46 四元数 @ 5ms (200Hz)
 cansend ${CAN_IF} 0CEF0808#4601060005000000
 if [ $? -ne 0 ]; then
     echo "[错误] CAN 报文发送失败，请检查 ${CAN_IF} 接口是否处于 UP 状态且波特率匹配当前 IMU。"
     exit 1
 fi
-sleep 0.5 
+sleep 0.5
 
+# PGN 0xFF41 航向角 → 关闭 (改用四元数推算)
 cansend ${CAN_IF} 0CEF0808#4101060000000000
 if [ $? -ne 0 ]; then
     echo "[错误] CAN 报文发送失败，请检查 ${CAN_IF} 接口是否处于 UP 状态且波特率匹配当前 IMU。"
     exit 1
 fi
-sleep 0.5 
+sleep 0.5
 
+# PGN 0xFF3D 俯仰/横滚 → 关闭 (改用四元数推算)
 cansend ${CAN_IF} 0CEF0808#3D01060000000000
 if [ $? -ne 0 ]; then
     echo "[错误] CAN 报文发送失败，请检查 ${CAN_IF} 接口是否处于 UP 状态且波特率匹配当前 IMU。"
     exit 1
 fi
-sleep 0.5 
+sleep 0.5
 
 echo "[2/4] 发送修改波特率为 1M 指令..."
 
+# 0x009A CAN 波特率: 0=1000K, 1=800K, 2=500K, 3=250K, 4=125K
 cansend ${CAN_IF} 0CEF0808#9A00060000000000
 if [ $? -ne 0 ]; then
     echo "[错误] CAN 报文发送失败，请检查 ${CAN_IF} 接口是否处于 UP 状态且波特率匹配当前 IMU。"
     exit 1
 fi
-sleep 0.5 
+sleep 0.5
 
 echo "[3/4] 发送保存配置指令..."
+# 0x0000 保存所有参数到 Flash
 cansend ${CAN_IF} 0CEF0808#0000060000000000
-sleep 1.0 
+sleep 1.0
 
 echo "[4/4] 发送设备复位指令..."
+# 0x0000 复位 (VAL=0xFF000000)
 cansend ${CAN_IF} 0CEF0808#00000600FF000000
-sleep 1.5 
+sleep 1.5
 
 echo "================================================="
 echo "[成功] IMU 指令下发完毕！"
@@ -99,4 +119,64 @@ echo "你的主机 ${CAN_IF} 接口仍停留在旧的波特率。"
 echo "请手动运行以下命令，将主机的 CAN 接口切换到 1M："
 echo "  sudo ip link set ${CAN_IF} down"
 echo "  sudo ip link set ${CAN_IF} up type can bitrate 1000000"
-echo "================================================="
\ No newline at end of file
+echo "================================================="
+
+# ============================================================================
+# 完整 PGN 周期配置参考 (J1939 配置帧: 0x0CEF[DA][SA])
+# ============================================================================
+# 格式: cansend <IFACE> 0CEF0808#[addr低8][addr高8][0x06写][0x00][VAL_4字节_LE]
+# 写入后需执行 保存(0x0000 VAL=0) + 复位(0x0000 VAL=0xFF) 才生效
+# VAL=0 关闭; 5/10/20/50/100/200/500/1000 ms 为推荐周期; 0x00008000=SYNC_IN触发
+#
+# ═══ PGN 数据输出周期 ═══
+#
+#   PGN     寄存器   说明                         示例 (10ms=100Hz)
+#   ─────────────────────────────────────────────────────────────────
+#   0xFF2F  0x012F   时间信息 (UTC/运行时间)       2F0106000A000000
+#   0xFF34  0x0134   三轴加速度                    340106000A000000
+#   0xFF37  0x0137   三轴角速度                    370106000A000000
+#   0xFF3A  0x013A   三轴磁场强度 (仅9轴产品)      3A0106000A000000
+#   0xFF3D  0x013D   俯仰/横滚角                   3D0106000A000000
+#   0xFF41  0x0141   航向角                        410106000A000000
+#   0xFF46  0x0146   四元数                        460106000A000000
+#   0xFF4A  0x014A   倾角仪输出 (倾角仪产品)       4A0106000A000000
+#   0xFF43  0x0143   温度                          4301060064000000  ← 100ms 足够
+#   0xFF5A  0x015A   CANFD 帧 0 (仅 CANFD 产品)    5A0106000A000000
+#
+# ═══ 系统控制 ═══
+#
+#   功能                       数据载荷
+#   ─────────────────────────────────────────────────────────────────
+#   全局使能节点数据输出        9D00060001000000
+#   全局关闭节点数据输出        9D00060000000000
+#   CAN 波特率 1000K            9A00060000000000
+#   CAN 波特率 800K             9A00060001000000
+#   CAN 波特率 500K             9A00060002000000
+#   CAN 波特率 250K             9A00060003000000
+#   CAN 波特率 125K             9A00060004000000
+#   J1939 节点 ID (推荐 1-126)  9C000600[ID]000000  (例 ID=8: 9C00060008000000)
+#   倾角仪 X 轴方向 默认        9E00060000000000
+#   倾角仪 X 轴方向 反向        9E00060001000000
+#   倾角仪 Y 轴方向 默认        9F00060000000000
+#   倾角仪 Y 轴方向 反向        9F00060001000000
+#   姿态控制-航向复位           A500060001000000
+#   姿态控制-设置相对零点       A500060002000000
+#   姿态控制-自动校平           A500060003000000
+#   姿态控制-取消校平           A500060005000000
+#   保存所有参数到 Flash        0000060000000000
+#   恢复出厂设置 (自动保存复位) 0000060001000000
+#   复位                        00000600FF000000
+#
+# ═══ 快速开启全部核心数据 (粘贴到终端) ═══
+#
+#   IF=can0
+#   sudo ip link set $IF up type can bitrate 500000
+#   cansend $IF 0CEF0808#340106000A000000  # 加速度 10ms
+#   cansend $IF 0CEF0808#370106000A000000  # 角速度 10ms
+#   cansend $IF 0CEF0808#3D0106000A000000  # 俯仰/横滚 10ms
+#   cansend $IF 0CEF0808#410106000A000000  # 航向角 10ms
+#   cansend $IF 0CEF0808#460106000A000000  # 四元数 10ms
+#   cansend $IF 0CEF0808#3A0106000A000000  # 磁场 10ms (9轴产品)
+#   cansend $IF 0CEF0808#4301060064000000  # 温度 100ms
+#   cansend $IF 0CEF0808#0000060000000000  # 保存
+#   cansend $IF 0CEF0808#00000600FF000000  # 复位
diff --git a/scripts/test_imu.py b/scripts/test_imu.py
new file mode 100644
index 0000000..76d2ebd
--- /dev/null
+++ b/scripts/test_imu.py
@@ -0,0 +1,270 @@
+#!/usr/bin/env python3
+"""IMU 测试工具 — 必须指定型号和通信接口。
+
+Usage:
+  python3 scripts/test_imu.py MCT7123 --can can1 -d 5
+  python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM0 -d 5
+  python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 -d 5
+  python3 scripts/test_imu.py --list
+"""
+import sys, os, time, argparse, glob, struct
+
+RAD2DEG = 57.29578
+DEG2RAD = 1.0 / RAD2DEG
+
+DEFAULTS = {
+    'MCT7123': {'baudrate': 921600, 'id': 1, 'can': 'can0'},
+    'HIPNUC':  {'baudrate': 115200,  'id': 8, 'can': 'can0'},
+}
+
+def scan_serial():
+    """返回可用的串口 (ttyUSB / ttyAMA)."""
+    devs = []
+    for p in sorted(glob.glob('/dev/ttyUSB*')) + sorted(glob.glob('/dev/ttyAMA*')):
+        if os.path.exists(p):
+            devs.append(p)
+    return devs
+
+def scan_slcan():
+    """返回可能的 CANable/SLCAN 串口。"""
+    return [p for p in sorted(glob.glob('/dev/ttyACM*')) if os.path.exists(p)]
+
+def scan_can():
+    """返回系统中已经注册的 SocketCAN 网络接口。"""
+    interfaces = []
+    for type_path in sorted(glob.glob('/sys/class/net/*/type')):
+        try:
+            with open(type_path, encoding='ascii') as type_file:
+                if type_file.read().strip() == '280':  # Linux ARPHRD_CAN
+                    interfaces.append(type_path.split('/')[-2])
+        except OSError:
+            continue
+    return interfaces
+
+def crc16_ccitt(data):
+    crc = 0xffff
+    for value in data:
+        crc ^= value << 8
+        for _ in range(8):
+            crc = ((crc << 1) ^ 0x1021) & 0xffff if crc & 0x8000 else (crc << 1) & 0xffff
+    return crc
+
+class MCT7123SlcanDriver:
+    """通过 CANable2 的 SLCAN-FD 协议读取 MCT7123。"""
+
+    def __init__(self, device):
+        try:
+            import can
+        except ImportError as exc:
+            raise RuntimeError('缺少 python-can，请先安装: pip install python-can') from exc
+
+        channel = device if '@' in device else f'{device}@115200'
+        self.bus = can.interface.Bus(
+            interface='slcan', channel=channel, bitrate=500000,
+            sleep_after_open=0.5)
+        self.bus.set_bitrate(500000, data_bitrate=2000000)
+        self.gyr = [0.0, 0.0, 0.0]
+        self.acc = [0.0, 0.0, 0.0]
+        self.mag = [0.0, 0.0, 0.0]
+        self.quat = [1.0, 0.0, 0.0, 0.0]
+        self.euler = [0.0, 0.0, 0.0]
+        self.temperature = 0.0
+        self.cycle = 0
+
+    def poll(self, timeout=0.05):
+        msg = self.bus.recv(timeout)
+        if msg is None or not msg.is_fd or len(msg.data) != 64:
+            return False
+        payload = bytes(msg.data)
+        if crc16_ccitt(payload[:62]) != struct.unpack_from('<H', payload, 62)[0]:
+            return False
+
+        can_id = msg.arbitration_id & 0x7ff
+        if can_id == 0x181:
+            values = struct.unpack_from('<10f', payload, 8)
+            self.gyr = [v * DEG2RAD for v in values[0:3]]
+            self.acc = list(values[3:6])
+            self.mag = list(values[6:9])
+            self.temperature = values[9]
+            self.cycle = payload[61]
+        elif can_id == 0x182:
+            roll, pitch, yaw, qx, qy, qz, qw, temp = struct.unpack_from('<8f', payload, 8)
+            self.euler = [roll, pitch, yaw]
+            self.quat = [qw, qx, qy, qz]
+            self.temperature = temp
+        else:
+            return False
+        return True
+
+    def shutdown(self):
+        try:
+            self.bus.shutdown()
+        except Exception:
+            pass
+
+    def get_ang_vel(self): return self.gyr
+    def get_lin_acc(self): return self.acc
+    def get_mag(self): return self.mag
+    def get_quat(self): return self.quat
+    def get_euler(self): return self.euler
+    def get_temperature(self): return self.temperature
+    def get_cycle(self): return self.cycle
+
+def main():
+    ap = argparse.ArgumentParser(
+        description='IMU 测试工具 — MCT7123 / HIPNUC（必须指定通信接口）',
+        epilog='示例: python3 scripts/test_imu.py MCT7123 --can can1 -d 5',
+        formatter_class=argparse.RawDescriptionHelpFormatter)
+    ap.add_argument('type', nargs='?', default=None, metavar='TYPE',
+                    help='IMU 型号 (MCT7123 / HIPNUC; 不提供则提示用法)')
+    interface = ap.add_mutually_exclusive_group()
+    interface.add_argument('--serial', metavar='DEV', help='指定 IMU 原生串口设备')
+    interface.add_argument('--can', metavar='IFACE', help='指定 SocketCAN 接口 (如 can0)')
+    interface.add_argument('--slcan', metavar='DEV',
+                           help='指定 CANable2 SLCAN-FD 串口 (如 /dev/ttyACM0)')
+    ap.add_argument('--list', action='store_true', help='列出可用设备')
+    ap.add_argument('-d', '--duration', type=float, default=0,
+                    help='运行时长 秒 (0=无限循环)')
+    ap.add_argument('-i', '--interval', type=float, default=0.5,
+                    help='打印间隔 秒 (默认 0.5)')
+    ap.add_argument('--id', type=int, default=None,
+                    help='CAN 源地址 (默认按型号: HIPNUC=8, MCT7123=1)')
+    ap.add_argument('-a', '--all', action='store_true',
+                    help='显示全部: Gyr/Acc/Mag/Quat/Euler/Temp/Cycle')
+    ap.add_argument('-q', '--quiet', action='store_true',
+                    help='安静模式, 只打印速率')
+    ap.add_argument('-b', '--build-dir', default=None,
+                    help='build 目录 (默认仓库根目录下的 build/)')
+    args = ap.parse_args()
+
+    if args.list:
+        serials = scan_serial()
+        slcans = scan_slcan()
+        cans = scan_can()
+        print('串口:', ', '.join(serials) if serials else '(无)')
+        print('SLCAN:', ', '.join(slcans) if slcans else '(无)')
+        print('CAN:  ', ', '.join(cans) if cans else '(无)')
+        return
+
+    if args.type is None:
+        print('请指定 IMU 型号: MCT7123 或 HIPNUC', file=sys.stderr)
+        print('示例: python3 scripts/test_imu.py MCT7123 --can can1 -d 5', file=sys.stderr)
+        sys.exit(1)
+
+    if args.type not in ('MCT7123', 'HIPNUC'):
+        print(f'无效型号: {args.type}, 可选: MCT7123, HIPNUC', file=sys.stderr)
+        sys.exit(1)
+
+    if not (args.serial or args.can or args.slcan):
+        print('请用 --serial、--can 或 --slcan 明确指定通信接口', file=sys.stderr)
+        sys.exit(1)
+
+    # Determine interface + device + baudrate
+    imu_type = args.type
+    baudrate = DEFAULTS[imu_type]['baudrate']
+    iface_type = 'slcanfd' if args.slcan else \
+                 'canfd' if (imu_type == 'MCT7123' and args.can) else \
+                 'can'   if (imu_type == 'HIPNUC'  and args.can) else \
+                 'serial'
+
+    if args.slcan:
+        if imu_type != 'MCT7123':
+            print('--slcan 当前只支持 MCT7123', file=sys.stderr)
+            sys.exit(1)
+        device = args.slcan
+        baudrate = 500000
+    elif args.serial:
+        device = args.serial
+    elif args.can:
+        device = args.can
+
+    imu_id = args.id if args.id is not None else DEFAULTS[imu_type]['id']
+
+    forever = (args.duration <= 0)
+    rate_text = {
+        'slcanfd': '500K/2M',
+        'canfd': 'CAN FD',
+        'can': 'CAN',
+        'serial': f'{baudrate}bps',
+    }[iface_type]
+    print(f'[{args.type}] {iface_type}:{device} {rate_text} '
+          f'{"∞" if forever else f"{args.duration}s"}', end=' ', flush=True)
+    try:
+        if iface_type == 'slcanfd':
+            imu = MCT7123SlcanDriver(device)
+        else:
+            # Silence C++ spdlog noise
+            sys.stdout.flush()
+            os.dup2(os.open(os.devnull, os.O_WRONLY), 2)
+            repo_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+            sys.path.insert(0, args.build_dir or os.path.join(repo_root, 'build'))
+            import imu_py
+            imu = imu_py.IMUDriver.create_imu(imu_id, iface_type, device,
+                                              args.type, baudrate)
+    except (ImportError, RuntimeError, OSError) as e:
+        print(f'FAIL: {e}', flush=True)
+        sys.exit(1)
+    time.sleep(0.3)
+    print('OK\n')
+
+    if not args.quiet:
+        if args.all:
+            hdr = (f'{"Time":>6s}  {"GyrX":>7s} {"GyrY":>7s} {"GyrZ":>7s}  '
+                   f'{"AccX":>7s} {"AccY":>7s} {"AccZ":>7s}  '
+                   f'{"MagX":>7s} {"MagY":>7s} {"MagZ":>7s}  '
+                   f'{"R":>7s} {"P":>7s} {"Y":>7s}  '
+                   f'{"Qw":>7s} {"Qx":>7s} {"Qy":>7s} {"Qz":>7s}  {"Temp":>5s}  Cyc')
+            unt = (f'{"(s)":>6s}  {"(°/s)":>7s} {"(°/s)":>7s} {"(°/s)":>7s}  '
+                   f'{"(m/s²)":>7s} {"(m/s²)":>7s} {"(m/s²)":>7s}  '
+                   f'{"(uT)":>7s} {"(uT)":>7s} {"(uT)":>7s}  '
+                   f'{"(°)":>7s} {"(°)":>7s} {"(°)":>7s}  '
+                   f'{"":>7s} {"":>7s} {"":>7s} {"":>7s}  {"(°C)":>5s}')
+        else:
+            hdr = f'{"Time":>6s}  {"R":>8s} {"P":>8s} {"Y":>8s}  {"Temp":>5s}'
+            unt = f'{"(s)":>6s}  {"(°)":>8s} {"(°)":>8s} {"(°)":>8s}  {"(°C)":>5s}'
+        print(hdr)
+        print(unt)
+        print('─' * len(hdr))
+
+    start = time.time()
+    last = 0
+    cnt = 0
+
+    try:
+        while forever or time.time() - start < args.duration:
+            if iface_type == 'slcanfd' and not imu.poll(0.05):
+                continue
+            g = imu.get_ang_vel()
+            a = imu.get_lin_acc()
+            m = imu.get_mag()
+            q = imu.get_quat()
+            e = imu.get_euler()
+            t = imu.get_temperature()
+            c = imu.get_cycle()
+            cnt += 1
+            now = time.time()
+            if not args.quiet and now - last >= args.interval:
+                el = now - start
+                if args.all:
+                    gd = [v * RAD2DEG for v in g]
+                    print(f'{el:6.2f}  {gd[0]:7.2f} {gd[1]:7.2f} {gd[2]:7.2f}  '
+                          f'{a[0]:7.3f} {a[1]:7.3f} {a[2]:7.3f}  '
+                          f'{m[0]:7.1f} {m[1]:7.1f} {m[2]:7.1f}  '
+                          f'{e[0]:7.2f} {e[1]:7.2f} {e[2]:7.2f}  '
+                          f'{q[0]:7.3f} {q[1]:7.3f} {q[2]:7.3f} {q[3]:7.3f}  {t:5.1f}  {c:3d}')
+                else:
+                    print(f'{el:6.2f}  {e[0]:8.2f} {e[1]:8.2f} {e[2]:8.2f}  {t:5.1f}')
+                last = now
+            time.sleep(0.0005)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if iface_type == 'slcanfd':
+            imu.shutdown()
+
+    elapsed = time.time() - start
+    print(f'\n{args.type} {iface_type}  {elapsed:.1f}s  {cnt} reads  {cnt/elapsed:.0f} Hz  ', end='')
+    print('OK' if cnt > 0 else 'FAIL')
+
+if __name__ == '__main__':
+    main()
diff --git a/src/drivers/CMakeLists.txt b/src/drivers/CMakeLists.txt
index 1787674..de8fd99 100644
--- a/src/drivers/CMakeLists.txt
+++ b/src/drivers/CMakeLists.txt
@@ -1,3 +1,4 @@
 # SPDX-License-Identifier: GPL-3.0
 # Copyright (C) 2026 Luo1imasi
-add_subdirectory(hipnuc)
\ No newline at end of file
+add_subdirectory(hipnuc)
+add_subdirectory(mct7123)
\ No newline at end of file
diff --git a/src/drivers/hipnuc/canopen_parser.c b/src/drivers/hipnuc/canopen_parser.c
index e9bfb35..0cbe366 100644
--- a/src/drivers/hipnuc/canopen_parser.c
+++ b/src/drivers/hipnuc/canopen_parser.c
@@ -8,7 +8,7 @@
 #define TPDO6_BASE  0x680
 #define TPDO7_BASE  0x780
 
-static int parse_canopen_accel(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_canopen_accel(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->acc_x = raw[0] * 0.001f * 9.81f;
@@ -17,7 +17,7 @@ static int parse_canopen_accel(const hipnuc_can_frame_t *frame, can_sensor_data_
     return CAN_MSG_ACCEL;
 }
 
-static int parse_canopen_gyro(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_canopen_gyro(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->gyr_x = raw[0] * 0.1f * 0.017453f;
@@ -26,7 +26,7 @@ static int parse_canopen_gyro(const hipnuc_can_frame_t *frame, can_sensor_data_t
     return CAN_MSG_GYRO;
 }
 
-static int parse_canopen_euler(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_canopen_euler(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->roll = raw[0] * 0.01f * 0.017453f;
@@ -35,7 +35,7 @@ static int parse_canopen_euler(const hipnuc_can_frame_t *frame, can_sensor_data_
     return CAN_MSG_EULER;
 }
 
-static int parse_canopen_quat(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_canopen_quat(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->quat_w = raw[0] * 0.0001f;
@@ -45,14 +45,14 @@ static int parse_canopen_quat(const hipnuc_can_frame_t *frame, can_sensor_data_t
     return CAN_MSG_QUAT;
 }
 
-static int parse_canopen_pressure(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_canopen_pressure(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw = (int32_t*)frame->data;
     data->pressure = *raw;
     return CAN_MSG_PRESSURE;
 }
 
-static int parse_canopen_inclination(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_canopen_inclination(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw = (int32_t*)frame->data;
     data->incli_x = raw[0] * 0.01f;
@@ -60,7 +60,7 @@ static int parse_canopen_inclination(const hipnuc_can_frame_t *frame, can_sensor
     return CAN_MSG_INCLI;
 }
 
-int canopen_parse_frame(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+int canopen_parse_frame(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     if (!frame || !data) return CAN_MSG_ERROR;
 
diff --git a/src/drivers/hipnuc/canopen_parser.h b/src/drivers/hipnuc/canopen_parser.h
index 5edfd95..df4fed5 100644
--- a/src/drivers/hipnuc/canopen_parser.h
+++ b/src/drivers/hipnuc/canopen_parser.h
@@ -4,8 +4,9 @@
 #define CANOPEN_PARSER_H
 
 #include "hipnuc_can_common.h"
+#include "imu_sensor_data.h"
 
-int canopen_parse_frame(const hipnuc_can_frame_t *frame, can_sensor_data_t *data);
+int canopen_parse_frame(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data);
 
 #endif
 
diff --git a/src/drivers/hipnuc/hipnuc_can_common.c b/src/drivers/hipnuc/hipnuc_can_common.c
index 60feef1..93ad273 100644
--- a/src/drivers/hipnuc/hipnuc_can_common.c
+++ b/src/drivers/hipnuc/hipnuc_can_common.c
@@ -12,7 +12,7 @@ static void json_append(can_json_output_t *out, const char *fmt, ...)
     if (written > 0) out->length += written;
 }
 
-int hipnuc_can_to_json(const can_sensor_data_t *data, int msg_type, can_json_output_t *output)
+int hipnuc_can_to_json(const imu_sensor_data_t *data, int msg_type, can_json_output_t *output)
 {
     if (!data || !output) return -1;
     output->length = 0;
diff --git a/src/drivers/hipnuc/hipnuc_can_common.h b/src/drivers/hipnuc/hipnuc_can_common.h
index 5f853a6..d3ffb0e 100644
--- a/src/drivers/hipnuc/hipnuc_can_common.h
+++ b/src/drivers/hipnuc/hipnuc_can_common.h
@@ -6,6 +6,8 @@
 #include <stdint.h>
 #include <stddef.h>
 
+#include "imu_sensor_data.h"
+
 typedef struct {
     uint32_t can_id;
     uint8_t can_dlc;
@@ -34,59 +36,12 @@ typedef struct {
 #define CAN_MSG_GNSS_STATUS 14
 #define CAN_MSG_UNKNOWN     99
 
-typedef struct {
-    uint8_t node_id;
-    uint64_t hw_ts_us;
-    float acc_x;
-    float acc_y;
-    float acc_z;
-    float gyr_x;
-    float gyr_y;
-    float gyr_z;
-    float mag_x;
-    float mag_y;
-    float mag_z;
-    float quat_w;
-    float quat_x;
-    float quat_y;
-    float quat_z;
-    float roll;
-    float pitch;
-    float imu_yaw;
-    float incli_x;
-    float incli_y;
-    float temperature;
-    float pressure;
-    uint8_t utc_year;
-    uint8_t utc_month;
-    uint8_t utc_day;
-    uint8_t hours;
-    uint8_t minutes;
-    uint8_t seconds;
-    uint16_t milliseconds;
-    uint32_t timestamp_ms;
-    double ins_lat;
-    double ins_lon;
-    double ins_msl;
-    float undulation;
-    float diff_age_s;
-    float ins_vel_e;
-    float ins_vel_n;
-    float ins_vel_u;
-    float ins_speed;
-    uint8_t solq_pos;
-    uint8_t solq_heading;
-    uint8_t nv_pos;
-    uint8_t nv_heading;
-    uint8_t ins_status;
-} can_sensor_data_t;
-
 typedef struct {
     char buffer[512];
     size_t length;
 } can_json_output_t;
 
-int hipnuc_can_to_json(const can_sensor_data_t *data, int msg_type, can_json_output_t *output);
+int hipnuc_can_to_json(const imu_sensor_data_t *data, int msg_type, can_json_output_t *output);
 uint8_t hipnuc_can_extract_node_id(uint32_t can_id);
 
 typedef enum {
diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp
index 3ed125f..2394397 100644
--- a/src/drivers/hipnuc/hipnuc_imu_driver.cpp
+++ b/src/drivers/hipnuc/hipnuc_imu_driver.cpp
@@ -13,10 +13,8 @@ HipnucIMUDriver::HipnucIMUDriver(uint16_t imu_id, const std::string& interface_t
     } else if (interface_type_ == "can") {
         can_ = IMUSocketCAN::get_instance(interface_);
         CanCbkFunc can_callback = std::bind(&HipnucIMUDriver::can_rx_cbk, this, std::placeholders::_1);
-        can_->add_can_callback(can_callback, imu_id_);
-        can_->set_key_extractor([](const can_frame &frame) -> CanCbkId {
-            return frame.can_id & 0x7F;
-        });
+        can_subscription_ =
+            CanCallbackSubscription(can_, CAN_CALLBACK_WILDCARD, can_callback);
     } else {
         throw std::runtime_error("Hipnuc driver only support CAN and SERIAL interface");
     }
@@ -25,17 +23,23 @@ HipnucIMUDriver::HipnucIMUDriver(uint16_t imu_id, const std::string& interface_t
 HipnucIMUDriver::~HipnucIMUDriver() {
     if (interface_type_ == "serial" && serial_) {
         serial_->close();
-    } else if (interface_type_ == "can" && can_) {
-        can_->remove_can_callback(imu_id_);
     }
 }
 
-void HipnucIMUDriver::can_rx_cbk(const can_frame& rx_frame) {
+void HipnucIMUDriver::can_rx_cbk(const canfd_frame& rx_frame) {
+    /* Only process classic CAN frames (len <= 8).
+     * CANFD frames on the same bus are handled by other drivers (e.g. MCT7123). */
+    if (rx_frame.len > 8 ||
+        !(rx_frame.can_id & HIPNUC_CAN_EFF_FLAG) ||
+        hipnuc_can_extract_node_id(rx_frame.can_id) != imu_id_) {
+        return;
+    }
+
     hipnuc_can_frame_t frame;
     frame.can_id = rx_frame.can_id;
-    frame.can_dlc = rx_frame.can_dlc;
+    frame.can_dlc = rx_frame.len;
     memcpy(frame.data, rx_frame.data, 8);
-    
+
     std::unique_lock<std::shared_mutex> lock(imu_mutex_);
     
     int ret = hipnuc_j1939_parse_frame(&frame, &sensor_data_);
@@ -70,6 +74,17 @@ void HipnucIMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) {
             sensor_data_.acc_x = raw_.hi91.acc[0] * GRA_ACC;
             sensor_data_.acc_y = raw_.hi91.acc[1] * GRA_ACC;
             sensor_data_.acc_z = raw_.hi91.acc[2] * GRA_ACC;
+
+            sensor_data_.roll  = raw_.hi91.roll;
+            sensor_data_.pitch = raw_.hi91.pitch;
+            sensor_data_.imu_yaw = raw_.hi91.yaw;
+
+            sensor_data_.mag_x = raw_.hi91.mag[0];
+            sensor_data_.mag_y = raw_.hi91.mag[1];
+            sensor_data_.mag_z = raw_.hi91.mag[2];
+
+            sensor_data_.temperature = raw_.hi91.temp;
+            sensor_data_.timestamp_ms = raw_.hi91.system_time;
         }
     }
 }
@@ -92,4 +107,23 @@ std::vector<float> HipnucIMUDriver::get_lin_acc() {
 float HipnucIMUDriver::get_temperature() {
     std::shared_lock<std::shared_mutex> lock(imu_mutex_);
     return sensor_data_.temperature;
-}
\ No newline at end of file
+}
+
+std::vector<float> HipnucIMUDriver::get_euler() {
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw};
+}
+
+std::vector<float> HipnucIMUDriver::get_mag() {
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z};
+}
+
+uint64_t HipnucIMUDriver::get_timestamp() {
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return (uint64_t)sensor_data_.timestamp_ms * 1000ULL;
+}
+
+uint8_t HipnucIMUDriver::get_cycle() {
+    return 0;  /* HiPNUC protocol has no cycle counter */
+}
diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp
index b334ae6..ad09728 100644
--- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp
+++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp
@@ -26,12 +26,16 @@ class HipnucIMUDriver : public IMUDriver {
     HipnucIMUDriver(uint16_t imu_id, const std::string& interface_type, const std::string& interface, const int baudrate=0);
     ~HipnucIMUDriver();
 
-    void can_rx_cbk(const can_frame& rx_frame);
+    void can_rx_cbk(const canfd_frame& rx_frame);
     void serial_rx_cbk(const uint8_t* data, size_t length);
     std::vector<float> get_ang_vel() override;
     std::vector<float> get_quat() override;
     std::vector<float> get_lin_acc() override;
+    std::vector<float> get_mag() override;
+    std::vector<float> get_euler() override;
+    uint64_t get_timestamp() override;
     float get_temperature() override;
+    uint8_t get_cycle() override;
 
    private:
     int baudrate_;
@@ -40,6 +44,7 @@ class HipnucIMUDriver : public IMUDriver {
     mutable std::shared_mutex imu_mutex_;
     std::shared_ptr<IMUSocketCAN> can_;
     std::shared_ptr<IMUSerialPort> serial_;
-    can_sensor_data_t sensor_data_;
+    imu_sensor_data_t sensor_data_;
     hipnuc_raw_t raw_;
-};
\ No newline at end of file
+    CanCallbackSubscription can_subscription_;
+};
diff --git a/src/drivers/hipnuc/hipnuc_j1939_parser.c b/src/drivers/hipnuc/hipnuc_j1939_parser.c
index 59088a2..c53f2d8 100644
--- a/src/drivers/hipnuc/hipnuc_j1939_parser.c
+++ b/src/drivers/hipnuc/hipnuc_j1939_parser.c
@@ -14,7 +14,7 @@
 #define J1939_PGN_QUAT       0xFF46
 #define J1939_PGN_INCLINE    0xFF4A
 
-static int parse_j1939_time(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_time(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     const uint8_t *raw = frame->data;
     data->utc_year = raw[0];
@@ -28,7 +28,7 @@ static int parse_j1939_time(const hipnuc_can_frame_t *frame, can_sensor_data_t *
     return CAN_MSG_TIME;
 }
 
-static int parse_j1939_accel(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_accel(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->acc_x = raw[0] * 0.00048828f;
@@ -37,7 +37,7 @@ static int parse_j1939_accel(const hipnuc_can_frame_t *frame, can_sensor_data_t
     return CAN_MSG_ACCEL;
 }
 
-static int parse_j1939_gyro(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_gyro(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->gyr_x = raw[0] * 0.061035f;
@@ -46,7 +46,7 @@ static int parse_j1939_gyro(const hipnuc_can_frame_t *frame, can_sensor_data_t *
     return CAN_MSG_GYRO;
 }
 
-static int parse_j1939_mag(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_mag(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->mag_x = raw[0] * 0.030517f;
@@ -55,7 +55,7 @@ static int parse_j1939_mag(const hipnuc_can_frame_t *frame, can_sensor_data_t *d
     return CAN_MSG_MAG;
 }
 
-static int parse_j1939_pitch_roll(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_pitch_roll(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw = (int32_t*)frame->data;
     data->roll = raw[0] * 0.001f;
@@ -63,14 +63,14 @@ static int parse_j1939_pitch_roll(const hipnuc_can_frame_t *frame, can_sensor_da
     return CAN_MSG_PITCH_ROLL;
 }
 
-static int parse_j1939_yaw(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_yaw(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw = (int32_t*)frame->data;
     data->imu_yaw = raw[0] * 0.001f;
     return CAN_MSG_YAW;
 }
 
-static int parse_j1939_quat(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_quat(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->quat_w = raw[0] * 0.0001f;
@@ -80,7 +80,7 @@ static int parse_j1939_quat(const hipnuc_can_frame_t *frame, can_sensor_data_t *
     return CAN_MSG_QUAT;
 }
 
-static int parse_j1939_inclination(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_inclination(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw = (int32_t*)frame->data;
     data->incli_x = raw[0] * 0.001f;
@@ -88,7 +88,7 @@ static int parse_j1939_inclination(const hipnuc_can_frame_t *frame, can_sensor_d
     return CAN_MSG_INCLI;
 }
 
-static int parse_j1939_env(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_env(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw16 = (int16_t*)frame->data;
     int32_t *raw32 = (int32_t*)(frame->data + 4);
@@ -97,7 +97,7 @@ static int parse_j1939_env(const hipnuc_can_frame_t *frame, can_sensor_data_t *d
     return CAN_MSG_PRESSURE;
 }
 
-static int parse_j1939_lonlat(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_lonlat(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw = (int32_t*)frame->data;
     data->ins_lat = raw[0] * 1e-7;
@@ -105,7 +105,7 @@ static int parse_j1939_lonlat(const hipnuc_can_frame_t *frame, can_sensor_data_t
     return CAN_MSG_GNSS_POS;
 }
 
-static int parse_j1939_alt(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_alt(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int32_t *raw32 = (int32_t*)frame->data;
     int16_t *raw16 = (int16_t*)frame->data;
@@ -115,7 +115,7 @@ static int parse_j1939_alt(const hipnuc_can_frame_t *frame, can_sensor_data_t *d
     return CAN_MSG_GNSS_POS;
 }
 
-static int parse_j1939_status(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_status(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     const uint8_t *raw = frame->data;
     data->solq_pos = raw[0];
@@ -126,7 +126,7 @@ static int parse_j1939_status(const hipnuc_can_frame_t *frame, can_sensor_data_t
     return CAN_MSG_GNSS_STATUS;
 }
 
-static int parse_j1939_vel(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+static int parse_j1939_vel(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     int16_t *raw = (int16_t*)frame->data;
     data->ins_vel_e = raw[0] * 0.01f;
@@ -136,7 +136,7 @@ static int parse_j1939_vel(const hipnuc_can_frame_t *frame, can_sensor_data_t *d
     return CAN_MSG_GNSS_VEL;
 }
 
-int hipnuc_j1939_parse_frame(const hipnuc_can_frame_t *frame, can_sensor_data_t *data)
+int hipnuc_j1939_parse_frame(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data)
 {
     if (!frame || !data) return CAN_MSG_ERROR;
     if (!(frame->can_id & HIPNUC_CAN_EFF_FLAG)) return CAN_MSG_UNKNOWN;
diff --git a/src/drivers/hipnuc/hipnuc_j1939_parser.h b/src/drivers/hipnuc/hipnuc_j1939_parser.h
index 234ff6d..2fbea19 100644
--- a/src/drivers/hipnuc/hipnuc_j1939_parser.h
+++ b/src/drivers/hipnuc/hipnuc_j1939_parser.h
@@ -2,7 +2,8 @@
 #define HIPNUC_J1939_PARSER_H
 
 #include "hipnuc_can_common.h"
+#include "imu_sensor_data.h"
 
-int hipnuc_j1939_parse_frame(const hipnuc_can_frame_t *frame, can_sensor_data_t *data);
+int hipnuc_j1939_parse_frame(const hipnuc_can_frame_t *frame, imu_sensor_data_t *data);
 
 #endif
diff --git a/src/drivers/mct7123/CMakeLists.txt b/src/drivers/mct7123/CMakeLists.txt
new file mode 100644
index 0000000..72d7135
--- /dev/null
+++ b/src/drivers/mct7123/CMakeLists.txt
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-3.0
+add_library(mct7123_imu STATIC
+    mct7123_dec.c
+    mct7123_serial_driver.cpp
+    mct7123_can_driver.cpp
+)
+target_include_directories(mct7123_imu
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../hipnuc>
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/src>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(mct7123_imu PUBLIC ${PUBLIC_DEPENDENCIES} imu_protocol)
diff --git a/src/drivers/mct7123/mct7123_can_driver.cpp b/src/drivers/mct7123/mct7123_can_driver.cpp
new file mode 100644
index 0000000..64ceda0
--- /dev/null
+++ b/src/drivers/mct7123/mct7123_can_driver.cpp
@@ -0,0 +1,141 @@
+#include "mct7123_can_driver.hpp"
+#include <cstring>
+
+Mct7123CanDriver::Mct7123CanDriver(uint16_t imu_id, const std::string& interface)
+    : IMUDriver(), interface_(interface),
+      callback_ids_{imu_id, static_cast<CanCbkId>(imu_id + 1),
+                    static_cast<CanCbkId>(imu_id + 2)}
+{
+    imu_id_ = imu_id;
+    memset(&sensor_data_, 0, sizeof(sensor_data_));
+
+    can_ = IMUSocketCAN::get_instance(interface_);
+    CanCbkFunc can_cb = std::bind(&Mct7123CanDriver::can_rx_cbk, this, std::placeholders::_1);
+    can_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId {
+        return frame.can_id & 0x7F;
+    });
+    // CAN_ID_Config is a base ID: +1 is IMU, +2 is attitude, +3 is config.
+    // imu_id is the low seven bits of the first data frame ID (normally 1).
+    for (size_t i = 0; i < callback_ids_.size(); ++i) {
+        can_subscriptions_[i] = CanCallbackSubscription(can_, callback_ids_[i], can_cb);
+    }
+}
+
+Mct7123CanDriver::~Mct7123CanDriver() = default;
+
+void Mct7123CanDriver::can_rx_cbk(const canfd_frame& rx_frame)
+{
+    /* MD7123 CANFD: full 64-byte payload per frame, frame ID selects type.
+     * 0x181 = IMU data, 0x182 = Attitude data, 0x183 = Config.
+     * Payload layout identical to UART; CRC16 over bytes 0..61. */
+    if (rx_frame.len < 64) return;
+
+    const uint8_t *payload = rx_frame.data;
+
+    /* Validate CRC16 over payload[0..61] */
+    uint16_t crc_expected = (uint16_t)payload[62] | ((uint16_t)payload[63] << 8);
+    if (mct7123_crc16(payload, 62) != crc_expected) return;
+
+    std::unique_lock<std::shared_mutex> lock(imu_mutex_);
+
+    uint32_t id = rx_frame.can_id & CAN_SFF_MASK;
+
+    switch (id) {
+    case 0x181: {
+        float gx, gy, gz, ax, ay, az, mx, my, mz, t;
+        uint64_t ts_us;
+        uint8_t cyc;
+        mct7123_parse_imu(payload, &gx, &gy, &gz,
+                          &ax, &ay, &az, &mx, &my, &mz, &t, &ts_us, &cyc);
+        sensor_data_.gyr_x = gx * DEG_TO_RAD;
+        sensor_data_.gyr_y = gy * DEG_TO_RAD;
+        sensor_data_.gyr_z = gz * DEG_TO_RAD;
+        sensor_data_.acc_x = ax;
+        sensor_data_.acc_y = ay;
+        sensor_data_.acc_z = az;
+        sensor_data_.mag_x = mx;
+        sensor_data_.mag_y = my;
+        sensor_data_.mag_z = mz;
+        sensor_data_.temperature = t;
+        sensor_data_.timestamp_ms = (uint32_t)(ts_us / 1000ULL);
+        sensor_data_.cycle = cyc;
+        break;
+    }
+    case 0x182: {
+        float roll, pitch, yaw, qw, qx, qy, qz, t;
+        uint32_t fusion_status;
+        uint64_t ts_us;
+        uint8_t cyc;
+        mct7123_parse_att(payload, &roll, &pitch, &yaw,
+                          &qw, &qx, &qy, &qz, &t,
+                          sensor_data_.running_status, &fusion_status, &ts_us, &cyc);
+        sensor_data_.quat_w = qw;
+        sensor_data_.quat_x = qx;
+        sensor_data_.quat_y = qy;
+        sensor_data_.quat_z = qz;
+        sensor_data_.roll  = roll;
+        sensor_data_.pitch = pitch;
+        sensor_data_.imu_yaw = yaw;
+        sensor_data_.temperature = t;
+        sensor_data_.timestamp_ms = (uint32_t)(ts_us / 1000ULL);
+        sensor_data_.cycle = cyc;
+        break;
+    }
+    case 0x183: {
+        mct7123_parse_cfg(payload, nullptr, nullptr, nullptr, nullptr,
+                          nullptr, nullptr, nullptr);
+        break;
+    }
+    default:
+        break;
+    }
+}
+
+std::vector<float> Mct7123CanDriver::get_ang_vel()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.gyr_x, sensor_data_.gyr_y, sensor_data_.gyr_z};
+}
+
+std::vector<float> Mct7123CanDriver::get_quat()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.quat_w, sensor_data_.quat_x,
+            sensor_data_.quat_y, sensor_data_.quat_z};
+}
+
+std::vector<float> Mct7123CanDriver::get_lin_acc()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.acc_x, sensor_data_.acc_y, sensor_data_.acc_z};
+}
+
+std::vector<float> Mct7123CanDriver::get_mag()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z};
+}
+
+std::vector<float> Mct7123CanDriver::get_euler()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw};
+}
+
+uint64_t Mct7123CanDriver::get_timestamp()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return (uint64_t)sensor_data_.timestamp_ms * 1000ULL;
+}
+
+float Mct7123CanDriver::get_temperature()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return sensor_data_.temperature;
+}
+
+uint8_t Mct7123CanDriver::get_cycle()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return sensor_data_.cycle;
+}
diff --git a/src/drivers/mct7123/mct7123_can_driver.hpp b/src/drivers/mct7123/mct7123_can_driver.hpp
new file mode 100644
index 0000000..98c34c1
--- /dev/null
+++ b/src/drivers/mct7123/mct7123_can_driver.hpp
@@ -0,0 +1,45 @@
+#pragma once
+
+extern "C" {
+#include "mct7123_dec.h"
+}
+
+#include <memory>
+#include <array>
+#include <string>
+#include <shared_mutex>
+
+#include "imu_driver.hpp"
+#include "protocol/can/socket_can.hpp"
+
+#ifndef DEG_TO_RAD
+#define DEG_TO_RAD  (0.01745329f)
+#endif
+
+class Mct7123CanDriver : public IMUDriver {
+   public:
+    Mct7123CanDriver(uint16_t imu_id, const std::string& interface);
+    ~Mct7123CanDriver();
+
+    void can_rx_cbk(const canfd_frame& rx_frame);
+
+    std::vector<float> get_ang_vel() override;
+    std::vector<float> get_quat() override;
+    std::vector<float> get_lin_acc() override;
+    std::vector<float> get_mag() override;
+    std::vector<float> get_euler() override;
+    uint64_t get_timestamp() override;
+    float get_temperature() override;
+    uint8_t get_cycle() override;
+
+   private:
+    static constexpr size_t kFrameTypeCount = 3;
+
+    std::string interface_;
+    std::array<CanCbkId, kFrameTypeCount> callback_ids_;
+    mutable std::shared_mutex imu_mutex_;
+    std::shared_ptr<IMUSocketCAN> can_;
+
+    imu_sensor_data_t sensor_data_;
+    std::array<CanCallbackSubscription, kFrameTypeCount> can_subscriptions_;
+};
diff --git a/src/drivers/mct7123/mct7123_dec.c b/src/drivers/mct7123/mct7123_dec.c
new file mode 100644
index 0000000..ceee0a9
--- /dev/null
+++ b/src/drivers/mct7123/mct7123_dec.c
@@ -0,0 +1,178 @@
+/*
+ * SPDX-License-Identifier: GPL-3.0
+ *
+ * MCT MD7123 byte-level UART protocol decoder.
+ */
+
+#include "mct7123_dec.h"
+#include <string.h>
+#include <stdio.h>
+
+/* CRC16-CCITT: poly 0x1021, init 0xFFFF */
+uint16_t mct7123_crc16(const uint8_t *data, size_t len)
+{
+    uint16_t crc = 0xFFFF;
+    for (size_t i = 0; i < len; i++) {
+        crc ^= (uint16_t)data[i] << 8;
+        for (int j = 0; j < 8; j++) {
+            if (crc & 0x8000)
+                crc = (crc << 1) ^ 0x1021;
+            else
+                crc <<= 1;
+        }
+    }
+    return crc;
+}
+
+static uint16_t le16(const uint8_t *p)
+{
+    return (uint16_t)p[0] | ((uint16_t)p[1] << 8);
+}
+
+static uint64_t le64(const uint8_t *p)
+{
+    uint64_t v;
+    memcpy(&v, p, 8);
+    return v;
+}
+
+static float r4(const uint8_t *p)
+{
+    float v;
+    memcpy(&v, p, 4);
+    return v;
+}
+
+int mct7123_input(mct7123_raw_t *raw, uint8_t data)
+{
+    /* state 0: waiting for 0xEB */
+    if (raw->idx == 0) {
+        if (data == 0xEB) {
+            raw->buf[raw->idx++] = data;
+        }
+        return 0;
+    }
+
+    /* state 1: expect 0x90 */
+    if (raw->idx == 1) {
+        if (data == 0x90) {
+            raw->buf[raw->idx++] = data;
+        } else {
+            /* false sync, restart */
+            raw->idx = 0;
+            if (data == 0xEB) {
+                raw->buf[raw->idx++] = data;
+            }
+        }
+        return 0;
+    }
+
+    /* state 2: MsgID */
+    if (raw->idx == 2) {
+        raw->msg_id = data;
+        raw->buf[raw->idx++] = data;
+        return 0;
+    }
+
+    /* state 3: payload length (must be 0x40) */
+    if (raw->idx == 3) {
+        if (data != MCT7123_PAYLOAD_LEN) {
+            /* invalid length, restart */
+            raw->idx = 0;
+            return -1;
+        }
+        raw->payload_len = data;
+        raw->buf[raw->idx++] = data;
+        return 0;
+    }
+
+    /* state 4: collecting payload (64 bytes) */
+    if (raw->idx < 4 + MCT7123_PAYLOAD_LEN) {
+        raw->payload[raw->idx - 4] = data;
+        raw->buf[raw->idx++] = data;
+        return 0;
+    }
+
+    /* state 5: tail (must be 0xED) */
+    if (raw->idx == 4 + MCT7123_PAYLOAD_LEN) {
+        raw->buf[raw->idx++] = data;
+        if (data != 0xED) {
+            raw->idx = 0;
+            return -1;
+        }
+
+        /* verify CRC16 over payload[0..61], CRC at payload[62..63] */
+        uint16_t crc_expected = le16(&raw->payload[62]);
+        uint16_t crc_calc = mct7123_crc16(raw->payload, 62);
+        if (crc_calc != crc_expected) {
+            raw->idx = 0;
+            return -1;
+        }
+
+        raw->idx = 0;
+        return 1;  /* complete frame */
+    }
+
+    return 0;
+}
+
+void mct7123_parse_imu(const uint8_t payload[64],
+                       float *gyr_x, float *gyr_y, float *gyr_z,
+                       float *acc_x, float *acc_y, float *acc_z,
+                       float *mag_x, float *mag_y, float *mag_z,
+                       float *temp, uint64_t *systimer_us, uint8_t *cycle)
+{
+    if (systimer_us) memcpy(systimer_us, payload, 8);
+    *gyr_x = r4(payload + 8);
+    *gyr_y = r4(payload + 12);
+    *gyr_z = r4(payload + 16);
+    *acc_x = r4(payload + 20);
+    *acc_y = r4(payload + 24);
+    *acc_z = r4(payload + 28);
+    *mag_x = r4(payload + 32);
+    *mag_y = r4(payload + 36);
+    *mag_z = r4(payload + 40);
+    *temp  = r4(payload + 44);
+    if (cycle) *cycle = payload[61];
+}
+
+void mct7123_parse_att(const uint8_t payload[64],
+                       float *roll, float *pitch, float *yaw,
+                       float *qw, float *qx, float *qy, float *qz,
+                       float *temp, uint8_t *running_status, uint32_t *fusion_status,
+                       uint64_t *systimer_us, uint8_t *cycle)
+{
+    if (systimer_us) memcpy(systimer_us, payload, 8);
+    *roll  = r4(payload + 8);
+    *pitch = r4(payload + 12);
+    *yaw   = r4(payload + 16);
+    *qx    = r4(payload + 20);
+    *qy    = r4(payload + 24);
+    *qz    = r4(payload + 28);
+    *qw    = r4(payload + 32);
+    *temp  = r4(payload + 36);
+
+    if (fusion_status) {
+        uint32_t fs;
+        memcpy(&fs, payload + 38, 4);
+        *fusion_status = fs;
+    }
+    if (running_status) {
+        memcpy(running_status, payload + 42, 6);
+    }
+    if (cycle) *cycle = payload[61];
+}
+
+void mct7123_parse_cfg(const uint8_t payload[64],
+                       uint8_t *sentence_ctrl, uint8_t *output_rate,
+                       uint8_t *orientation, uint8_t *fusion_mode,
+                       double *latitude, double *longitude, float *height)
+{
+    if (sentence_ctrl) *sentence_ctrl = payload[0];
+    if (output_rate)   *output_rate   = payload[1];
+    if (orientation)   *orientation   = payload[2];
+    if (fusion_mode)   *fusion_mode   = payload[3];
+    if (latitude)      memcpy(latitude,  payload + 4,  8);
+    if (longitude)     memcpy(longitude, payload + 12, 8);
+    if (height)        memcpy(height,    payload + 20, 4);
+}
diff --git a/src/drivers/mct7123/mct7123_dec.h b/src/drivers/mct7123/mct7123_dec.h
new file mode 100644
index 0000000..809a91f
--- /dev/null
+++ b/src/drivers/mct7123/mct7123_dec.h
@@ -0,0 +1,76 @@
+/*
+ * SPDX-License-Identifier: GPL-3.0
+ *
+ * MCT MD7123 Decoder Library
+ * Byte-level UART protocol decoder for MD7123 inertial measurement unit.
+ *
+ * Frame format (ref: MD7123 User Manual V2.0):
+ *   Header:  0xEB 0x90
+ *   Msg_ID:  1 byte (0x81=IMU raw, 0x82=Attitude, 0x83=Config)
+ *   Length:  1 byte (fixed 0x40 = 64 byte payload)
+ *   Payload: 64 bytes
+ *   Tail:    0xED
+ *   CRC16:   CCITT (poly 0x1021, init 0xFFFF) over payload[0..61]
+ */
+
+#ifndef __MCT7123_DEC_H__
+#define __MCT7123_DEC_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+#define MCT7123_MSG_IMU_DATA  0x81
+#define MCT7123_MSG_ATT_DATA  0x82
+#define MCT7123_MSG_CFG_DATA  0x83
+
+#define MCT7123_PAYLOAD_LEN   64
+#define MCT7123_BUF_SIZE      (2 + 1 + 1 + MCT7123_PAYLOAD_LEN + 1)  /* 69 */
+
+typedef struct {
+    uint8_t  buf[MCT7123_BUF_SIZE];
+    int      idx;
+
+    uint8_t  msg_id;
+    uint8_t  payload_len;
+    uint8_t  payload[MCT7123_PAYLOAD_LEN];
+} mct7123_raw_t;
+
+/**
+ * Process one byte, returns:
+ *  1  = complete frame decoded
+ *  0  = need more data
+ * -1  = error (CRC mismatch, sync lost, etc.)
+ */
+int mct7123_input(mct7123_raw_t *raw, uint8_t data);
+
+/** CRC16-CCITT over payload bytes (poly 0x1021, init 0xFFFF). */
+uint16_t mct7123_crc16(const uint8_t *data, size_t len);
+
+/** Parse payload into float fields after successful mct7123_input(). */
+void mct7123_parse_imu(const uint8_t payload[64],
+                       float *gyr_x, float *gyr_y, float *gyr_z,
+                       float *acc_x, float *acc_y, float *acc_z,
+                       float *mag_x, float *mag_y, float *mag_z,
+                       float *temp, uint64_t *systimer_us, uint8_t *cycle);
+
+void mct7123_parse_att(const uint8_t payload[64],
+                       float *roll, float *pitch, float *yaw,
+                       float *qw, float *qx, float *qy, float *qz,
+                       float *temp, uint8_t *running_status, uint32_t *fusion_status,
+                       uint64_t *systimer_us, uint8_t *cycle);
+
+/** MD7123 config frame (Msg_ID 0x83 / CAN ID 0x183). All pointers may be NULL. */
+void mct7123_parse_cfg(const uint8_t payload[64],
+                       uint8_t *sentence_ctrl, uint8_t *output_rate,
+                       uint8_t *orientation, uint8_t *fusion_mode,
+                       double *latitude, double *longitude, float *height);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MCT7123_DEC_H__ */
diff --git a/src/drivers/mct7123/mct7123_serial_driver.cpp b/src/drivers/mct7123/mct7123_serial_driver.cpp
new file mode 100644
index 0000000..39151e9
--- /dev/null
+++ b/src/drivers/mct7123/mct7123_serial_driver.cpp
@@ -0,0 +1,129 @@
+#include "mct7123_serial_driver.hpp"
+#include <cstring>
+
+Mct7123SerialDriver::Mct7123SerialDriver(uint16_t imu_id, const std::string& interface, int baudrate)
+    : IMUDriver(), baudrate_(baudrate), interface_(interface)
+{
+    imu_id_ = imu_id;
+    memset(&raw_, 0, sizeof(raw_));
+    memset(&sensor_data_, 0, sizeof(sensor_data_));
+
+    serial_ = IMUSerialPort::open(interface_, baudrate_);
+    IMUSerialPort::SerialCbkFunc cb = std::bind(
+        &Mct7123SerialDriver::serial_rx_cbk, this,
+        std::placeholders::_1, std::placeholders::_2);
+    serial_->set_serial_callback(cb);
+}
+
+Mct7123SerialDriver::~Mct7123SerialDriver()
+{
+    if (serial_) serial_->close();
+}
+
+void Mct7123SerialDriver::serial_rx_cbk(const uint8_t* data, size_t length)
+{
+    std::unique_lock<std::shared_mutex> lock(imu_mutex_);
+
+    for (size_t i = 0; i < length; i++) {
+        if (mct7123_input(&raw_, data[i]) == 1) {
+            switch (raw_.msg_id) {
+            case MCT7123_MSG_IMU_DATA: {
+                float gx, gy, gz, ax, ay, az, mx, my, mz, t;
+                uint64_t ts_us;
+                uint8_t cyc;
+                mct7123_parse_imu(raw_.payload, &gx, &gy, &gz,
+                                  &ax, &ay, &az, &mx, &my, &mz, &t, &ts_us, &cyc);
+                sensor_data_.gyr_x = gx * DEG_TO_RAD;
+                sensor_data_.gyr_y = gy * DEG_TO_RAD;
+                sensor_data_.gyr_z = gz * DEG_TO_RAD;
+                sensor_data_.acc_x = ax;
+                sensor_data_.acc_y = ay;
+                sensor_data_.acc_z = az;
+                sensor_data_.mag_x = mx;
+                sensor_data_.mag_y = my;
+                sensor_data_.mag_z = mz;
+                sensor_data_.temperature = t;
+                sensor_data_.timestamp_ms = (uint32_t)(ts_us / 1000ULL);
+                sensor_data_.cycle = cyc;
+                break;
+            }
+            case MCT7123_MSG_ATT_DATA: {
+                float roll, pitch, yaw, qw, qx, qy, qz, t;
+                uint32_t fusion_status;
+                uint64_t ts_us;
+                uint8_t cyc;
+                mct7123_parse_att(raw_.payload, &roll, &pitch, &yaw,
+                                  &qw, &qx, &qy, &qz, &t,
+                                  sensor_data_.running_status, &fusion_status, &ts_us, &cyc);
+                sensor_data_.quat_w = qw;
+                sensor_data_.quat_x = qx;
+                sensor_data_.quat_y = qy;
+                sensor_data_.quat_z = qz;
+                sensor_data_.roll  = roll;
+                sensor_data_.pitch = pitch;
+                sensor_data_.imu_yaw = yaw;
+                sensor_data_.temperature = t;
+                sensor_data_.timestamp_ms = (uint32_t)(ts_us / 1000ULL);
+                sensor_data_.cycle = cyc;
+                break;
+            }
+            case MCT7123_MSG_CFG_DATA: {
+                mct7123_parse_cfg(raw_.payload, nullptr, nullptr, nullptr, nullptr,
+                                  nullptr, nullptr, nullptr);
+                break;
+            }
+            default:
+                break;
+            }
+        }
+    }
+}
+
+std::vector<float> Mct7123SerialDriver::get_ang_vel()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.gyr_x, sensor_data_.gyr_y, sensor_data_.gyr_z};
+}
+
+std::vector<float> Mct7123SerialDriver::get_quat()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.quat_w, sensor_data_.quat_x,
+            sensor_data_.quat_y, sensor_data_.quat_z};
+}
+
+std::vector<float> Mct7123SerialDriver::get_lin_acc()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.acc_x, sensor_data_.acc_y, sensor_data_.acc_z};
+}
+
+std::vector<float> Mct7123SerialDriver::get_mag()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z};
+}
+
+std::vector<float> Mct7123SerialDriver::get_euler()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw};
+}
+
+uint64_t Mct7123SerialDriver::get_timestamp()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return (uint64_t)sensor_data_.timestamp_ms * 1000ULL;
+}
+
+float Mct7123SerialDriver::get_temperature()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return sensor_data_.temperature;
+}
+
+uint8_t Mct7123SerialDriver::get_cycle()
+{
+    std::shared_lock<std::shared_mutex> lock(imu_mutex_);
+    return sensor_data_.cycle;
+}
diff --git a/src/drivers/mct7123/mct7123_serial_driver.hpp b/src/drivers/mct7123/mct7123_serial_driver.hpp
new file mode 100644
index 0000000..154040d
--- /dev/null
+++ b/src/drivers/mct7123/mct7123_serial_driver.hpp
@@ -0,0 +1,42 @@
+#pragma once
+
+extern "C" {
+#include "mct7123_dec.h"
+}
+
+#include <memory>
+#include <string>
+#include <shared_mutex>
+
+#include "imu_driver.hpp"
+#include "protocol/serial/serial_port.hpp"
+
+#ifndef DEG_TO_RAD
+#define DEG_TO_RAD  (0.01745329f)
+#endif
+
+class Mct7123SerialDriver : public IMUDriver {
+   public:
+    Mct7123SerialDriver(uint16_t imu_id, const std::string& interface, int baudrate);
+    ~Mct7123SerialDriver();
+
+    void serial_rx_cbk(const uint8_t* data, size_t length);
+
+    std::vector<float> get_ang_vel() override;
+    std::vector<float> get_quat() override;
+    std::vector<float> get_lin_acc() override;
+    std::vector<float> get_mag() override;
+    std::vector<float> get_euler() override;
+    uint64_t get_timestamp() override;
+    float get_temperature() override;
+    uint8_t get_cycle() override;
+
+   private:
+    int baudrate_;
+    std::string interface_;
+    mutable std::shared_mutex imu_mutex_;
+    std::shared_ptr<IMUSerialPort> serial_;
+
+    mct7123_raw_t raw_;
+    imu_sensor_data_t sensor_data_;
+};
diff --git a/src/imu_driver.cpp b/src/imu_driver.cpp
index 2dc3a7d..e12594f 100644
--- a/src/imu_driver.cpp
+++ b/src/imu_driver.cpp
@@ -11,11 +11,21 @@
 #include "imu_driver.hpp"
 
 #include "drivers/hipnuc/hipnuc_imu_driver.hpp"
+#include "drivers/mct7123/mct7123_serial_driver.hpp"
+#include "drivers/mct7123/mct7123_can_driver.hpp"
 
 std::shared_ptr<IMUDriver> IMUDriver::create_imu(uint16_t imu_id, const std::string& interface_type, const std::string& interface,
                                                 const std::string& imu_type, const int baudrate) {
     if (imu_type == "HIPNUC") {
         return std::make_shared<HipnucIMUDriver>(imu_id, interface_type, interface, baudrate);
+    } else if (imu_type == "MCT7123") {
+        if (interface_type == "serial") {
+            return std::make_shared<Mct7123SerialDriver>(imu_id, interface, baudrate);
+        } else if (interface_type == "canfd") {
+            return std::make_shared<Mct7123CanDriver>(imu_id, interface);
+        } else {
+            throw std::runtime_error("MCT7123: interface_type must be 'serial' or 'canfd'");
+        }
     } else {
         throw std::runtime_error("IMU type not supported");
     }
diff --git a/src/protocol/can/socket_can.cpp b/src/protocol/can/socket_can.cpp
index 4763b26..ced6562 100644
--- a/src/protocol/can/socket_can.cpp
+++ b/src/protocol/can/socket_can.cpp
@@ -10,6 +10,8 @@
 
 #include "socket_can.hpp"
 
+#include <algorithm>
+
 std::shared_ptr<spdlog::logger> IMUSocketCAN::logger_ = nullptr;
 std::unordered_map<std::string, std::shared_ptr<IMUSocketCAN>> IMUSocketCAN::instances_;
 
@@ -27,6 +29,12 @@ void IMUSocketCAN::open(std::string interface) {
         throw std::runtime_error("Failed to create CAN socket");
     }
 
+    /* Enable CAN FD frame reception (backward compatible with classic CAN) */
+    int canfd_on = 1;
+    if (setsockopt(sockfd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0) {
+        logger_->warn("CAN FD not supported on interface {}, falling back to classic CAN", interface);
+    }
+
     strncpy(if_request_.ifr_name, interface.c_str(), IFNAMSIZ);
     if (ioctl(sockfd_, SIOCGIFINDEX, &if_request_) == -1) {
         logger_->error("Unable to detect CAN interface {}", interface);
@@ -62,12 +70,12 @@ void IMUSocketCAN::open(std::string interface) {
         pthread_setname_np(pthread_self(), "can_rx");
         struct sched_param sp{}; sp.sched_priority = 80;
         if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp) != 0) {
-            logger_->error("Failed to set realtime priority for IMU CAN RX thread");
+            logger_->warn("Failed to set realtime priority for IMU CAN RX thread");
         }
         fd_set descriptors;
         int maxfd = sockfd_;
         struct timeval timeout;
-        can_frame rx_frame;
+        canfd_frame rx_frame;
 
         while (receiving_) {
             FD_ZERO(&descriptors);
@@ -84,7 +92,7 @@ void IMUSocketCAN::open(std::string interface) {
             }
             if (sel_ret == 1) {
                 while (true){
-                    int len = ::read(sockfd_, &rx_frame, CAN_MTU);
+                    int len = ::read(sockfd_, &rx_frame, sizeof(canfd_frame));
                     if (len < 0) {
                         if (errno == EAGAIN || errno == EWOULDBLOCK) {
                             break; 
@@ -95,17 +103,44 @@ void IMUSocketCAN::open(std::string interface) {
                     if (len == 0){
                         break;
                     }
-                    CanCbkFunc callback_to_run;
+                    std::vector<CanCbkEntryPtr> callbacks_to_run;
                     {
                         std::lock_guard<std::mutex> lock(can_callback_mutex_);
                         CanCbkId key = key_extractor_(rx_frame);
                         auto it = can_callback_list_.find(key);
                         if (it != can_callback_list_.end()) {
-                            callback_to_run = it->second;
+                            callbacks_to_run = it->second;
+                        }
+                        auto wildcard_it =
+                            can_callback_list_.find(CAN_CALLBACK_WILDCARD);
+                        if (wildcard_it != can_callback_list_.end() &&
+                            key != CAN_CALLBACK_WILDCARD) {
+                            callbacks_to_run.insert(
+                                callbacks_to_run.end(),
+                                wildcard_it->second.begin(),
+                                wildcard_it->second.end());
                         }
                     }
-                    if (callback_to_run) {
-                        callback_to_run(rx_frame);
+                    for (const auto &entry : callbacks_to_run) {
+                        {
+                            std::lock_guard<std::mutex> lock(entry->mutex);
+                            if (!entry->enabled) continue;
+                            ++entry->running_count;
+                        }
+
+                        try {
+                            entry->callback(rx_frame);
+                        } catch (const std::exception &e) {
+                            logger_->error("CAN callback error: {}", e.what());
+                        } catch (...) {
+                            logger_->error("CAN callback error: unknown exception");
+                        }
+
+                        {
+                            std::lock_guard<std::mutex> lock(entry->mutex);
+                            --entry->running_count;
+                            if (entry->running_count == 0) entry->cv.notify_all();
+                        }
                     }
                 }
             }
@@ -127,22 +162,71 @@ void IMUSocketCAN::close() {
     sockfd_ = INIT_FD;
 }
 
-void IMUSocketCAN::add_can_callback(const CanCbkFunc callback, const CanCbkId id) {
+CanCbkToken IMUSocketCAN::add_can_callback(const CanCbkFunc callback, const CanCbkId id) {
     std::lock_guard<std::mutex> lock(can_callback_mutex_);
-    can_callback_list_[id] = callback;
+    const CanCbkToken token = next_callback_token_++;
+    auto entry = std::make_shared<CanCbkEntry>();
+    entry->token = token;
+    entry->callback = callback;
+    can_callback_list_[id].push_back(std::move(entry));
+    return token;
 }
 
-void IMUSocketCAN::remove_can_callback(CanCbkId id) {
-    std::lock_guard<std::mutex> lock(can_callback_mutex_);
-    can_callback_list_.erase(id);
+void IMUSocketCAN::remove_can_callback(CanCbkId id, CanCbkToken token) {
+    CanCbkEntryPtr removed_entry;
+    {
+        std::lock_guard<std::mutex> lock(can_callback_mutex_);
+        auto it = can_callback_list_.find(id);
+        if (it == can_callback_list_.end()) return;
+
+        auto &entries = it->second;
+        auto entry_it = std::find_if(
+            entries.begin(), entries.end(),
+            [token](const CanCbkEntryPtr &entry) { return entry->token == token; });
+        if (entry_it == entries.end()) return;
+
+        removed_entry = *entry_it;
+        entries.erase(entry_it);
+        if (entries.empty()) can_callback_list_.erase(it);
+    }
+
+    // Do not call this synchronously from the callback being removed.
+    std::unique_lock<std::mutex> lock(removed_entry->mutex);
+    removed_entry->enabled = false;
+    removed_entry->cv.wait(lock, [&removed_entry] {
+        return removed_entry->running_count == 0;
+    });
 }
 
 void IMUSocketCAN::clear_can_callbacks() {
-    std::lock_guard<std::mutex> lock(can_callback_mutex_);
-    can_callback_list_.clear();
+    std::vector<CanCbkEntryPtr> removed_entries;
+    {
+        std::lock_guard<std::mutex> lock(can_callback_mutex_);
+        for (const auto &item : can_callback_list_) {
+            removed_entries.insert(removed_entries.end(),
+                                   item.second.begin(), item.second.end());
+        }
+        can_callback_list_.clear();
+    }
+
+    for (const auto &entry : removed_entries) {
+        std::unique_lock<std::mutex> lock(entry->mutex);
+        entry->enabled = false;
+        entry->cv.wait(lock, [&entry] { return entry->running_count == 0; });
+    }
 }
 
 void IMUSocketCAN::set_key_extractor(CanCbkKeyExtractor extractor) {
     std::lock_guard<std::mutex> lock(can_callback_mutex_);
     key_extractor_ = std::move(extractor);
 }
+
+int IMUSocketCAN::send(const canfd_frame &frame) {
+    if (sockfd_ == INIT_FD) return -1;
+    ssize_t n = ::write(sockfd_, &frame, CANFD_MTU);
+    if (n < 0) {
+        logger_->warn("CAN send error on {}: {}", interface_, strerror(errno));
+        return -1;
+    }
+    return (int)n;
+}
diff --git a/src/protocol/can/socket_can.hpp b/src/protocol/can/socket_can.hpp
index 9ca85c8..10cade9 100644
--- a/src/protocol/can/socket_can.hpp
+++ b/src/protocol/can/socket_can.hpp
@@ -5,13 +5,16 @@
  * @file socket_can.hpp
  * @brief SocketCAN interface declaration for CAN/CAN FD frame reception.
  * @details Provides the IMUSocketCAN singleton class that wraps Linux
- *          SocketCAN socket operations and delivers parsed frames
- *          to registered callback handlers.
+ *          SocketCAN socket operations and delivers both classic CAN
+ *          (len <= 8) and CAN FD (len up to 64) frames to registered
+ *          callbacks via the unified canfd_frame struct.
+ *          Callbacks are responsible for filtering by frame length.
  */
 
 #pragma once
 
 #include <linux/can.h>
+#include <linux/can/raw.h>
 #include <net/if.h>
 #include <pthread.h>
 #include <spdlog/sinks/stdout_color_sinks.h>
@@ -23,6 +26,7 @@
 
 #include <atomic>
 #include <cstdbool>
+#include <condition_variable>
 #include <cstdio>
 #include <cstring>
 #include <memory>
@@ -33,13 +37,28 @@
 constexpr const int INIT_FD = -1;
 constexpr const int TIMEOUT_SEC = 0;
 constexpr const int TIMEOUT_USEC = 1000;
-using CanCbkFunc = std::function<void(const can_frame &)>;
+using CanCbkFunc = std::function<void(const canfd_frame &)>;
 using CanCbkId = uint16_t;
-using CanCbkMap = std::unordered_map<CanCbkId, CanCbkFunc>;
-using CanCbkKeyExtractor = std::function<CanCbkId(const can_frame &)>;
+using CanCbkToken = uint64_t;
+constexpr CanCbkId CAN_CALLBACK_WILDCARD = UINT16_MAX;
+struct CanCbkEntry {
+    CanCbkToken token;
+    CanCbkFunc callback;
+    std::mutex mutex;
+    std::condition_variable cv;
+    bool enabled{true};
+    size_t running_count{0};
+};
+using CanCbkEntryPtr = std::shared_ptr<CanCbkEntry>;
+using CanCbkMap = std::unordered_map<CanCbkId, std::vector<CanCbkEntryPtr>>;
+using CanCbkKeyExtractor = std::function<CanCbkId(const canfd_frame &)>;
+
+class CanCallbackSubscription;
 
 class IMUSocketCAN {
    private:
+    friend class CanCallbackSubscription;
+
     std::string interface_;  // The network interface name
     int sockfd_ = -1;        // The file descriptor for the CAN socket
     std::atomic<bool> receiving_;
@@ -51,7 +70,8 @@ class IMUSocketCAN {
     std::thread receiver_thread_;
     CanCbkMap can_callback_list_;
     std::mutex can_callback_mutex_;
-    CanCbkKeyExtractor key_extractor_ = [](const can_frame &frame) -> CanCbkId {
+    CanCbkToken next_callback_token_{1};
+    CanCbkKeyExtractor key_extractor_ = [](const canfd_frame &frame) -> CanCbkId {
         return static_cast<CanCbkId>(frame.can_id);
     };
 
@@ -63,6 +83,9 @@ class IMUSocketCAN {
     static std::shared_ptr<spdlog::logger> logger_;
     static std::unordered_map<std::string, std::shared_ptr<IMUSocketCAN>> instances_;
 
+    CanCbkToken add_can_callback(const CanCbkFunc callback, const CanCbkId id);
+    void remove_can_callback(const CanCbkId id, const CanCbkToken token);
+
    public:
     IMUSocketCAN(const IMUSocketCAN &) = delete;
     IMUSocketCAN &operator=(const IMUSocketCAN &) = delete;
@@ -83,8 +106,49 @@ class IMUSocketCAN {
     }
     void open(std::string interface);
     void close();
-    void add_can_callback(const CanCbkFunc callback, const CanCbkId id);
-    void remove_can_callback(const CanCbkId id);
     void clear_can_callbacks();
     void set_key_extractor(CanCbkKeyExtractor extractor);
+    int send(const canfd_frame &frame);
+};
+
+class CanCallbackSubscription {
+   public:
+    CanCallbackSubscription() = default;
+    CanCallbackSubscription(std::shared_ptr<IMUSocketCAN> can, CanCbkId id,
+                            const CanCbkFunc &callback)
+        : can_(std::move(can)), id_(id) {
+        if (can_) token_ = can_->add_can_callback(callback, id_);
+    }
+
+    ~CanCallbackSubscription() { reset(); }
+
+    CanCallbackSubscription(const CanCallbackSubscription &) = delete;
+    CanCallbackSubscription &operator=(const CanCallbackSubscription &) = delete;
+
+    CanCallbackSubscription(CanCallbackSubscription &&other) noexcept
+        : can_(std::move(other.can_)), id_(other.id_), token_(other.token_) {
+        other.token_ = 0;
+    }
+
+    CanCallbackSubscription &operator=(CanCallbackSubscription &&other) noexcept {
+        if (this != &other) {
+            reset();
+            can_ = std::move(other.can_);
+            id_ = other.id_;
+            token_ = other.token_;
+            other.token_ = 0;
+        }
+        return *this;
+    }
+
+    void reset() {
+        if (can_ && token_ != 0) can_->remove_can_callback(id_, token_);
+        token_ = 0;
+        can_.reset();
+    }
+
+   private:
+    std::shared_ptr<IMUSocketCAN> can_;
+    CanCbkId id_{0};
+    CanCbkToken token_{0};
 };
diff --git a/src/protocol/serial/serial_port.cpp b/src/protocol/serial/serial_port.cpp
index ab5c4ff..4a15271 100644
--- a/src/protocol/serial/serial_port.cpp
+++ b/src/protocol/serial/serial_port.cpp
@@ -77,7 +77,7 @@ void IMUSerialPort::init() {
         pthread_setname_np(pthread_self(), "serial_rx");
         struct sched_param sp{}; sp.sched_priority = 80;
         if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp) != 0) {
-            if (logger_) logger_->error("Failed to set realtime priority for IMU serial RX");
+            if (logger_) logger_->warn("Failed to set realtime priority for IMU serial RX");
         } 
         uint8_t buf[BUF_SIZE] = {0};
         
diff --git a/src/pybind_module.cpp b/src/pybind_module.cpp
index a88836d..9758ea0 100644
--- a/src/pybind_module.cpp
+++ b/src/pybind_module.cpp
@@ -4,8 +4,9 @@
 /**
  * @file pybind_module.cpp
  * @brief Python bindings for the IMU driver library via pybind11.
- * @details Exposes IMUDriver and sensor data accessors to Python
- *          as the imu_py module.
+ * @details Exposes IMUDriver and all sensor data accessors to Python
+ *          as the imu_py module. Each method includes inline docstring
+ *          visible via help() in Python.
  */
 
 #include <pybind11/pybind11.h>
@@ -15,19 +16,38 @@
 namespace py = pybind11;
 
 PYBIND11_MODULE(imu_py, m) {
-    m.doc() = "IMU Driver Python SDK"; 
+    m.doc() = "IMU Driver Python SDK";
 
     py::class_<IMUDriver, std::shared_ptr<IMUDriver>>(m, "IMUDriver")
         .def(py::init<>())
-        .def_static("create_imu", &IMUDriver::create_imu, 
+        .def_static("create_imu", &IMUDriver::create_imu,
             py::arg("imu_id"),
             py::arg("interface_type"),
             py::arg("interface"),
             py::arg("imu_type"),
-            py::arg("baudrate") = 0)
-        .def("get_imu_id", &IMUDriver::get_imu_id)
-        .def("get_ang_vel", &IMUDriver::get_ang_vel)
-        .def("get_quat", &IMUDriver::get_quat)
-        .def("get_lin_acc", &IMUDriver::get_lin_acc)
-        .def("get_temperature", &IMUDriver::get_temperature);
+            py::arg("baudrate") = 0,
+            "工厂方法: create_imu(imu_id, interface_type, interface, imu_type, baudrate=0)\n"
+            "  imu_id         int        节点 ID (CAN 模式用于多设备路由)\n"
+            "  interface_type \"serial\"|\"can\"|\"canfd\"\n"
+            "  interface      str        设备路径 (/dev/ttyUSB0, can0 …)\n"
+            "  imu_type       \"MCT7123\"|\"HIPNUC\"\n"
+            "  baudrate       int        serial 必须, can/canfd 忽略")
+        .def("get_imu_id", &IMUDriver::get_imu_id,
+            "IMU 节点 ID — CAN 模式用于多设备路由")
+        .def("get_ang_vel", &IMUDriver::get_ang_vel,
+            "角速度 [x, y, z]     rad/s  — 陀螺仪输出")
+        .def("get_quat", &IMUDriver::get_quat,
+            "姿态四元数 [w, x, y, z]       — IMU 姿态解算输出")
+        .def("get_lin_acc", &IMUDriver::get_lin_acc,
+            "线加速度 [x, y, z]   m/s²   — 加速度计输出")
+        .def("get_mag", &IMUDriver::get_mag,
+            "磁场强度 [x, y, z]   uT     — 磁力计输出 (9 轴模式下有效)")
+        .def("get_euler", &IMUDriver::get_euler,
+            "欧拉角 [roll, pitch, yaw] 度 — IMU 硬件直接输出")
+        .def("get_timestamp", &IMUDriver::get_timestamp,
+            "系统时间戳           us     — IMU 内部时钟 (开机累积)")
+        .def("get_temperature", &IMUDriver::get_temperature,
+            "传感器温度           °C")
+        .def("get_cycle", &IMUDriver::get_cycle,
+            "帧计数器 0-255 — 检测丢帧");
 }