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.
2026-08-19 11:55:42 +09:00
commit feddbf00b3
4 changed files with 251 additions and 0 deletions
@@ -0,0 +1,2 @@
+build/
+*.csv
@@ -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)
@@ -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 <cerrno>
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <fcntl.h>
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <termios.h>
+#include <unistd.h>
+
+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<int16_t>(static_cast<uint16_t>(lo) | (static_cast<uint16_t>(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<double>(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;
+}