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

xbox_motor_control.cpp

@@ -5,7 +5,9 @@
 #include <chrono>
 #include <cmath>
 #include <csignal>
+#include <ctime>
 #include <fcntl.h>
+#include <filesystem>
 #include <fstream>
 #include <iostream>
 #include <map>
@@ -315,33 +317,39 @@ public:
 
   bool readFeedback(float &l_fb, float &r_fb, int32_t &l_tick,
                     int32_t &r_tick, float &l_torque_a, float &r_torque_a,
-                    uint16_t &err_l, uint16_t &err_r) {
+                    uint16_t &err_l, uint16_t &err_r, int &l_temp_c,
+                    int &r_temp_c) {
     std::vector<uint16_t> regs;
-    // 0x20A5~0x20AE 10레지스터 연속 읽기: 에러코드(0x20A5/0x20A6)부터
-    // 포지션 틱, RPM 피드백, 실제 토크(전류)까지 한 번의 통신으로 확보.
+    // 0x20A4~0x20AE 11레지스터 연속 읽기: 모터온도(0x20A4)부터 에러코드
+    // (0x20A5/0x20A6), 포지션 틱, RPM 피드백, 실제 토크(전류)까지 한 번의
+    // 통신으로 확보. 0x20A4는 기존 읽기 범위 바로 앞이라 추가 통신 없이
+    // 온도까지 같이 받아온다(과열 사전감지용, doc/02 §읽기전용상태 참고).
     // 무부하(공중에 뜬) 바퀴는 전류가 급격히 낮아지므로 슬립 진단에 사용,
     // 에러코드는 과전류/과부하 등 드라이버 알람 발생 시 원인 진단에 사용.
-    if (port_->readRegs(slave_id_, 0x20A5, 10, regs)) {
-      err_l = regs[0];
-      err_r = regs[1];
+    if (port_->readRegs(slave_id_, 0x20A4, 11, regs)) {
+      l_temp_c = static_cast<int8_t>((regs[0] >> 8) & 0xFF);
+      r_temp_c = static_cast<int8_t>(regs[0] & 0xFF);
+
+      err_l = regs[1];
+      err_r = regs[2];
 
       // 수정: uint32_t -> int32_t 캐스팅은 2의 보수 표현에서 안전하게 부호가
       // 재해석됨. 기존의 "val - 0x100000000ULL" 방식은 uint64_t 승격 후
       // int32_t로 축소 캐스팅하는 구현정의 동작(현실적으로는 대부분 동작하지만
       // 명확성/이식성이 떨어짐)이라 제거함.
-      uint32_t val_l = (static_cast<uint32_t>(regs[2]) << 16) | regs[3];
+      uint32_t val_l = (static_cast<uint32_t>(regs[3]) << 16) | regs[4];
       l_tick = static_cast<int32_t>(val_l);
 
-      uint32_t val_r = (static_cast<uint32_t>(regs[4]) << 16) | regs[5];
+      uint32_t val_r = (static_cast<uint32_t>(regs[5]) << 16) | regs[6];
       r_tick = static_cast<int32_t>(val_r);
 
-      int16_t vl = static_cast<int16_t>(regs[6]);
-      int16_t vr = static_cast<int16_t>(regs[7]);
+      int16_t vl = static_cast<int16_t>(regs[7]);
+      int16_t vr = static_cast<int16_t>(regs[8]);
       l_fb = static_cast<float>(vl) * 0.1f; // 0.1RPM 단위 -> RPM
       r_fb = static_cast<float>(vr) * 0.1f;
 
-      int16_t tl = static_cast<int16_t>(regs[8]);
-      int16_t tr = static_cast<int16_t>(regs[9]);
+      int16_t tl = static_cast<int16_t>(regs[9]);
+      int16_t tr = static_cast<int16_t>(regs[10]);
       l_torque_a = static_cast<float>(tl) * 0.1f; // 0.1A 단위 -> A
       r_torque_a = static_cast<float>(tr) * 0.1f;
       return true;
@@ -349,6 +357,17 @@ public:
     return false;
   }
 
+  // 드라이버 자체 온도(0x20B0, 0.1℃). 열 시정수가 초 단위로 느려서 매 틱
+  // 읽을 필요가 없어 별도 저빈도 폴링용으로 분리(readFeedback 연속범위와
+  // 떨어져 있어 합치면 통신 1회가 늘어남).
+  bool readDriverTemp(float &temp_c) {
+    std::vector<uint16_t> regs;
+    if (!port_->readRegs(slave_id_, 0x20B0, 1, regs))
+      return false;
+    temp_c = static_cast<int16_t>(regs[0]) * 0.1f;
+    return true;
+  }
+
   // 정격전류(0x2033/0x2063)·최대전류(0x2034/0x2064) 실측 조회. 채널당
   // 2레지스터씩 떨어져 있어 L/R 두 번 읽음(설정값이라 시작 시 1회만 조회).
   bool readCurrentLimits(uint16_t &rated_l, uint16_t &max_l, uint16_t &rated_r,
@@ -628,7 +647,9 @@ struct termios2 { // <asm/termbits.h>의 커널 ABI와 동일 레이아웃 (glib
 };
 
 struct RcConfig {
-  std::string port = "/dev/ttyUSB0"; // 현재 고정 배선: RC 수신기(XR1) = ttyUSB0
+  // udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된
+  // 심볼릭 링크. ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름은 고정.
+  std::string port = "/dev/ttyRC";
   int baud = 420000; // CRSF 표준 보드레이트
 
   int ch_steer = 1;    // CH1: 좌우 조향
@@ -914,20 +935,285 @@ private:
   }
 };
 
+// -----------------------------------------------------------------------------
+// WitMotion HWT905-RS232 IMU 리더 (doc/06-imu-integration-plan.md Phase 1)
+// RS485/Modbus 변형과 달리 이 RS232 변형은 액티브 푸시 방식 바이너리 프로토콜을
+// 쓴다: [0]=0x55 헤더, [1]=타입(0x51 가속도/0x52 자이로/0x53 각도/0x54 지자기),
+// [2..9]=int16×4(LE, XYZ+예약), [10]=체크섬(0~9바이트 합의 하위 8비트).
+// imu/imu_test에서 실기로 검증된 프로토콜/스케일을 그대로 이식했다.
+// Phase 1 범위: 순수 관측만 한다 — 제어 루프에는 절대 개입하지 않는다.
+// -----------------------------------------------------------------------------
+struct ImuConfig {
+  // udev 규칙으로 고정된 심볼릭 링크 (모터=/dev/ttyMOTOR, RC=/dev/ttyRC와 별도)
+  std::string port = "/dev/ttyIMU";
+  int baud = 9600;                   // HWT905-232 공장 출하 기본값(실기 확인됨)
+  int failsafe_timeout_ms = 500;     // 이 시간 이상 미수신 시 미연결로 표시
+};
+
+class ImuReader {
+public:
+  struct Snapshot {
+    double accel[3] = {0, 0, 0}; // g (x,y,z)
+    double gyro[3] = {0, 0, 0};  // deg/s (x,y,z)
+    double angle[3] = {0, 0, 0}; // deg (roll, pitch, yaw)
+    bool valid = false;
+  };
+
+  explicit ImuReader(const ImuConfig &cfg) : cfg_(cfg) {}
+  ~ImuReader() { stop(); }
+
+  void start() {
+    running_ = true;
+    thread_ = std::thread(&ImuReader::run, this);
+  }
+
+  void stop() {
+    running_ = false;
+    if (thread_.joinable())
+      thread_.join();
+    closePort();
+  }
+
+  bool isConnected() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!port_open_ || !has_angle_)
+      return false;
+    auto now = std::chrono::steady_clock::now();
+    return std::chrono::duration_cast<std::chrono::milliseconds>(
+               now - last_rx_time_)
+               .count() <= cfg_.failsafe_timeout_ms;
+  }
+
+  Snapshot getSnapshot() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    Snapshot s;
+    for (int i = 0; i < 3; ++i) {
+      s.accel[i] = accel_[i];
+      s.gyro[i] = gyro_[i];
+      s.angle[i] = angle_[i];
+    }
+    s.valid = isConnectedLocked();
+    return s;
+  }
+
+private:
+  ImuConfig cfg_;
+  std::atomic<bool> running_{false};
+  std::thread thread_;
+  int fd_ = -1;
+
+  std::mutex mutex_;
+  double accel_[3] = {0, 0, 0};
+  double gyro_[3] = {0, 0, 0};
+  double angle_[3] = {0, 0, 0};
+  bool has_accel_ = false, has_gyro_ = false, has_angle_ = false;
+  std::chrono::steady_clock::time_point last_rx_time_{};
+  bool port_open_ = false;
+
+  static constexpr uint8_t kFrameHeader = 0x55;
+  static constexpr uint8_t kTypeAccel = 0x51;
+  static constexpr uint8_t kTypeGyro = 0x52;
+  static constexpr uint8_t kTypeAngle = 0x53;
+
+  bool isConnectedLocked() const {
+    if (!port_open_ || !has_angle_)
+      return false;
+    auto now = std::chrono::steady_clock::now();
+    return std::chrono::duration_cast<std::chrono::milliseconds>(
+               now - last_rx_time_)
+               .count() <= cfg_.failsafe_timeout_ms;
+  }
+
+  static 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));
+  }
+
+  static speed_t baudToSpeed(int baud) {
+    switch (baud) {
+    case 4800:
+      return B4800;
+    case 19200:
+      return B19200;
+    case 38400:
+      return B38400;
+    case 57600:
+      return B57600;
+    case 115200:
+      return B115200;
+    case 230400:
+      return B230400;
+    default:
+      return B9600;
+    }
+  }
+
+  bool openPort() {
+    fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd_ < 0)
+      return false;
+    fcntl(fd_, F_SETFL, 0); // 블로킹 읽기로 전환
+
+    struct termios options;
+    if (tcgetattr(fd_, &options) != 0) {
+      close(fd_);
+      fd_ = -1;
+      return false;
+    }
+
+    speed_t speed = baudToSpeed(cfg_.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.5초 바이트 간 타임아웃 (종료 감지용)
+
+    tcflush(fd_, TCIFLUSH);
+    if (tcsetattr(fd_, TCSANOW, &options) != 0) {
+      close(fd_);
+      fd_ = -1;
+      return false;
+    }
+    return true;
+  }
+
+  void closePort() {
+    if (fd_ >= 0) {
+      close(fd_);
+      fd_ = -1;
+    }
+    std::lock_guard<std::mutex> lock(mutex_);
+    port_open_ = false;
+  }
+
+  // 검증된 11바이트 프레임 하나를 상태에 반영. 각도 프레임 완료 시 true.
+  bool applyFrame(const uint8_t *frame) {
+    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]);
+        accel_[i] = raw / 32768.0 * 16.0; // g
+      }
+      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]);
+        gyro_[i] = raw / 32768.0 * 2000.0; // deg/s
+      }
+      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]);
+        angle_[i] = raw / 32768.0 * 180.0; // deg
+      }
+      has_angle_ = true;
+      return true;
+    default:
+      return false; // 시간/쿼터니언/GPS 등: Phase 1에서는 무시
+    }
+  }
+
+  void run() {
+    auto last_open_attempt =
+        std::chrono::steady_clock::now() - std::chrono::milliseconds(1001);
+    uint8_t buf[11];
+    size_t buf_len = 0;
+
+    while (running_) {
+      if (fd_ < 0) {
+        auto now = std::chrono::steady_clock::now();
+        if (std::chrono::duration_cast<std::chrono::milliseconds>(
+                now - last_open_attempt)
+                .count() > 1000) {
+          last_open_attempt = now;
+          if (openPort()) {
+            std::lock_guard<std::mutex> lock(mutex_);
+            port_open_ = true;
+            std::cout << "\n[정보] IMU 포트(" << cfg_.port << ") 연결 성공.\n";
+          } else {
+            std::cerr << "\n[경고] IMU 포트(" << cfg_.port
+                      << ") 열기 실패. 1초 후 재시도.\n";
+          }
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+        buf_len = 0;
+        continue;
+      }
+
+      uint8_t byte;
+      ssize_t n = read(fd_, &byte, 1);
+      if (n <= 0) {
+        if (n < 0 && errno != EAGAIN && errno != EINTR) {
+          std::cerr << "\n[경고] IMU 포트 통신 오류. 재연결 시도...\n";
+          closePort();
+          buf_len = 0;
+        }
+        continue; // n==0: VTIME 타임아웃, running_ 재확인 후 계속 대기
+      }
+
+      if (buf_len == 0) {
+        if (byte != kFrameHeader)
+          continue; // 헤더 대기하며 재동기화
+        buf[buf_len++] = byte;
+        continue;
+      }
+
+      buf[buf_len++] = byte;
+      if (buf_len < 11)
+        continue;
+
+      uint8_t sum = 0;
+      for (int i = 0; i < 10; ++i)
+        sum += buf[i];
+      if (sum != buf[10]) {
+        buf_len = 0; // 체크섬 불일치: 프레임 폐기 후 재동기화
+        continue;
+      }
+
+      bool angle_done;
+      {
+        std::lock_guard<std::mutex> lock(mutex_);
+        angle_done = applyFrame(buf);
+        if (angle_done)
+          last_rx_time_ = std::chrono::steady_clock::now();
+      }
+      buf_len = 0;
+      (void)angle_done;
+    }
+  }
+};
+
 int main(int argc, char **argv) {
   std::signal(SIGINT, signalHandler);
   std::signal(SIGTERM, signalHandler);
 
-  std::string port1 = "/dev/ttyUSB1"; // 현재 고정 배선: 모터 드라이버 = ttyUSB1
+  // udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된
+  // 심볼릭 링크. ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름은 고정.
+  std::string port1 = "/dev/ttyMOTOR";
   std::string port2 = "";
   uint8_t id1 = 1;
   uint8_t id2 = 2;
   bool bcast_mode = false;
   RcConfig rc_cfg;
   float max_spin_v = 0.30f;       // m/s
-  float track_width = 0.576f;     // 좌우 바퀴 중심 거리 576mm
-  float wheelbase = 0.3684f;      // 전후 바퀴 중심 거리 368.4mm
   float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름
+  float k_skid_override = -1.0f;  // 0 이상이면 Phase 5a 기본 캘리브레이션값을 대체
+  float effective_w_override = -1.0f; // 0 이상이면 Phase 5a 기본 캘리브레이션값을 대체
   // 가속은 역기전력 보호회로가 하드웨어로 구성되어 목표값에 1:1로 즉시
   // 추종하므로 별도의 가속 한계값이 없다. decel_rate/jerk_rate(및
   // spin_jerk_rate)는 감속에만 적용된다.
@@ -939,6 +1225,27 @@ int main(int argc, char **argv) {
                                    // 살짝 벗어나게 해 스크럽 마찰 저항을 줄임)
   float bumper_offset = 0.045f;   // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리
 
+  // Phase 3 — 직진 헤딩 홀드 PI 트림 (doc/06-imu-integration-plan.md §Phase 3).
+  // 조향 중립 + 라이다 회피 미개입 + 실제 주행 중일 때만 개입해, 진입 순간의
+  // fused_yaw_deg를 목표로 고정하고 그로부터 벗어난 만큼을 PI로 보정한다.
+  bool heading_hold_enabled = true;
+  float heading_kp = 0.6f;         // rad/s per rad 오차
+  float heading_ki = 0.15f;        // rad/s per (rad·s) 적분 오차
+  float heading_max_trim = 0.15f;  // rad/s (트림 상한 — 의도적 조향을 압도하지 않도록)
+
+  // Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (doc §Phase 4). 지령
+  // omega 대비 바퀴 피드백과 무관한 IMU 원시 gyro_z 비율이 임계값 밑으로
+  // 지속되면 "전신 슬립"(빙판/젖은 잔디 등 4륜 동시 헛돎)으로 판정해 라이다
+  // 회피의 speed_scale과 같은 패턴으로 속도를 일시적으로 낮춘다. 제자리
+  // 회전은 반력 토크로 인한 정상적인 스크럽 손실(지령 대비 40~60% 미달도
+  // 흔함)이 있어 문턱값을 낮게(=크게 미달일 때만) 잡아 정상 회전을 오탐하지
+  // 않게 한다.
+  bool whole_slip_enabled = true;
+  float whole_slip_min_omega = 0.15f;    // rad/s (이 미만 지령은 판정 보류)
+  float whole_slip_ratio_threshold = 0.15f; // |imu_omega|/|omega| 가 이 밑이면 슬립 후보
+  int whole_slip_debounce_ticks = 15;    // 틱 (100Hz 기준 150ms) 연속돼야 확정
+  float whole_slip_speed_scale = 0.5f;   // 슬립 확정 시 v_x/omega에 곱하는 배율
+
   // 바퀴별 이상(들뜸/걸림) 판정 파라미터: 속도 폐루프 특성상 "지령 대비 실제
   // 속도" 하나만으로는 무부하(들뜸) 판정이 안 되므로, 동료 바퀴 대비 전류
   // 편차와 함께 두 축으로 판정한다.
@@ -951,13 +1258,21 @@ int main(int argc, char **argv) {
                                        // 지연이 동반되면 걸림/과부하로 판정
   int airborne_debounce_ticks = 5;    // 틱 (100Hz 기준 50ms) 연속 들뜸 판정
                                        // 시에만 실제로 목표속도를 낮춤
-  std::string log_path = ""; // 비어있으면 로깅 비활성화. --log <파일경로>로 지정
+  // 비어있으면 프로그램 시작 시 logs/drive_YYYYmmdd_HHMMSS.csv로 자동 생성된다
+  // (매 주행마다 RC/IMU/모터 로깅을 자동으로 별도 파일에 남기기 위함).
+  // --log <파일경로>로 직접 지정하거나 --no_log로 완전히 끌 수 있다.
+  std::string log_path = "";
+  bool no_log = false;
   // 실측 결과 정격15A/최대30A(공장 기본값) 그대로였음. 정격 위로 5A 여유는
   // 남기되, 걸림/과부하 상황에서 30A까지 밀어붙이며 3초씩 버티다 과부하
   // 알람이 터지는 걸 막기 위해 기본값을 20A로 낮춤. --max_current_a로 조정 가능.
   // 음수를 주면 미변경(공장/기존 설정 유지).
   float max_current_a = 20.0f;
 
+  // IMU 파라미터 (doc/06-imu-integration-plan.md Phase 1: 순수 관측)
+  bool use_imu = true;
+  ImuConfig imu_cfg;
+
   // LiDAR Parameters & Robot Physical Specs (User Specification)
   bool use_lidar = true;
   std::string lidar_config = "mid360s_config.json";
@@ -987,10 +1302,10 @@ int main(int argc, char **argv) {
       id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
     else if (arg == "--id2" && i + 1 < argc)
       id2 = static_cast<uint8_t>(std::stoi(argv[++i]));
-    else if (arg == "--track_width" && i + 1 < argc)
-      track_width = std::stof(argv[++i]);
-    else if (arg == "--wheelbase" && i + 1 < argc)
-      wheelbase = std::stof(argv[++i]);
+    else if (arg == "--k_skid" && i + 1 < argc)
+      k_skid_override = std::stof(argv[++i]);
+    else if (arg == "--effective_w" && i + 1 < argc)
+      effective_w_override = std::stof(argv[++i]);
     else if (arg == "--radius" && i + 1 < argc)
       wheel_radius = std::stof(argv[++i]);
     else if (arg == "--bumper" && i + 1 < argc)
@@ -1015,6 +1330,8 @@ int main(int argc, char **argv) {
       airborne_debounce_ticks = std::stoi(argv[++i]);
     else if (arg == "--log" && i + 1 < argc)
       log_path = argv[++i];
+    else if (arg == "--no_log")
+      no_log = true;
     else if (arg == "--max_current_a" && i + 1 < argc)
       max_current_a = std::stof(argv[++i]);
     else if (arg == "--bcast" || arg == "--broadcast")
@@ -1041,6 +1358,12 @@ int main(int argc, char **argv) {
       max_z = std::stof(argv[++i]);
     else if (arg == "--no_lidar")
       use_lidar = false;
+    else if (arg == "--imu_port" && i + 1 < argc)
+      imu_cfg.port = argv[++i];
+    else if (arg == "--imu_baud" && i + 1 < argc)
+      imu_cfg.baud = std::stoi(argv[++i]);
+    else if (arg == "--no_imu")
+      use_imu = false;
     else if (arg == "--rc_port" && i + 1 < argc)
       rc_cfg.port = argv[++i];
     else if (arg == "--rc_baud" && i + 1 < argc)
@@ -1085,15 +1408,55 @@ int main(int argc, char **argv) {
       rc_cfg.failsafe_timeout_ms = std::stoi(argv[++i]);
     else if (arg == "--max_spin_v" && i + 1 < argc)
       max_spin_v = std::stof(argv[++i]);
+    else if (arg == "--no_heading_hold")
+      heading_hold_enabled = false;
+    else if (arg == "--heading_kp" && i + 1 < argc)
+      heading_kp = std::stof(argv[++i]);
+    else if (arg == "--heading_ki" && i + 1 < argc)
+      heading_ki = std::stof(argv[++i]);
+    else if (arg == "--heading_max_trim" && i + 1 < argc)
+      heading_max_trim = std::stof(argv[++i]);
+    else if (arg == "--no_whole_slip")
+      whole_slip_enabled = false;
+    else if (arg == "--whole_slip_min_omega" && i + 1 < argc)
+      whole_slip_min_omega = std::stof(argv[++i]);
+    else if (arg == "--whole_slip_ratio" && i + 1 < argc)
+      whole_slip_ratio_threshold = std::stof(argv[++i]);
+    else if (arg == "--whole_slip_debounce_ticks" && i + 1 < argc)
+      whole_slip_debounce_ticks = std::stoi(argv[++i]);
+    else if (arg == "--whole_slip_speed_scale" && i + 1 < argc)
+      whole_slip_speed_scale = std::stof(argv[++i]);
+  }
+
+  // --log를 직접 지정하지 않았으면 이 저장소(프로젝트 폴더) 안의 logs/에
+  // 타임스탬프 파일명으로 자동 생성한다 — 매 주행마다 RC/IMU/모터 로깅을
+  // 빠짐없이 남기기 위함. --no_log로 완전히 끌 수 있다.
+  if (log_path.empty() && !no_log) {
+    std::filesystem::create_directories("logs");
+    std::time_t now_c = std::time(nullptr);
+    char ts_buf[32];
+    std::strftime(ts_buf, sizeof(ts_buf), "%Y%m%d_%H%M%S", std::localtime(&now_c));
+    log_path = std::string("logs/drive_") + ts_buf + ".csv";
   }
 
   constexpr float PI_VAL = 3.14159265358979323846f;
   float rpm_per_ms = 60.0f / (2.0f * PI_VAL * wheel_radius);
 
-  // 4WD 유효 선회 계수
-  float effective_w =
-      std::sqrt(track_width * track_width + wheelbase * wheelbase);
-  float k_skid = (track_width + (wheelbase * wheelbase) / track_width) / 2.0f;
+  // effective_w — Phase 5a 정적 캘리브레이션. 기하학적 공식값(대각선 길이,
+  // 0.684)은 제자리 회전 실측 대비 작았다. 전용 회전 로그 2개(무결점
+  // 405틱, 두 로그 개별 중앙값 0.855/0.904로 서로 일치)에서 역산한
+  // 중앙값 0.87을 대신 사용한다. --effective_w로 재조정 가능.
+  float effective_w = (effective_w_override >= 0.0f) ? effective_w_override : 0.87f;
+  // k_skid — Phase 5a 정적 캘리브레이션(doc/06-imu-integration-plan.md §5a):
+  // 기하학적 공식값(트랙폭/휠베이스로만 계산, 0.406)은 실측 대비 계속
+  // 작게 나왔다(회전이 지령보다 항상 덜 도는 원인 중 하나). 두 차례
+  // 실주행 로그(커브 선회 중 무결점 총 5,150틱)에서 바퀴 실측 속도차 대
+  // IMU 실측 회전율로 역산한 중앙값이 0.51 적용 전 0.510, 적용 후에도
+  // 0.517로 수렴해 그대로 0.51을 유지한다. 회전 반경에 따라
+  // 0.44(완만한 커브)~0.53(급한 커브)로 흔들리는 편이라 0.51은 그
+  // 절충값 — 반경별로 정밀하게 맞추려면 Phase 5b(온라인 ICR 추정)가
+  // 필요하다. --k_skid로 재조정 가능.
+  float k_skid = (k_skid_override >= 0.0f) ? k_skid_override : 0.51f;
 
   std::cout << "\n============================================================="
                "=====\n";
@@ -1129,20 +1492,27 @@ int main(int argc, char **argv) {
   std::cout
       << "==================================================================\n";
 
-  // CSV 로깅: --log <경로> 지정 시 매 틱 지령/피드백/전류/휠상태를 파일로 기록.
-  // 실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로, 문제가 된
-  // 구간(턱 넘는 지점 등)을 나중에 잘라서 분석하기 위한 용도.
+  // CSV 로깅: 기본적으로 매 주행마다 logs/에 자동 저장된다(위 auto-path 로직).
+  // RC 채널, IMU(자이로/각도), 모터 지령/피드백/전류/휠상태/엔코더 raw tick까지
+  // 매 틱 전부 기록한다. 실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로,
+  // 문제가 된 구간(턱 넘는 지점 등)이나 IMU-엔코더 융합용 데이터를 나중에 잘라서
+  // 분석하기 위한 용도.
   std::ofstream log_stream;
   if (!log_path.empty()) {
     log_stream.open(log_path);
     if (log_stream.is_open()) {
       log_stream << "t_ms,state,rc_steer,rc_throttle,rc_ok,brake,vmax,vx,omega,"
                     "ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,"
+                    "imu_ok,imu_gyro_z,imu_roll,imu_pitch,imu_yaw,imu_yaw_rel,"
                     "cmd_fl,cmd_fr,cmd_rl,cmd_rr,"
                     "fb_fl,fb_fr,fb_rl,fb_rr,"
                     "amp_fl,amp_fr,amp_rl,amp_rr,"
+                    "temp_fl,temp_fr,temp_rl,temp_rr,temp_drv_front,temp_drv_rear,"
                     "stat_fl,stat_fr,stat_rl,stat_rr,"
                     "err_fl,err_fr,err_rl,err_rr,"
+                    "tick_fl,tick_fr,tick_rl,tick_rr,"
+                    "wheel_omega,imu_omega,omega_residual,fused_yaw,odom_x,odom_y,"
+                    "heading_hold,heading_target,heading_trim,whole_slip,"
                     "dist_axle,comm_ms\n";
       std::cout << "  - CSV 로그 기록: " << log_path << "\n";
     } else {
@@ -1174,6 +1544,11 @@ int main(int argc, char **argv) {
                  "동시에 읽으면 RC 데이터 파싱이 깨질 수 있으니 서로 다른 "
                  "포트로 지정하세요.\n\n";
   }
+  if (use_imu && (imu_cfg.port == port1 || imu_cfg.port == rc_cfg.port)) {
+    std::cout << "\n[경고] --imu_port(" << imu_cfg.port
+              << ")가 모터 또는 RC 수신기 포트와 동일합니다. 서로 다른 "
+                 "포트로 지정하세요.\n\n";
+  }
 
   RcReceiver rc(rc_cfg);
   rc.start();
@@ -1181,6 +1556,14 @@ int main(int argc, char **argv) {
             << rc_cfg.port << "). 신호 수신 전까지는 안전을 위해 브레이크가 "
                "걸린 상태로 대기합니다.\n";
 
+  ImuReader imu(imu_cfg);
+  if (use_imu) {
+    imu.start();
+    std::cout << "[정보] IMU(HWT905-RS232) 리더 시작 (" << imu_cfg.port << " @"
+              << imu_cfg.baud << "bps). Phase 1: 관측 전용, 제어에는 개입하지 "
+                 "않습니다.\n";
+  }
+
   SerialPort sp1, sp2;
   if (!sp1.openPort(port1))
     return 1;
@@ -1268,7 +1651,16 @@ int main(int argc, char **argv) {
       return target_vel;
     }
 
-    bool is_accelerating = std::abs(target_vel) > std::abs(current_vel);
+    // 같은 방향으로 더 빨라지는 경우만 즉시 스냅한다. 부호가 바뀌는 역전은
+    // (현재속도가 0이 아닌데 목표가 반대부호로 바뀌는 경우) 저크 제한 감속
+    // 경로로 보낸다 — 정지 직전 저속 구간에서 RC/조향 노이즈만으로 목표가
+    // 순간 반대부호로 흔들려도 그대로 즉시 스냅되면 "전진→후진→전진"처럼
+    // 튀는 현상이 생기고, 실제 반전 요청이라도 0을 관통하는 순간 스냅은
+    // 역기전력 스파이크가 오히려 일반 가속보다 더 크다.
+    bool same_direction =
+        (current_vel == 0.0f) || (target_vel * current_vel > 0.0f);
+    bool is_accelerating =
+        same_direction && (std::abs(target_vel) > std::abs(current_vel));
     if (is_accelerating) {
       current_accel = 0.0f;
       return target_vel;
@@ -1307,6 +1699,14 @@ int main(int argc, char **argv) {
   bool just_landed_rl = false, just_landed_rr = false;
   char wheel_status_str[5] = "OOOO";
   bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부
+  // 드라이버 온도(0x20B0)는 열 시정수가 초 단위로 느려 매 틱 통신할
+  // 필요가 없다 — 30틱(실측 ~27Hz 기준 약 1.1초)마다만 갱신.
+  float front_driver_temp_c = 0.0f, rear_driver_temp_c = 0.0f;
+  int driver_temp_poll_counter = 0;
+  constexpr int kDriverTempPollTicks = 30;
+  // 하드웨어 스펙(doc/01) 동작온도 상한 50°C, 드라이버 자체 과열 보호
+  // 임계값 기본 80°C 대비 여유를 두고 조기 경보를 띄우는 기준.
+  constexpr int kOverheatWarnC = 60;
   bool prev_brake_on = false; // 브레이크(CH5/Fail-safe) 직전 틱 상태
   bool prev_rc_ok = false;    // RC 연결 직전 틱 상태 (Fail-safe 로그용)
   float omega_max = (effective_w > 0.0f) ? (max_spin_v / (effective_w / 2.0f))
@@ -1329,6 +1729,40 @@ int main(int argc, char **argv) {
   bool steer_initialized = false, throttle_initialized = false;
   int steer_reject_streak = 0, throttle_reject_streak = 0;
 
+  // IMU Phase 1: 순수 관측용 표시 오프셋. 로봇 전원이 켜질 때(또는 이
+  // 프로그램이 시작될 때) IMU가 처음 보고하는 절대 yaw를 "정면(0°)"
+  // 기준으로 저장해, 화면에는 그 시점 대비 상대 yaw를 보여준다. 이건
+  // 순수 표시용 계산일 뿐 제어 로직에는 전혀 관여하지 않는다 — 헤딩을
+  // 실제로 잠그거나 보정하는 건 Phase 3의 몫이다.
+  double imu_yaw_offset = 0.0;
+  bool imu_yaw_offset_set = false;
+
+  // Phase 2 — 헤딩 적분 + x,y 오도메트리 (여전히 순수 관측, 제어 미개입).
+  // 처음엔 IMU 내장 AHRS가 계산해 주는 imu_yaw_rel을 θ로 그대로 썼으나,
+  // 실측 루프백(한 바퀴 돌아 제자리 복귀) 주행 로그로 검증한 결과 그 값이
+  // 같은 장치의 원시 자이로(gyro_z)와도 1초 평균 구간 기준 부호 일치율이
+  // 72%에 불과함이 드러났다(회전 중 모터 전류로 인한 지자기 간섭 등으로
+  // 추정 — 저가 AHRS의 흔한 증상). 실제로 그 루프백 로그에서 odom이 시작
+  // 위치로 전혀 돌아오지 못했다. 반면 바퀴 인코더 기반 wheel_omega와 원시
+  // gyro_z는 부호 일치율 99.3%로 서로 강하게 신뢰할 수 있음을 같은 로그에서
+  // 확인했다. 그래서 θ는 IMU 융합 yaw 대신 wheel_omega와 원시 gyro_z의
+  // 평균을 자체 적분한 fused_yaw_deg를 쓴다(IMU 미연결 시엔 wheel_omega만
+  // 사용). IMU 융합 yaw(imu_yaw_rel)는 비교용으로 HUD/CSV에 계속 남긴다.
+  double odom_x = 0.0, odom_y = 0.0; // m, Phase 1 오프셋 설정 시점 기준
+  double fused_yaw_deg = 0.0;        // deg, wheel_omega+gyro_z 자체 적분 헤딩
+  auto last_odom_time = std::chrono::steady_clock::now();
+
+  // Phase 3 — 헤딩 홀드 PI 상태. 적분 windup 방지용 상한은 트림 상한을
+  // Ki로 나눠 "적분항 단독으로도 트림 상한을 넘지 않는" 값으로 고정한다.
+  bool heading_hold_active_prev = false;
+  double heading_target_deg = 0.0;
+  double heading_error_integral = 0.0;
+  const float heading_integral_limit =
+      (heading_ki > 1e-6f) ? (heading_max_trim / heading_ki) : 0.0f;
+
+  // Phase 4 — 전신 슬립 디바운스 카운터
+  int whole_slip_count = 0;
+
   while (g_running) {
     // ---------------------------------------------------------------------
     // RC 수신기 채널 읽기 (doc/joystick.md §2, §3 기능 명세서 기준)
@@ -1388,6 +1822,25 @@ int main(int argc, char **argv) {
         raw_ch[ch] = 0; // 아직 수신 못한 채널은 0으로 표시
     }
 
+    // IMU Phase 1: 순수 관측. 제어 로직에는 관여하지 않는다.
+    ImuReader::Snapshot imu_snap = use_imu ? imu.getSnapshot() : ImuReader::Snapshot{};
+    if (imu_snap.valid && !imu_yaw_offset_set) {
+      imu_yaw_offset = imu_snap.angle[2];
+      imu_yaw_offset_set = true;
+      std::cout << "\n[정보] IMU 정면 기준 설정: 시작 시점 yaw " << imu_yaw_offset
+                << "도를 0도(정면)로 저장했습니다.\n";
+    }
+    double imu_yaw_rel = 0.0;
+    if (imu_yaw_offset_set) {
+      imu_yaw_rel = imu_snap.angle[2] - imu_yaw_offset;
+      // [-180, 180) 범위로 정규화
+      while (imu_yaw_rel > 180.0) imu_yaw_rel -= 360.0;
+      while (imu_yaw_rel < -180.0) imu_yaw_rel += 360.0;
+    }
+    // 바퀴 피드백과 무관한 IMU 원시 요레이트. Phase 2(휠-IMU 융합 헤딩)와
+    // Phase 4(지령-IMU 전신 슬립 감지, 명령 결정 이전에 필요)가 공유한다.
+    float imu_omega_rad = imu_snap.gyro[2] * (PI_VAL / 180.0f); // rad/s
+
     if (!rc_ok && prev_rc_ok) {
       std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms
                 << "ms 이상 미수신) -> 비상 정지.\n";
@@ -1404,6 +1857,7 @@ int main(int argc, char **argv) {
 
     float v_x = 0.0f;   // 선속도 m/s
     float omega = 0.0f; // 각속도 rad/s
+    bool user_steer_centered = false; // Phase 3 헤딩 홀드 진입 조건용
 
     if (!brake_on) {
       // 3.3 전후진 속도 계산 (CH2) — 실측 결과 raw 값이 높을수록 전진(+),
@@ -1438,6 +1892,7 @@ int main(int argc, char **argv) {
                   static_cast<float>(rc_cfg.steer_max - rc_cfg.steer_center_hi)) *
                  omega_max;
       }
+      user_steer_centered = (omega == 0.0f);
     }
 
     // ---------------------------------------------------------------------
@@ -1445,11 +1900,12 @@ int main(int argc, char **argv) {
     // (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작)
     // ---------------------------------------------------------------------
     std::string lidar_telemetry = "LIDAR:OFF";
+    float avoid_omega_offset = 0.0f; // Phase 3 헤딩 홀드가 라이다 회피와
+                                     // 충돌하지 않도록 블록 밖에서도 확인
     if (use_lidar) {
       ObstacleStatus obs = lidar_detector.getStatus();
       if (obs.connected) {
         float speed_scale = 1.0f;
-        float avoid_omega_offset = 0.0f;
 
         // 1. Forward / Backward Automatic Stopping
         if (v_x > 0.01f) {
@@ -1509,6 +1965,68 @@ int main(int argc, char **argv) {
       }
     }
 
+    // ---------------------------------------------------------------------
+    // Phase 3 — 직진 헤딩 홀드 (doc/06-imu-integration-plan.md §Phase 3).
+    // 조향 중립 + 라이다 회피 미개입 + 실제 주행 중일 때만 개입한다. 진입
+    // 순간의 fused_yaw_deg를 목표 헤딩으로 고정(lock)하고, 이후 그로부터
+    // 벗어난 만큼(오차)을 작은 PI로 보정해 omega에 트림을 더한다. 사용자
+    // 조향/정지/라이다 회피가 개입하는 즉시 해제되고 적분항도 리셋된다.
+    // Phase 2 검증(실측 루프백 로그)에서 wheel_omega/gyro_z 부호 일치율이
+    // 99.3%로 확인된 fused_yaw_deg를 피드백으로 쓴다 — IMU 단독 융합 yaw는
+    // 회전 중 신뢰도가 낮아 제어 피드백에서 제외했다(imu_yaw_rel은 HUD/CSV
+    // 비교용으로만 남김).
+    bool heading_hold_condition = heading_hold_enabled && !brake_on &&
+                                  user_steer_centered &&
+                                  avoid_omega_offset == 0.0f && v_x != 0.0f;
+    float heading_trim_omega = 0.0f; // HUD/CSV 노출용
+    if (heading_hold_condition) {
+      if (!heading_hold_active_prev) {
+        heading_target_deg = fused_yaw_deg;
+        heading_error_integral = 0.0;
+      }
+      double heading_error_deg = heading_target_deg - fused_yaw_deg;
+      while (heading_error_deg > 180.0) heading_error_deg -= 360.0;
+      while (heading_error_deg < -180.0) heading_error_deg += 360.0;
+      double heading_error_rad = heading_error_deg * (PI_VAL / 180.0);
+
+      heading_error_integral += heading_error_rad * dt;
+      heading_error_integral = std::clamp(
+          heading_error_integral,
+          static_cast<double>(-heading_integral_limit),
+          static_cast<double>(heading_integral_limit));
+
+      heading_trim_omega = static_cast<float>(
+          heading_kp * heading_error_rad + heading_ki * heading_error_integral);
+      heading_trim_omega =
+          std::clamp(heading_trim_omega, -heading_max_trim, heading_max_trim);
+      omega += heading_trim_omega;
+    } else {
+      heading_error_integral = 0.0;
+    }
+    heading_hold_active_prev = heading_hold_condition;
+
+    // ---------------------------------------------------------------------
+    // Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (doc §Phase 4). 지령
+    // omega(라이다 회피/헤딩 홀드 트림까지 반영된 최종값) 대비 바퀴 피드백과
+    // 완전히 독립적인 IMU 원시 gyro_z의 비율이 문턱값 밑으로
+    // whole_slip_debounce_ticks 틱 이상 지속되면 "전신 슬립"으로 확정하고,
+    // 라이다 회피의 speed_scale과 같은 패턴으로 v_x/omega를 일시적으로
+    // 낮춘다. 문턱값을 낮게(0.15) 잡아 제자리 회전의 정상적인 스크럽 손실
+    // (지령 대비 40~60% 미달도 흔함)과 혼동하지 않고 사실상 전혀 못 도는
+    // 극단적 슬립만 잡아낸다.
+    bool whole_slip_now = false;
+    if (whole_slip_enabled && imu_snap.valid &&
+        std::abs(omega) > whole_slip_min_omega) {
+      float slip_ratio = std::abs(imu_omega_rad) / std::abs(omega);
+      whole_slip_now = slip_ratio < whole_slip_ratio_threshold;
+    }
+    whole_slip_count = whole_slip_now ? whole_slip_count + 1 : 0;
+    bool whole_slip_active = whole_slip_count >= whole_slip_debounce_ticks;
+    if (whole_slip_active) {
+      v_x *= whole_slip_speed_scale;
+      omega *= whole_slip_speed_scale;
+    }
+
     // 제자리 회전 여부: CH2(전후진) 입력이 완전히 중립(v_x == 0.0f)이면서
     // CH1(조향) 입력이 있을 때만 진입한다. v_x는 데드존 처리 시 정확히
     // 0.0f로 설정되므로 등호 비교가 안전하다.
@@ -1541,6 +2059,13 @@ int main(int argc, char **argv) {
     // 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 0으로 낮춰
     // 헛돌이를 억제한다. 접지력이 회복되어 전류가 정상으로 돌아오면 다음
     // 판정 틱에서 자동으로 해제되어 원래 지령으로 복귀한다.
+    // 단, 제자리 회전 중에는 반력 토크로 인한 대각선 하중 이동(FL+RR 또는
+    // FR+RL 쌍이 동시에 가벼워짐)이 정상적인 현상인데 이걸 "들뜸"으로 오判定해
+    // 목표 속도를 꺼버리면 4륜 중 사실상 2륜만 구동되어 회전력이 반토막
+    // 난다(실측 로그에서 전체 회전 틱의 38%가 대각선 쌍 동시 들뜸 판정이었고,
+    // 그 결과 실제 회전율이 지령 대비 44~58%에 그쳤음). 그래서 제자리 회전
+    // 중에는 판정(HUD 표시용)은 유지하되 속도를 꺾는 개입만 끈다.
+    if (!is_spin_turn) {
       if (airborne_fl)
         target_fl = 0.0f;
       if (airborne_fr)
@@ -1549,6 +2074,7 @@ int main(int argc, char **argv) {
         target_rl = 0.0f;
       if (airborne_rr)
         target_rr = 0.0f;
+    }
 
     // 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시
     // 0으로 스냅 후 브레이크를 잠근다. 해제되면 STOPPED 상태에서 아래
@@ -1647,12 +2173,21 @@ int main(int argc, char **argv) {
     float fl_amp = 0, fr_amp = 0, rl_amp = 0, rr_amp = 0;
     int32_t fl_tick = 0, fr_tick = 0, rl_tick = 0, rr_tick = 0;
     uint16_t err_f_l = 0, err_f_r = 0, err_r_l = 0, err_r_r = 0;
+    int fl_temp = 0, fr_temp = 0, rl_temp = 0, rr_temp = 0;
 
     driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick, fl_amp, fr_amp,
-                              err_f_l, err_f_r);
+                              err_f_l, err_f_r, fl_temp, fr_temp);
     if (driver_rear_ptr)
       driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick, rl_amp,
-                                    rr_amp, err_r_l, err_r_r);
+                                    rr_amp, err_r_l, err_r_r, rl_temp, rr_temp);
+
+    // 드라이버(기판) 온도는 저빈도로만 갱신(위 kDriverTempPollTicks 주석 참고).
+    if (++driver_temp_poll_counter >= kDriverTempPollTicks) {
+      driver_temp_poll_counter = 0;
+      driver_front.readDriverTemp(front_driver_temp_c);
+      if (driver_rear_ptr)
+        driver_rear_ptr->readDriverTemp(rear_driver_temp_c);
+    }
 
     // 드라이버 알람(과전류/과부하 등) 감지: 새로 발생한 알람만 콘솔에
     // 한 번 크게 출력한다 (매 틱 갱신되는 HUD 줄과 별개의 고정 줄).
@@ -1776,6 +2311,40 @@ int main(int argc, char **argv) {
     float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);
     (void)dist_bumper;
 
+    // Phase 2 — 휠 기반 ω(엔코더 피드백) vs IMU 기반 ω(자이로) 비교 +
+    // x,y 오도메트리 적분. 실측 검증(실기 로그) 결과 부호 규약이 일치함:
+    // 우회전(+omega)일 때 wheel_omega/imu_gyro_z 둘 다 양수로 나온다.
+    // 이 둘의 차이(잔차)가 §6.3-4의 슬립 지표다. 여전히 제어에는 관여하지
+    // 않는다 — HUD/CSV 노출까지만.
+    auto odom_now = std::chrono::steady_clock::now();
+    float odom_dt = std::chrono::duration<float>(odom_now - last_odom_time).count();
+    last_odom_time = odom_now;
+
+    float v_l_meas = ((fl_fb + rl_fb) / 2.0f) / rpm_per_ms;   // m/s
+    float v_r_meas = -((fr_fb + rr_fb) / 2.0f) / rpm_per_ms;  // m/s (부호 규약: fr/rr는 -v_r로 지령됨)
+    float v_x_meas = (v_l_meas + v_r_meas) / 2.0f;
+    float wheel_omega = (k_skid > 0.0f)
+                            ? (v_r_meas - v_l_meas) / (2.0f * k_skid)
+                            : 0.0f; // rad/s
+    float omega_residual = wheel_omega - imu_omega_rad;
+
+    // θ 적분원: wheel_omega와 원시 gyro_z의 평균(둘의 부호/크기 일치율이
+    // 실측상 99.3%로 높음). IMU가 끊겼을 땐 wheel_omega만으로 대체한다.
+    float fused_omega_rad =
+        imu_snap.valid ? (wheel_omega + imu_omega_rad) / 2.0f : wheel_omega;
+
+    if (imu_yaw_offset_set && odom_dt > 0.0f && odom_dt < 0.5f) {
+      // odom_dt>=0.5s: Fail-safe 재연결 등으로 틱이 크게 벌어진 비정상
+      // 구간은 위치 적분에서 제외해 순간 도약을 막는다.
+      fused_yaw_deg += fused_omega_rad * (180.0 / PI_VAL) * odom_dt;
+      while (fused_yaw_deg > 180.0) fused_yaw_deg -= 360.0;
+      while (fused_yaw_deg < -180.0) fused_yaw_deg += 360.0;
+
+      double theta_rad = fused_yaw_deg * (PI_VAL / 180.0);
+      odom_x += v_x_meas * odom_dt * std::cos(theta_rad);
+      odom_y += v_x_meas * odom_dt * std::sin(theta_rad);
+    }
+
     if (log_stream.is_open()) {
       float t_ms = std::chrono::duration<float, std::milli>(
                        std::chrono::steady_clock::now() - log_start_time)
@@ -1786,13 +2355,26 @@ int main(int argc, char **argv) {
                 << omega << ',' << raw_ch[1] << ',' << raw_ch[2] << ','
                 << raw_ch[3] << ',' << raw_ch[4] << ',' << raw_ch[5] << ','
                 << raw_ch[6] << ',' << raw_ch[7] << ',' << raw_ch[8] << ','
+                << (imu_snap.valid ? 1 : 0) << ',' << imu_snap.gyro[2] << ','
+                << imu_snap.angle[0] << ',' << imu_snap.angle[1] << ','
+                << imu_snap.angle[2] << ',' << imu_yaw_rel << ','
                 << cmd_fl << ',' << cmd_fr << ',' << cmd_rl
                 << ',' << cmd_rr << ',' << fl_fb << ',' << fr_fb << ','
                 << rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp
-                << ',' << rl_amp << ',' << rr_amp << ',' << wheel_status_str[0]
+                << ',' << rl_amp << ',' << rr_amp
+                << ',' << fl_temp << ',' << fr_temp << ',' << rl_temp << ','
+                << rr_temp << ',' << front_driver_temp_c << ','
+                << rear_driver_temp_c << ',' << wheel_status_str[0]
                 << ',' << wheel_status_str[1] << ',' << wheel_status_str[2]
                 << ',' << wheel_status_str[3] << ',' << err_f_l << ','
                 << err_f_r << ',' << err_r_l << ',' << err_r_r << ','
+                << fl_tick << ',' << fr_tick << ',' << rl_tick << ',' << rr_tick
+                << ',' << wheel_omega << ',' << imu_omega_rad << ','
+                << omega_residual << ',' << fused_yaw_deg << ','
+                << odom_x << ',' << odom_y << ','
+                << (heading_hold_condition ? 1 : 0) << ',' << heading_target_deg
+                << ',' << heading_trim_omega << ','
+                << (whole_slip_active ? 1 : 0) << ','
                 << dist_axle << ',' << comm_ms << '\n';
     }
 
@@ -1803,15 +2385,30 @@ int main(int argc, char **argv) {
         (v_max_now <= rc_cfg.v_max_low + 0.001f) ? "LOW " : "HIGH";
     printf("\r\033[K[%-8s] RC:%s%s "
           "C1:%4d C2:%4d C3:%4d C4:%4d C5:%4d C6:%4d C7:%4d C8:%4d "
-          "SPD:%s VX:%+4.2f OMG:%+4.2f | AXLE:%5.3fm | %-28s | "
+          "SPD:%s VX:%+4.2f OMG:%+4.2f | IMU:%s GZ:%+6.2f YAW(imu):%+6.1f | "
+          "WO:%+5.2f IO:%+5.2f YAW(fus):%+6.1f ODO:(%+5.2f,%+5.2f) | "
+          "HH:%s TRG:%+6.1f TRM:%+5.2f%s | "
+          "AXLE:%5.3fm | %-28s | "
           "I(A):FL%+4.1f FR%+4.1f RL%+4.1f RR%+4.1f | "
+          "T(C):FL%3d FR%3d RL%3d RR%3d DRV%4.1f/%4.1f%s | "
           "WHL(FL/FR/RL/RR):%s%s | %4.1fms",
           state.c_str(), rc_ok ? "OK" : "LOST", brake_on ? "[BRK]" : "     ",
           raw_ch[1], raw_ch[2], raw_ch[3], raw_ch[4], raw_ch[5], raw_ch[6],
           raw_ch[7], raw_ch[8], speed_mode_str, v_x, omega,
+          imu_snap.valid ? "OK  " : "LOST", imu_snap.gyro[2], imu_yaw_rel,
+          wheel_omega, imu_omega_rad, fused_yaw_deg, odom_x, odom_y,
+          heading_hold_condition ? "ON " : "off", heading_target_deg,
+          heading_trim_omega, whole_slip_active ? " [!!전신슬립!!]" : "",
           dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
-          rl_amp, rr_amp, wheel_status_str,
-          fault_active ? " [!!알람!!]" : "", comm_ms);
+          rl_amp, rr_amp, fl_temp, fr_temp, rl_temp, rr_temp,
+          front_driver_temp_c, rear_driver_temp_c,
+          (fl_temp >= kOverheatWarnC || fr_temp >= kOverheatWarnC ||
+           rl_temp >= kOverheatWarnC || rr_temp >= kOverheatWarnC ||
+           front_driver_temp_c >= kOverheatWarnC ||
+           rear_driver_temp_c >= kOverheatWarnC)
+              ? " [!!고온!!]"
+              : "",
+          wheel_status_str, fault_active ? " [!!알람!!]" : "", comm_ms);
     fflush(stdout);
 
     std::this_thread::sleep_for(
@@ -1837,6 +2434,8 @@ int main(int argc, char **argv) {
   }
 
   rc.stop();
+  if (use_imu)
+    imu.stop();
 
   std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
   return 0;