From feddbf00b39490814c00ed124d618c6a69b89173 Mon Sep 17 00:00:00 2001 From: robin Date: Wed, 19 Aug 2026 11:55:42 +0900 Subject: [PATCH] Add HWT905-RS232 IMU reader (Phase 1: observation only, no ROS) Standalone C++/termios serial reader for the WitMotion HWT905 IMU (9600bps active-push protocol), printing accel/gyro/angle to stdout with optional CSV logging. --- .gitignore | 2 + CMakeLists.txt | 13 +++ readme.md | 0 src/main.cpp | 236 +++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 251 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 readme.md create mode 100644 src/main.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ce5b2fd --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +*.csv diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bdb2879 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.10) +project(duru_imu_ws CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +add_compile_options(-Wall -Wextra) + +add_executable(imu_reader src/main.cpp) diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..e69de29 diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..bed989d --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,236 @@ +// HWT905-RS232 (WitMotion) IMU reader — Phase 1: pure observation, no control loop. +// Reads the sensor's active-push protocol (0x55-prefixed packets) over a plain +// serial port (RS232-over-USB, e.g. /dev/ttyUSB0) and prints accel/gyro/angle to +// stdout, optionally logging to CSV. Not Modbus — the RS485 variant uses Modbus, +// this RS232 variant does not. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +constexpr uint8_t kFrameHeader = 0x55; +constexpr uint8_t kTypeAccel = 0x51; +constexpr uint8_t kTypeGyro = 0x52; +constexpr uint8_t kTypeAngle = 0x53; +constexpr uint8_t kTypeMag = 0x54; + +struct ImuState { + double accel[3] = {0, 0, 0}; // g + double gyro[3] = {0, 0, 0}; // deg/s + double angle[3] = {0, 0, 0}; // deg (roll, pitch, yaw) + double mag[3] = {0, 0, 0}; // raw counts + bool has_accel = false, has_gyro = false, has_angle = false, has_mag = false; +}; + +int16_t toInt16(uint8_t lo, uint8_t hi) { + return static_cast(static_cast(lo) | (static_cast(hi) << 8)); +} + +speed_t baudToSpeed(int baud) { + switch (baud) { + case 4800: return B4800; + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + default: + std::cerr << "[경고] 지원하지 않는 baud " << baud << ", 115200으로 대체\n"; + return B115200; + } +} + +int openSerialPort(const std::string &port, int baud) { + int fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "[오류] 포트 열기 실패: " << port << " (" << std::strerror(errno) << ")\n"; + return -1; + } + fcntl(fd, F_SETFL, 0); // switch back to blocking reads + + struct termios options; + if (tcgetattr(fd, &options) != 0) { + std::cerr << "[오류] tcgetattr 실패\n"; + close(fd); + return -1; + } + + speed_t speed = baudToSpeed(baud); + cfsetispeed(&options, speed); + cfsetospeed(&options, speed); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_cflag &= ~CRTSCTS; + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_iflag &= ~(IXON | IXOFF | IXANY); + options.c_iflag &= ~(INLCR | ICRNL); + options.c_oflag &= ~OPOST; + + options.c_cc[VMIN] = 1; + options.c_cc[VTIME] = 5; // 0.5s inter-byte timeout + + tcflush(fd, TCIFLUSH); + if (tcsetattr(fd, TCSANOW, &options) != 0) { + std::cerr << "[오류] tcsetattr 실패\n"; + close(fd); + return -1; + } + return fd; +} + +// Parses one validated 11-byte WitMotion frame (frame[0]==0x55, checksum ok) +// into the running ImuState. Returns true if this frame completed an +// accel+gyro+angle group worth printing (i.e. it was an angle frame). +bool applyFrame(const uint8_t *frame, ImuState &state) { + const uint8_t type = frame[1]; + switch (type) { + case kTypeAccel: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.accel[i] = raw / 32768.0 * 16.0; // g + } + state.has_accel = true; + return false; + case kTypeGyro: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.gyro[i] = raw / 32768.0 * 2000.0; // deg/s + } + state.has_gyro = true; + return false; + case kTypeAngle: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.angle[i] = raw / 32768.0 * 180.0; // deg + } + state.has_angle = true; + return true; + case kTypeMag: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.mag[i] = raw; + } + state.has_mag = true; + return false; + default: + return false; // time/quaternion/GPS frames etc. — ignored in Phase 1 + } +} + +} // namespace + +int main(int argc, char **argv) { + std::setvbuf(stdout, nullptr, _IOLBF, 4096); // line-buffer even when piped + + std::string port = "/dev/ttyUSB0"; + int baud = 9600; // HWT905-232 factory default (confirmed against this unit) + std::string log_path; + + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--port" && i + 1 < argc) { + port = argv[++i]; + } else if (arg == "--baud" && i + 1 < argc) { + baud = std::stoi(argv[++i]); + } else if (arg == "--log" && i + 1 < argc) { + log_path = argv[++i]; + } else if (arg == "--help") { + std::cout << "사용법: " << argv[0] + << " [--port /dev/ttyUSB0] [--baud 115200] [--log out.csv]\n"; + return 0; + } + } + + int fd = openSerialPort(port, baud); + if (fd < 0) return 1; + + std::ofstream log_file; + if (!log_path.empty()) { + log_file.open(log_path); + if (!log_file) { + std::cerr << "[오류] 로그 파일 열기 실패: " << log_path << "\n"; + return 1; + } + log_file << "t_s,ax_g,ay_g,az_g,gx_dps,gy_dps,gz_dps,roll_deg,pitch_deg,yaw_deg\n"; + } + + std::cout << "포트 " << port << " @ " << baud << "bps 에서 IMU 데이터 수신 시작 " + << "(Ctrl+C로 종료)\n"; + std::cout.flush(); + + const auto t_start = std::chrono::steady_clock::now(); + ImuState state; + uint8_t buf[11]; + size_t buf_len = 0; + + while (true) { + uint8_t byte; + ssize_t n = read(fd, &byte, 1); + if (n <= 0) { + if (n < 0 && errno != EAGAIN && errno != EINTR) { + std::cerr << "[오류] 시리얼 읽기 실패: " << std::strerror(errno) << "\n"; + break; + } + continue; + } + + if (buf_len == 0) { + if (byte != kFrameHeader) continue; // resync: wait for header + buf[buf_len++] = byte; + continue; + } + + buf[buf_len++] = byte; + if (buf_len < 11) continue; + + // Full 11-byte candidate frame collected — validate checksum. + uint8_t sum = 0; + for (int i = 0; i < 10; ++i) sum += buf[i]; + if (sum != buf[10]) { + // Checksum mismatch: resync by sliding one byte and rescanning for 0x55. + std::cerr << "[경고] 체크섬 불일치, 프레임 폐기\n"; + buf_len = 0; + continue; + } + + bool print_now = applyFrame(buf, state); + buf_len = 0; + + if (print_now && state.has_accel && state.has_gyro && state.has_angle) { + double t_s = std::chrono::duration(std::chrono::steady_clock::now() - t_start).count(); + std::printf( + "t=%7.3fs accel[g]=(%+.3f,%+.3f,%+.3f) gyro[dps]=(%+7.2f,%+7.2f,%+7.2f) " + "angle[deg]=(roll=%+7.2f,pitch=%+7.2f,yaw=%+7.2f)\n", + t_s, state.accel[0], state.accel[1], state.accel[2], state.gyro[0], state.gyro[1], + state.gyro[2], state.angle[0], state.angle[1], state.angle[2]); + + if (log_file) { + log_file << t_s << ',' << state.accel[0] << ',' << state.accel[1] << ',' + << state.accel[2] << ',' << state.gyro[0] << ',' << state.gyro[1] << ',' + << state.gyro[2] << ',' << state.angle[0] << ',' << state.angle[1] << ',' + << state.angle[2] << '\n'; + log_file.flush(); + } + } + } + + close(fd); + return 0; +}