From 84992fe5e96da18fbfad77deb820bf742bfb3f12 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 15:48:53 +0800 Subject: [PATCH 01/17] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=EF=BC=9A=E6=94=AF=E6=8C=81=20MCT7123=20=E4=B8=8E=20HiPNUC=20?= =?UTF-8?q?=E5=A4=9A=E6=8E=A5=E5=8F=A3=E5=85=B1=E5=AD=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 建立 IMU 驱动库的基础工程结构，同时提供 C++ 接口与 Python 绑定。 实现 MCT7123、HiPNUC 的串口、经典 CAN 和 CAN FD 通信，并补充构建、打包、测试及项目文档。 --- CMakeLists.txt | 4 +- README.md | 97 +++++++++++ include/imu_driver.hpp | 10 +- src/drivers/CMakeLists.txt | 3 +- src/drivers/hipnuc/hipnuc_imu_driver.cpp | 38 ++++- src/drivers/hipnuc/hipnuc_imu_driver.hpp | 5 +- src/drivers/mct7123/CMakeLists.txt | 14 ++ src/drivers/mct7123/mct7123_dec.c | 162 +++++++++++++++++++ src/drivers/mct7123/mct7123_dec.h | 67 ++++++++ src/drivers/mct7123/mct7123_imu_driver.cpp | 180 +++++++++++++++++++++ src/drivers/mct7123/mct7123_imu_driver.hpp | 47 ++++++ src/imu_driver.cpp | 3 + src/protocol/can/socket_can.cpp | 12 +- src/protocol/can/socket_can.hpp | 13 +- src/protocol/serial/serial_port.cpp | 2 +- src/pybind_module.cpp | 38 +++-- test_imu.py | 84 ++++++++++ 17 files changed, 750 insertions(+), 29 deletions(-) create mode 100644 README.md create mode 100644 src/drivers/mct7123/CMakeLists.txt create mode 100644 src/drivers/mct7123/mct7123_dec.c create mode 100644 src/drivers/mct7123/mct7123_dec.h create mode 100644 src/drivers/mct7123/mct7123_imu_driver.cpp create mode 100644 src/drivers/mct7123/mct7123_imu_driver.hpp create mode 100644 test_imu.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 078ff7e..2afc4b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,11 +40,11 @@ target_include_directories(imu $ ) -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 diff --git a/README.md b/README.md new file mode 100644 index 0000000..e7b4582 --- /dev/null +++ b/README.md @@ -0,0 +1,97 @@ +# roboparty_imu + +RPO 双足机器人 IMU 驱动库 — C++ / Python 绑定 + +## 支持的 IMU + +| 类型 | 接口 | 协议 | +|------|------|------| +| **HIPNUC** (HI226/HI229 等) | serial, can | HiPNUC 私有协议 (5A A5 帧头) + CAN J1939 | +| **MCT7123** (MD7123) | serial, canfd | EB 90 固定 64B 帧 / CANFD 64B 一体帧 | + +## Python 测试脚本 + +```bash +# MCT7123 串口 (默认 921600bps) +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 + +# MCT7123 CANFD +python3 test_imu.py MCT7123 canfd can0 + +# 仅打摘要，安静模式 +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 5 -q + +# 无限循环，Ctrl+C 退出 +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 0 + +# HIPNUC 串口 +python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 + +# HIPNUC 经典 CAN +python3 test_imu.py HIPNUC can can0 +``` + +参数: + +``` +python3 test_imu.py -h + type MCT7123 | HIPNUC + interface 接口: serial | can | canfd + device 设备路径 (/dev/ttyUSB0, can0 等) + baudrate 波特率 (serial 必须; can/canfd 忽略) + -d SECONDS 运行时长 (0=无限循环, Ctrl+C 退出) + -i SECONDS 打印间隔 (默认 0.5s) + -q 安静模式, 只打印摘要 +``` + +## Python API + +```python +import imu_py + +# 创建 IMU 实例 +imu = imu_py.IMUDriver.create_imu( + imu_id=1, # 节点 ID (CAN 模式用于多设备路由) + interface_type="serial", # "serial" | "can" | "canfd" + interface="/dev/ttyUSB0", # 设备路径 + imu_type="MCT7123", # "MCT7123" | "HIPNUC" + baudrate=921600 # serial 必须, can/canfd 忽略 +) + +# 读取数据 (线程安全) +gyr = imu.get_ang_vel() # [x, y, z] rad/s 角速度 +acc = imu.get_lin_acc() # [x, y, z] m/s² 线加速度 +mag = imu.get_mag() # [x, y, z] uT 磁场强度 (9 轴模式) +quat = imu.get_quat() # [w, x, y, z] 姿态四元数 +euler = imu.get_euler() # [roll, pitch, yaw] 度 +ts = imu.get_timestamp() # us 系统时间戳 +temp = imu.get_temperature() # °C 传感器温度 +``` + +> `help(imu_py.IMUDriver)` 查看完整文档。 + +## C++ API + +```cpp +#include "imu_driver.hpp" + +auto imu = IMUDriver::create_imu(1, "serial", "/dev/ttyUSB0", "MCT7123", 921600); + +std::vector gyr = imu->get_ang_vel(); // [x,y,z] rad/s +std::vector acc = imu->get_lin_acc(); // [x,y,z] m/s² +std::vector mag = imu->get_mag(); // [x,y,z] uT +std::vector quat = imu->get_quat(); // [w,x,y,z] +std::vector euler = imu->get_euler(); // [roll,pitch,yaw] 度 +uint64_t ts = imu->get_timestamp(); // us +float temp = imu->get_temperature(); // °C +``` + +## MCT7123 坐标系说明 + +MD7123 使用**坐标系 9**（手册默认值）： + +- 标签 +Uz → X 正 +- 标签 -Uy → Y 正 +- 标签 +Ux → Z 正 + +机器人安装：正面朝右，接插件朝天。静止时 AccZ ≈ -9.81 m/s²（重力）。 diff --git a/include/imu_driver.hpp b/include/imu_driver.hpp index 2b0e341..5afb213 100644 --- a/include/imu_driver.hpp +++ b/include/imu_driver.hpp @@ -43,6 +43,9 @@ class IMUDriver { virtual std::vector get_ang_vel() { return ang_vel_; } virtual std::vector get_quat() { return quat_; } virtual std::vector get_lin_acc() { return lin_acc_; } + virtual std::vector get_mag() { return mag_; } + virtual std::vector get_euler() { return euler_; } + virtual uint64_t get_timestamp() { return timestamp_; } virtual float get_temperature() { return temperature_; } protected: @@ -50,7 +53,10 @@ class IMUDriver { uint16_t imu_id_; std::vector quat_{0.f, 0.f, 0.f, 0.f}; // w, x, y, z - std::vector ang_vel_{0.f, 0.f, 0.f}; // x, y, z - std::vector lin_acc_{0.f, 0.f, 0.f}; // x, y, z + std::vector ang_vel_{0.f, 0.f, 0.f}; // x, y, z rad/s + std::vector lin_acc_{0.f, 0.f, 0.f}; // x, y, z m/s² + std::vector mag_{0.f, 0.f, 0.f}; // x, y, z uT + std::vector euler_{0.f, 0.f, 0.f}; // roll, pitch, yaw (deg) + uint64_t timestamp_{0}; // us float temperature_{0.f}; // temperature }; \ No newline at end of file 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/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp index 3ed125f..779abaa 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.cpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.cpp @@ -14,7 +14,7 @@ HipnucIMUDriver::HipnucIMUDriver(uint16_t imu_id, const std::string& interface_t 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 { + can_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId { return frame.can_id & 0x7F; }); } else { @@ -30,12 +30,16 @@ HipnucIMUDriver::~HipnucIMUDriver() { } } -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) 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 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,19 @@ std::vector HipnucIMUDriver::get_lin_acc() { float HipnucIMUDriver::get_temperature() { std::shared_lock lock(imu_mutex_); return sensor_data_.temperature; +} + +std::vector HipnucIMUDriver::get_euler() { + std::shared_lock lock(imu_mutex_); + return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw}; +} + +std::vector HipnucIMUDriver::get_mag() { + std::shared_lock lock(imu_mutex_); + return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z}; +} + +uint64_t HipnucIMUDriver::get_timestamp() { + std::shared_lock lock(imu_mutex_); + return (uint64_t)sensor_data_.timestamp_ms * 1000ULL; } \ No newline at end of file diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp index b334ae6..603eed1 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp @@ -26,11 +26,14 @@ 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 get_ang_vel() override; std::vector get_quat() override; std::vector get_lin_acc() override; + std::vector get_mag() override; + std::vector get_euler() override; + uint64_t get_timestamp() override; float get_temperature() override; private: diff --git a/src/drivers/mct7123/CMakeLists.txt b/src/drivers/mct7123/CMakeLists.txt new file mode 100644 index 0000000..fa5daac --- /dev/null +++ b/src/drivers/mct7123/CMakeLists.txt @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: GPL-3.0 +add_library(mct7123_imu STATIC + mct7123_dec.c + mct7123_imu_driver.cpp +) +target_include_directories(mct7123_imu + PUBLIC + $ + $ + $ + $ + $ +) +target_link_libraries(mct7123_imu PUBLIC ${PUBLIC_DEPENDENCIES} imu_protocol) diff --git a/src/drivers/mct7123/mct7123_dec.c b/src/drivers/mct7123/mct7123_dec.c new file mode 100644 index 0000000..576c34a --- /dev/null +++ b/src/drivers/mct7123/mct7123_dec.c @@ -0,0 +1,162 @@ +/* + * SPDX-License-Identifier: GPL-3.0 + * + * MCT MD7123 byte-level UART protocol decoder. + */ + +#include "mct7123_dec.h" +#include +#include + +/* CRC16-CCITT: poly 0x1021, init 0xFFFF */ +static uint16_t crc16_ccitt(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 = crc16_ccitt(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) +{ + 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); +} + +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) +{ + 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); + } +} diff --git a/src/drivers/mct7123/mct7123_dec.h b/src/drivers/mct7123/mct7123_dec.h new file mode 100644 index 0000000..46c6e6b --- /dev/null +++ b/src/drivers/mct7123/mct7123_dec.h @@ -0,0 +1,67 @@ +/* + * 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 +#include + +#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); + +/** 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); + +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); + +#ifdef __cplusplus +} +#endif + +#endif /* __MCT7123_DEC_H__ */ diff --git a/src/drivers/mct7123/mct7123_imu_driver.cpp b/src/drivers/mct7123/mct7123_imu_driver.cpp new file mode 100644 index 0000000..a833a4d --- /dev/null +++ b/src/drivers/mct7123/mct7123_imu_driver.cpp @@ -0,0 +1,180 @@ +#include "mct7123_imu_driver.hpp" +#include +#include + +Mct7123IMUDriver::Mct7123IMUDriver(uint16_t imu_id, const std::string& interface_type, + const std::string& interface, const int baudrate) + : IMUDriver(), interface_type_(interface_type), interface_(interface) +{ + imu_id_ = imu_id; + memset(&raw_, 0, sizeof(raw_)); + memset(&sensor_data_, 0, sizeof(sensor_data_)); + + if (interface_type_ == "serial") { + baudrate_ = baudrate; + serial_ = IMUSerialPort::open(interface_, baudrate_); + IMUSerialPort::SerialCbkFunc cb = std::bind( + &Mct7123IMUDriver::serial_rx_cbk, this, + std::placeholders::_1, std::placeholders::_2); + serial_->set_serial_callback(cb); + } else if (interface_type_ == "canfd") { + can_ = IMUSocketCAN::get_instance(interface_); + CanCbkFunc can_cb = std::bind(&Mct7123IMUDriver::can_rx_cbk, this, std::placeholders::_1); + can_->add_can_callback(can_cb, imu_id_); + can_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId { + return frame.can_id & 0x7F; + }); + } else { + throw std::runtime_error("MCT7123 driver supports SERIAL and CANFD interfaces"); + } +} + +Mct7123IMUDriver::~Mct7123IMUDriver() +{ + if (serial_) serial_->close(); + if (can_) can_->remove_can_callback(imu_id_); +} + +void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) +{ + std::unique_lock 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; + mct7123_parse_imu(raw_.payload, &gx, &gy, &gz, + &ax, &ay, &az, &mx, &my, &mz, &t, &ts_us); + 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); + break; + } + case MCT7123_MSG_ATT_DATA: { + float roll, pitch, yaw, qw, qx, qy, qz, t; + uint32_t fusion_status; + uint64_t ts_us; + mct7123_parse_att(raw_.payload, &roll, &pitch, &yaw, + &qw, &qx, &qy, &qz, &t, + nullptr, &fusion_status, &ts_us); + 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); + break; + } + default: + break; + } + } + } +} + +void Mct7123IMUDriver::can_rx_cbk(const canfd_frame& rx_frame) +{ + if (rx_frame.len < 64) return; + + std::unique_lock lock(imu_mutex_); + + uint32_t id = rx_frame.can_id & CAN_SFF_MASK; + const uint8_t *payload = rx_frame.data; + + switch (id) { + case 0x181: { + float gx, gy, gz, ax, ay, az, mx, my, mz, t; + uint64_t ts_us; + mct7123_parse_imu(payload, &gx, &gy, &gz, + &ax, &ay, &az, &mx, &my, &mz, &t, &ts_us); + 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); + break; + } + case 0x182: { + float roll, pitch, yaw, qw, qx, qy, qz, t; + uint32_t fusion_status; + uint64_t ts_us; + mct7123_parse_att(payload, &roll, &pitch, &yaw, + &qw, &qx, &qy, &qz, &t, + nullptr, &fusion_status, &ts_us); + 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); + break; + } + default: + break; + } +} + +std::vector Mct7123IMUDriver::get_ang_vel() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.gyr_x, sensor_data_.gyr_y, sensor_data_.gyr_z}; +} + +std::vector Mct7123IMUDriver::get_quat() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.quat_w, sensor_data_.quat_x, + sensor_data_.quat_y, sensor_data_.quat_z}; +} + +std::vector Mct7123IMUDriver::get_lin_acc() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.acc_x, sensor_data_.acc_y, sensor_data_.acc_z}; +} + +std::vector Mct7123IMUDriver::get_mag() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z}; +} + +std::vector Mct7123IMUDriver::get_euler() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw}; +} + +uint64_t Mct7123IMUDriver::get_timestamp() +{ + std::shared_lock lock(imu_mutex_); + return (uint64_t)sensor_data_.timestamp_ms * 1000ULL; +} + +float Mct7123IMUDriver::get_temperature() +{ + std::shared_lock lock(imu_mutex_); + return sensor_data_.temperature; +} diff --git a/src/drivers/mct7123/mct7123_imu_driver.hpp b/src/drivers/mct7123/mct7123_imu_driver.hpp new file mode 100644 index 0000000..70ef7c9 --- /dev/null +++ b/src/drivers/mct7123/mct7123_imu_driver.hpp @@ -0,0 +1,47 @@ +#pragma once + +extern "C" { +#include "mct7123_dec.h" +} + +#include +#include +#include + +#include "imu_driver.hpp" +#include "protocol/can/socket_can.hpp" +#include "protocol/serial/serial_port.hpp" +#include "drivers/hipnuc/hipnuc_can_common.h" + +#ifndef DEG_TO_RAD +#define DEG_TO_RAD (0.01745329f) +#endif + +class Mct7123IMUDriver : public IMUDriver { + public: + Mct7123IMUDriver(uint16_t imu_id, const std::string& interface_type, + const std::string& interface, const int baudrate=0); + ~Mct7123IMUDriver(); + + void serial_rx_cbk(const uint8_t* data, size_t length); + void can_rx_cbk(const canfd_frame& rx_frame); + + std::vector get_ang_vel() override; + std::vector get_quat() override; + std::vector get_lin_acc() override; + std::vector get_mag() override; + std::vector get_euler() override; + uint64_t get_timestamp() override; + float get_temperature() override; + + private: + int baudrate_; + std::string interface_type_; + std::string interface_; + mutable std::shared_mutex imu_mutex_; + std::shared_ptr serial_; + std::shared_ptr can_; + + mct7123_raw_t raw_; + can_sensor_data_t sensor_data_; +}; diff --git a/src/imu_driver.cpp b/src/imu_driver.cpp index 2dc3a7d..ecd5949 100644 --- a/src/imu_driver.cpp +++ b/src/imu_driver.cpp @@ -11,11 +11,14 @@ #include "imu_driver.hpp" #include "drivers/hipnuc/hipnuc_imu_driver.hpp" +#include "drivers/mct7123/mct7123_imu_driver.hpp" std::shared_ptr 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(imu_id, interface_type, interface, baudrate); + } else if (imu_type == "MCT7123") { + return std::make_shared(imu_id, interface_type, interface, baudrate); } 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..a19ce41 100644 --- a/src/protocol/can/socket_can.cpp +++ b/src/protocol/can/socket_can.cpp @@ -27,6 +27,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 +68,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 +90,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; diff --git a/src/protocol/can/socket_can.hpp b/src/protocol/can/socket_can.hpp index 9ca85c8..171faf5 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 +#include #include #include #include @@ -33,10 +36,10 @@ constexpr const int INIT_FD = -1; constexpr const int TIMEOUT_SEC = 0; constexpr const int TIMEOUT_USEC = 1000; -using CanCbkFunc = std::function; +using CanCbkFunc = std::function; using CanCbkId = uint16_t; using CanCbkMap = std::unordered_map; -using CanCbkKeyExtractor = std::function; +using CanCbkKeyExtractor = std::function; class IMUSocketCAN { private: @@ -51,7 +54,7 @@ class IMUSocketCAN { std::thread receiver_thread_; CanCbkMap can_callback_list_; std::mutex can_callback_mutex_; - CanCbkKeyExtractor key_extractor_ = [](const can_frame &frame) -> CanCbkId { + CanCbkKeyExtractor key_extractor_ = [](const canfd_frame &frame) -> CanCbkId { return static_cast(frame.can_id); }; 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..e242cbf 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 @@ -15,19 +16,36 @@ namespace py = pybind11; PYBIND11_MODULE(imu_py, m) { - m.doc() = "IMU Driver Python SDK"; + m.doc() = "IMU Driver Python SDK"; py::class_>(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"); } diff --git a/test_imu.py b/test_imu.py new file mode 100644 index 0000000..d3c20c7 --- /dev/null +++ b/test_imu.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +"""IMU test reader — MCT7123 / HIPNUC, serial / CAN / CANFD. + +Usage: + python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 + python3 test_imu.py MCT7123 canfd can0 + python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 + python3 test_imu.py HIPNUC can can0 + +Options: -d SEC (0=forever) -i SEC -q -b BUILD_DIR +""" +import sys, os, time, argparse + +# Silence spdlog noise on stderr +sys.stdout.flush() +os.dup2(os.open(os.devnull, os.O_WRONLY), 2) + +RAD2DEG = 57.29578 + +def main(): + ap = argparse.ArgumentParser(description='IMU test reader') + ap.add_argument('type', choices=['MCT7123', 'HIPNUC']) + ap.add_argument('interface', choices=['serial', 'can', 'canfd']) + ap.add_argument('device') + ap.add_argument('baudrate', type=int, nargs='?', default=0) + ap.add_argument('-d', '--duration', type=float, default=0, + help='seconds (0=forever, Ctrl+C to stop)') + ap.add_argument('-i', '--interval', type=float, default=0.5) + ap.add_argument('-q', '--quiet', action='store_true') + ap.add_argument('-b', '--build-dir', default=None) + args = ap.parse_args() + + sys.path.insert(0, args.build_dir or + os.path.join(os.path.dirname(__file__) or '.', 'build')) + try: + import imu_py + except ImportError: + sys.exit('ERROR: imu_py not found. Build first.') + + forever = (args.duration <= 0) + print(f'[{args.type}] {args.interface}:{args.device} ' + f'{"∞" if forever else args.duration}s', end=' ', flush=True) + try: + imu = imu_py.IMUDriver.create_imu(1, args.interface, args.device, + args.type, args.baudrate) + except RuntimeError as e: + sys.exit(f'FAIL {e}') + time.sleep(0.3) + print('OK\n') + + start = time.time() + last = 0 + cnt = 0 + + try: + while forever or time.time() - start < args.duration: + 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() + ts = imu.get_timestamp() + cnt += 1 + now = time.time() + if not args.quiet and now - last >= args.interval: + el = now - start + print(f'── {el:6.1f}s ts {ts/1000:8.0f}ms') + print(f' GyrX {g[0]*RAD2DEG:7.2f} GyrY {g[1]*RAD2DEG:7.2f} GyrZ {g[2]*RAD2DEG:7.2f} °/s') + print(f' AccX {a[0]:7.3f} AccY {a[1]:7.3f} AccZ {a[2]:7.3f} m/s²') + print(f' MagX {m[0]:7.1f} MagY {m[1]:7.1f} MagZ {m[2]:7.1f} uT') + print(f' R {e[0]:7.2f} P {e[1]:7.2f} Y {e[2]:7.2f} °') + print(f' Qw {q[0]:7.3f} Qx {q[1]:7.3f} Qy {q[2]:7.3f} Qz {q[3]:7.3f} Temp {t:5.1f}°C') + last = now + time.sleep(0.0005) + except KeyboardInterrupt: + pass + + elapsed = time.time() - start + print(f'\n{args.type} {args.interface} {elapsed:.1f}s {cnt} reads {cnt/elapsed:.0f} Hz ', end='') + print('OK' if cnt > 0 else 'FAIL') + +if __name__ == '__main__': + main() From 34ce57eda2e39c81a5ce849ddc96508bcf594326 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 16:03:04 +0800 Subject: [PATCH 02/17] =?UTF-8?q?=E5=AE=8C=E5=96=84=20CAN=20=E9=80=9A?= =?UTF-8?q?=E4=BF=A1=E4=B8=8E=20MCT7123=20=E9=85=8D=E7=BD=AE=E5=B8=A7?= =?UTF-8?q?=E5=A4=84=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 为 SocketCAN 增加发送能力，并公开 CRC16 校验接口供配置流程复用。 补充 MCT7123 配置帧解析，同时统一串口与 CAN FD 模式下的回调和数据处理行为。 --- README.md | 97 +++++++++++++--------- src/drivers/mct7123/mct7123_dec.c | 18 +++- src/drivers/mct7123/mct7123_dec.h | 9 ++ src/drivers/mct7123/mct7123_imu_driver.cpp | 20 ++++- src/protocol/can/socket_can.cpp | 10 +++ src/protocol/can/socket_can.hpp | 1 + 6 files changed, 115 insertions(+), 40 deletions(-) diff --git a/README.md b/README.md index e7b4582..da8c062 100644 --- a/README.md +++ b/README.md @@ -6,70 +6,79 @@ RPO 双足机器人 IMU 驱动库 — C++ / Python 绑定 | 类型 | 接口 | 协议 | |------|------|------| -| **HIPNUC** (HI226/HI229 等) | serial, can | HiPNUC 私有协议 (5A A5 帧头) + CAN J1939 | | **MCT7123** (MD7123) | serial, canfd | EB 90 固定 64B 帧 / CANFD 64B 一体帧 | +| **HIPNUC** (HI226/HI229 等) | serial, can | HiPNUC 私有协议 (5A A5 帧头) + CAN J1939 | -## Python 测试脚本 +## 构建 ```bash -# MCT7123 串口 (默认 921600bps) -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 +mkdir build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release +make -j$(nproc) +``` -# MCT7123 CANFD -python3 test_imu.py MCT7123 canfd can0 +也可以通过 colcon 在 ROS2 workspace 中构建：`source /opt/ros/humble/setup.bash && colcon build`。 -# 仅打摘要，安静模式 -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 5 -q +## 快速上手 -# 无限循环，Ctrl+C 退出 +```bash +# 连接 IMU 后直接运行测试脚本 python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 0 -# HIPNUC 串口 -python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 +# 安静模式, 只打印速率摘要 +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -q -# HIPNUC 经典 CAN -python3 test_imu.py HIPNUC can can0 +# HIPNUC +python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 ``` -参数: - +输出示例： ``` -python3 test_imu.py -h - type MCT7123 | HIPNUC - interface 接口: serial | can | canfd - device 设备路径 (/dev/ttyUSB0, can0 等) - baudrate 波特率 (serial 必须; can/canfd 忽略) - -d SECONDS 运行时长 (0=无限循环, Ctrl+C 退出) - -i SECONDS 打印间隔 (默认 0.5s) - -q 安静模式, 只打印摘要 +── 1.0s ts 5678ms + GyrX 0.03 GyrY -0.02 GyrZ 0.01 °/s + AccX 0.015 AccY -0.021 AccZ 9.813 m/s² + MagX 0.0 MagY 0.0 MagZ 0.0 uT + R -0.15 P 0.08 Y -20.01 ° + Qw 0.985 Qx -0.001 Qy 0.001 Qz -0.174 Temp 35.6°C ``` +| 参数 | 说明 | +|------|------| +| `type` | `MCT7123` / `HIPNUC` | +| `interface` | `serial` / `can` / `canfd` | +| `-d 0` | 无限循环 (Ctrl+C 退出) | +| `-q` | 只打印摘要 | + ## Python API +所有方法线程安全, 可通过 `help(imu_py.IMUDriver)` 查看行内文档。 + +| 方法 | 返回 | 说明 | +|------|------|------| +| `get_ang_vel()` | `[x, y, z]` rad/s | 角速度 | +| `get_lin_acc()` | `[x, y, z]` m/s² | 线加速度 | +| `get_mag()` | `[x, y, z]` uT | 磁场强度 (9 轴模式) | +| `get_quat()` | `[w, x, y, z]` | 姿态四元数 | +| `get_euler()` | `[roll, pitch, yaw]` ° | 欧拉角 (硬件直接输出) | +| `get_timestamp()` | `int` us | IMU 内部时钟 | +| `get_temperature()` | `float` °C | 传感器温度 | + ```python import imu_py -# 创建 IMU 实例 imu = imu_py.IMUDriver.create_imu( - imu_id=1, # 节点 ID (CAN 模式用于多设备路由) + imu_id=1, interface_type="serial", # "serial" | "can" | "canfd" - interface="/dev/ttyUSB0", # 设备路径 + interface="/dev/ttyUSB0", imu_type="MCT7123", # "MCT7123" | "HIPNUC" baudrate=921600 # serial 必须, can/canfd 忽略 ) -# 读取数据 (线程安全) -gyr = imu.get_ang_vel() # [x, y, z] rad/s 角速度 -acc = imu.get_lin_acc() # [x, y, z] m/s² 线加速度 -mag = imu.get_mag() # [x, y, z] uT 磁场强度 (9 轴模式) -quat = imu.get_quat() # [w, x, y, z] 姿态四元数 +gyr = imu.get_ang_vel() # [x, y, z] rad/s +acc = imu.get_lin_acc() # [x, y, z] m/s² euler = imu.get_euler() # [roll, pitch, yaw] 度 -ts = imu.get_timestamp() # us 系统时间戳 -temp = imu.get_temperature() # °C 传感器温度 ``` -> `help(imu_py.IMUDriver)` 查看完整文档。 - ## C++ API ```cpp @@ -86,7 +95,21 @@ uint64_t ts = imu->get_timestamp(); // us float temp = imu->get_temperature(); // °C ``` -## MCT7123 坐标系说明 +## CAN / CANFD + +同一条 CAN 总线可以同时挂载 HIPNUC (经典 CAN) 和 MCT7123 (CANFD)，各自按帧长度分流。 + +```python +# HIPNUC 经典 CAN (≤8 字节) +imu = imu_py.IMUDriver.create_imu(0, "can", "can0", "HIPNUC") + +# MCT7123 CANFD (64 字节) +imu = imu_py.IMUDriver.create_imu(1, "canfd", "can0", "MCT7123") +``` + +C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧，用于配置/命令下发。 + +## MCT7123 坐标系 MD7123 使用**坐标系 9**（手册默认值）： @@ -94,4 +117,4 @@ MD7123 使用**坐标系 9**（手册默认值）： - 标签 -Uy → Y 正 - 标签 +Ux → Z 正 -机器人安装：正面朝右，接插件朝天。静止时 AccZ ≈ -9.81 m/s²（重力）。 +机器人安装：正面朝右，接插件朝天。静止时 AccZ ≈ -9.81 m/s²。 diff --git a/src/drivers/mct7123/mct7123_dec.c b/src/drivers/mct7123/mct7123_dec.c index 576c34a..667ecb9 100644 --- a/src/drivers/mct7123/mct7123_dec.c +++ b/src/drivers/mct7123/mct7123_dec.c @@ -9,7 +9,7 @@ #include /* CRC16-CCITT: poly 0x1021, init 0xFFFF */ -static uint16_t crc16_ccitt(const uint8_t *data, size_t len) +uint16_t mct7123_crc16(const uint8_t *data, size_t len) { uint16_t crc = 0xFFFF; for (size_t i = 0; i < len; i++) { @@ -103,7 +103,7 @@ int mct7123_input(mct7123_raw_t *raw, uint8_t data) /* verify CRC16 over payload[0..61], CRC at payload[62..63] */ uint16_t crc_expected = le16(&raw->payload[62]); - uint16_t crc_calc = crc16_ccitt(raw->payload, 62); + uint16_t crc_calc = mct7123_crc16(raw->payload, 62); if (crc_calc != crc_expected) { raw->idx = 0; return -1; @@ -160,3 +160,17 @@ void mct7123_parse_att(const uint8_t payload[64], memcpy(running_status, payload + 42, 6); } } + +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 index 46c6e6b..da87d16 100644 --- a/src/drivers/mct7123/mct7123_dec.h +++ b/src/drivers/mct7123/mct7123_dec.h @@ -47,6 +47,9 @@ typedef struct { */ 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, @@ -60,6 +63,12 @@ void mct7123_parse_att(const uint8_t payload[64], float *temp, uint8_t *running_status, uint32_t *fusion_status, uint64_t *systimer_us); +/** 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 diff --git a/src/drivers/mct7123/mct7123_imu_driver.cpp b/src/drivers/mct7123/mct7123_imu_driver.cpp index a833a4d..703ac9a 100644 --- a/src/drivers/mct7123/mct7123_imu_driver.cpp +++ b/src/drivers/mct7123/mct7123_imu_driver.cpp @@ -78,6 +78,11 @@ void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) sensor_data_.timestamp_ms = (uint32_t)(ts_us / 1000ULL); break; } + case MCT7123_MSG_CFG_DATA: { + mct7123_parse_cfg(raw_.payload, nullptr, nullptr, nullptr, nullptr, + nullptr, nullptr, nullptr); + break; + } default: break; } @@ -87,12 +92,20 @@ void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) void Mct7123IMUDriver::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 lock(imu_mutex_); uint32_t id = rx_frame.can_id & CAN_SFF_MASK; - const uint8_t *payload = rx_frame.data; switch (id) { case 0x181: { @@ -131,6 +144,11 @@ void Mct7123IMUDriver::can_rx_cbk(const canfd_frame& rx_frame) sensor_data_.timestamp_ms = (uint32_t)(ts_us / 1000ULL); break; } + case 0x183: { + mct7123_parse_cfg(payload, nullptr, nullptr, nullptr, nullptr, + nullptr, nullptr, nullptr); + break; + } default: break; } diff --git a/src/protocol/can/socket_can.cpp b/src/protocol/can/socket_can.cpp index a19ce41..7affda8 100644 --- a/src/protocol/can/socket_can.cpp +++ b/src/protocol/can/socket_can.cpp @@ -152,3 +152,13 @@ void IMUSocketCAN::set_key_extractor(CanCbkKeyExtractor extractor) { std::lock_guard 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 171faf5..9771be5 100644 --- a/src/protocol/can/socket_can.hpp +++ b/src/protocol/can/socket_can.hpp @@ -90,4 +90,5 @@ class IMUSocketCAN { void remove_can_callback(const CanCbkId id); void clear_can_callbacks(); void set_key_extractor(CanCbkKeyExtractor extractor); + int send(const canfd_frame &frame); }; From 4fca4687b7f337cff93845d9567583f2feaf9817 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 16:09:32 +0800 Subject: [PATCH 03/17] =?UTF-8?q?=E4=BC=98=E5=8C=96=20IMU=20=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E6=95=B0=E6=8D=AE=E7=9A=84=E6=98=BE=E7=A4=BA=E6=96=B9?= =?UTF-8?q?=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 将测试结果调整为对齐的表格格式，便于持续观察各项传感器数据，并同步更新 README 输出示例。 默认仅显示欧拉角和温度，同时保留 -a 选项用于展示陀螺仪、加速度、磁场和四元数等完整数据。 --- README.md | 15 ++++++++------- test_imu.py | 45 +++++++++++++++++++++++++++++++++------------ 2 files changed, 41 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index da8c062..6d6e6dd 100644 --- a/README.md +++ b/README.md @@ -32,16 +32,17 @@ python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -q python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 ``` -输出示例： +输出示例（默认仅欧拉角+温度）： ``` -── 1.0s ts 5678ms - GyrX 0.03 GyrY -0.02 GyrZ 0.01 °/s - AccX 0.015 AccY -0.021 AccZ 9.813 m/s² - MagX 0.0 MagY 0.0 MagZ 0.0 uT - R -0.15 P 0.08 Y -20.01 ° - Qw 0.985 Qx -0.001 Qy 0.001 Qz -0.174 Temp 35.6°C + 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 ``` +`-a` 显示完整表格（陀螺/加表/磁力计/四元数+欧拉角）。 + | 参数 | 说明 | |------|------| | `type` | `MCT7123` / `HIPNUC` | diff --git a/test_imu.py b/test_imu.py index d3c20c7..0dd1410 100644 --- a/test_imu.py +++ b/test_imu.py @@ -6,12 +6,9 @@ python3 test_imu.py MCT7123 canfd can0 python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 python3 test_imu.py HIPNUC can can0 - -Options: -d SEC (0=forever) -i SEC -q -b BUILD_DIR """ import sys, os, time, argparse -# Silence spdlog noise on stderr sys.stdout.flush() os.dup2(os.open(os.devnull, os.O_WRONLY), 2) @@ -24,9 +21,12 @@ def main(): ap.add_argument('device') ap.add_argument('baudrate', type=int, nargs='?', default=0) ap.add_argument('-d', '--duration', type=float, default=0, - help='seconds (0=forever, Ctrl+C to stop)') + help='seconds (0=forever)') ap.add_argument('-i', '--interval', type=float, default=0.5) - ap.add_argument('-q', '--quiet', action='store_true') + ap.add_argument('-a', '--all', action='store_true', + help='show all: Gyr/Acc/Mag/Quat + Euler') + ap.add_argument('-q', '--quiet', action='store_true', + help='summary only') ap.add_argument('-b', '--build-dir', default=None) args = ap.parse_args() @@ -48,6 +48,25 @@ def main(): 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}') + 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 @@ -60,17 +79,19 @@ def main(): q = imu.get_quat() e = imu.get_euler() t = imu.get_temperature() - ts = imu.get_timestamp() cnt += 1 now = time.time() if not args.quiet and now - last >= args.interval: el = now - start - print(f'── {el:6.1f}s ts {ts/1000:8.0f}ms') - print(f' GyrX {g[0]*RAD2DEG:7.2f} GyrY {g[1]*RAD2DEG:7.2f} GyrZ {g[2]*RAD2DEG:7.2f} °/s') - print(f' AccX {a[0]:7.3f} AccY {a[1]:7.3f} AccZ {a[2]:7.3f} m/s²') - print(f' MagX {m[0]:7.1f} MagY {m[1]:7.1f} MagZ {m[2]:7.1f} uT') - print(f' R {e[0]:7.2f} P {e[1]:7.2f} Y {e[2]:7.2f} °') - print(f' Qw {q[0]:7.3f} Qx {q[1]:7.3f} Qy {q[2]:7.3f} Qz {q[3]:7.3f} Temp {t:5.1f}°C') + 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}') + 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: From 6fb7b3c4a6ff39d02a4fc20413c85ca85104f99a Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 16:17:28 +0800 Subject: [PATCH 04/17] =?UTF-8?q?=E8=A1=A5=E9=BD=90=E5=B8=A7=E8=AE=A1?= =?UTF-8?q?=E6=95=B0=E5=99=A8=E4=B8=8E=E8=BF=90=E8=A1=8C=E7=8A=B6=E6=80=81?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E9=93=BE=E8=B7=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 在解码器、驱动基类和 Python 绑定中补充 Cycle 计数器与 Running_Status 状态存储。 同步扩展测试脚本，使帧连续性和设备运行状态能够从底层解析一直传递到测试输出。 --- include/imu_driver.hpp | 4 +++- src/drivers/hipnuc/hipnuc_can_common.h | 2 ++ src/drivers/hipnuc/hipnuc_imu_driver.cpp | 4 ++++ src/drivers/hipnuc/hipnuc_imu_driver.hpp | 1 + src/drivers/mct7123/mct7123_dec.c | 6 ++++-- src/drivers/mct7123/mct7123_dec.h | 4 ++-- src/drivers/mct7123/mct7123_imu_driver.cpp | 23 +++++++++++++++++----- src/drivers/mct7123/mct7123_imu_driver.hpp | 1 + src/pybind_module.cpp | 4 +++- test_imu.py | 5 +++-- 10 files changed, 41 insertions(+), 13 deletions(-) diff --git a/include/imu_driver.hpp b/include/imu_driver.hpp index 5afb213..30c1233 100644 --- a/include/imu_driver.hpp +++ b/include/imu_driver.hpp @@ -47,6 +47,7 @@ class IMUDriver { virtual std::vector get_euler() { return euler_; } virtual uint64_t get_timestamp() { return timestamp_; } virtual float get_temperature() { return temperature_; } + virtual uint8_t get_cycle() { return cycle_; } protected: std::shared_ptr logger_; @@ -58,5 +59,6 @@ class IMUDriver { std::vector mag_{0.f, 0.f, 0.f}; // x, y, z uT std::vector euler_{0.f, 0.f, 0.f}; // roll, pitch, yaw (deg) uint64_t timestamp_{0}; // us - float temperature_{0.f}; // temperature + float temperature_{0.f}; + uint8_t cycle_{0}; // frame cycle counter }; \ No newline at end of file diff --git a/src/drivers/hipnuc/hipnuc_can_common.h b/src/drivers/hipnuc/hipnuc_can_common.h index 5f853a6..7acd1b8 100644 --- a/src/drivers/hipnuc/hipnuc_can_common.h +++ b/src/drivers/hipnuc/hipnuc_can_common.h @@ -79,6 +79,8 @@ typedef struct { uint8_t nv_pos; uint8_t nv_heading; uint8_t ins_status; + uint8_t running_status[6]; + uint8_t cycle; } can_sensor_data_t; typedef struct { diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp index 779abaa..2208dda 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.cpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.cpp @@ -122,4 +122,8 @@ std::vector HipnucIMUDriver::get_mag() { uint64_t HipnucIMUDriver::get_timestamp() { std::shared_lock lock(imu_mutex_); return (uint64_t)sensor_data_.timestamp_ms * 1000ULL; +} + +uint8_t HipnucIMUDriver::get_cycle() { + return 0; /* HiPNUC protocol has no cycle counter */ } \ No newline at end of file diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp index 603eed1..1c0ee04 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp @@ -35,6 +35,7 @@ class HipnucIMUDriver : public IMUDriver { std::vector get_euler() override; uint64_t get_timestamp() override; float get_temperature() override; + uint8_t get_cycle() override; private: int baudrate_; diff --git a/src/drivers/mct7123/mct7123_dec.c b/src/drivers/mct7123/mct7123_dec.c index 667ecb9..ceee0a9 100644 --- a/src/drivers/mct7123/mct7123_dec.c +++ b/src/drivers/mct7123/mct7123_dec.c @@ -120,7 +120,7 @@ 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) + float *temp, uint64_t *systimer_us, uint8_t *cycle) { if (systimer_us) memcpy(systimer_us, payload, 8); *gyr_x = r4(payload + 8); @@ -133,13 +133,14 @@ void mct7123_parse_imu(const uint8_t payload[64], *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) + uint64_t *systimer_us, uint8_t *cycle) { if (systimer_us) memcpy(systimer_us, payload, 8); *roll = r4(payload + 8); @@ -159,6 +160,7 @@ void mct7123_parse_att(const uint8_t payload[64], if (running_status) { memcpy(running_status, payload + 42, 6); } + if (cycle) *cycle = payload[61]; } void mct7123_parse_cfg(const uint8_t payload[64], diff --git a/src/drivers/mct7123/mct7123_dec.h b/src/drivers/mct7123/mct7123_dec.h index da87d16..809a91f 100644 --- a/src/drivers/mct7123/mct7123_dec.h +++ b/src/drivers/mct7123/mct7123_dec.h @@ -55,13 +55,13 @@ 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); + 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); + 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], diff --git a/src/drivers/mct7123/mct7123_imu_driver.cpp b/src/drivers/mct7123/mct7123_imu_driver.cpp index 703ac9a..faf8c3d 100644 --- a/src/drivers/mct7123/mct7123_imu_driver.cpp +++ b/src/drivers/mct7123/mct7123_imu_driver.cpp @@ -1,5 +1,4 @@ #include "mct7123_imu_driver.hpp" -#include #include Mct7123IMUDriver::Mct7123IMUDriver(uint16_t imu_id, const std::string& interface_type, @@ -45,8 +44,9 @@ void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) 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); + &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; @@ -58,15 +58,17 @@ void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) 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, - nullptr, &fusion_status, &ts_us); + sensor_data_.running_status, &fusion_status, &ts_us, &cyc); sensor_data_.quat_w = qw; sensor_data_.quat_x = qx; sensor_data_.quat_y = qy; @@ -76,6 +78,7 @@ void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) 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: { @@ -111,8 +114,9 @@ void Mct7123IMUDriver::can_rx_cbk(const canfd_frame& rx_frame) 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); + &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; @@ -124,15 +128,17 @@ void Mct7123IMUDriver::can_rx_cbk(const canfd_frame& rx_frame) 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, - nullptr, &fusion_status, &ts_us); + sensor_data_.running_status, &fusion_status, &ts_us, &cyc); sensor_data_.quat_w = qw; sensor_data_.quat_x = qx; sensor_data_.quat_y = qy; @@ -142,6 +148,7 @@ void Mct7123IMUDriver::can_rx_cbk(const canfd_frame& rx_frame) 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: { @@ -196,3 +203,9 @@ float Mct7123IMUDriver::get_temperature() std::shared_lock lock(imu_mutex_); return sensor_data_.temperature; } + +uint8_t Mct7123IMUDriver::get_cycle() +{ + std::shared_lock lock(imu_mutex_); + return sensor_data_.cycle; +} diff --git a/src/drivers/mct7123/mct7123_imu_driver.hpp b/src/drivers/mct7123/mct7123_imu_driver.hpp index 70ef7c9..7522db7 100644 --- a/src/drivers/mct7123/mct7123_imu_driver.hpp +++ b/src/drivers/mct7123/mct7123_imu_driver.hpp @@ -33,6 +33,7 @@ class Mct7123IMUDriver : public IMUDriver { std::vector get_euler() override; uint64_t get_timestamp() override; float get_temperature() override; + uint8_t get_cycle() override; private: int baudrate_; diff --git a/src/pybind_module.cpp b/src/pybind_module.cpp index e242cbf..9758ea0 100644 --- a/src/pybind_module.cpp +++ b/src/pybind_module.cpp @@ -47,5 +47,7 @@ PYBIND11_MODULE(imu_py, m) { .def("get_timestamp", &IMUDriver::get_timestamp, "系统时间戳 us — IMU 内部时钟 (开机累积)") .def("get_temperature", &IMUDriver::get_temperature, - "传感器温度 °C"); + "传感器温度 °C") + .def("get_cycle", &IMUDriver::get_cycle, + "帧计数器 0-255 — 检测丢帧"); } diff --git a/test_imu.py b/test_imu.py index 0dd1410..4ed8e69 100644 --- a/test_imu.py +++ b/test_imu.py @@ -54,7 +54,7 @@ def main(): 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}') + 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} ' @@ -79,6 +79,7 @@ def main(): 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: @@ -89,7 +90,7 @@ def main(): 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}') + 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 From deab1a1cf24c01733b624130b0d407cd57b0a402 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 16:22:29 +0800 Subject: [PATCH 05/17] =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=8D=8F=E8=AE=AE?= =?UTF-8?q?=E8=AF=B4=E6=98=8E=E4=B8=8E=E8=AE=BE=E5=A4=87=E5=9D=90=E6=A0=87?= =?UTF-8?q?=E7=B3=BB=E6=96=87=E6=A1=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 补充 CRC、CAN FD、Cycle 计数器和 MCT7123 坐标系等关键协议说明。 修正文档中的接口描述与示例，并记录相关功能验证结果，便于部署和排查问题。 --- README.md | 80 ++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 53 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index 6d6e6dd..18cc0bb 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,10 @@ RPO 双足机器人 IMU 驱动库 — C++ / Python 绑定 ## 支持的 IMU -| 类型 | 接口 | 协议 | -|------|------|------| -| **MCT7123** (MD7123) | serial, canfd | EB 90 固定 64B 帧 / CANFD 64B 一体帧 | -| **HIPNUC** (HI226/HI229 等) | serial, can | HiPNUC 私有协议 (5A A5 帧头) + CAN J1939 | +| 类型 | 接口 | 帧格式 | CRC | +|------|------|--------|-----| +| **MCT7123** (MD7123) | serial, canfd | EB 90 固定 64B + ED | CCITT (poly 0x1021) | +| **HIPNUC** (HI226/HI229 等) | serial, can | 5A A5 变长 + CAN J1939 | 无 | ## 构建 @@ -17,22 +17,27 @@ cmake .. -DCMAKE_BUILD_TYPE=Release make -j$(nproc) ``` -也可以通过 colcon 在 ROS2 workspace 中构建：`source /opt/ros/humble/setup.bash && colcon build`。 - ## 快速上手 ```bash -# 连接 IMU 后直接运行测试脚本 -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 0 +# 连接 IMU 后直接运行 (默认无限循环) +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 + +# 仅欧拉角 + 温度 (默认输出) +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 5 -# 安静模式, 只打印速率摘要 -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -q +# 全部数据: 陀螺/加表/磁力计/四元数/欧拉角/温度/Cycle +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -a + +# 安静模式, 只打速率 +python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 5 -q # HIPNUC python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 ``` -输出示例（默认仅欧拉角+温度）： +输出示例 (默认仅欧拉角+温度): + ``` Time R P Y Temp (s) (°) (°) (°) (°C) @@ -41,43 +46,55 @@ python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 1.00 -0.16 -2.07 -11.47 36.2 ``` -`-a` 显示完整表格（陀螺/加表/磁力计/四元数+欧拉角）。 +`-a` 完整输出 (含 Cycle 计数器): + +``` + Time GyrX GyrY GyrZ AccX AccY AccZ MagX MagY MagZ R P Y Qw Qx Qy Qz Temp Cyc + (s) (°/s) (°/s) (°/s) (m/s²) (m/s²) (m/s²) (uT) (uT) (uT) (°) (°) (°) (°C) +─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────── + 0.00 0.00 -0.01 0.04 -0.029 -0.048 9.810 0.0 0.0 0.0 -0.22 -0.12 -51.32 0.901 -0.002 -0.000 -0.433 35.4 215 + 1.50 0.03 0.03 -0.00 -0.027 -0.041 9.811 0.0 0.0 0.0 -0.22 -0.12 -51.32 0.901 -0.002 -0.000 -0.433 35.4 2 +``` | 参数 | 说明 | |------|------| | `type` | `MCT7123` / `HIPNUC` | | `interface` | `serial` / `can` / `canfd` | -| `-d 0` | 无限循环 (Ctrl+C 退出) | -| `-q` | 只打印摘要 | +| `-d SECONDS` | 运行时长, 0=无限 (Ctrl+C 退出) | +| `-a` | 显示全部 20 列 (Gyr/Acc/Mag/Quat/Euler/Temp/Cycle) | +| `-q` | 安静模式, 只打速率摘要 | ## Python API -所有方法线程安全, 可通过 `help(imu_py.IMUDriver)` 查看行内文档。 +所有方法线程安全 (`shared_mutex`), 可通过 `help(imu_py.IMUDriver)` 查看行内文档。 | 方法 | 返回 | 说明 | |------|------|------| -| `get_ang_vel()` | `[x, y, z]` rad/s | 角速度 | +| `get_ang_vel()` | `[x, y, z]` rad/s | 角速度 (陀螺仪) | | `get_lin_acc()` | `[x, y, z]` m/s² | 线加速度 | | `get_mag()` | `[x, y, z]` uT | 磁场强度 (9 轴模式) | | `get_quat()` | `[w, x, y, z]` | 姿态四元数 | | `get_euler()` | `[roll, pitch, yaw]` ° | 欧拉角 (硬件直接输出) | -| `get_timestamp()` | `int` us | IMU 内部时钟 | +| `get_timestamp()` | `int` us | IMU 内部时钟 (开机累积) | | `get_temperature()` | `float` °C | 传感器温度 | +| `get_cycle()` | `int` 0-255 | 帧计数器 (检测丢帧, MCT7123 专用) | ```python import imu_py imu = imu_py.IMUDriver.create_imu( - imu_id=1, + imu_id=1, # 节点 ID (CAN 模式下回调路由) interface_type="serial", # "serial" | "can" | "canfd" - interface="/dev/ttyUSB0", + interface="/dev/ttyUSB0", # 设备路径 imu_type="MCT7123", # "MCT7123" | "HIPNUC" - baudrate=921600 # serial 必须, can/canfd 忽略 + baudrate=921600 # serial 必须; can/canfd 忽略 ) +# 读取数据 gyr = imu.get_ang_vel() # [x, y, z] rad/s acc = imu.get_lin_acc() # [x, y, z] m/s² euler = imu.get_euler() # [roll, pitch, yaw] 度 +cycle = imu.get_cycle() # 帧计数 (丢帧检测) ``` ## C++ API @@ -94,21 +111,22 @@ std::vector quat = imu->get_quat(); // [w,x,y,z] std::vector euler = imu->get_euler(); // [roll,pitch,yaw] 度 uint64_t ts = imu->get_timestamp(); // us float temp = imu->get_temperature(); // °C +uint8_t cycle = imu->get_cycle(); // 0-255 ``` ## CAN / CANFD -同一条 CAN 总线可以同时挂载 HIPNUC (经典 CAN) 和 MCT7123 (CANFD)，各自按帧长度分流。 +同一条 CAN 总线可同时挂载 HIPNUC (经典 CAN, ≤8 字节) 和 MCT7123 (CANFD, 64 字节), 各自按帧长度分流。 ```python -# HIPNUC 经典 CAN (≤8 字节) -imu = imu_py.IMUDriver.create_imu(0, "can", "can0", "HIPNUC") +# HIPNUC 经典 CAN (J1939) +imu_h = imu_py.IMUDriver.create_imu(0, "can", "can0", "HIPNUC") -# MCT7123 CANFD (64 字节) -imu = imu_py.IMUDriver.create_imu(1, "canfd", "can0", "MCT7123") +# MCT7123 CANFD (0x181=IMU, 0x182=Att, 0x183=Config) +imu_m = imu_py.IMUDriver.create_imu(1, "canfd", "can0", "MCT7123") ``` -C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧，用于配置/命令下发。 +C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧。 ## MCT7123 坐标系 @@ -118,4 +136,12 @@ MD7123 使用**坐标系 9**（手册默认值）： - 标签 -Uy → Y 正 - 标签 +Ux → Z 正 -机器人安装：正面朝右，接插件朝天。静止时 AccZ ≈ -9.81 m/s²。 +机器人安装：正面朝右，接插件朝天。静止时 AccZ ≈ +9.81 m/s² (重力反作用力，Z 轴朝上)。 + +## 协议参考 + +| 帧 ID | 类型 | 频率 | +|--------|------|------| +| 0x81 | IMU 原始数据 (陀螺/加表/磁力计) | 100 Hz | +| 0x82 | 姿态数据 (欧拉角/四元数) | 100 Hz | +| 0x83 | 配置交互 | 事件触发 | From 0dc5049235ba7ba79f913e8b163a65ef73e92fcc Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 16:45:43 +0800 Subject: [PATCH 06/17] =?UTF-8?q?=E7=AE=80=E5=8C=96=20IMU=20=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E8=84=9A=E6=9C=AC=E7=9A=84=E5=8F=82=E6=95=B0=E4=B8=8E?= =?UTF-8?q?=E8=AE=BE=E5=A4=87=E9=80=89=E6=8B=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 提供中文帮助、自动波特率和可见的错误提示，并将型号简化为主要启动参数。 增加 --list、--can 和 --serial 选项及常见串口自动扫描，同时更新 README 中的使用示例。 --- README.md | 36 ++++++++++------- test_imu.py | 110 +++++++++++++++++++++++++++++++++++++++------------- 2 files changed, 105 insertions(+), 41 deletions(-) diff --git a/README.md b/README.md index 18cc0bb..107f4fe 100644 --- a/README.md +++ b/README.md @@ -20,20 +20,24 @@ make -j$(nproc) ## 快速上手 ```bash -# 连接 IMU 后直接运行 (默认无限循环) -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 +# 只需指定型号，自动探测串口 +python3 test_imu.py MCT7123 -# 仅欧拉角 + 温度 (默认输出) -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 5 +# 指定 CAN 口 +python3 test_imu.py MCT7123 --can can0 +python3 test_imu.py HIPNUC --can can0 -# 全部数据: 陀螺/加表/磁力计/四元数/欧拉角/温度/Cycle -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -a +# 指定串口 +python3 test_imu.py MCT7123 --serial /dev/ttyUSB1 -# 安静模式, 只打速率 -python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 -d 5 -q +# 列出可用设备 +python3 test_imu.py --list -# HIPNUC -python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 +# 全部数据 +python3 test_imu.py MCT7123 -a + +# 安静模式 +python3 test_imu.py MCT7123 -d 5 -q ``` 输出示例 (默认仅欧拉角+温度): @@ -58,11 +62,13 @@ python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 | 参数 | 说明 | |------|------| -| `type` | `MCT7123` / `HIPNUC` | -| `interface` | `serial` / `can` / `canfd` | -| `-d SECONDS` | 运行时长, 0=无限 (Ctrl+C 退出) | -| `-a` | 显示全部 20 列 (Gyr/Acc/Mag/Quat/Euler/Temp/Cycle) | -| `-q` | 安静模式, 只打速率摘要 | +| `TYPE` | 唯一必填: `MCT7123` / `HIPNUC` | +| `--serial DEV` | 指定串口 (默认自动扫描 /dev/ttyUSB*) | +| `--can IFACE` | 指定 CAN/CANFD 口 (如 can0) | +| `-d SEC` | 运行时长, 0=无限 (Ctrl+C 退出) | +| `-a` | 显示全部 20 列 | +| `-q` | 安静模式, 只打速率 | +| `--list` | 列出可用串口设备 | ## Python API diff --git a/test_imu.py b/test_imu.py index 4ed8e69..77cedcb 100644 --- a/test_imu.py +++ b/test_imu.py @@ -1,50 +1,108 @@ #!/usr/bin/env python3 -"""IMU test reader — MCT7123 / HIPNUC, serial / CAN / CANFD. +"""IMU 测试工具 — 只需指定型号，自动探测接口。 Usage: - python3 test_imu.py MCT7123 serial /dev/ttyUSB0 921600 - python3 test_imu.py MCT7123 canfd can0 - python3 test_imu.py HIPNUC serial /dev/ttyUSB0 115200 - python3 test_imu.py HIPNUC can can0 + python3 test_imu.py MCT7123 # 自动探测 + python3 test_imu.py MCT7123 --can can0 # 指定 CAN 口 + python3 test_imu.py MCT7123 --serial /dev/ttyUSB0 # 指定串口 + python3 test_imu.py --list # 列出设备 """ -import sys, os, time, argparse - -sys.stdout.flush() -os.dup2(os.open(os.devnull, os.O_WRONLY), 2) +import sys, os, time, argparse, glob RAD2DEG = 57.29578 +DEFAULTS = { + 'MCT7123': {'baudrate': 921600, 'iface': 'serial'}, + 'HIPNUC': {'baudrate': 115200, 'iface': 'serial'}, +} + +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 main(): - ap = argparse.ArgumentParser(description='IMU test reader') - ap.add_argument('type', choices=['MCT7123', 'HIPNUC']) - ap.add_argument('interface', choices=['serial', 'can', 'canfd']) - ap.add_argument('device') - ap.add_argument('baudrate', type=int, nargs='?', default=0) + ap = argparse.ArgumentParser( + description='IMU 测试工具 — MCT7123 / HIPNUC', + epilog='示例: python3 test_imu.py MCT7123', + formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument('type', nargs='?', default=None, metavar='TYPE', + help='IMU 型号 (MCT7123 / HIPNUC; 不提供则提示用法)') + ap.add_argument('--serial', metavar='DEV', help='指定串口设备') + ap.add_argument('--can', metavar='IFACE', help='指定 CAN/CANFD 接口 (如 can0)') + ap.add_argument('--list', action='store_true', help='列出可用设备') ap.add_argument('-d', '--duration', type=float, default=0, - help='seconds (0=forever)') - ap.add_argument('-i', '--interval', type=float, default=0.5) + help='运行时长 秒 (0=无限循环)') + ap.add_argument('-i', '--interval', type=float, default=0.5, + help='打印间隔 秒 (默认 0.5)') ap.add_argument('-a', '--all', action='store_true', - help='show all: Gyr/Acc/Mag/Quat + Euler') + help='显示全部: Gyr/Acc/Mag/Quat/Euler/Temp/Cycle') ap.add_argument('-q', '--quiet', action='store_true', - help='summary only') - ap.add_argument('-b', '--build-dir', default=None) + help='安静模式, 只打印速率') + ap.add_argument('-b', '--build-dir', default=None, + help='build 目录 (默认 ./build)') args = ap.parse_args() + if args.list: + serials = scan_serial() + print('串口:', ', '.join(serials) if serials else '(无)') + print('CAN: (使用 --can can0 指定)') + return + + if args.type is None: + print('请指定 IMU 型号: MCT7123 或 HIPNUC', file=sys.stderr) + print('示例: python3 test_imu.py MCT7123', 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) + + # Determine interface + device + baudrate + imu_type = args.type + baudrate = DEFAULTS[imu_type]['baudrate'] + iface_type = 'canfd' if (imu_type == 'MCT7123' and args.can) else \ + 'can' if (imu_type == 'HIPNUC' and args.can) else \ + 'serial' + + if args.serial: + device = args.serial + elif args.can: + device = args.can + else: + # Auto-detect: try serial first + serials = scan_serial() + if serials: + device = serials[0] + iface_type = 'serial' + else: + print('未找到串口设备，请用 --can 或 --serial 指定', file=sys.stderr) + sys.exit(1) + + # Silence C++ spdlog noise + sys.stdout.flush() + os.dup2(os.open(os.devnull, os.O_WRONLY), 2) + sys.path.insert(0, args.build_dir or os.path.join(os.path.dirname(__file__) or '.', 'build')) try: import imu_py except ImportError: - sys.exit('ERROR: imu_py not found. Build first.') + print('ERROR: imu_py not found. 请先编译', flush=True) + os._exit(1) forever = (args.duration <= 0) - print(f'[{args.type}] {args.interface}:{args.device} ' - f'{"∞" if forever else args.duration}s', end=' ', flush=True) + print(f'[{args.type}] {iface_type}:{device} {baudrate}bps ' + f'{"∞" if forever else f"{args.duration}s"}', end=' ', flush=True) try: - imu = imu_py.IMUDriver.create_imu(1, args.interface, args.device, - args.type, args.baudrate) + imu = imu_py.IMUDriver.create_imu(1, iface_type, device, + args.type, baudrate) except RuntimeError as e: - sys.exit(f'FAIL {e}') + print(f'FAIL: {e}', flush=True) + os._exit(1) time.sleep(0.3) print('OK\n') @@ -99,7 +157,7 @@ def main(): pass elapsed = time.time() - start - print(f'\n{args.type} {args.interface} {elapsed:.1f}s {cnt} reads {cnt/elapsed:.0f} Hz ', end='') + 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__': From b93b4f694021f79883ff7fb31119fc2905dabb35 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 18:09:30 +0800 Subject: [PATCH 07/17] =?UTF-8?q?=E9=87=8D=E6=9E=84=E4=BC=A0=E6=84=9F?= =?UTF-8?q?=E5=99=A8=E6=95=B0=E6=8D=AE=E7=BB=93=E6=9E=84=E4=BB=A5=E8=A7=A3?= =?UTF-8?q?=E9=99=A4=E9=A9=B1=E5=8A=A8=E8=80=A6=E5=90=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增兼容 C 与 C++ 的 imu_sensor_data_t 公共数据结构，替代原有 can_sensor_data_t。 清理 MCT7123 对 HiPNUC 头文件的跨驱动依赖，并统一更新解析器、驱动和公共接口中的类型引用。 --- include/imu_driver.hpp | 29 +++++------- include/imu_sensor_data.h | 46 +++++++++++++++++++ src/drivers/hipnuc/canopen_parser.c | 14 +++--- src/drivers/hipnuc/canopen_parser.h | 3 +- src/drivers/hipnuc/hipnuc_can_common.c | 2 +- src/drivers/hipnuc/hipnuc_can_common.h | 53 ++-------------------- src/drivers/hipnuc/hipnuc_imu_driver.hpp | 2 +- src/drivers/hipnuc/hipnuc_j1939_parser.c | 28 ++++++------ src/drivers/hipnuc/hipnuc_j1939_parser.h | 3 +- src/drivers/mct7123/mct7123_imu_driver.hpp | 3 +- 10 files changed, 88 insertions(+), 95 deletions(-) create mode 100644 include/imu_sensor_data.h diff --git a/include/imu_driver.hpp b/include/imu_driver.hpp index 30c1233..dbf3d7f 100644 --- a/include/imu_driver.hpp +++ b/include/imu_driver.hpp @@ -22,6 +22,8 @@ #include #include +#include "imu_sensor_data.h" + class IMUDriver { public: @@ -40,25 +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 get_ang_vel() { return ang_vel_; } - virtual std::vector get_quat() { return quat_; } - virtual std::vector get_lin_acc() { return lin_acc_; } - virtual std::vector get_mag() { return mag_; } - virtual std::vector get_euler() { return euler_; } - virtual uint64_t get_timestamp() { return timestamp_; } - virtual float get_temperature() { return temperature_; } - virtual uint8_t get_cycle() { return cycle_; } + virtual std::vector get_ang_vel() { return {0.f, 0.f, 0.f}; } + virtual std::vector get_quat() { return {0.f, 0.f, 0.f, 0.f}; } + virtual std::vector get_lin_acc() { return {0.f, 0.f, 0.f}; } + virtual std::vector get_mag() { return {0.f, 0.f, 0.f}; } + virtual std::vector 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 logger_; - uint16_t imu_id_; - - std::vector quat_{0.f, 0.f, 0.f, 0.f}; // w, x, y, z - std::vector ang_vel_{0.f, 0.f, 0.f}; // x, y, z rad/s - std::vector lin_acc_{0.f, 0.f, 0.f}; // x, y, z m/s² - std::vector mag_{0.f, 0.f, 0.f}; // x, y, z uT - std::vector euler_{0.f, 0.f, 0.f}; // roll, pitch, yaw (deg) - uint64_t timestamp_{0}; // us - float temperature_{0.f}; - uint8_t cycle_{0}; // frame cycle counter + 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 + +#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/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 7acd1b8..d3ffb0e 100644 --- a/src/drivers/hipnuc/hipnuc_can_common.h +++ b/src/drivers/hipnuc/hipnuc_can_common.h @@ -6,6 +6,8 @@ #include #include +#include "imu_sensor_data.h" + typedef struct { uint32_t can_id; uint8_t can_dlc; @@ -34,61 +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; - uint8_t running_status[6]; - uint8_t cycle; -} 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.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp index 1c0ee04..bbfc6ba 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp @@ -44,6 +44,6 @@ class HipnucIMUDriver : public IMUDriver { mutable std::shared_mutex imu_mutex_; std::shared_ptr can_; std::shared_ptr serial_; - can_sensor_data_t sensor_data_; + imu_sensor_data_t sensor_data_; hipnuc_raw_t raw_; }; \ No newline at end of file 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/mct7123_imu_driver.hpp b/src/drivers/mct7123/mct7123_imu_driver.hpp index 7522db7..95471ca 100644 --- a/src/drivers/mct7123/mct7123_imu_driver.hpp +++ b/src/drivers/mct7123/mct7123_imu_driver.hpp @@ -11,7 +11,6 @@ extern "C" { #include "imu_driver.hpp" #include "protocol/can/socket_can.hpp" #include "protocol/serial/serial_port.hpp" -#include "drivers/hipnuc/hipnuc_can_common.h" #ifndef DEG_TO_RAD #define DEG_TO_RAD (0.01745329f) @@ -44,5 +43,5 @@ class Mct7123IMUDriver : public IMUDriver { std::shared_ptr can_; mct7123_raw_t raw_; - can_sensor_data_t sensor_data_; + imu_sensor_data_t sensor_data_; }; From 8239d6fa49a227f079ef379879c52d344f43c531 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Wed, 29 Jul 2026 18:15:35 +0800 Subject: [PATCH 08/17] =?UTF-8?q?=E6=8B=86=E5=88=86=20MCT7123=20=E4=B8=B2?= =?UTF-8?q?=E5=8F=A3=E4=B8=8E=20CAN=20=E9=A9=B1=E5=8A=A8=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 将原有 MCT7123 驱动拆分为独立的串口子类和 CAN 子类，各自管理连接与接收回调。 工厂根据接口类型创建对应实现，同时继续复用公共解码器，降低不同传输方式之间的耦合。 --- src/drivers/mct7123/CMakeLists.txt | 3 +- src/drivers/mct7123/mct7123_can_driver.cpp | 138 ++++++++++++ src/drivers/mct7123/mct7123_can_driver.hpp | 40 ++++ src/drivers/mct7123/mct7123_imu_driver.cpp | 211 ------------------ src/drivers/mct7123/mct7123_serial_driver.cpp | 129 +++++++++++ ...u_driver.hpp => mct7123_serial_driver.hpp} | 11 +- src/imu_driver.cpp | 11 +- 7 files changed, 321 insertions(+), 222 deletions(-) create mode 100644 src/drivers/mct7123/mct7123_can_driver.cpp create mode 100644 src/drivers/mct7123/mct7123_can_driver.hpp delete mode 100644 src/drivers/mct7123/mct7123_imu_driver.cpp create mode 100644 src/drivers/mct7123/mct7123_serial_driver.cpp rename src/drivers/mct7123/{mct7123_imu_driver.hpp => mct7123_serial_driver.hpp} (69%) diff --git a/src/drivers/mct7123/CMakeLists.txt b/src/drivers/mct7123/CMakeLists.txt index fa5daac..72d7135 100644 --- a/src/drivers/mct7123/CMakeLists.txt +++ b/src/drivers/mct7123/CMakeLists.txt @@ -1,7 +1,8 @@ # SPDX-License-Identifier: GPL-3.0 add_library(mct7123_imu STATIC mct7123_dec.c - mct7123_imu_driver.cpp + mct7123_serial_driver.cpp + mct7123_can_driver.cpp ) target_include_directories(mct7123_imu PUBLIC diff --git a/src/drivers/mct7123/mct7123_can_driver.cpp b/src/drivers/mct7123/mct7123_can_driver.cpp new file mode 100644 index 0000000..35e223e --- /dev/null +++ b/src/drivers/mct7123/mct7123_can_driver.cpp @@ -0,0 +1,138 @@ +#include "mct7123_can_driver.hpp" +#include + +Mct7123CanDriver::Mct7123CanDriver(uint16_t imu_id, const std::string& interface) + : IMUDriver(), interface_(interface) +{ + 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_->add_can_callback(can_cb, imu_id_); + can_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId { + return frame.can_id & 0x7F; + }); +} + +Mct7123CanDriver::~Mct7123CanDriver() +{ + if (can_) can_->remove_can_callback(imu_id_); +} + +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 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 Mct7123CanDriver::get_ang_vel() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.gyr_x, sensor_data_.gyr_y, sensor_data_.gyr_z}; +} + +std::vector Mct7123CanDriver::get_quat() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.quat_w, sensor_data_.quat_x, + sensor_data_.quat_y, sensor_data_.quat_z}; +} + +std::vector Mct7123CanDriver::get_lin_acc() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.acc_x, sensor_data_.acc_y, sensor_data_.acc_z}; +} + +std::vector Mct7123CanDriver::get_mag() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z}; +} + +std::vector Mct7123CanDriver::get_euler() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw}; +} + +uint64_t Mct7123CanDriver::get_timestamp() +{ + std::shared_lock lock(imu_mutex_); + return (uint64_t)sensor_data_.timestamp_ms * 1000ULL; +} + +float Mct7123CanDriver::get_temperature() +{ + std::shared_lock lock(imu_mutex_); + return sensor_data_.temperature; +} + +uint8_t Mct7123CanDriver::get_cycle() +{ + std::shared_lock 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..25ce4ab --- /dev/null +++ b/src/drivers/mct7123/mct7123_can_driver.hpp @@ -0,0 +1,40 @@ +#pragma once + +extern "C" { +#include "mct7123_dec.h" +} + +#include +#include +#include + +#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 get_ang_vel() override; + std::vector get_quat() override; + std::vector get_lin_acc() override; + std::vector get_mag() override; + std::vector get_euler() override; + uint64_t get_timestamp() override; + float get_temperature() override; + uint8_t get_cycle() override; + + private: + std::string interface_; + mutable std::shared_mutex imu_mutex_; + std::shared_ptr can_; + + imu_sensor_data_t sensor_data_; +}; diff --git a/src/drivers/mct7123/mct7123_imu_driver.cpp b/src/drivers/mct7123/mct7123_imu_driver.cpp deleted file mode 100644 index faf8c3d..0000000 --- a/src/drivers/mct7123/mct7123_imu_driver.cpp +++ /dev/null @@ -1,211 +0,0 @@ -#include "mct7123_imu_driver.hpp" -#include - -Mct7123IMUDriver::Mct7123IMUDriver(uint16_t imu_id, const std::string& interface_type, - const std::string& interface, const int baudrate) - : IMUDriver(), interface_type_(interface_type), interface_(interface) -{ - imu_id_ = imu_id; - memset(&raw_, 0, sizeof(raw_)); - memset(&sensor_data_, 0, sizeof(sensor_data_)); - - if (interface_type_ == "serial") { - baudrate_ = baudrate; - serial_ = IMUSerialPort::open(interface_, baudrate_); - IMUSerialPort::SerialCbkFunc cb = std::bind( - &Mct7123IMUDriver::serial_rx_cbk, this, - std::placeholders::_1, std::placeholders::_2); - serial_->set_serial_callback(cb); - } else if (interface_type_ == "canfd") { - can_ = IMUSocketCAN::get_instance(interface_); - CanCbkFunc can_cb = std::bind(&Mct7123IMUDriver::can_rx_cbk, this, std::placeholders::_1); - can_->add_can_callback(can_cb, imu_id_); - can_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId { - return frame.can_id & 0x7F; - }); - } else { - throw std::runtime_error("MCT7123 driver supports SERIAL and CANFD interfaces"); - } -} - -Mct7123IMUDriver::~Mct7123IMUDriver() -{ - if (serial_) serial_->close(); - if (can_) can_->remove_can_callback(imu_id_); -} - -void Mct7123IMUDriver::serial_rx_cbk(const uint8_t* data, size_t length) -{ - std::unique_lock 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; - } - } - } -} - -void Mct7123IMUDriver::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 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 Mct7123IMUDriver::get_ang_vel() -{ - std::shared_lock lock(imu_mutex_); - return {sensor_data_.gyr_x, sensor_data_.gyr_y, sensor_data_.gyr_z}; -} - -std::vector Mct7123IMUDriver::get_quat() -{ - std::shared_lock lock(imu_mutex_); - return {sensor_data_.quat_w, sensor_data_.quat_x, - sensor_data_.quat_y, sensor_data_.quat_z}; -} - -std::vector Mct7123IMUDriver::get_lin_acc() -{ - std::shared_lock lock(imu_mutex_); - return {sensor_data_.acc_x, sensor_data_.acc_y, sensor_data_.acc_z}; -} - -std::vector Mct7123IMUDriver::get_mag() -{ - std::shared_lock lock(imu_mutex_); - return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z}; -} - -std::vector Mct7123IMUDriver::get_euler() -{ - std::shared_lock lock(imu_mutex_); - return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw}; -} - -uint64_t Mct7123IMUDriver::get_timestamp() -{ - std::shared_lock lock(imu_mutex_); - return (uint64_t)sensor_data_.timestamp_ms * 1000ULL; -} - -float Mct7123IMUDriver::get_temperature() -{ - std::shared_lock lock(imu_mutex_); - return sensor_data_.temperature; -} - -uint8_t Mct7123IMUDriver::get_cycle() -{ - std::shared_lock lock(imu_mutex_); - return sensor_data_.cycle; -} 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 + +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 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 Mct7123SerialDriver::get_ang_vel() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.gyr_x, sensor_data_.gyr_y, sensor_data_.gyr_z}; +} + +std::vector Mct7123SerialDriver::get_quat() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.quat_w, sensor_data_.quat_x, + sensor_data_.quat_y, sensor_data_.quat_z}; +} + +std::vector Mct7123SerialDriver::get_lin_acc() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.acc_x, sensor_data_.acc_y, sensor_data_.acc_z}; +} + +std::vector Mct7123SerialDriver::get_mag() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.mag_x, sensor_data_.mag_y, sensor_data_.mag_z}; +} + +std::vector Mct7123SerialDriver::get_euler() +{ + std::shared_lock lock(imu_mutex_); + return {sensor_data_.roll, sensor_data_.pitch, sensor_data_.imu_yaw}; +} + +uint64_t Mct7123SerialDriver::get_timestamp() +{ + std::shared_lock lock(imu_mutex_); + return (uint64_t)sensor_data_.timestamp_ms * 1000ULL; +} + +float Mct7123SerialDriver::get_temperature() +{ + std::shared_lock lock(imu_mutex_); + return sensor_data_.temperature; +} + +uint8_t Mct7123SerialDriver::get_cycle() +{ + std::shared_lock lock(imu_mutex_); + return sensor_data_.cycle; +} diff --git a/src/drivers/mct7123/mct7123_imu_driver.hpp b/src/drivers/mct7123/mct7123_serial_driver.hpp similarity index 69% rename from src/drivers/mct7123/mct7123_imu_driver.hpp rename to src/drivers/mct7123/mct7123_serial_driver.hpp index 95471ca..154040d 100644 --- a/src/drivers/mct7123/mct7123_imu_driver.hpp +++ b/src/drivers/mct7123/mct7123_serial_driver.hpp @@ -9,21 +9,18 @@ extern "C" { #include #include "imu_driver.hpp" -#include "protocol/can/socket_can.hpp" #include "protocol/serial/serial_port.hpp" #ifndef DEG_TO_RAD #define DEG_TO_RAD (0.01745329f) #endif -class Mct7123IMUDriver : public IMUDriver { +class Mct7123SerialDriver : public IMUDriver { public: - Mct7123IMUDriver(uint16_t imu_id, const std::string& interface_type, - const std::string& interface, const int baudrate=0); - ~Mct7123IMUDriver(); + Mct7123SerialDriver(uint16_t imu_id, const std::string& interface, int baudrate); + ~Mct7123SerialDriver(); void serial_rx_cbk(const uint8_t* data, size_t length); - void can_rx_cbk(const canfd_frame& rx_frame); std::vector get_ang_vel() override; std::vector get_quat() override; @@ -36,11 +33,9 @@ class Mct7123IMUDriver : public IMUDriver { private: int baudrate_; - std::string interface_type_; std::string interface_; mutable std::shared_mutex imu_mutex_; std::shared_ptr serial_; - std::shared_ptr can_; mct7123_raw_t raw_; imu_sensor_data_t sensor_data_; diff --git a/src/imu_driver.cpp b/src/imu_driver.cpp index ecd5949..e12594f 100644 --- a/src/imu_driver.cpp +++ b/src/imu_driver.cpp @@ -11,14 +11,21 @@ #include "imu_driver.hpp" #include "drivers/hipnuc/hipnuc_imu_driver.hpp" -#include "drivers/mct7123/mct7123_imu_driver.hpp" +#include "drivers/mct7123/mct7123_serial_driver.hpp" +#include "drivers/mct7123/mct7123_can_driver.hpp" std::shared_ptr 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(imu_id, interface_type, interface, baudrate); } else if (imu_type == "MCT7123") { - return std::make_shared(imu_id, interface_type, interface, baudrate); + if (interface_type == "serial") { + return std::make_shared(imu_id, interface, baudrate); + } else if (interface_type == "canfd") { + return std::make_shared(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"); } From d825fcb383229851c05d6059112d5e27e6aea15a Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Thu, 30 Jul 2026 14:50:05 +0800 Subject: [PATCH 09/17] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=20MCT7123=20CAN=20FD?= =?UTF-8?q?=20=E5=A4=9A=E7=B1=BB=E5=B8=A7=E7=9A=84=E5=9B=9E=E8=B0=83?= =?UTF-8?q?=E8=B7=AF=E7=94=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 扩展 CAN FD 帧标识匹配范围，使原始数据、姿态数据和配置响应均能进入对应驱动。 避免仅处理单一帧类型时造成姿态或配置数据缺失。 --- src/drivers/mct7123/mct7123_can_driver.cpp | 16 +++++++++++++--- src/drivers/mct7123/mct7123_can_driver.hpp | 4 ++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/drivers/mct7123/mct7123_can_driver.cpp b/src/drivers/mct7123/mct7123_can_driver.cpp index 35e223e..f81f4b3 100644 --- a/src/drivers/mct7123/mct7123_can_driver.cpp +++ b/src/drivers/mct7123/mct7123_can_driver.cpp @@ -2,22 +2,32 @@ #include Mct7123CanDriver::Mct7123CanDriver(uint16_t imu_id, const std::string& interface) - : IMUDriver(), interface_(interface) + : IMUDriver(), interface_(interface), + callback_ids_{imu_id, static_cast(imu_id + 1), + static_cast(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_->add_can_callback(can_cb, imu_id_); 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 (CanCbkId callback_id : callback_ids_) { + can_->add_can_callback(can_cb, callback_id); + } } Mct7123CanDriver::~Mct7123CanDriver() { - if (can_) can_->remove_can_callback(imu_id_); + if (can_) { + for (CanCbkId callback_id : callback_ids_) { + can_->remove_can_callback(callback_id); + } + } } void Mct7123CanDriver::can_rx_cbk(const canfd_frame& rx_frame) diff --git a/src/drivers/mct7123/mct7123_can_driver.hpp b/src/drivers/mct7123/mct7123_can_driver.hpp index 25ce4ab..a088787 100644 --- a/src/drivers/mct7123/mct7123_can_driver.hpp +++ b/src/drivers/mct7123/mct7123_can_driver.hpp @@ -5,6 +5,7 @@ extern "C" { } #include +#include #include #include @@ -32,7 +33,10 @@ class Mct7123CanDriver : public IMUDriver { uint8_t get_cycle() override; private: + static constexpr size_t kFrameTypeCount = 3; + std::string interface_; + std::array callback_ids_; mutable std::shared_mutex imu_mutex_; std::shared_ptr can_; From 9247b5a0c8c9917c3ac9813e402820939b48c25f Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Thu, 30 Jul 2026 15:34:11 +0800 Subject: [PATCH 10/17] =?UTF-8?q?=E5=AE=8C=E5=96=84=20CAN=20=E5=9B=9E?= =?UTF-8?q?=E8=B0=83=E8=AE=A2=E9=98=85=E7=9A=84=E5=B9=B6=E5=8F=91=E4=B8=8E?= =?UTF-8?q?=E7=94=9F=E5=91=BD=E5=91=A8=E6=9C=9F=E7=AE=A1=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 支持同一路由键注册多个独立订阅者，并允许每个驱动精确注销自身回调。 注销时等待活动回调结束，并使用 CanCallbackSubscription 自动管理生命周期，避免析构并发和遗漏清理。 --- TODO.md | 25 +++++++ src/drivers/hipnuc/hipnuc_imu_driver.cpp | 6 +- src/drivers/hipnuc/hipnuc_imu_driver.hpp | 3 +- src/drivers/mct7123/mct7123_can_driver.cpp | 13 +--- src/drivers/mct7123/mct7123_can_driver.hpp | 1 + src/protocol/can/socket_can.cpp | 81 +++++++++++++++++++--- src/protocol/can/socket_can.hpp | 65 ++++++++++++++++- 7 files changed, 165 insertions(+), 29 deletions(-) create mode 100644 TODO.md diff --git a/TODO.md b/TODO.md new file mode 100644 index 0000000..4e02188 --- /dev/null +++ b/TODO.md @@ -0,0 +1,25 @@ +# TODO + +## CAN 回调机制 + +- [x] 支持同一个 CAN 路由 key 注册多个回调。 +- [x] 注册时返回唯一 token，注销时使用 `id + token` 精确删除订阅，避免不同 IMU 或驱动互相覆盖、误删回调。 +- [x] 将手工管理的 `id + token` 升级为 RAII Subscription，由订阅对象在析构时自动注销。 +- [x] 注销订阅时禁止新回调进入并等待已经开始执行的回调结束，避免驱动析构与接收线程并发导致 use-after-free。 +- [ ] 明确禁止在回调内部同步注销自身，或为该场景增加无死锁的延迟注销机制。 +- [ ] 增加不依赖真实 CAN 设备的回调测试，覆盖同 key 多订阅、独立注销和并发注销。 + +## 时间戳 + +- [ ] MCT7123 直接保存并返回原始 64 位微秒时间戳，避免转换为 32 位毫秒后丢失精度并在约 49.7 天后回绕。 + +## 测试工具 + +- [ ] `test_imu.py` 根据传感器帧或 cycle/timestamp 变化统计接收速率，不再把 getter 调用次数当作传感器帧数。 +- [ ] 未收到有效数据时报告失败，避免设备只成功打开就显示 `OK`。 + +## 构建与发布 + +- [ ] CMake 不要强制启用 `ccache`；仅在检测到命令存在时使用，并允许调用方指定 compiler launcher。 +- [ ] 不要强制覆盖 `CMAKE_BUILD_TYPE`，保证 Debug 等外部构建配置生效。 +- [ ] 检查并补充 ROS/ament 导出的 `mct7123_imu` 目标。 diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp index 2208dda..f54d78c 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.cpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.cpp @@ -13,10 +13,10 @@ 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 canfd_frame &frame) -> CanCbkId { return frame.can_id & 0x7F; }); + can_subscription_ = CanCallbackSubscription(can_, imu_id_, can_callback); } else { throw std::runtime_error("Hipnuc driver only support CAN and SERIAL interface"); } @@ -25,8 +25,6 @@ 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_); } } @@ -126,4 +124,4 @@ uint64_t HipnucIMUDriver::get_timestamp() { uint8_t HipnucIMUDriver::get_cycle() { return 0; /* HiPNUC protocol has no cycle counter */ -} \ No newline at end of file +} diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp index bbfc6ba..ad09728 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp @@ -46,4 +46,5 @@ class HipnucIMUDriver : public IMUDriver { std::shared_ptr serial_; imu_sensor_data_t sensor_data_; hipnuc_raw_t raw_; -}; \ No newline at end of file + CanCallbackSubscription can_subscription_; +}; diff --git a/src/drivers/mct7123/mct7123_can_driver.cpp b/src/drivers/mct7123/mct7123_can_driver.cpp index f81f4b3..64ceda0 100644 --- a/src/drivers/mct7123/mct7123_can_driver.cpp +++ b/src/drivers/mct7123/mct7123_can_driver.cpp @@ -16,19 +16,12 @@ Mct7123CanDriver::Mct7123CanDriver(uint16_t imu_id, const std::string& interface }); // 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 (CanCbkId callback_id : callback_ids_) { - can_->add_can_callback(can_cb, callback_id); + for (size_t i = 0; i < callback_ids_.size(); ++i) { + can_subscriptions_[i] = CanCallbackSubscription(can_, callback_ids_[i], can_cb); } } -Mct7123CanDriver::~Mct7123CanDriver() -{ - if (can_) { - for (CanCbkId callback_id : callback_ids_) { - can_->remove_can_callback(callback_id); - } - } -} +Mct7123CanDriver::~Mct7123CanDriver() = default; void Mct7123CanDriver::can_rx_cbk(const canfd_frame& rx_frame) { diff --git a/src/drivers/mct7123/mct7123_can_driver.hpp b/src/drivers/mct7123/mct7123_can_driver.hpp index a088787..98c34c1 100644 --- a/src/drivers/mct7123/mct7123_can_driver.hpp +++ b/src/drivers/mct7123/mct7123_can_driver.hpp @@ -41,4 +41,5 @@ class Mct7123CanDriver : public IMUDriver { std::shared_ptr can_; imu_sensor_data_t sensor_data_; + std::array can_subscriptions_; }; diff --git a/src/protocol/can/socket_can.cpp b/src/protocol/can/socket_can.cpp index 7affda8..34ef5c4 100644 --- a/src/protocol/can/socket_can.cpp +++ b/src/protocol/can/socket_can.cpp @@ -10,6 +10,8 @@ #include "socket_can.hpp" +#include + std::shared_ptr IMUSocketCAN::logger_ = nullptr; std::unordered_map> IMUSocketCAN::instances_; @@ -101,17 +103,35 @@ void IMUSocketCAN::open(std::string interface) { if (len == 0){ break; } - CanCbkFunc callback_to_run; + std::vector callbacks_to_run; { std::lock_guard 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; } } - if (callback_to_run) { - callback_to_run(rx_frame); + for (const auto &entry : callbacks_to_run) { + { + std::lock_guard 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 lock(entry->mutex); + --entry->running_count; + if (entry->running_count == 0) entry->cv.notify_all(); + } } } } @@ -133,19 +153,58 @@ 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 lock(can_callback_mutex_); - can_callback_list_[id] = callback; + const CanCbkToken token = next_callback_token_++; + auto entry = std::make_shared(); + 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 lock(can_callback_mutex_); - can_callback_list_.erase(id); +void IMUSocketCAN::remove_can_callback(CanCbkId id, CanCbkToken token) { + CanCbkEntryPtr removed_entry; + { + std::lock_guard 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 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 lock(can_callback_mutex_); - can_callback_list_.clear(); + std::vector removed_entries; + { + std::lock_guard 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 lock(entry->mutex); + entry->enabled = false; + entry->cv.wait(lock, [&entry] { return entry->running_count == 0; }); + } } void IMUSocketCAN::set_key_extractor(CanCbkKeyExtractor extractor) { diff --git a/src/protocol/can/socket_can.hpp b/src/protocol/can/socket_can.hpp index 9771be5..a65752e 100644 --- a/src/protocol/can/socket_can.hpp +++ b/src/protocol/can/socket_can.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -38,11 +39,25 @@ constexpr const int TIMEOUT_SEC = 0; constexpr const int TIMEOUT_USEC = 1000; using CanCbkFunc = std::function; using CanCbkId = uint16_t; -using CanCbkMap = std::unordered_map; +using CanCbkToken = uint64_t; +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; +using CanCbkMap = std::unordered_map>; using CanCbkKeyExtractor = std::function; +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 receiving_; @@ -54,6 +69,7 @@ class IMUSocketCAN { std::thread receiver_thread_; CanCbkMap can_callback_list_; std::mutex can_callback_mutex_; + CanCbkToken next_callback_token_{1}; CanCbkKeyExtractor key_extractor_ = [](const canfd_frame &frame) -> CanCbkId { return static_cast(frame.can_id); }; @@ -66,6 +82,9 @@ class IMUSocketCAN { static std::shared_ptr logger_; static std::unordered_map> 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; @@ -86,9 +105,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 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 can_; + CanCbkId id_{0}; + CanCbkToken token_{0}; +}; From a2955924080f0a7dc3f906c9c66f1ff492983ef0 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Thu, 30 Jul 2026 18:21:23 +0800 Subject: [PATCH 11/17] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=BB=98=E8=AE=A4=20CA?= =?UTF-8?q?N=20=E6=8E=A2=E6=B5=8B=E4=B8=8E=E8=8A=82=E7=82=B9=E7=BC=96?= =?UTF-8?q?=E5=8F=B7=E8=A6=86=E7=9B=96=E9=80=89=E9=A1=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 测试脚本在没有发现串口时自动使用对应型号的默认 CAN 接口和节点编号。 新增 --id 参数以覆盖默认节点编号，并同步更新 README 中的调用示例。 --- README.md | 20 +++++++++++--------- test_imu.py | 15 +++++++++++---- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 107f4fe..e4acccc 100644 --- a/README.md +++ b/README.md @@ -20,12 +20,13 @@ make -j$(nproc) ## 快速上手 ```bash -# 只需指定型号，自动探测串口 -python3 test_imu.py MCT7123 +# 只需指定型号，自动探测接口（串口优先，无串口则走默认 CAN） +python3 test_imu.py MCT7123 # → can0, id=1 +python3 test_imu.py HIPNUC # → can0, id=8 -# 指定 CAN 口 -python3 test_imu.py MCT7123 --can can0 -python3 test_imu.py HIPNUC --can can0 +# 指定 CAN 口或 ID +python3 test_imu.py HIPNUC --can can1 +python3 test_imu.py HIPNUC --id 9 # 指定串口 python3 test_imu.py MCT7123 --serial /dev/ttyUSB1 @@ -34,7 +35,7 @@ python3 test_imu.py MCT7123 --serial /dev/ttyUSB1 python3 test_imu.py --list # 全部数据 -python3 test_imu.py MCT7123 -a +python3 test_imu.py HIPNUC -a # 安静模式 python3 test_imu.py MCT7123 -d 5 -q @@ -64,7 +65,8 @@ python3 test_imu.py MCT7123 -d 5 -q |------|------| | `TYPE` | 唯一必填: `MCT7123` / `HIPNUC` | | `--serial DEV` | 指定串口 (默认自动扫描 /dev/ttyUSB*) | -| `--can IFACE` | 指定 CAN/CANFD 口 (如 can0) | +| `--can IFACE` | 指定 CAN/CANFD 口 (无串口时默认 can0) | +| `--id N` | CAN 源地址/节点 ID (默认: HIPNUC=8, MCT7123=1) | | `-d SEC` | 运行时长, 0=无限 (Ctrl+C 退出) | | `-a` | 显示全部 20 列 | | `-q` | 安静模式, 只打速率 | @@ -125,8 +127,8 @@ uint8_t cycle = imu->get_cycle(); // 0-255 同一条 CAN 总线可同时挂载 HIPNUC (经典 CAN, ≤8 字节) 和 MCT7123 (CANFD, 64 字节), 各自按帧长度分流。 ```python -# HIPNUC 经典 CAN (J1939) -imu_h = imu_py.IMUDriver.create_imu(0, "can", "can0", "HIPNUC") +# HIPNUC 经典 CAN (J1939, SA 通常为 8) +imu_h = imu_py.IMUDriver.create_imu(8, "can", "can0", "HIPNUC") # MCT7123 CANFD (0x181=IMU, 0x182=Att, 0x183=Config) imu_m = imu_py.IMUDriver.create_imu(1, "canfd", "can0", "MCT7123") diff --git a/test_imu.py b/test_imu.py index 77cedcb..2373775 100644 --- a/test_imu.py +++ b/test_imu.py @@ -12,8 +12,8 @@ RAD2DEG = 57.29578 DEFAULTS = { - 'MCT7123': {'baudrate': 921600, 'iface': 'serial'}, - 'HIPNUC': {'baudrate': 115200, 'iface': 'serial'}, + 'MCT7123': {'baudrate': 921600, 'id': 1, 'can': 'can0'}, + 'HIPNUC': {'baudrate': 115200, 'id': 8, 'can': 'can0'}, } def scan_serial(): @@ -38,6 +38,8 @@ def main(): 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', @@ -73,15 +75,20 @@ def main(): elif args.can: device = args.can else: - # Auto-detect: try serial first + # Auto-detect: try serial first, fall back to default CAN serials = scan_serial() if serials: device = serials[0] iface_type = 'serial' + elif DEFAULTS[imu_type].get('can'): + device = DEFAULTS[imu_type]['can'] + iface_type = 'canfd' if imu_type == 'MCT7123' else 'can' else: print('未找到串口设备，请用 --can 或 --serial 指定', file=sys.stderr) sys.exit(1) + imu_id = args.id if args.id is not None else DEFAULTS[imu_type]['id'] + # Silence C++ spdlog noise sys.stdout.flush() os.dup2(os.open(os.devnull, os.O_WRONLY), 2) @@ -98,7 +105,7 @@ def main(): print(f'[{args.type}] {iface_type}:{device} {baudrate}bps ' f'{"∞" if forever else f"{args.duration}s"}', end=' ', flush=True) try: - imu = imu_py.IMUDriver.create_imu(1, iface_type, device, + imu = imu_py.IMUDriver.create_imu(imu_id, iface_type, device, args.type, baudrate) except RuntimeError as e: print(f'FAIL: {e}', flush=True) From a7b0960ef0e2ee6fb0158c9bda786d3f850c18a7 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Thu, 30 Jul 2026 19:17:36 +0800 Subject: [PATCH 12/17] =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=B9=B6=E6=A0=A1?= =?UTF-8?q?=E6=AD=A3=20HiPNUC=20J1939=20=E5=88=9D=E5=A7=8B=E5=8C=96?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=E8=AF=B4=E6=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 为初始化命令补充寄存器、参数组、数据周期和系统控制说明，并增加核心数据快速启用示例。 依据设备手册校正配置项对应关系，确保脚本注释、配置值与实际协议一致。 --- init_imu.sh | 94 +++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 87 insertions(+), 7 deletions(-) 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 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 # 复位 From 5115689a4b2a69519dedc116af40028e1c539704 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Fri, 31 Jul 2026 13:51:18 +0800 Subject: [PATCH 13/17] =?UTF-8?q?=E6=95=B4=E7=90=86=20IMU=20=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E8=84=9A=E6=9C=AC=E5=B9=B6=E5=A2=9E=E5=8A=A0=20CANabl?= =?UTF-8?q?e2=20=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 将测试工具移动到 scripts 目录，统一项目脚本的存放位置并更新 README 调用路径。 增加 CANable2 的 SLCAN-FD 连接、CRC 校验和 MCT7123 帧解析能力，同时保持原有串口与 SocketCAN 流程。 --- README.md | 16 ++-- TODO.md | 25 ----- test_imu.py => scripts/test_imu.py | 148 +++++++++++++++++++++++------ 3 files changed, 127 insertions(+), 62 deletions(-) delete mode 100644 TODO.md rename test_imu.py => scripts/test_imu.py (52%) diff --git a/README.md b/README.md index e4acccc..cbe70a1 100644 --- a/README.md +++ b/README.md @@ -21,24 +21,24 @@ make -j$(nproc) ```bash # 只需指定型号，自动探测接口（串口优先，无串口则走默认 CAN） -python3 test_imu.py MCT7123 # → can0, id=1 -python3 test_imu.py HIPNUC # → can0, id=8 +python3 scripts/test_imu.py MCT7123 # → can0, id=1 +python3 scripts/test_imu.py HIPNUC # → can0, id=8 # 指定 CAN 口或 ID -python3 test_imu.py HIPNUC --can can1 -python3 test_imu.py HIPNUC --id 9 +python3 scripts/test_imu.py HIPNUC --can can1 +python3 scripts/test_imu.py HIPNUC --id 9 # 指定串口 -python3 test_imu.py MCT7123 --serial /dev/ttyUSB1 +python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB1 # 列出可用设备 -python3 test_imu.py --list +python3 scripts/test_imu.py --list # 全部数据 -python3 test_imu.py HIPNUC -a +python3 scripts/test_imu.py HIPNUC -a # 安静模式 -python3 test_imu.py MCT7123 -d 5 -q +python3 scripts/test_imu.py MCT7123 -d 5 -q ``` 输出示例 (默认仅欧拉角+温度): diff --git a/TODO.md b/TODO.md deleted file mode 100644 index 4e02188..0000000 --- a/TODO.md +++ /dev/null @@ -1,25 +0,0 @@ -# TODO - -## CAN 回调机制 - -- [x] 支持同一个 CAN 路由 key 注册多个回调。 -- [x] 注册时返回唯一 token，注销时使用 `id + token` 精确删除订阅，避免不同 IMU 或驱动互相覆盖、误删回调。 -- [x] 将手工管理的 `id + token` 升级为 RAII Subscription，由订阅对象在析构时自动注销。 -- [x] 注销订阅时禁止新回调进入并等待已经开始执行的回调结束，避免驱动析构与接收线程并发导致 use-after-free。 -- [ ] 明确禁止在回调内部同步注销自身，或为该场景增加无死锁的延迟注销机制。 -- [ ] 增加不依赖真实 CAN 设备的回调测试，覆盖同 key 多订阅、独立注销和并发注销。 - -## 时间戳 - -- [ ] MCT7123 直接保存并返回原始 64 位微秒时间戳，避免转换为 32 位毫秒后丢失精度并在约 49.7 天后回绕。 - -## 测试工具 - -- [ ] `test_imu.py` 根据传感器帧或 cycle/timestamp 变化统计接收速率，不再把 getter 调用次数当作传感器帧数。 -- [ ] 未收到有效数据时报告失败，避免设备只成功打开就显示 `OK`。 - -## 构建与发布 - -- [ ] CMake 不要强制启用 `ccache`；仅在检测到命令存在时使用，并允许调用方指定 compiler launcher。 -- [ ] 不要强制覆盖 `CMAKE_BUILD_TYPE`，保证 Debug 等外部构建配置生效。 -- [ ] 检查并补充 ROS/ament 导出的 `mct7123_imu` 目标。 diff --git a/test_imu.py b/scripts/test_imu.py similarity index 52% rename from test_imu.py rename to scripts/test_imu.py index 2373775..25c2a5d 100644 --- a/test_imu.py +++ b/scripts/test_imu.py @@ -2,14 +2,16 @@ """IMU 测试工具 — 只需指定型号，自动探测接口。 Usage: - python3 test_imu.py MCT7123 # 自动探测 - python3 test_imu.py MCT7123 --can can0 # 指定 CAN 口 - python3 test_imu.py MCT7123 --serial /dev/ttyUSB0 # 指定串口 - python3 test_imu.py --list # 列出设备 + python3 scripts/test_imu.py MCT7123 # 自动探测 + python3 scripts/test_imu.py MCT7123 --can can0 # 指定 CAN 口 + python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM1 # CANable2 CAN FD + python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 # 指定串口 + python3 scripts/test_imu.py --list # 列出设备 """ -import sys, os, time, argparse, glob +import sys, os, time, argparse, glob, struct RAD2DEG = 57.29578 +DEG2RAD = 1.0 / RAD2DEG DEFAULTS = { 'MCT7123': {'baudrate': 921600, 'id': 1, 'can': 'can0'}, @@ -24,15 +26,91 @@ def scan_serial(): 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 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(' Date: Fri, 31 Jul 2026 14:17:35 +0800 Subject: [PATCH 14/17] =?UTF-8?q?=E6=95=B4=E7=90=86=20IMU=20=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E5=B7=A5=E5=85=B7=E4=B8=8E=E6=9E=84=E5=BB=BA=E8=AF=B4?= =?UTF-8?q?=E6=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 重新编排功能概览、构建、测试工具、API、CAN 与坐标系说明。补充 CANable2、SLCAN-FD、完整命令参数和依赖信息，并统一术语、示例及格式。 --- README.md | 285 ++++++++++++++++++++++++++++++-------------- scripts/test_imu.py | 32 +++-- 2 files changed, 208 insertions(+), 109 deletions(-) diff --git a/README.md b/README.md index cbe70a1..01cdcaf 100644 --- a/README.md +++ b/README.md @@ -1,49 +1,148 @@ # roboparty_imu -RPO 双足机器人 IMU 驱动库 — C++ / Python 绑定 +RoboParty 双足机器人 IMU 驱动库，提供 C++ 接口和 Python 绑定，支持 +MCT7123（MD7123）与 HiPNUC 系列设备。 -## 支持的 IMU +## 功能概览 -| 类型 | 接口 | 帧格式 | CRC | -|------|------|--------|-----| -| **MCT7123** (MD7123) | serial, canfd | EB 90 固定 64B + ED | CCITT (poly 0x1021) | -| **HIPNUC** (HI226/HI229 等) | serial, can | 5A A5 变长 + CAN J1939 | 无 | +| 设备 | 通信接口 | 数据帧 | 校验 | +| --- | --- | --- | --- | +| MCT7123（MD7123） | 串口、CAN FD、CANable2 SLCAN-FD | `EB 90` 开头的固定 64 字节帧 | CRC16-CCITT（多项式 `0x1021`） | +| HiPNUC（HI226、HI229 等） | 串口、经典 CAN（J1939） | `5A A5` 开头的变长帧或 J1939 帧 | 无 | + +主要能力： + +- 同一条 CAN 总线同时接入经典 CAN 和 CAN FD 设备。 +- 自动区分并路由 MCT7123 原始数据、姿态数据和配置响应。 +- 支持多个驱动订阅同一 CAN 路由，并安全管理回调生命周期。 +- 提供角速度、加速度、磁场、四元数、欧拉角、温度和帧计数器等数据。 +- 提供命令行测试工具，可明确选择串口、SocketCAN 或 CANable2。 ## 构建 +### 使用 ROS 2 和 colcon + +以 ROS 2 Humble 和 Ubuntu 为例，先安装构建依赖： + ```bash -mkdir build && cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j$(nproc) +sudo apt update +sudo apt 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 -# 只需指定型号，自动探测接口（串口优先，无串口则走默认 CAN） -python3 scripts/test_imu.py MCT7123 # → can0, id=1 -python3 scripts/test_imu.py HIPNUC # → can0, id=8 +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` 指定其他构建目录。 + +## 测试工具 + +测试脚本位于 `scripts/test_imu.py`。 -# 指定 CAN 口或 ID -python3 scripts/test_imu.py HIPNUC --can can1 -python3 scripts/test_imu.py HIPNUC --id 9 +### 指定通信接口 -# 指定串口 +必须同时指定型号和通信接口： + +```bash +# 原生串口 python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB1 -# 列出可用设备 +# SocketCAN +python3 scripts/test_imu.py MCT7123 --can can0 +python3 scripts/test_imu.py HIPNUC --can can1 --id 9 + +# CANable2 SLCAN-FD（目前仅支持 MCT7123） +python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM1 +``` + +`--slcan` 模式直接通过 Python 读取 CANable2，需要额外安装 +[`python-can`](https://python-can.readthedocs.io/)： + +```bash +python3 -m pip install python-can +``` + +CANable2 默认使用 `500 kbit/s` 仲裁速率和 `2 Mbit/s` 数据速率。脚本不会自动 +选择 SLCAN 设备，需要显式传入 `--slcan`。 + +### 常用操作 + +```bash +# 列出检测到的原生串口和 SLCAN 串口 python3 scripts/test_imu.py --list -# 全部数据 +# 显示全部传感器数据 python3 scripts/test_imu.py HIPNUC -a -# 安静模式 +# 运行 5 秒，只输出读取速率 python3 scripts/test_imu.py MCT7123 -d 5 -q + +# 每 0.2 秒打印一次 +python3 scripts/test_imu.py MCT7123 -i 0.2 ``` -输出示例 (默认仅欧拉角+温度): +默认输出仅包含欧拉角和温度： -``` +```text Time R P Y Temp (s) (°) (°) (°) (°C) ───────────────────────────────────────── @@ -51,105 +150,109 @@ python3 scripts/test_imu.py MCT7123 -d 5 -q 1.00 -0.16 -2.07 -11.47 36.2 ``` -`-a` 完整输出 (含 Cycle 计数器): - -``` - Time GyrX GyrY GyrZ AccX AccY AccZ MagX MagY MagZ R P Y Qw Qx Qy Qz Temp Cyc - (s) (°/s) (°/s) (°/s) (m/s²) (m/s²) (m/s²) (uT) (uT) (uT) (°) (°) (°) (°C) -─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────── - 0.00 0.00 -0.01 0.04 -0.029 -0.048 9.810 0.0 0.0 0.0 -0.22 -0.12 -51.32 0.901 -0.002 -0.000 -0.433 35.4 215 - 1.50 0.03 0.03 -0.00 -0.027 -0.041 9.811 0.0 0.0 0.0 -0.22 -0.12 -51.32 0.901 -0.002 -0.000 -0.433 35.4 2 -``` +### 参数 | 参数 | 说明 | -|------|------| -| `TYPE` | 唯一必填: `MCT7123` / `HIPNUC` | -| `--serial DEV` | 指定串口 (默认自动扫描 /dev/ttyUSB*) | -| `--can IFACE` | 指定 CAN/CANFD 口 (无串口时默认 can0) | -| `--id N` | CAN 源地址/节点 ID (默认: HIPNUC=8, MCT7123=1) | -| `-d SEC` | 运行时长, 0=无限 (Ctrl+C 退出) | -| `-a` | 显示全部 20 列 | -| `-q` | 安静模式, 只打速率 | -| `--list` | 列出可用串口设备 | +| --- | --- | +| `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` | 仅输出读取速率 | +| `-b, --build-dir DIR` | 指定包含 `imu_py` 的构建目录 | ## Python API -所有方法线程安全 (`shared_mutex`), 可通过 `help(imu_py.IMUDriver)` 查看行内文档。 - -| 方法 | 返回 | 说明 | -|------|------|------| -| `get_ang_vel()` | `[x, y, z]` rad/s | 角速度 (陀螺仪) | -| `get_lin_acc()` | `[x, y, z]` m/s² | 线加速度 | -| `get_mag()` | `[x, y, z]` uT | 磁场强度 (9 轴模式) | -| `get_quat()` | `[w, x, y, z]` | 姿态四元数 | -| `get_euler()` | `[roll, pitch, yaw]` ° | 欧拉角 (硬件直接输出) | -| `get_timestamp()` | `int` us | IMU 内部时钟 (开机累积) | -| `get_temperature()` | `float` °C | 传感器温度 | -| `get_cycle()` | `int` 0-255 | 帧计数器 (检测丢帧, MCT7123 专用) | +通过 `IMUDriver.create_imu()` 创建驱动： ```python import imu_py imu = imu_py.IMUDriver.create_imu( - imu_id=1, # 节点 ID (CAN 模式下回调路由) - interface_type="serial", # "serial" | "can" | "canfd" - interface="/dev/ttyUSB0", # 设备路径 - imu_type="MCT7123", # "MCT7123" | "HIPNUC" - baudrate=921600 # serial 必须; can/canfd 忽略 + imu_id=1, + interface_type="serial", # "serial"、"can" 或 "canfd" + interface="/dev/ttyUSB0", # 串口路径或 SocketCAN 接口名 + imu_type="MCT7123", # "MCT7123" 或 "HIPNUC" + baudrate=921600, # 串口必填；CAN 和 CAN FD 模式忽略 ) -# 读取数据 -gyr = imu.get_ang_vel() # [x, y, z] rad/s -acc = imu.get_lin_acc() # [x, y, z] m/s² -euler = imu.get_euler() # [roll, pitch, yaw] 度 -cycle = imu.get_cycle() # 帧计数 (丢帧检测) +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` | IMU 内部时钟，单位为微秒 | +| `get_temperature()` | `float` | 摄氏度 | +| `get_cycle()` | `int` | `0`–`255` 帧计数器 | + +可通过 `help(imu_py.IMUDriver)` 查看 Python 行内文档。 + ## C++ API ```cpp #include "imu_driver.hpp" -auto imu = IMUDriver::create_imu(1, "serial", "/dev/ttyUSB0", "MCT7123", 921600); - -std::vector gyr = imu->get_ang_vel(); // [x,y,z] rad/s -std::vector acc = imu->get_lin_acc(); // [x,y,z] m/s² -std::vector mag = imu->get_mag(); // [x,y,z] uT -std::vector quat = imu->get_quat(); // [w,x,y,z] -std::vector euler = imu->get_euler(); // [roll,pitch,yaw] 度 -uint64_t ts = imu->get_timestamp(); // us -float temp = imu->get_temperature(); // °C -uint8_t cycle = imu->get_cycle(); // 0-255 +auto imu = IMUDriver::create_imu( + 1, "serial", "/dev/ttyUSB0", "MCT7123", 921600); + +std::vector angular_velocity = imu->get_ang_vel(); +std::vector linear_acceleration = imu->get_lin_acc(); +std::vector magnetic_field = imu->get_mag(); +std::vector quaternion = imu->get_quat(); +std::vector euler = imu->get_euler(); +uint64_t timestamp = imu->get_timestamp(); +float temperature = imu->get_temperature(); +uint8_t cycle = imu->get_cycle(); ``` -## CAN / CANFD +## CAN 与 CAN FD -同一条 CAN 总线可同时挂载 HIPNUC (经典 CAN, ≤8 字节) 和 MCT7123 (CANFD, 64 字节), 各自按帧长度分流。 +同一条 CAN 总线可以同时挂载 HiPNUC 经典 CAN 设备和 MCT7123 CAN FD 设备。 +驱动按照帧类型和长度分别路由数据。 ```python -# HIPNUC 经典 CAN (J1939, SA 通常为 8) -imu_h = imu_py.IMUDriver.create_imu(8, "can", "can0", "HIPNUC") +# HiPNUC 经典 CAN；J1939 源地址通常为 8 +hipnuc = imu_py.IMUDriver.create_imu( + 8, "can", "can0", "HIPNUC") -# MCT7123 CANFD (0x181=IMU, 0x182=Att, 0x183=Config) -imu_m = imu_py.IMUDriver.create_imu(1, "canfd", "can0", "MCT7123") +# MCT7123 CAN FD +mct7123 = imu_py.IMUDriver.create_imu( + 1, "canfd", "can0", "MCT7123") ``` -C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧。 +MCT7123 使用以下 CAN 标识： -## MCT7123 坐标系 +| CAN 标识 | 类型 | 频率 | +| --- | --- | --- | +| `0x181` | IMU 原始数据：陀螺仪、加速度计、磁力计 | 100 Hz | +| `0x182` | 姿态数据：欧拉角、四元数 | 100 Hz | +| `0x183` | 配置交互 | 事件触发 | -MD7123 使用**坐标系 9**（手册默认值）： +C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧。 -- 标签 +Uz → X 正 -- 标签 -Uy → Y 正 -- 标签 +Ux → Z 正 +## MCT7123 坐标系 -机器人安装：正面朝右，接插件朝天。静止时 AccZ ≈ +9.81 m/s² (重力反作用力，Z 轴朝上)。 +MCT7123 默认使用坐标系 9： -## 协议参考 +- 标签 `+Uz` 对应 X 轴正方向。 +- 标签 `-Uy` 对应 Y 轴正方向。 +- 标签 `+Ux` 对应 Z 轴正方向。 -| 帧 ID | 类型 | 频率 | -|--------|------|------| -| 0x81 | IMU 原始数据 (陀螺/加表/磁力计) | 100 Hz | -| 0x82 | 姿态数据 (欧拉角/四元数) | 100 Hz | -| 0x83 | 配置交互 | 事件触发 | +机器人安装方向为正面朝右、接插件朝天。静止时 `AccZ` 约为 +`+9.81 m/s²`，表示 Z 轴朝上时测得的重力反作用力。 diff --git a/scripts/test_imu.py b/scripts/test_imu.py index 25c2a5d..9c7c6cf 100644 --- a/scripts/test_imu.py +++ b/scripts/test_imu.py @@ -1,8 +1,7 @@ #!/usr/bin/env python3 -"""IMU 测试工具 — 只需指定型号，自动探测接口。 +"""IMU 测试工具 — 必须指定型号和通信接口。 Usage: - python3 scripts/test_imu.py MCT7123 # 自动探测 python3 scripts/test_imu.py MCT7123 --can can0 # 指定 CAN 口 python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM1 # CANable2 CAN FD python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 # 指定串口 @@ -101,8 +100,8 @@ def get_cycle(self): return self.cycle def main(): ap = argparse.ArgumentParser( - description='IMU 测试工具 — MCT7123 / HIPNUC', - epilog='示例: python3 scripts/test_imu.py MCT7123', + description='IMU 测试工具 — MCT7123 / HIPNUC（必须指定通信接口）', + epilog='示例: python3 scripts/test_imu.py MCT7123 --can can0', formatter_class=argparse.RawDescriptionHelpFormatter) ap.add_argument('type', nargs='?', default=None, metavar='TYPE', help='IMU 型号 (MCT7123 / HIPNUC; 不提供则提示用法)') @@ -136,13 +135,17 @@ def main(): if args.type is None: print('请指定 IMU 型号: MCT7123 或 HIPNUC', file=sys.stderr) - print('示例: python3 scripts/test_imu.py MCT7123', file=sys.stderr) + print('示例: python3 scripts/test_imu.py MCT7123 --can can0', 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'] @@ -161,23 +164,16 @@ def main(): device = args.serial elif args.can: device = args.can - else: - # Auto-detect: try serial first, fall back to default CAN - serials = scan_serial() - if serials: - device = serials[0] - iface_type = 'serial' - elif DEFAULTS[imu_type].get('can'): - device = DEFAULTS[imu_type]['can'] - iface_type = 'canfd' if imu_type == 'MCT7123' else 'can' - else: - print('未找到串口设备，请用 --can 或 --serial 指定', file=sys.stderr) - sys.exit(1) imu_id = args.id if args.id is not None else DEFAULTS[imu_type]['id'] forever = (args.duration <= 0) - rate_text = '500K/2M' if iface_type == 'slcanfd' else f'{baudrate}bps' + 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: From 7ef19692f99936059a3843eb9bd38902e265965d Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Fri, 31 Jul 2026 15:41:31 +0800 Subject: [PATCH 15/17] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=20HiPNUC=20CAN=20?= =?UTF-8?q?=E5=9B=9E=E8=B0=83=E8=B7=AF=E7=94=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 为 SocketCAN 增加通配回调订阅，由 HiPNUC 在自身回调中按 J1939 扩展帧和完整源地址过滤。避免共享 ID 提取器导致回调丢失，同时保持 MCT7123 的精确 CAN FD 路由不变。 --- src/drivers/hipnuc/hipnuc_imu_driver.cpp | 12 +++++++----- src/protocol/can/socket_can.cpp | 9 +++++++++ src/protocol/can/socket_can.hpp | 1 + 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp index f54d78c..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_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId { - return frame.can_id & 0x7F; - }); - can_subscription_ = CanCallbackSubscription(can_, imu_id_, can_callback); + can_subscription_ = + CanCallbackSubscription(can_, CAN_CALLBACK_WILDCARD, can_callback); } else { throw std::runtime_error("Hipnuc driver only support CAN and SERIAL interface"); } @@ -31,7 +29,11 @@ HipnucIMUDriver::~HipnucIMUDriver() { 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) return; + 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; diff --git a/src/protocol/can/socket_can.cpp b/src/protocol/can/socket_can.cpp index 34ef5c4..ced6562 100644 --- a/src/protocol/can/socket_can.cpp +++ b/src/protocol/can/socket_can.cpp @@ -111,6 +111,15 @@ void IMUSocketCAN::open(std::string interface) { if (it != can_callback_list_.end()) { 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()); + } } for (const auto &entry : callbacks_to_run) { { diff --git a/src/protocol/can/socket_can.hpp b/src/protocol/can/socket_can.hpp index a65752e..10cade9 100644 --- a/src/protocol/can/socket_can.hpp +++ b/src/protocol/can/socket_can.hpp @@ -40,6 +40,7 @@ constexpr const int TIMEOUT_USEC = 1000; using CanCbkFunc = std::function; using CanCbkId = uint16_t; using CanCbkToken = uint64_t; +constexpr CanCbkId CAN_CALLBACK_WILDCARD = UINT16_MAX; struct CanCbkEntry { CanCbkToken token; CanCbkFunc callback; From 2e5c1b599e31d524dbe38efa4c86095c8a11a595 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Fri, 31 Jul 2026 15:45:32 +0800 Subject: [PATCH 16/17] =?UTF-8?q?=E8=A1=A5=E5=85=85=20IMU=20=E9=BB=98?= =?UTF-8?q?=E8=AE=A4=E9=80=9A=E4=BF=A1=E5=8F=82=E6=95=B0=E8=AF=B4=E6=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 在 README 中明确 MCT7123 与 HiPNUC 的串口波特率、CAN 仲裁速率、CAN FD 数据速率和默认节点编号，并补充带注释的 SocketCAN 拉起命令及节点编号覆盖示例。 --- README.md | 92 +++++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 83 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 01cdcaf..eb7b31d 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ RoboParty 双足机器人 IMU 驱动库，提供 C++ 接口和 Python 绑定，支持 MCT7123（MD7123）与 HiPNUC 系列设备。 -## 功能概览 +## 1. 功能概览 | 设备 | 通信接口 | 数据帧 | 校验 | | --- | --- | --- | --- | @@ -18,7 +18,7 @@ MCT7123（MD7123）与 HiPNUC 系列设备。 - 提供角速度、加速度、磁场、四元数、欧拉角、温度和帧计数器等数据。 - 提供命令行测试工具，可明确选择串口、SocketCAN 或 CANable2。 -## 构建 +## 2. 构建 ### 使用 ROS 2 和 colcon @@ -94,21 +94,95 @@ 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 接口需要在运行测试脚本前配置并拉起，脚本不会自动修改系统网络接口。 + +MCT7123： + +```bash +# 500 kbit/s 仲裁段，2 Mbit/s 数据段，启用 CAN FD +sudo ip link set can0 down +sudo ip link set can0 type can bitrate 500000 dbitrate 2000000 fd on +sudo ip link set can0 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 +``` + +### 配置 HiPNUC 数据输出 + +测试工具的默认界面显示欧拉角和温度。如果设备只启用了加速度、角速度和 +四元数 PGN，默认界面会全部显示为 `0`，但这不表示 CAN 通信或解析失败。 +使用 `--all` 可以查看当前已经启用的数据： + +```bash +python3 scripts/test_imu.py HIPNUC --can can0 --id 8 --all +``` + +下面的命令以节点 ID `8` 为例，开启俯仰/横滚、航向角和温度输出： + +```bash +# 0xFF3D：俯仰/横滚，周期 10 ms（100 Hz） +cansend can0 0CEF0808#3D0106000A000000 + +# 0xFF41：航向角，周期 10 ms（100 Hz） +cansend can0 0CEF0808#410106000A000000 + +# 0xFF43：温度，周期 100 ms（10 Hz） +cansend can0 0CEF0808#4301060064000000 + +# 保存全部参数到 Flash +cansend can0 0CEF0808#0000060000000000 + +# 复位设备，使配置生效 +cansend can0 0CEF0808#00000600FF000000 +``` + +以上命令需要安装 `can-utils`： + +```bash +sudo apt 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 # 原生串口 -python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB1 +python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 # SocketCAN python3 scripts/test_imu.py MCT7123 --can can0 -python3 scripts/test_imu.py HIPNUC --can can1 --id 9 +python3 scripts/test_imu.py HIPNUC --can can0 --id 8 # CANable2 SLCAN-FD（目前仅支持 MCT7123） python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM1 @@ -166,7 +240,7 @@ python3 scripts/test_imu.py MCT7123 -i 0.2 | `-q, --quiet` | 仅输出读取速率 | | `-b, --build-dir DIR` | 指定包含 `imu_py` 的构建目录 | -## Python API +## 4. Python API 通过 `IMUDriver.create_imu()` 创建驱动： @@ -203,7 +277,7 @@ cycle = imu.get_cycle() 可通过 `help(imu_py.IMUDriver)` 查看 Python 行内文档。 -## C++ API +## 5. C++ API ```cpp #include "imu_driver.hpp" @@ -221,7 +295,7 @@ float temperature = imu->get_temperature(); uint8_t cycle = imu->get_cycle(); ``` -## CAN 与 CAN FD +## 6. CAN 与 CAN FD 同一条 CAN 总线可以同时挂载 HiPNUC 经典 CAN 设备和 MCT7123 CAN FD 设备。 驱动按照帧类型和长度分别路由数据。 @@ -246,7 +320,7 @@ MCT7123 使用以下 CAN 标识： C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧。 -## MCT7123 坐标系 +## 7. MCT7123 坐标系 MCT7123 默认使用坐标系 9： From 3aebaf7eea972cc4e80f7460ba60909349609a2e Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Fri, 31 Jul 2026 16:31:21 +0800 Subject: [PATCH 17/17] =?UTF-8?q?=E5=AE=8C=E5=96=84=20IMU=20=E4=BD=BF?= =?UTF-8?q?=E7=94=A8=E6=96=87=E6=A1=A3=E4=B8=8E=E6=B5=8B=E8=AF=95=E5=B7=A5?= =?UTF-8?q?=E5=85=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 4 +- README.md | 136 ++++++++++++++++++++++---------------------- scripts/test_imu.py | 29 +++++++--- 3 files changed, 92 insertions(+), 77 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2afc4b4..09795fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 $ diff --git a/README.md b/README.md index eb7b31d..1ae0901 100644 --- a/README.md +++ b/README.md @@ -7,8 +7,8 @@ MCT7123（MD7123）与 HiPNUC 系列设备。 | 设备 | 通信接口 | 数据帧 | 校验 | | --- | --- | --- | --- | -| MCT7123（MD7123） | 串口、CAN FD、CANable2 SLCAN-FD | `EB 90` 开头的固定 64 字节帧 | CRC16-CCITT（多项式 `0x1021`） | -| HiPNUC（HI226、HI229 等） | 串口、经典 CAN（J1939） | `5A A5` 开头的变长帧或 J1939 帧 | 无 | +| MCT7123（MD7123） | 串口、CAN FD、CANable2 SLCAN-FD | 串口为 69 字节封装帧；CAN FD 为 64 字节载荷 | CRC16-CCITT（多项式 `0x1021`） | +| HiPNUC（HI226、HI229 等） | 串口、经典 CAN（J1939） | `5A A5` 开头的变长帧或 J1939 帧 | 串口使用 CRC16；J1939 无额外应用层校验 | 主要能力： @@ -25,8 +25,8 @@ MCT7123（MD7123）与 HiPNUC 系列设备。 以 ROS 2 Humble 和 Ubuntu 为例，先安装构建依赖： ```bash -sudo apt update -sudo apt install \ +sudo apt-get update +sudo apt-get install \ build-essential \ cmake \ ccache \ @@ -100,20 +100,22 @@ cmake --build build -j"$(nproc)" ### 默认通信参数 -| 型号 | 原生串口 | CAN 仲裁速率 | CAN FD 数据速率 | 默认节点编号 | +| 型号 | 原生串口 | 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： +MCT7123（以下使用 `can1`，避免与示例中的 HiPNUC `can0` 冲突）： ```bash # 500 kbit/s 仲裁段，2 Mbit/s 数据段，启用 CAN FD -sudo ip link set can0 down -sudo ip link set can0 type can bitrate 500000 dbitrate 2000000 fd on -sudo ip link set can0 up +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： @@ -125,6 +127,32 @@ 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 数据输出 测试工具的默认界面显示欧拉角和温度。如果设备只启用了加速度、角速度和 @@ -132,32 +160,28 @@ sudo ip link set can0 up 使用 `--all` 可以查看当前已经启用的数据： ```bash -python3 scripts/test_imu.py HIPNUC --can can0 --id 8 --all +python3 scripts/test_imu.py HIPNUC --can can0 --id 8 --all -d 5 ``` 下面的命令以节点 ID `8` 为例，开启俯仰/横滚、航向角和温度输出： ```bash -# 0xFF3D：俯仰/横滚，周期 10 ms（100 Hz） -cansend can0 0CEF0808#3D0106000A000000 - -# 0xFF41：航向角，周期 10 ms（100 Hz） -cansend can0 0CEF0808#410106000A000000 - -# 0xFF43：温度，周期 100 ms（10 Hz） -cansend can0 0CEF0808#4301060064000000 - -# 保存全部参数到 Flash -cansend can0 0CEF0808#0000060000000000 - -# 复位设备，使配置生效 -cansend can0 0CEF0808#00000600FF000000 +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 install can-utils +sudo apt-get install can-utils ``` 配置成功后，总线上应出现以下 J1939 报文： @@ -169,49 +193,23 @@ sudo apt install can-utils | `0x0CFF4308` | `0xFF43` | 温度 | 100 ms | 配置帧 `0x0CEF0808` 中，目标地址和源地址均为 `8`。设备使用其他节点编号时， -需要相应修改配置帧 CAN ID。航向角显示 `360°` 与 `0°` 表示同一方向，属于 -正常的角度回绕。 - -### 指定通信接口 - -必须同时指定型号和通信接口： - -```bash -# 原生串口 -python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 - -# SocketCAN -python3 scripts/test_imu.py MCT7123 --can can0 -python3 scripts/test_imu.py HIPNUC --can can0 --id 8 - -# CANable2 SLCAN-FD（目前仅支持 MCT7123） -python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM1 -``` - -`--slcan` 模式直接通过 Python 读取 CANable2，需要额外安装 -[`python-can`](https://python-can.readthedocs.io/)： - -```bash -python3 -m pip install python-can -``` - -CANable2 默认使用 `500 kbit/s` 仲裁速率和 `2 Mbit/s` 数据速率。脚本不会自动 -选择 SLCAN 设备，需要显式传入 `--slcan`。 +需要相应修改配置帧 CAN ID；执行这些命令不会修改节点编号本身。航向角显示 +`360°` 与 `0°` 表示同一方向，属于正常的角度回绕。 ### 常用操作 ```bash -# 列出检测到的原生串口和 SLCAN 串口 +# 列出检测到的原生串口、SLCAN 串口和 SocketCAN 接口 python3 scripts/test_imu.py --list # 显示全部传感器数据 -python3 scripts/test_imu.py HIPNUC -a +python3 scripts/test_imu.py HIPNUC --can can0 --id 8 -a -d 5 # 运行 5 秒，只输出读取速率 -python3 scripts/test_imu.py MCT7123 -d 5 -q +python3 scripts/test_imu.py MCT7123 --can can1 -d 5 -q # 每 0.2 秒打印一次 -python3 scripts/test_imu.py MCT7123 -i 0.2 +python3 scripts/test_imu.py MCT7123 --can can1 -i 0.2 -d 5 ``` 默认输出仅包含欧拉角和温度： @@ -224,6 +222,9 @@ python3 scripts/test_imu.py MCT7123 -i 0.2 1.00 -0.16 -2.07 -11.47 36.2 ``` +当前脚本末尾显示的 `reads/s` 是 getter 轮询速率，不是传感器实际输出帧率。 +判断传感器帧率应统计 CAN 报文、Cycle 计数器或时间戳变化。 + ### 参数 | 参数 | 说明 | @@ -237,7 +238,7 @@ python3 scripts/test_imu.py MCT7123 -i 0.2 | `-d, --duration SEC` | 运行时长；`0` 表示持续运行 | | `-i, --interval SEC` | 输出间隔，默认为 `0.5` 秒 | | `-a, --all` | 显示全部传感器数据 | -| `-q, --quiet` | 仅输出读取速率 | +| `-q, --quiet` | 仅输出 getter 轮询速率 | | `-b, --build-dir DIR` | 指定包含 `imu_py` 的构建目录 | ## 4. Python API @@ -271,9 +272,9 @@ cycle = imu.get_cycle() | `get_mag()` | `[x, y, z]` | `µT`，九轴模式下有效 | | `get_quat()` | `[w, x, y, z]` | 姿态四元数 | | `get_euler()` | `[roll, pitch, yaw]` | 度 | -| `get_timestamp()` | `int` | IMU 内部时钟，单位为微秒 | +| `get_timestamp()` | `int` | 协议时间戳换算为微秒；设备未输出时间戳时为 `0` | | `get_temperature()` | `float` | 摄氏度 | -| `get_cycle()` | `int` | `0`–`255` 帧计数器 | +| `get_cycle()` | `int` | MCT7123 的 `0`–`255` 帧计数器；HiPNUC 固定返回 `0` | 可通过 `help(imu_py.IMUDriver)` 查看 Python 行内文档。 @@ -297,29 +298,28 @@ uint8_t cycle = imu->get_cycle(); ## 6. CAN 与 CAN FD -同一条 CAN 总线可以同时挂载 HiPNUC 经典 CAN 设备和 MCT7123 CAN FD 设备。 -驱动按照帧类型和长度分别路由数据。 +下面示例使用两个 SocketCAN 接口，和前文 OPI5 测试接线一致。设备也可以 +接在同一条总线上，但必须使用相同仲裁速率，且主机控制器和总线都支持 +CAN FD。驱动按照帧类型和长度分别路由数据。 ```python # HiPNUC 经典 CAN；J1939 源地址通常为 8 hipnuc = imu_py.IMUDriver.create_imu( 8, "can", "can0", "HIPNUC") -# MCT7123 CAN FD +# MCT7123 CAN FD；当前示例连接在 can1 mct7123 = imu_py.IMUDriver.create_imu( - 1, "canfd", "can0", "MCT7123") + 1, "canfd", "can1", "MCT7123") ``` MCT7123 使用以下 CAN 标识： | CAN 标识 | 类型 | 频率 | | --- | --- | --- | -| `0x181` | IMU 原始数据：陀螺仪、加速度计、磁力计 | 100 Hz | -| `0x182` | 姿态数据：欧拉角、四元数 | 100 Hz | +| `0x181` | IMU 原始数据：陀螺仪、加速度计、磁力计 | 取决于设备输出配置 | +| `0x182` | 姿态数据：欧拉角、四元数 | 取决于设备输出配置 | | `0x183` | 配置交互 | 事件触发 | -C++ 端可通过 `IMUSocketCAN::get_instance("can0")->send(frame)` 发送 CAN 帧。 - ## 7. MCT7123 坐标系 MCT7123 默认使用坐标系 9： diff --git a/scripts/test_imu.py b/scripts/test_imu.py index 9c7c6cf..76d2ebd 100644 --- a/scripts/test_imu.py +++ b/scripts/test_imu.py @@ -2,10 +2,10 @@ """IMU 测试工具 — 必须指定型号和通信接口。 Usage: - python3 scripts/test_imu.py MCT7123 --can can0 # 指定 CAN 口 - python3 scripts/test_imu.py MCT7123 --slcan /dev/ttyACM1 # CANable2 CAN FD - python3 scripts/test_imu.py MCT7123 --serial /dev/ttyUSB0 # 指定串口 - python3 scripts/test_imu.py --list # 列出设备 + 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 @@ -29,6 +29,18 @@ 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: @@ -101,7 +113,7 @@ def get_cycle(self): return self.cycle def main(): ap = argparse.ArgumentParser( description='IMU 测试工具 — MCT7123 / HIPNUC（必须指定通信接口）', - epilog='示例: python3 scripts/test_imu.py MCT7123 --can can0', + 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; 不提供则提示用法)') @@ -109,7 +121,7 @@ def main(): 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/ttyACM1)') + 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=无限循环)') @@ -128,14 +140,15 @@ def main(): 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: (使用 --can can0 指定)') + 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 can0', 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'):