From 84011fca5292e7545ce14fdebdb733eabd6c0630 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Mon, 3 Aug 2026 15:42:47 +0800 Subject: [PATCH 1/5] Unify IMU sensor data and public interfaces --- include/imu_driver.hpp | 21 +++++----- 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 | 51 ++---------------------- src/drivers/hipnuc/hipnuc_imu_driver.cpp | 34 +++++++++++++++- src/drivers/hipnuc/hipnuc_imu_driver.hpp | 8 +++- src/drivers/hipnuc/hipnuc_j1939_parser.c | 28 ++++++------- src/drivers/hipnuc/hipnuc_j1939_parser.h | 3 +- src/pybind_module.cpp | 40 ++++++++++++++----- 11 files changed, 154 insertions(+), 96 deletions(-) create mode 100644 include/imu_sensor_data.h diff --git a/include/imu_driver.hpp b/include/imu_driver.hpp index 2b0e341..dbf3d7f 100644 --- a/include/imu_driver.hpp +++ b/include/imu_driver.hpp @@ -22,6 +22,8 @@ #include #include +#include "imu_sensor_data.h" + class IMUDriver { public: @@ -40,17 +42,16 @@ class IMUDriver { const std::string& imu_type, const int baudrate=0); virtual uint16_t get_imu_id() { return imu_id_; } - virtual std::vector get_ang_vel() { return ang_vel_; } - virtual std::vector get_quat() { return quat_; } - virtual std::vector get_lin_acc() { return lin_acc_; } - virtual float get_temperature() { return temperature_; } + 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 - std::vector lin_acc_{0.f, 0.f, 0.f}; // x, y, z - float temperature_{0.f}; // temperature + uint16_t imu_id_{0}; }; \ No newline at end of file diff --git a/include/imu_sensor_data.h b/include/imu_sensor_data.h new file mode 100644 index 0000000..22bd52b --- /dev/null +++ b/include/imu_sensor_data.h @@ -0,0 +1,46 @@ +/* SPDX-License-Identifier: GPL-3.0 */ +/* Copyright (C) 2026 Luo1imasi */ + +/** + * @file imu_sensor_data.h + * @brief Unified IMU sensor data struct — C/C++ compatible. + * Single buffer all transports (serial, CAN, CANFD) write to. + */ + +#ifndef IMU_SENSOR_DATA_H +#define IMU_SENSOR_DATA_H + +#include + +#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 5f853a6..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,59 +36,12 @@ typedef struct { #define CAN_MSG_GNSS_STATUS 14 #define CAN_MSG_UNKNOWN 99 -typedef struct { - uint8_t node_id; - uint64_t hw_ts_us; - float acc_x; - float acc_y; - float acc_z; - float gyr_x; - float gyr_y; - float gyr_z; - float mag_x; - float mag_y; - float mag_z; - float quat_w; - float quat_x; - float quat_y; - float quat_z; - float roll; - float pitch; - float imu_yaw; - float incli_x; - float incli_y; - float temperature; - float pressure; - uint8_t utc_year; - uint8_t utc_month; - uint8_t utc_day; - uint8_t hours; - uint8_t minutes; - uint8_t seconds; - uint16_t milliseconds; - uint32_t timestamp_ms; - double ins_lat; - double ins_lon; - double ins_msl; - float undulation; - float diff_age_s; - float ins_vel_e; - float ins_vel_n; - float ins_vel_u; - float ins_speed; - uint8_t solq_pos; - uint8_t solq_heading; - uint8_t nv_pos; - uint8_t nv_heading; - uint8_t ins_status; -} can_sensor_data_t; - typedef struct { char buffer[512]; size_t length; } can_json_output_t; -int hipnuc_can_to_json(const can_sensor_data_t *data, int msg_type, can_json_output_t *output); +int hipnuc_can_to_json(const imu_sensor_data_t *data, int msg_type, can_json_output_t *output); uint8_t hipnuc_can_extract_node_id(uint32_t can_id); typedef enum { diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp index 3ed125f..b29d49e 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.cpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.cpp @@ -35,7 +35,7 @@ void HipnucIMUDriver::can_rx_cbk(const can_frame& rx_frame) { frame.can_id = rx_frame.can_id; frame.can_dlc = rx_frame.can_dlc; memcpy(frame.data, rx_frame.data, 8); - + std::unique_lock lock(imu_mutex_); int ret = hipnuc_j1939_parse_frame(&frame, &sensor_data_); @@ -70,6 +70,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 +103,23 @@ std::vector HipnucIMUDriver::get_lin_acc() { float HipnucIMUDriver::get_temperature() { std::shared_lock lock(imu_mutex_); return sensor_data_.temperature; -} \ No newline at end of file +} + +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; +} + +uint8_t HipnucIMUDriver::get_cycle() { + return 0; /* HiPNUC protocol has no cycle counter */ +} diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp index b334ae6..e705e72 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp @@ -31,7 +31,11 @@ class HipnucIMUDriver : public IMUDriver { 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: int baudrate_; @@ -40,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/pybind_module.cpp b/src/pybind_module.cpp index a88836d..9758ea0 100644 --- a/src/pybind_module.cpp +++ b/src/pybind_module.cpp @@ -4,8 +4,9 @@ /** * @file pybind_module.cpp * @brief Python bindings for the IMU driver library via pybind11. - * @details Exposes IMUDriver and sensor data accessors to Python - * as the imu_py module. + * @details Exposes IMUDriver and all sensor data accessors to Python + * as the imu_py module. Each method includes inline docstring + * visible via help() in Python. */ #include @@ -15,19 +16,38 @@ namespace py = pybind11; PYBIND11_MODULE(imu_py, m) { - m.doc() = "IMU Driver Python SDK"; + m.doc() = "IMU Driver Python SDK"; py::class_>(m, "IMUDriver") .def(py::init<>()) - .def_static("create_imu", &IMUDriver::create_imu, + .def_static("create_imu", &IMUDriver::create_imu, py::arg("imu_id"), py::arg("interface_type"), py::arg("interface"), py::arg("imu_type"), - py::arg("baudrate") = 0) - .def("get_imu_id", &IMUDriver::get_imu_id) - .def("get_ang_vel", &IMUDriver::get_ang_vel) - .def("get_quat", &IMUDriver::get_quat) - .def("get_lin_acc", &IMUDriver::get_lin_acc) - .def("get_temperature", &IMUDriver::get_temperature); + py::arg("baudrate") = 0, + "工厂方法: create_imu(imu_id, interface_type, interface, imu_type, baudrate=0)\n" + " imu_id int 节点 ID (CAN 模式用于多设备路由)\n" + " interface_type \"serial\"|\"can\"|\"canfd\"\n" + " interface str 设备路径 (/dev/ttyUSB0, can0 …)\n" + " imu_type \"MCT7123\"|\"HIPNUC\"\n" + " baudrate int serial 必须, can/canfd 忽略") + .def("get_imu_id", &IMUDriver::get_imu_id, + "IMU 节点 ID — CAN 模式用于多设备路由") + .def("get_ang_vel", &IMUDriver::get_ang_vel, + "角速度 [x, y, z] rad/s — 陀螺仪输出") + .def("get_quat", &IMUDriver::get_quat, + "姿态四元数 [w, x, y, z] — IMU 姿态解算输出") + .def("get_lin_acc", &IMUDriver::get_lin_acc, + "线加速度 [x, y, z] m/s² — 加速度计输出") + .def("get_mag", &IMUDriver::get_mag, + "磁场强度 [x, y, z] uT — 磁力计输出 (9 轴模式下有效)") + .def("get_euler", &IMUDriver::get_euler, + "欧拉角 [roll, pitch, yaw] 度 — IMU 硬件直接输出") + .def("get_timestamp", &IMUDriver::get_timestamp, + "系统时间戳 us — IMU 内部时钟 (开机累积)") + .def("get_temperature", &IMUDriver::get_temperature, + "传感器温度 °C") + .def("get_cycle", &IMUDriver::get_cycle, + "帧计数器 0-255 — 检测丢帧"); } From 6cfc72f1f0ce8ad2e931a8ab703c9dcd6752b774 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Mon, 3 Aug 2026 15:43:05 +0800 Subject: [PATCH 2/5] Add automatic CAN callback lifecycle management --- src/drivers/hipnuc/hipnuc_imu_driver.cpp | 20 ++-- src/drivers/hipnuc/hipnuc_imu_driver.hpp | 3 +- src/protocol/can/socket_can.cpp | 112 ++++++++++++++++++++--- src/protocol/can/socket_can.hpp | 80 ++++++++++++++-- 4 files changed, 184 insertions(+), 31 deletions(-) diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.cpp b/src/drivers/hipnuc/hipnuc_imu_driver.cpp index b29d49e..2394397 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.cpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.cpp @@ -13,10 +13,8 @@ HipnucIMUDriver::HipnucIMUDriver(uint16_t imu_id, const std::string& interface_t } else if (interface_type_ == "can") { can_ = IMUSocketCAN::get_instance(interface_); CanCbkFunc can_callback = std::bind(&HipnucIMUDriver::can_rx_cbk, this, std::placeholders::_1); - can_->add_can_callback(can_callback, imu_id_); - can_->set_key_extractor([](const can_frame &frame) -> CanCbkId { - return frame.can_id & 0x7F; - }); + can_subscription_ = + CanCallbackSubscription(can_, CAN_CALLBACK_WILDCARD, can_callback); } else { throw std::runtime_error("Hipnuc driver only support CAN and SERIAL interface"); } @@ -25,15 +23,21 @@ HipnucIMUDriver::HipnucIMUDriver(uint16_t imu_id, const std::string& interface_t HipnucIMUDriver::~HipnucIMUDriver() { if (interface_type_ == "serial" && serial_) { serial_->close(); - } else if (interface_type_ == "can" && can_) { - can_->remove_can_callback(imu_id_); } } -void HipnucIMUDriver::can_rx_cbk(const can_frame& rx_frame) { +void HipnucIMUDriver::can_rx_cbk(const canfd_frame& rx_frame) { + /* Only process classic CAN frames (len <= 8). + * CANFD frames on the same bus are handled by other drivers (e.g. MCT7123). */ + if (rx_frame.len > 8 || + !(rx_frame.can_id & HIPNUC_CAN_EFF_FLAG) || + hipnuc_can_extract_node_id(rx_frame.can_id) != imu_id_) { + return; + } + hipnuc_can_frame_t frame; frame.can_id = rx_frame.can_id; - frame.can_dlc = rx_frame.can_dlc; + frame.can_dlc = rx_frame.len; memcpy(frame.data, rx_frame.data, 8); std::unique_lock lock(imu_mutex_); diff --git a/src/drivers/hipnuc/hipnuc_imu_driver.hpp b/src/drivers/hipnuc/hipnuc_imu_driver.hpp index e705e72..ad09728 100644 --- a/src/drivers/hipnuc/hipnuc_imu_driver.hpp +++ b/src/drivers/hipnuc/hipnuc_imu_driver.hpp @@ -26,7 +26,7 @@ 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; @@ -46,4 +46,5 @@ class HipnucIMUDriver : public IMUDriver { std::shared_ptr serial_; imu_sensor_data_t sensor_data_; hipnuc_raw_t raw_; + CanCallbackSubscription can_subscription_; }; diff --git a/src/protocol/can/socket_can.cpp b/src/protocol/can/socket_can.cpp index 4763b26..ced6562 100644 --- a/src/protocol/can/socket_can.cpp +++ b/src/protocol/can/socket_can.cpp @@ -10,6 +10,8 @@ #include "socket_can.hpp" +#include + std::shared_ptr IMUSocketCAN::logger_ = nullptr; std::unordered_map> IMUSocketCAN::instances_; @@ -27,6 +29,12 @@ void IMUSocketCAN::open(std::string interface) { throw std::runtime_error("Failed to create CAN socket"); } + /* Enable CAN FD frame reception (backward compatible with classic CAN) */ + int canfd_on = 1; + if (setsockopt(sockfd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0) { + logger_->warn("CAN FD not supported on interface {}, falling back to classic CAN", interface); + } + strncpy(if_request_.ifr_name, interface.c_str(), IFNAMSIZ); if (ioctl(sockfd_, SIOCGIFINDEX, &if_request_) == -1) { logger_->error("Unable to detect CAN interface {}", interface); @@ -62,12 +70,12 @@ void IMUSocketCAN::open(std::string interface) { pthread_setname_np(pthread_self(), "can_rx"); struct sched_param sp{}; sp.sched_priority = 80; if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp) != 0) { - logger_->error("Failed to set realtime priority for IMU CAN RX thread"); + logger_->warn("Failed to set realtime priority for IMU CAN RX thread"); } fd_set descriptors; int maxfd = sockfd_; struct timeval timeout; - can_frame rx_frame; + canfd_frame rx_frame; while (receiving_) { FD_ZERO(&descriptors); @@ -84,7 +92,7 @@ void IMUSocketCAN::open(std::string interface) { } if (sel_ret == 1) { while (true){ - int len = ::read(sockfd_, &rx_frame, CAN_MTU); + int len = ::read(sockfd_, &rx_frame, sizeof(canfd_frame)); if (len < 0) { if (errno == EAGAIN || errno == EWOULDBLOCK) { break; @@ -95,17 +103,44 @@ void IMUSocketCAN::open(std::string interface) { if (len == 0){ break; } - CanCbkFunc callback_to_run; + std::vector 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; + } + auto wildcard_it = + can_callback_list_.find(CAN_CALLBACK_WILDCARD); + if (wildcard_it != can_callback_list_.end() && + key != CAN_CALLBACK_WILDCARD) { + callbacks_to_run.insert( + callbacks_to_run.end(), + wildcard_it->second.begin(), + wildcard_it->second.end()); } } - if (callback_to_run) { - callback_to_run(rx_frame); + for (const auto &entry : callbacks_to_run) { + { + std::lock_guard 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(); + } } } } @@ -127,22 +162,71 @@ void IMUSocketCAN::close() { sockfd_ = INIT_FD; } -void IMUSocketCAN::add_can_callback(const CanCbkFunc callback, const CanCbkId id) { +CanCbkToken IMUSocketCAN::add_can_callback(const CanCbkFunc callback, const CanCbkId id) { std::lock_guard 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) { 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 9ca85c8..10cade9 100644 --- a/src/protocol/can/socket_can.hpp +++ b/src/protocol/can/socket_can.hpp @@ -5,13 +5,16 @@ * @file socket_can.hpp * @brief SocketCAN interface declaration for CAN/CAN FD frame reception. * @details Provides the IMUSocketCAN singleton class that wraps Linux - * SocketCAN socket operations and delivers parsed frames - * to registered callback handlers. + * SocketCAN socket operations and delivers both classic CAN + * (len <= 8) and CAN FD (len up to 64) frames to registered + * callbacks via the unified canfd_frame struct. + * Callbacks are responsible for filtering by frame length. */ #pragma once #include +#include #include #include #include @@ -23,6 +26,7 @@ #include #include +#include #include #include #include @@ -33,13 +37,28 @@ constexpr const int INIT_FD = -1; constexpr const int TIMEOUT_SEC = 0; constexpr const int TIMEOUT_USEC = 1000; -using CanCbkFunc = std::function; +using CanCbkFunc = std::function; using CanCbkId = uint16_t; -using CanCbkMap = std::unordered_map; -using CanCbkKeyExtractor = std::function; +using CanCbkToken = uint64_t; +constexpr CanCbkId CAN_CALLBACK_WILDCARD = UINT16_MAX; +struct CanCbkEntry { + CanCbkToken token; + CanCbkFunc callback; + std::mutex mutex; + std::condition_variable cv; + bool enabled{true}; + size_t running_count{0}; +}; +using CanCbkEntryPtr = std::shared_ptr; +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_; @@ -51,7 +70,8 @@ class IMUSocketCAN { std::thread receiver_thread_; CanCbkMap can_callback_list_; std::mutex can_callback_mutex_; - CanCbkKeyExtractor key_extractor_ = [](const can_frame &frame) -> CanCbkId { + CanCbkToken next_callback_token_{1}; + CanCbkKeyExtractor key_extractor_ = [](const canfd_frame &frame) -> CanCbkId { return static_cast(frame.can_id); }; @@ -63,6 +83,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; @@ -83,8 +106,49 @@ class IMUSocketCAN { } void open(std::string interface); void close(); - void add_can_callback(const CanCbkFunc callback, const CanCbkId id); - void remove_can_callback(const CanCbkId id); void clear_can_callbacks(); void set_key_extractor(CanCbkKeyExtractor extractor); + int send(const canfd_frame &frame); +}; + +class CanCallbackSubscription { + public: + CanCallbackSubscription() = default; + CanCallbackSubscription(std::shared_ptr 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 4dd5db7ca68a2e185790000b5ddbeeb9c2d6a5ad Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Mon, 3 Aug 2026 15:43:25 +0800 Subject: [PATCH 3/5] Add MCT7123 serial and CAN FD drivers --- CMakeLists.txt | 10 +- cmake/roboparty_imuConfig.cmake.in | 1 + src/drivers/CMakeLists.txt | 3 +- src/drivers/mct7123/CMakeLists.txt | 15 ++ src/drivers/mct7123/mct7123_can_driver.cpp | 141 ++++++++++++++ src/drivers/mct7123/mct7123_can_driver.hpp | 45 +++++ src/drivers/mct7123/mct7123_dec.c | 178 ++++++++++++++++++ src/drivers/mct7123/mct7123_dec.h | 76 ++++++++ src/drivers/mct7123/mct7123_serial_driver.cpp | 129 +++++++++++++ src/drivers/mct7123/mct7123_serial_driver.hpp | 42 +++++ src/imu_driver.cpp | 10 + src/protocol/serial/serial_port.cpp | 2 +- 12 files changed, 646 insertions(+), 6 deletions(-) create mode 100644 src/drivers/mct7123/CMakeLists.txt create mode 100644 src/drivers/mct7123/mct7123_can_driver.cpp create mode 100644 src/drivers/mct7123/mct7123_can_driver.hpp 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_serial_driver.cpp create mode 100644 src/drivers/mct7123/mct7123_serial_driver.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 078ff7e..31e4d8b 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 @@ -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 $ @@ -97,7 +99,7 @@ install(TARGETS imu_py ) if(ament_cmake_FOUND) - ament_export_libraries(imu hipnuc_imu imu_protocol) + ament_export_libraries(imu hipnuc_imu mct7123_imu imu_protocol) ament_export_include_directories(include) ament_package() endif() diff --git a/cmake/roboparty_imuConfig.cmake.in b/cmake/roboparty_imuConfig.cmake.in index 9a5a439..245d9d6 100644 --- a/cmake/roboparty_imuConfig.cmake.in +++ b/cmake/roboparty_imuConfig.cmake.in @@ -14,6 +14,7 @@ if(NOT TARGET roboparty_imu::roboparty_imu) target_link_libraries(roboparty_imu::roboparty_imu INTERFACE roboparty_imu::imu roboparty_imu::hipnuc_imu + roboparty_imu::mct7123_imu roboparty_imu::imu_protocol ) endif() 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/mct7123/CMakeLists.txt b/src/drivers/mct7123/CMakeLists.txt new file mode 100644 index 0000000..72d7135 --- /dev/null +++ b/src/drivers/mct7123/CMakeLists.txt @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: GPL-3.0 +add_library(mct7123_imu STATIC + mct7123_dec.c + mct7123_serial_driver.cpp + mct7123_can_driver.cpp +) +target_include_directories(mct7123_imu + PUBLIC + $ + $ + $ + $ + $ +) +target_link_libraries(mct7123_imu PUBLIC ${PUBLIC_DEPENDENCIES} imu_protocol) diff --git a/src/drivers/mct7123/mct7123_can_driver.cpp b/src/drivers/mct7123/mct7123_can_driver.cpp new file mode 100644 index 0000000..64ceda0 --- /dev/null +++ b/src/drivers/mct7123/mct7123_can_driver.cpp @@ -0,0 +1,141 @@ +#include "mct7123_can_driver.hpp" +#include + +Mct7123CanDriver::Mct7123CanDriver(uint16_t imu_id, const std::string& 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_->set_key_extractor([](const canfd_frame &frame) -> CanCbkId { + return frame.can_id & 0x7F; + }); + // CAN_ID_Config is a base ID: +1 is IMU, +2 is attitude, +3 is config. + // imu_id is the low seven bits of the first data frame ID (normally 1). + for (size_t i = 0; i < callback_ids_.size(); ++i) { + can_subscriptions_[i] = CanCallbackSubscription(can_, callback_ids_[i], can_cb); + } +} + +Mct7123CanDriver::~Mct7123CanDriver() = default; + +void Mct7123CanDriver::can_rx_cbk(const canfd_frame& rx_frame) +{ + /* MD7123 CANFD: full 64-byte payload per frame, frame ID selects type. + * 0x181 = IMU data, 0x182 = Attitude data, 0x183 = Config. + * Payload layout identical to UART; CRC16 over bytes 0..61. */ + if (rx_frame.len < 64) return; + + const uint8_t *payload = rx_frame.data; + + /* Validate CRC16 over payload[0..61] */ + uint16_t crc_expected = (uint16_t)payload[62] | ((uint16_t)payload[63] << 8); + if (mct7123_crc16(payload, 62) != crc_expected) return; + + std::unique_lock 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..98c34c1 --- /dev/null +++ b/src/drivers/mct7123/mct7123_can_driver.hpp @@ -0,0 +1,45 @@ +#pragma once + +extern "C" { +#include "mct7123_dec.h" +} + +#include +#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: + static constexpr size_t kFrameTypeCount = 3; + + std::string interface_; + std::array callback_ids_; + mutable std::shared_mutex imu_mutex_; + std::shared_ptr can_; + + imu_sensor_data_t sensor_data_; + std::array can_subscriptions_; +}; diff --git a/src/drivers/mct7123/mct7123_dec.c b/src/drivers/mct7123/mct7123_dec.c new file mode 100644 index 0000000..ceee0a9 --- /dev/null +++ b/src/drivers/mct7123/mct7123_dec.c @@ -0,0 +1,178 @@ +/* + * SPDX-License-Identifier: GPL-3.0 + * + * MCT MD7123 byte-level UART protocol decoder. + */ + +#include "mct7123_dec.h" +#include +#include + +/* CRC16-CCITT: poly 0x1021, init 0xFFFF */ +uint16_t mct7123_crc16(const uint8_t *data, size_t len) +{ + uint16_t crc = 0xFFFF; + for (size_t i = 0; i < len; i++) { + crc ^= (uint16_t)data[i] << 8; + for (int j = 0; j < 8; j++) { + if (crc & 0x8000) + crc = (crc << 1) ^ 0x1021; + else + crc <<= 1; + } + } + return crc; +} + +static uint16_t le16(const uint8_t *p) +{ + return (uint16_t)p[0] | ((uint16_t)p[1] << 8); +} + +static uint64_t le64(const uint8_t *p) +{ + uint64_t v; + memcpy(&v, p, 8); + return v; +} + +static float r4(const uint8_t *p) +{ + float v; + memcpy(&v, p, 4); + return v; +} + +int mct7123_input(mct7123_raw_t *raw, uint8_t data) +{ + /* state 0: waiting for 0xEB */ + if (raw->idx == 0) { + if (data == 0xEB) { + raw->buf[raw->idx++] = data; + } + return 0; + } + + /* state 1: expect 0x90 */ + if (raw->idx == 1) { + if (data == 0x90) { + raw->buf[raw->idx++] = data; + } else { + /* false sync, restart */ + raw->idx = 0; + if (data == 0xEB) { + raw->buf[raw->idx++] = data; + } + } + return 0; + } + + /* state 2: MsgID */ + if (raw->idx == 2) { + raw->msg_id = data; + raw->buf[raw->idx++] = data; + return 0; + } + + /* state 3: payload length (must be 0x40) */ + if (raw->idx == 3) { + if (data != MCT7123_PAYLOAD_LEN) { + /* invalid length, restart */ + raw->idx = 0; + return -1; + } + raw->payload_len = data; + raw->buf[raw->idx++] = data; + return 0; + } + + /* state 4: collecting payload (64 bytes) */ + if (raw->idx < 4 + MCT7123_PAYLOAD_LEN) { + raw->payload[raw->idx - 4] = data; + raw->buf[raw->idx++] = data; + return 0; + } + + /* state 5: tail (must be 0xED) */ + if (raw->idx == 4 + MCT7123_PAYLOAD_LEN) { + raw->buf[raw->idx++] = data; + if (data != 0xED) { + raw->idx = 0; + return -1; + } + + /* verify CRC16 over payload[0..61], CRC at payload[62..63] */ + uint16_t crc_expected = le16(&raw->payload[62]); + uint16_t crc_calc = mct7123_crc16(raw->payload, 62); + if (crc_calc != crc_expected) { + raw->idx = 0; + return -1; + } + + raw->idx = 0; + return 1; /* complete frame */ + } + + return 0; +} + +void mct7123_parse_imu(const uint8_t payload[64], + float *gyr_x, float *gyr_y, float *gyr_z, + float *acc_x, float *acc_y, float *acc_z, + float *mag_x, float *mag_y, float *mag_z, + float *temp, uint64_t *systimer_us, uint8_t *cycle) +{ + if (systimer_us) memcpy(systimer_us, payload, 8); + *gyr_x = r4(payload + 8); + *gyr_y = r4(payload + 12); + *gyr_z = r4(payload + 16); + *acc_x = r4(payload + 20); + *acc_y = r4(payload + 24); + *acc_z = r4(payload + 28); + *mag_x = r4(payload + 32); + *mag_y = r4(payload + 36); + *mag_z = r4(payload + 40); + *temp = r4(payload + 44); + if (cycle) *cycle = payload[61]; +} + +void mct7123_parse_att(const uint8_t payload[64], + float *roll, float *pitch, float *yaw, + float *qw, float *qx, float *qy, float *qz, + float *temp, uint8_t *running_status, uint32_t *fusion_status, + uint64_t *systimer_us, uint8_t *cycle) +{ + if (systimer_us) memcpy(systimer_us, payload, 8); + *roll = r4(payload + 8); + *pitch = r4(payload + 12); + *yaw = r4(payload + 16); + *qx = r4(payload + 20); + *qy = r4(payload + 24); + *qz = r4(payload + 28); + *qw = r4(payload + 32); + *temp = r4(payload + 36); + + if (fusion_status) { + uint32_t fs; + memcpy(&fs, payload + 38, 4); + *fusion_status = fs; + } + if (running_status) { + memcpy(running_status, payload + 42, 6); + } + if (cycle) *cycle = payload[61]; +} + +void mct7123_parse_cfg(const uint8_t payload[64], + uint8_t *sentence_ctrl, uint8_t *output_rate, + uint8_t *orientation, uint8_t *fusion_mode, + double *latitude, double *longitude, float *height) +{ + if (sentence_ctrl) *sentence_ctrl = payload[0]; + if (output_rate) *output_rate = payload[1]; + if (orientation) *orientation = payload[2]; + if (fusion_mode) *fusion_mode = payload[3]; + if (latitude) memcpy(latitude, payload + 4, 8); + if (longitude) memcpy(longitude, payload + 12, 8); + if (height) memcpy(height, payload + 20, 4); +} diff --git a/src/drivers/mct7123/mct7123_dec.h b/src/drivers/mct7123/mct7123_dec.h new file mode 100644 index 0000000..809a91f --- /dev/null +++ b/src/drivers/mct7123/mct7123_dec.h @@ -0,0 +1,76 @@ +/* + * SPDX-License-Identifier: GPL-3.0 + * + * MCT MD7123 Decoder Library + * Byte-level UART protocol decoder for MD7123 inertial measurement unit. + * + * Frame format (ref: MD7123 User Manual V2.0): + * Header: 0xEB 0x90 + * Msg_ID: 1 byte (0x81=IMU raw, 0x82=Attitude, 0x83=Config) + * Length: 1 byte (fixed 0x40 = 64 byte payload) + * Payload: 64 bytes + * Tail: 0xED + * CRC16: CCITT (poly 0x1021, init 0xFFFF) over payload[0..61] + */ + +#ifndef __MCT7123_DEC_H__ +#define __MCT7123_DEC_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#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); + +/** CRC16-CCITT over payload bytes (poly 0x1021, init 0xFFFF). */ +uint16_t mct7123_crc16(const uint8_t *data, size_t len); + +/** Parse payload into float fields after successful mct7123_input(). */ +void mct7123_parse_imu(const uint8_t payload[64], + float *gyr_x, float *gyr_y, float *gyr_z, + float *acc_x, float *acc_y, float *acc_z, + float *mag_x, float *mag_y, float *mag_z, + float *temp, uint64_t *systimer_us, uint8_t *cycle); + +void mct7123_parse_att(const uint8_t payload[64], + float *roll, float *pitch, float *yaw, + float *qw, float *qx, float *qy, float *qz, + float *temp, uint8_t *running_status, uint32_t *fusion_status, + uint64_t *systimer_us, uint8_t *cycle); + +/** MD7123 config frame (Msg_ID 0x83 / CAN ID 0x183). All pointers may be NULL. */ +void mct7123_parse_cfg(const uint8_t payload[64], + uint8_t *sentence_ctrl, uint8_t *output_rate, + uint8_t *orientation, uint8_t *fusion_mode, + double *latitude, double *longitude, float *height); + +#ifdef __cplusplus +} +#endif + +#endif /* __MCT7123_DEC_H__ */ diff --git a/src/drivers/mct7123/mct7123_serial_driver.cpp b/src/drivers/mct7123/mct7123_serial_driver.cpp new file mode 100644 index 0000000..39151e9 --- /dev/null +++ b/src/drivers/mct7123/mct7123_serial_driver.cpp @@ -0,0 +1,129 @@ +#include "mct7123_serial_driver.hpp" +#include + +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_serial_driver.hpp b/src/drivers/mct7123/mct7123_serial_driver.hpp new file mode 100644 index 0000000..154040d --- /dev/null +++ b/src/drivers/mct7123/mct7123_serial_driver.hpp @@ -0,0 +1,42 @@ +#pragma once + +extern "C" { +#include "mct7123_dec.h" +} + +#include +#include +#include + +#include "imu_driver.hpp" +#include "protocol/serial/serial_port.hpp" + +#ifndef DEG_TO_RAD +#define DEG_TO_RAD (0.01745329f) +#endif + +class Mct7123SerialDriver : public IMUDriver { + public: + Mct7123SerialDriver(uint16_t imu_id, const std::string& interface, int baudrate); + ~Mct7123SerialDriver(); + + void serial_rx_cbk(const uint8_t* data, size_t length); + + std::vector 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: + int baudrate_; + std::string interface_; + mutable std::shared_mutex imu_mutex_; + std::shared_ptr serial_; + + mct7123_raw_t raw_; + imu_sensor_data_t sensor_data_; +}; diff --git a/src/imu_driver.cpp b/src/imu_driver.cpp index 2dc3a7d..e12594f 100644 --- a/src/imu_driver.cpp +++ b/src/imu_driver.cpp @@ -11,11 +11,21 @@ #include "imu_driver.hpp" #include "drivers/hipnuc/hipnuc_imu_driver.hpp" +#include "drivers/mct7123/mct7123_serial_driver.hpp" +#include "drivers/mct7123/mct7123_can_driver.hpp" std::shared_ptr IMUDriver::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") { + 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"); } 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}; From 19785df4a128c47c45d10e6d3887737747372072 Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Mon, 3 Aug 2026 14:05:29 +0800 Subject: [PATCH 4/5] Improve HiPNUC J1939 initialization configuration --- 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 cd94935d12a781b772a07fc3761a02d5aefa0b6d Mon Sep 17 00:00:00 2001 From: chang <1984715306@qq.com> Date: Mon, 3 Aug 2026 14:05:29 +0800 Subject: [PATCH 5/5] Improve IMU usage and protocol documentation --- README.md | 248 ++++++++++++++++++++++++++++++++++++++++++++++++ README.zh-CN.md | 244 +++++++++++++++++++++++++++++++++++++++++++++++ debian/control | 4 +- 3 files changed, 494 insertions(+), 2 deletions(-) create mode 100644 README.md create mode 100644 README.zh-CN.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..807a300 --- /dev/null +++ b/README.md @@ -0,0 +1,248 @@ +# roboparty_imu + +**English** | [简体中文](README.zh-CN.md) + +RoboParty biped robot IMU driver library with C++ APIs and Python bindings. +It supports MCT7123 (MD7123) and HiPNUC devices. + +## 1. Features + +| Device | Interface | Frame format | Validation | +| --- | --- | --- | --- | +| MCT7123 (MD7123) | Serial, CAN FD | 69-byte wrapped serial frames; 64-byte CAN FD payloads | CRC16-CCITT (polynomial `0x1021`) | +| HiPNUC (HI226, HI229, etc.) | Serial, classic CAN (J1939) | Variable-length frames beginning with `5A A5`, or J1939 frames | CRC16 for serial; no additional application-layer checksum for J1939 | + +Main capabilities: + +- Connect classic CAN and CAN FD devices through the same CAN interface. +- Distinguish and route MCT7123 raw-data, attitude, and configuration frames. +- Allow multiple drivers to subscribe to the same CAN route with managed callback lifetimes. +- Read angular velocity, linear acceleration, magnetic field, quaternion, Euler angles, temperature, timestamp, and frame counter data. + +## 2. Build + +### ROS 2 and colcon + +The following example targets ROS 2 Humble on Ubuntu. Install the build dependencies first: + +```bash +sudo apt-get update +sudo apt-get install \ + build-essential \ + cmake \ + ccache \ + python3-colcon-common-extensions \ + python3-dev \ + pybind11-dev \ + libfmt-dev \ + libspdlog-dev \ + ros-humble-ament-cmake +``` + +For another ROS 2 distribution, replace `humble` in +`ros-humble-ament-cmake` with the corresponding distribution name. + +Place the repository under the `src/` directory of a ROS 2 workspace: + +```text +imu_ws/ +└── src/ + └── imu/ +``` + +Run the following commands from the workspace root: + +```bash +cd ~/imu_ws + +# Replace humble with the installed ROS 2 distribution when necessary. +source /opt/ros/humble/setup.bash + +# Build this package only. +colcon build --symlink-install --packages-select roboparty_imu + +# Source the workspace. +source install/setup.bash +``` + +To confirm that ROS 2 recognizes the package: + +```bash +colcon list +``` + +The output should include: + +```text +roboparty_imu src/imu (ros.ament_cmake) +``` + +After changing the source, run `colcon build` again and source +`install/setup.bash` in each new terminal. + +### Standalone CMake + +A standalone build requires a C++17 compiler, CMake 3.12 or later, Python 3 +development headers, pybind11, fmt, spdlog, and ccache. + +Run the following commands from the repository root: + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j"$(nproc)" +``` + +The Python module is generated in the `build/` directory. + +## 3. Communication setup + +### Default parameters + +| Device | Serial baud rate | CAN arbitration rate | CAN FD data rate | Common node ID | +| --- | --- | --- | --- | --- | +| MCT7123 | `921600 bit/s` | `500 kbit/s` | `2 Mbit/s` | `1` | +| HiPNUC | `115200 bit/s` | `500 kbit/s` | Not applicable | `8` | + +The node ID is a device parameter. The `imu_id` passed when creating a driver +must match the device's current node ID. The values above are commonly used by +this project and are not written to the device automatically. + +### SocketCAN + +MCT7123 CAN FD interface: + +```bash +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 classic CAN interface: + +```bash +sudo ip link set can0 down +sudo ip link set can0 type can bitrate 500000 +sudo ip link set can0 up +``` + +The driver only reads from and writes to an already configured SocketCAN +interface. It does not change the bitrate or link state automatically. + +### HiPNUC J1939 output configuration + +The following example enables pitch/roll, heading, and temperature output for +node ID `8`: + +```bash +cansend can0 0CEF0808#3D0106000A000000 # Pitch/roll, 10 ms (100 Hz) +cansend can0 0CEF0808#410106000A000000 # Heading, 10 ms (100 Hz) +cansend can0 0CEF0808#4301060064000000 # Temperature, 100 ms (10 Hz) +cansend can0 0CEF0808#0000060000000000 # Save parameters to flash +cansend can0 0CEF0808#00000600FF000000 # Reset the device +``` + +The configuration payload format is +`[register low byte][register high byte][command 0x06][reserved][32-bit little-endian value]`. +For example, `3D 01 06 00 0A 00 00 00` writes a `10 ms` period to register +`0x013D`. In configuration CAN ID `0x0CEF0808`, both the destination and source +addresses are `8`; adjust the CAN ID when the device uses another node ID. + +## 4. Python API + +Create a driver with `IMUDriver.create_imu()`: + +```python +import imu_py + +imu = imu_py.IMUDriver.create_imu( + imu_id=1, + interface_type="serial", # MCT7123: "serial"/"canfd"; HiPNUC: "serial"/"can" + interface="/dev/ttyUSB0", # Serial device path or SocketCAN interface name + imu_type="MCT7123", # "MCT7123" or "HIPNUC" + baudrate=921600, # Required for serial; ignored for CAN/CAN FD +) + +angular_velocity = imu.get_ang_vel() +linear_acceleration = imu.get_lin_acc() +euler = imu.get_euler() +cycle = imu.get_cycle() +``` + +Supported device and interface combinations: + +| Device | `interface_type` | `interface` | +| --- | --- | --- | +| MCT7123 | `serial` | Serial device path, such as `/dev/ttyUSB0` | +| MCT7123 | `canfd` | SocketCAN interface name, such as `can1` | +| HiPNUC | `serial` | Serial device path, such as `/dev/ttyUSB0` | +| HiPNUC | `can` | SocketCAN interface name, such as `can0` | + +Public accessors: + +| Method | Return value | Unit or description | +| --- | --- | --- | +| `get_imu_id()` | `int` | CAN node ID | +| `get_ang_vel()` | `[x, y, z]` | `rad/s` | +| `get_lin_acc()` | `[x, y, z]` | `m/s²` | +| `get_mag()` | `[x, y, z]` | `µT`; available in 9-axis mode | +| `get_quat()` | `[w, x, y, z]` | Attitude quaternion | +| `get_euler()` | `[roll, pitch, yaw]` | Degrees | +| `get_timestamp()` | `int` | Protocol timestamp converted to microseconds; `0` if unavailable | +| `get_temperature()` | `float` | Degrees Celsius | +| `get_cycle()` | `int` | MCT7123 frame counter from `0` to `255`; always `0` for HiPNUC | + +Run `help(imu_py.IMUDriver)` to view the inline Python documentation. + +## 5. C++ API + +```cpp +#include "imu_driver.hpp" + +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(); +``` + +## 6. CAN and CAN FD + +The following example uses two SocketCAN interfaces. The devices may share one +bus when they use the same arbitration rate and the host controller and bus +support CAN FD. The driver routes frames according to their type and length. + +```python +# HiPNUC classic CAN; the J1939 source address is commonly 8. +hipnuc = imu_py.IMUDriver.create_imu( + 8, "can", "can0", "HIPNUC") + +# MCT7123 CAN FD connected to can1. +mct7123 = imu_py.IMUDriver.create_imu( + 1, "canfd", "can1", "MCT7123") +``` + +MCT7123 uses the following CAN identifiers: + +| CAN identifier | Type | Frequency | +| --- | --- | --- | +| `0x181` | Raw IMU data: gyroscope, accelerometer, and magnetometer | Determined by the device output configuration | +| `0x182` | Attitude data: Euler angles and quaternion | Determined by the device output configuration | +| `0x183` | Configuration exchange | Event-driven | + +## 7. MCT7123 coordinate system + +MCT7123 uses coordinate system 9 by default: + +- The `+Uz` label corresponds to the positive X-axis. +- The `-Uy` label corresponds to the positive Y-axis. +- The `+Ux` label corresponds to the positive Z-axis. + +For the robot installation, the front faces right and the connector faces up. +When stationary, `AccZ` is approximately `+9.81 m/s²`, representing the +reaction to gravity when the Z-axis points upward. diff --git a/README.zh-CN.md b/README.zh-CN.md new file mode 100644 index 0000000..eea75bd --- /dev/null +++ b/README.zh-CN.md @@ -0,0 +1,244 @@ +# roboparty_imu + +[English](README.md) | **简体中文** + +RoboParty 双足机器人 IMU 驱动库，提供 C++ 接口和 Python 绑定，支持 +MCT7123（MD7123）与 HiPNUC 系列设备。 + +## 1. 功能概览 + +| 设备 | 通信接口 | 数据帧 | 校验 | +| --- | --- | --- | --- | +| MCT7123（MD7123） | 串口、CAN FD | 串口为 69 字节封装帧；CAN FD 为 64 字节载荷 | CRC16-CCITT（多项式 `0x1021`） | +| HiPNUC（HI226、HI229 等） | 串口、经典 CAN（J1939） | `5A A5` 开头的变长帧或 J1939 帧 | 串口使用 CRC16；J1939 无额外应用层校验 | + +主要能力： + +- 同一条 CAN 总线同时接入经典 CAN 和 CAN FD 设备。 +- 自动区分并路由 MCT7123 原始数据、姿态数据和配置响应。 +- 支持多个驱动订阅同一 CAN 路由，并安全管理回调生命周期。 +- 提供角速度、加速度、磁场、四元数、欧拉角、温度和帧计数器等数据。 + +## 2. 构建 + +### 使用 ROS 2 和 colcon + +以 ROS 2 Humble 和 Ubuntu 为例，先安装构建依赖： + +```bash +sudo apt-get update +sudo apt-get install \ + build-essential \ + cmake \ + ccache \ + python3-colcon-common-extensions \ + python3-dev \ + pybind11-dev \ + libfmt-dev \ + libspdlog-dev \ + ros-humble-ament-cmake +``` + +使用其他 ROS 2 发行版时，将 `ros-humble-ament-cmake` 中的 `humble` +替换为对应的发行版名称。 + +推荐将仓库放在 ROS 2 工作区的 `src/` 目录下： + +```text +imu_ws/ +└── src/ + └── imu/ +``` + +在工作区根目录执行： + +```bash +cd ~/imu_ws + +# 按实际安装的 ROS 2 发行版修改 humble +source /opt/ros/humble/setup.bash + +# 只构建本包 +colcon build --symlink-install --packages-select roboparty_imu + +# 加载本工作区 +source install/setup.bash +``` + +如果已经加载 ROS 2 环境，也可以确认包是否被正确识别： + +```bash +colcon list +``` + +输出中应包含： + +```text +roboparty_imu src/imu (ros.ament_cmake) +``` + +修改代码后重新执行 `colcon build`，并在新终端中重新加载 +`install/setup.bash`。 + +### 使用独立 CMake + +不使用 ROS 2 时，需要提前安装支持 C++17 的编译器、CMake 3.12、 +Python 3 开发头文件、pybind11、fmt、spdlog 和 ccache。 + +在仓库根目录执行： + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j"$(nproc)" +``` + +独立构建完成后，Python 模块位于 `build/` 目录。 + +## 3. 通信配置 + +### 默认参数 + +| 型号 | 串口波特率 | CAN 仲裁速率 | CAN FD 数据速率 | 常用节点编号 | +| --- | --- | --- | --- | --- | +| MCT7123 | `921600 bit/s` | `500 kbit/s` | `2 Mbit/s` | `1` | +| HiPNUC | `115200 bit/s` | `500 kbit/s` | 不适用 | `8` | + +节点编号是设备参数，创建驱动时传入的 `imu_id` 必须与设备当前编号一致。 +上表是本项目常用值，不会由驱动自动写入设备。 + +### SocketCAN + +MCT7123 CAN FD 接口： + +```bash +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 经典 CAN 接口： + +```bash +sudo ip link set can0 down +sudo ip link set can0 type can bitrate 500000 +sudo ip link set can0 up +``` + +驱动只读写已配置的 SocketCAN 接口，不会自动修改接口的波特率或启停状态。 + +### HiPNUC J1939 输出配置 + +以节点 ID `8` 为例，以下命令开启俯仰/横滚、航向角和温度输出： + +```bash +cansend can0 0CEF0808#3D0106000A000000 # 俯仰/横滚，10 ms（100 Hz） +cansend can0 0CEF0808#410106000A000000 # 航向角，10 ms（100 Hz） +cansend can0 0CEF0808#4301060064000000 # 温度，100 ms（10 Hz） +cansend can0 0CEF0808#0000060000000000 # 保存参数到 Flash +cansend can0 0CEF0808#00000600FF000000 # 复位设备 +``` + +配置载荷格式为 +`[寄存器低字节][寄存器高字节][命令 0x06][保留][32 位小端值]`。 +例如 `3D 01 06 00 0A 00 00 00` 表示向寄存器 `0x013D` 写入 +`10 ms` 周期。配置 CAN ID `0x0CEF0808` 中的目标地址和源地址均为 +`8`；设备使用其他节点编号时需要相应修改 CAN ID。 + +## 4. Python API + +通过 `IMUDriver.create_imu()` 创建驱动： + +```python +import imu_py + +imu = imu_py.IMUDriver.create_imu( + imu_id=1, + interface_type="serial", # MCT7123: "serial"/"canfd"; HiPNUC: "serial"/"can" + interface="/dev/ttyUSB0", # 串口路径或 SocketCAN 接口名 + imu_type="MCT7123", # "MCT7123" 或 "HIPNUC" + baudrate=921600, # 串口必填；CAN 和 CAN FD 模式忽略 +) + +angular_velocity = imu.get_ang_vel() +linear_acceleration = imu.get_lin_acc() +euler = imu.get_euler() +cycle = imu.get_cycle() +``` + +型号与通信接口的对应关系： + +| 型号 | `interface_type` | `interface` | +| --- | --- | --- | +| MCT7123 | `serial` | 串口设备路径，如 `/dev/ttyUSB0` | +| MCT7123 | `canfd` | SocketCAN 接口名，如 `can1` | +| HiPNUC | `serial` | 串口设备路径，如 `/dev/ttyUSB0` | +| HiPNUC | `can` | SocketCAN 接口名，如 `can0` | + +公共读取接口： + +| 方法 | 返回值 | 单位或说明 | +| --- | --- | --- | +| `get_imu_id()` | `int` | CAN 节点编号 | +| `get_ang_vel()` | `[x, y, z]` | `rad/s` | +| `get_lin_acc()` | `[x, y, z]` | `m/s²` | +| `get_mag()` | `[x, y, z]` | `µT`，九轴模式下有效 | +| `get_quat()` | `[w, x, y, z]` | 姿态四元数 | +| `get_euler()` | `[roll, pitch, yaw]` | 度 | +| `get_timestamp()` | `int` | 协议时间戳换算为微秒；设备未输出时间戳时为 `0` | +| `get_temperature()` | `float` | 摄氏度 | +| `get_cycle()` | `int` | MCT7123 的 `0`–`255` 帧计数器；HiPNUC 固定返回 `0` | + +可通过 `help(imu_py.IMUDriver)` 查看 Python 行内文档。 + +## 5. C++ API + +```cpp +#include "imu_driver.hpp" + +auto imu = IMUDriver::create_imu( + 1, "serial", "/dev/ttyUSB0", "MCT7123", 921600); + +std::vector 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(); +``` + +## 6. CAN 与 CAN FD + +下面示例使用两个 SocketCAN 接口。设备也可以 +接在同一条总线上，但必须使用相同仲裁速率，且主机控制器和总线都支持 +CAN FD。驱动按照帧类型和长度分别路由数据。 + +```python +# HiPNUC 经典 CAN；J1939 源地址通常为 8 +hipnuc = imu_py.IMUDriver.create_imu( + 8, "can", "can0", "HIPNUC") + +# MCT7123 CAN FD；当前示例连接在 can1 +mct7123 = imu_py.IMUDriver.create_imu( + 1, "canfd", "can1", "MCT7123") +``` + +MCT7123 使用以下 CAN 标识： + +| CAN 标识 | 类型 | 频率 | +| --- | --- | --- | +| `0x181` | IMU 原始数据：陀螺仪、加速度计、磁力计 | 取决于设备输出配置 | +| `0x182` | 姿态数据：欧拉角、四元数 | 取决于设备输出配置 | +| `0x183` | 配置交互 | 事件触发 | + +## 7. MCT7123 坐标系 + +MCT7123 默认使用坐标系 9： + +- 标签 `+Uz` 对应 X 轴正方向。 +- 标签 `-Uy` 对应 Y 轴正方向。 +- 标签 `+Ux` 对应 Z 轴正方向。 + +机器人安装方向为正面朝右、接插件朝天。静止时 `AccZ` 约为 +`+9.81 m/s²`，表示 Z 轴朝上时测得的重力反作用力。 diff --git a/debian/control b/debian/control index a58b4a1..180de77 100644 --- a/debian/control +++ b/debian/control @@ -9,6 +9,6 @@ Package: roboparty-imu Architecture: any Depends: ${shlibs:Depends}, ${misc:Depends}, roboparty-base (>= 1.0.0) Description: RoboParty IMU driver - IMU driver with HIPNUC support (CAN and serial protocols). + IMU driver with HiPNUC serial and classic CAN support, and MCT7123 + serial and CAN FD support. Provides static libraries, headers and Python bindings. - Installs udev rules for IMU serial devices.