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/

doc/joystick.md

@@ -6,6 +6,12 @@
   * 하드웨어: USB to TTL 컨버터 (`/dev/ttyUSB*` 또는 COM 포트)
   * 수신기: XR1 V1.0
   * 조종기: RadioMaster Pocket
+  * **실측 확인된 시리얼 프로토콜: CRSF(Crossfire), 420000bps, 8N1.**
+    프레임 구조 `[Sync 0xC8][Length][FrameType][Payload][CRC8]`, FrameType
+    `0x16`(RC_CHANNELS_PACKED)의 22바이트 payload에 16채널이 11비트씩
+    리틀엔디안으로 패킹되어 raw `172~1811` 값이 `1000~2000us`로 선형
+    변환된다. (CRC8은 이 송신기에서 표준 DVB-S2 값과 안 맞아 검증 없이
+    Sync+Length 프레이밍만 신뢰함.)
 
 ---
 
@@ -15,10 +21,10 @@
 
 | 채널 | 입력 장치 | Raw Value 범위 | 할당 기능 | 동작 설명 |
 | :--- | :--- | :--- | :--- | :--- |
-| **CH1** | 롤/조향 스틱 | • `1001`: 좌측 끝<br>• `1480 ~ 1500`: 중앙 (데드존)<br>• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1480~1500`) 적용<br>• 좌회전(-) / 우회전(+) 정규화 |
-| **CH2** | 피치/스로틀 스틱 | • `1000`: 위 끝 (전진)<br>• `1500`: 중앙 (정지)<br>• `2000`: 아래 끝 (후진) | **전후진 (Throttle / Linear X)** | • `1000` 방향으로 이동 시 전진<br>• `2000` 방향으로 이동 시 후진<br>• CH7에서 결정된 최대 속도 기준으로 선형 매핑 |
-| **CH5** | 2단 스위치 | • `1011`: 미작동 (OFF)<br>• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)<br>• `1011` 수신 시 일반 주행 허용 |
-| **CH7** | 2단 스위치 | • `1988`: 위쪽 누름<br>• `1011`: 아래쪽 누름 | **속도 모드 선택 (Speed Mode)** | • `1988` (위쪽): 저속 모드 (최대 **0.3 m/s**)<br>• `1011` (아래쪽): 고속 모드 (최대 **0.6 m/s**) |
+| **CH1** | 롤/조향 스틱 | • `1005`: 좌측 끝<br>• `1481 ~ 1503`: 중앙 (데드존)<br>• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1481~1503`) 적용<br>• 좌회전(-) / 우회전(+) 정규화 |
+| **CH2** | 피치/스로틀 스틱 | • `1005`: 최대 후진<br>• `1500`: 중앙 (정지)<br>• `2000`: 최대 전진 | **전후진 (Throttle / Linear X)** | • 값이 높을수록 전진, 낮을수록 후진 (CH1과 반대 방향)<br>• CH6에서 결정된 최대 속도 기준으로 선형 매핑 |
+| **CH5** | 2단 스위치 | • `1012`: 미작동 (OFF)<br>• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)<br>• `1012` 수신 시 일반 주행 허용 |
+| **CH6** | 2단 스위치 | • `1832`: 저속 위치<br>• `1012~1090`: 고속 위치 | **속도 모드 선택 (Speed Mode)** | • `1832`(≥1500 부근): 저속 모드 (최대 **0.3 m/s**)<br>• `1012~1090`(<1500): 고속 모드 (최대 **1.5 m/s**) |
 
 ---
 
@@ -27,25 +33,25 @@
 ### 3.1 브레이크 최우선 로직 (CH5)
 * $\text{CH5} \ge 1500$ (누름 / `1988` 부근):
   $$\text{Linear Velocity} = 0.0, \quad \text{Angular Velocity} = 0.0$$
-* $\text{CH5} < 1500$ (안누름 / `1011` 부근): 정상 주행 로직 실행
+* $\text{CH5} < 1500$ (안누름 / `1012` 부근): 정상 주행 로직 실행
 
-### 3.2 속도 제한 설정 (CH7)
-* $\text{CH7} \ge 1500$ (위쪽 / `1988` 부근): $V_{\max} = 0.3 \text{ m/s}$ (저속 모드)
-* $\text{CH7} < 1500$ (아래쪽 / `1011` 부근): $V_{\max} = 0.6 \text{ m/s}$ (고속 모드)
+### 3.2 속도 제한 설정 (CH6)
+* $\text{CH6} \ge 1500$ (`1832` 부근): $V_{\max} = 0.3 \text{ m/s}$ (저속 모드)
+* $\text{CH6} < 1500$ (`1012~1090` 부근): $V_{\max} = 1.5 \text{ m/s}$ (고속 모드)
 
 ### 3.3 전후진 속도 계산 (CH2)
 * **데드존(Deadband):** $1480 \le \text{CH2} \le 1520 \rightarrow V_x = 0.0$
-* **전진 구간 ($1000 \le \text{CH2} < 1480$):**
-  $$V_x = \left( \frac{1480 - \text{CH2}}{1480 - 1000} \right) \times V_{\max}$$
-* **후진 구간 ($1520 < \text{CH2} \le 2000$):**
-  $$V_x = -\left( \frac{\text{CH2} - 1520}{2000 - 1520} \right) \times V_{\max}$$
+* **후진 구간 ($1005 \le \text{CH2} < 1480$):**
+  $$V_x = -\left( \frac{1480 - \text{CH2}}{1480 - 1005} \right) \times V_{\max}$$
+* **전진 구간 ($1520 < \text{CH2} \le 2000$):**
+  $$V_x = \left( \frac{\text{CH2} - 1520}{2000 - 1520} \right) \times V_{\max}$$
 
 ### 3.4 좌우 조향 각속도 계산 (CH1)
-* **데드존(Deadband):** $1480 \le \text{CH1} \le 1500 \rightarrow \omega_z = 0.0$
-* **좌회전 구간 ($1001 \le \text{CH1} < 1480$):**
-  $$\omega_z = -\left( \frac{1480 - \text{CH1}}{1480 - 1001} \right) \times \Omega_{\max}$$
-* **우회전 구간 ($1500 < \text{CH1} \le 2000$):**
-  $$\omega_z = +\left( \frac{\text{CH1} - 1500}{2000 - 1500} \right) \times \Omega_{\max}$$
+* **데드존(Deadband):** $1481 \le \text{CH1} \le 1503 \rightarrow \omega_z = 0.0$
+* **좌회전 구간 ($1005 \le \text{CH1} < 1481$):**
+  $$\omega_z = -\left( \frac{1481 - \text{CH1}}{1481 - 1005} \right) \times \Omega_{\max}$$
+* **우회전 구간 ($1503 < \text{CH1} \le 2000$):**
+  $$\omega_z = +\left( \frac{\text{CH1} - 1503}{2000 - 1503} \right) \times \Omega_{\max}$$
 
 ---
 

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

mac/mac_code

@@ -0,0 +1,79 @@
+import serial
+import time
+
+# 확인된 포트 경로 설정
+SERIAL_PORT = "/dev/cu.usbserial-0001"
+BAUD_RATE = 420000
+
+CRSF_SYNC = 0xC8
+CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16
+
+def parse_channels(payload):
+    if len(payload) < 22:
+        return []
+    
+    # 22바이트의 채널 데이터를 정수로 변환 후 11비트씩 언패킹
+    data = int.from_bytes(payload[:22], byteorder='little')
+    channels = []
+    for _ in range(16):
+        raw_val = data & 0x07FF  # 11-bit 마스크
+        # CRSF 원본 값(172~1811)을 RC 마이크로초(1000~2000µs) 단위로 변환
+        us_val = int((raw_val - 172) * (2000 - 1000) / (1811 - 172) + 1000)
+        channels.append(us_val)
+        data >>= 11
+    return channels
+
+def main():
+    try:
+        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=0.1)
+        print(f"Connected to {SERIAL_PORT} at {BAUD_RATE} baud.")
+        print("스틱과 스위치를 움직여 채널 값을 확인하세요. (종료: Ctrl+C)\n")
+    except Exception as e:
+        print(f"포트 연결 오류: {e}")
+        return
+
+    buf = bytearray()
+    while True:
+        try:
+            if ser.in_waiting:
+                buf.extend(ser.read(ser.in_waiting))
+
+                while len(buf) > 3:
+                    if buf[0] != CRSF_SYNC:
+                        buf.pop(0)
+                        continue
+
+                    length = buf[1]
+                    total_packet_len = length + 2
+
+                    if len(buf) < total_packet_len:
+                        break  # 전체 패킷 대기
+
+                    packet = buf[:total_packet_len]
+                    buf = buf[total_packet_len:]
+
+                    frame_type = packet[2]
+                    if frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
+                        payload = packet[3:-1]  # CRC 제외 payload
+                        channels = parse_channels(payload)
+                        
+                        # read_crsf.py 출력 부분 수정
+                        print(
+                            f"\rCH1: {channels[0]:4d} | CH2: {channels[1]:4d} | "
+                            f"CH3: {channels[2]:4d} | CH4: {channels[3]:4d} | "
+                            f"CH5(SA/SC): {channels[4]:4d} | CH6(SD): {channels[5]:4d} | "
+                            f"CH7: {channels[6]:4d} | CH8: {channels[7]:4d}", 
+                            end="", flush=True
+                            )
+
+        except KeyboardInterrupt:
+            print("\n모니터링 종료")
+            break
+        except Exception as e:
+            print(f"\n데이터 수신 오류: {e}")
+            break
+
+    ser.close()
+
+if __name__ == "__main__":
+    main()

run_4wd.sh

@@ -1,7 +1,11 @@
 #!/usr/bin/env bash
 
 # 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance
-PORT="${1:-/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
@@ -10,10 +14,12 @@ echo "=================================================================="
 echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템"
 echo "=================================================================="
 
-# 1. USB Latency 1ms 단축
+# 1. USB Latency 1ms 단축 (모터/RC/IMU 포트 전부)
 if [ -f "./set_low_latency.sh" ]; then
-    echo "[1/2] USB 시리얼 포트($PORT) 지연 시간 1ms 최적화 적용 중..."
+    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++ 바이너리 존재 여부 확인 및 컴파일
@@ -23,14 +29,17 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then
 fi
 
 echo "=================================================================="
-echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작 ($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 프로그램 실행 (추가 인자 전달 가능)
+# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port/--imu_port를 다시
+# 넘기면 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨)
 if [ $# -gt 1 ]; then
-    ./xbox_motor_control_cpp --port "$PORT" "${@:2}"
+    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT" "${@:2}"
 else
-    ./xbox_motor_control_cpp --port "$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