Merge feat/radiomaster-rc-control: IMU integration + reliability fixes

Brings in the x86-validated IMU integration (Phases 1-5a), udev-stable
serial device names, motor/driver temperature monitoring, and RC/
kinematics fixes from the completed x86 branch. Resolved conflicts by
keeping xbox_motor_control_cpp untracked (build artifact) and merging
both branches' .gitignore entries.

.gitignore

@@ -0,0 +1,3 @@
+xbox_motor_control_cpp
+.DS_Store
+logs/

imu/imu_test

@@ -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;
+}

run_4wd.sh

@@ -1,9 +1,11 @@
 #!/usr/bin/env bash
 
 # 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance
-# 현재 고정 배선: 모터 드라이버 = ttyUSB1, RC 수신기(XR1) = ttyUSB0
-PORT="${1:-/dev/ttyUSB1}"
-RC_PORT="${RC_PORT:-/dev/ttyUSB0}"
+# udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된 심볼릭
+# 링크 사용 — ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름들은 고정이다.
+PORT="${1:-/dev/ttyMOTOR}"
+RC_PORT="${RC_PORT:-/dev/ttyRC}"
+IMU_PORT="${IMU_PORT:-/dev/ttyIMU}"
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 
 cd "$SCRIPT_DIR" || exit 1
@@ -12,11 +14,12 @@ echo "=================================================================="
 echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템"
 echo "=================================================================="
 
-# 1. USB Latency 1ms 단축 (모터 포트 + RC 수신기 포트 둘 다)
+# 1. USB Latency 1ms 단축 (모터/RC/IMU 포트 전부)
 if [ -f "./set_low_latency.sh" ]; then
-    echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT)"
+    echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT / IMU: $IMU_PORT)"
     ./set_low_latency.sh "$PORT"
     ./set_low_latency.sh "$RC_PORT"
+    ./set_low_latency.sh "$IMU_PORT"
 fi
 
 # 2. C++ 바이너리 존재 여부 확인 및 컴파일
@@ -26,16 +29,17 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then
 fi
 
 echo "=================================================================="
-echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작"
-echo "  - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT"
-echo "  - 라이다 기능 비활성화: --no_lidar 옵션"
+echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + IMU(HWT905) + 라이다 장애물 회피 시작"
+echo "  - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT / IMU 포트: $IMU_PORT"
+echo "  - 라이다 기능 비활성화: --no_lidar 옵션 / IMU 비활성화: --no_imu 옵션"
+echo "  - 매 주행마다 logs/에 CSV 로그 자동 저장 (끄려면 --no_log)"
 echo "  - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C"
 echo "=================================================================="
 
-# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port를 다시 넘기면
-# 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨)
+# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port/--imu_port를 다시
+# 넘기면 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨)
 if [ $# -gt 1 ]; then
-    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" "${@:2}"
+    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT" "${@:2}"
 else
-    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT"
+    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT"
 fi

set_low_latency.sh

@@ -8,7 +8,10 @@ if [ ! -e "$PORT" ]; then
     exit 1
 fi
 
-DEV_NAME=$(basename "$PORT")
+# /dev/ttyMOTOR 같은 udev 고정 심볼릭 링크로 넘어올 수 있으므로, sysfs 조회에
+# 필요한 실제 장치명(ttyUSB0 등)으로 반드시 풀어준다 — 안 그러면
+# /sys/bus/usb-serial/devices/ttyMOTOR 경로가 존재하지 않아 항상 폴백으로 샌다.
+DEV_NAME=$(basename "$(readlink -f "$PORT")")
 LATENCY_PATH="/sys/bus/usb-serial/devices/$DEV_NAME/latency_timer"
 
 if [ -f "$LATENCY_PATH" ]; then