Rewrite LIDAR/SLAM plan doc to reflect fori_ws's existing implementation

The plan assumed no code existed yet, but the 2.5D-map autonomy stack
(URDF, terrain-aware speed control, Nav2 global/local costmaps +
collision monitor) is already built and running in the separate
fori_ws workspace. Reframes the doc around what's actually there and
the concrete gaps found (AMCL correction not wired into the nav TF
tree, skid-steer calibration not ported from xbox_motor_control.cpp).

.gitignore

@@ -0,0 +1 @@
+logs/

doc/07-lidar-slam-plan.md

@@ -0,0 +1,235 @@
+# 07. 2.5D 맵 기반 자율주행 — 현재 상태 정리와 다음 단계 (Phase 6)
+
+> **문서 성격 변경**: 이전 버전은 "계획안, 코드 미착수"를 전제로 썼다. 그러나 실제로는 별도
+> 워크스페이스 `~/fori_ws`에 이 문서가 계획하려던 내용의 상당 부분이 **이미 구현되어 동작
+> 중**이다(§7.0). 그래서 이 문서는 더 이상 "무엇을 어떻게 새로 만들 것인가"가 아니라 **"무엇이
+> 이미 있고, 무엇이 검증되지 않았고, 무엇을 다음에 해야 하는가"**를 정리하는 문서로 성격을
+> 바꾼다. [06번 문서](06-imu-integration-plan.md)의 RC 통합 작업과는 독립적으로 진행된 별도
+> 트랙이었다는 점에 유의한다.
+
+## 7.0 두 저장소의 관계 — 왜 이 문서가 실제와 어긋나 있었는가
+
+| 저장소 | 역할 | 상태 |
+| :--- | :--- | :--- |
+| `fori_zltech_motor_test`(이 레포) | RC 수동조종(Xbox/CRSF) + ZLAC8015D **Modbus RTU 프로토콜을 CRC 검증까지 포함해 실기체에서 검증**하는 테스트대. ROS2 없이 단일 C++ 프로그램(`xbox_motor_control.cpp`)으로 동작. | 계속 유지·개발 중([06번 문서](06-imu-integration-plan.md) Phase 1~5a) |
+| `~/fori_ws` | ROS2 Humble 기반 완전한 자율주행 통합 스택. livox_ros_driver2, FAST-LIO(LIO), Nav2, 커스텀 시리얼 브릿지, 웹 대시보드까지 포함. | **이미 상당 부분 구현·통합 완료**, 실기체 검증은 일부만 진행 |
+
+`~/fori_ws/fori_ws/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp` 상단 주석:
+
+> "only the low-level serial transport is replaced with the CRC-validated, short-poll
+> implementation verified on the real robot in
+> `zlac8015d_motor_test/fori_zltech_motor_test/xbox_motor_control.cpp`."
+
+즉 두 저장소는 이미 실제로 연결된 형제 프로젝트다 — 이 레포에서 검증한 Modbus 프로토콜을
+`fori_ws`가 가져다 ROS2 노드로 감쌌다. 이 문서(07번)는 그 사실을 모른 채 "URDF부터 새로
+작성해야 한다"는 전제로 쓰여 있었으므로, 이번에 전면 수정한다.
+
+## 7.1 사용자가 요청한 컨셉과 실제 구현 대조
+
+사용자가 이번에 요청한 자율주행 방향 — **이미 가진 2.5D 맵을 활용, 고도 정보로 오르막/내리막
+속도 조절, Mid-360S 그대로 사용, `pcd_gunsan_output` 맵 활용** — 은 다음과 같이 **이미 구현되어
+있다**.
+
+| 요청 사항 | 구현 상태 | 근거 파일 |
+| :--- | :--- | :--- |
+| 2.5D 맵(포인트별 고도 포함) | 구현됨. `pcd_gunsan.pcd`(9,791,311포인트, RGB 컬러 포함, ~1.05GB)를 `pcd_gridmap_converter`가 표준 Nav2 점유격자(`map.pgm/yaml`)와 별도 고도 그레이스케일 맵(`elevation_map.pgm/yaml`)으로 변환 | `~/pcd_gridmap_converter/src/pcd_to_gridmap_node.cpp`, 출력 `~/pcd_gunsan_output/` |
+| 오르막/내리막 속도 조절 | 구현됨. `TerrainSpeedNode`가 현재 위치+진행방향 0.5m 앞의 고도차(grade)를 계산해 오르막은 부스트(최대 1.3배), 내리막은 감속(최소 0.5배), EMA로 스무딩 | `~/fori_ws/fori_ws/src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py` |
+| Mid-360S 유지 | 그대로 사용. `livox_ros_driver2`가 `/livox/lidar`, `/livox/imu` 발행 | `~/fori_ws/fori_ws/src/livox_ros_driver2/` |
+| `pcd_gunsan_output` 맵 활용 | 이미 이 경로를 Nav2 `map_server`와 `terrain_speed_node`가 직접 참조 중. 사용자가 언급한 `/home/pcd_gunsan_output`은 실제로는 `/home/yoo/pcd_gunsan_output`이다(확인 완료) | `fori_nav2_params.yaml:3`, `fori_full.launch.py:18-21` |
+
+**`elevation_map.pgm` 디코딩 방식**(참고용): 픽셀 0=미상, 그 외에는
+`z = min_elevation + (pixel-1)/254*(max_elevation-min_elevation)`로 선형 복원. 현재 저장된
+범위는 `-1.99m ~ 3.00m`(`elevation_map.yaml`). 이 범위가 실제 지형 고도차를 다 담고 있는지는
+Z 관심영역(ROI) 파라미터(`min_height`/`max_height`)에 달려 있는데, 과거 한 번 이 값이 필터
+경계값과 정확히 일치해 잘렸을(clipping) 가능성이 제기되어 더 넓은 범위로 재변환한 이력이 있다
+— §7.5 Phase 2에서 재확인이 필요하다.
+
+## 7.2 판단 1 — "URDF 먼저, 그 다음 ros2_control" 순서가 맞는가
+
+**결론: URDF 부분은 맞고 이미 있다. ros2_control은 불필요하다 — 이미 더 가벼운 대안으로
+대체되어 잘 동작 중이다.**
+
+### URDF: 있음, 다만 표준 패턴과 다른 구조
+
+`~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf`에 Mid-360 라이다, Hikrobot 카메라,
+4바퀴(continuous joint)가 실측 치수로 정의되어 있다. 다만 **루트 링크가 `base_link`가 아니라
+`aft_mapped`**다 — FAST-LIO(LIO SLAM)가 런타임에 `camera_init → aft_mapped` TF를 동적으로
+발행하고, `base_link`는 그 아래에 고정 조인트로 매달린 구조(`aft_mapped_to_base`,
+`xyz="-0.26953 0 -0.41296" rpy="0 -0.277309 0"`, 라이다 실측 피치 보정 포함)다.
+
+이건 표준 로봇공학 URDF 패턴(고정된 `base_link`를 루트로 두고 오도메트리가 `odom→base_link`를
+발행)과 다르다 — 대신 "라이다 SLAM이 로봇의 기준 좌표계 자체를 소유하고, 로봇 형상은 그 자세를
+따라간다"는 구조다. 이 선택 자체는 실용적이다(LIO가 어차피 가장 정확한 자세 추정원이므로).
+다만 §7.4에서 다루는 TF 트리 충돌 문제의 근본 원인이기도 하다.
+
+### ros2_control: 불필요 — 더 가벼운 직접 시리얼 브릿지가 이미 그 자리를 대신함
+
+이전 버전 문서는 `hardware_interface::SystemInterface`를 구현하는 ros2_control 플러그인을
+써야 한다고 했다. 실제로는 그렇게 하지 않았다. 대신 `fori_serial_bridge_cpp::ForiSerialBridge`
+(일반 `rclcpp::Node`)가:
+
+* `/cmd_vel`(Twist)을 직접 구독
+* 20Hz 제어 루프에서 가감속 램프 + IMU 요레이트 PI 보정 + 차동 킨매틱스 → 좌우 RPM 산출
+* ZLAC8015D에 RS485 Modbus RTU로 직접 명령(레지스터 `0x2088` 속도 쓰기, `0x20AD` 피드백 읽기)
+* `/odom_wheels`, `/joint_states`, `/battery_state` 발행
+
+이 구조로 이미 Nav2와 통합되어 동작 중이다(`serial_bridge_node.cpp:112-134`). ros2_control의
+핵심 이점은 (1) `diff_drive_controller` 같은 표준 컨트롤러 재사용, (2) `controller_manager`의
+표준 생명주기 관리인데, 이 로봇은 애초에 4륜 스킷스티어라 표준 컨트롤러가 정확히 맞지 않고(이전
+버전 문서도 §7.4.4에서 이 점을 지적했다), 커스텀 로직(가감속 램프, IMU PI 보정, 배터리 필터링,
+모의주행 폴백)이 필요해 결국 커스텀 코드를 짜야 하는 건 ros2_control을 쓰든 안 쓰든 마찬가지다.
+**지금 상태에서 ros2_control로 다시 감싸는 건 실질적 이득 없이 이미 검증된 경로를
+재작성하는 것**이므로 우선순위가 낮다. 유지를 권장한다.
+
+### 다만 놓친 부분 하나 — 스킷스티어 캘리브레이션 미이식
+
+`fori_serial_bridge_cpp`의 킨매틱스는 순수 기하학적 차동구동 공식을 쓴다
+(`v_left = v - w*wheel_base/2`, `wheel_base=0.374`m, `serial_bridge_node.cpp:344-346`). 반면
+이 레포의 `xbox_motor_control.cpp`는 [06번 문서](06-imu-integration-plan.md) Phase 5a에서
+실측으로 캘리브레이션한 `effective_w=0.87`, `k_skid=0.51`(`xbox_motor_control.cpp:1449,1459`)을
+쓴다 — 이 값들은 스킷스티어 로봇의 실제 타이어-지면 마찰로 인한 유효 회전반경이 순수 기하학적
+트랙폭(0.374m)과 다르다는 걸 실측으로 확인한 결과다. **`fori_ws`의 회전 킨매틱스는 이 캘리브레이션을
+이식받지 못했다** — 즉 Nav2가 "이만큼 회전해라"라고 지시했을 때 실제 회전량이 계산값과 얼마나
+다른지가 검증되지 않은 상태다. 이건 §7.5 Phase 1의 작업 항목으로 넣는다.
+
+## 7.3 판단 2 — Nav2 글로벌맵 + 로컬맵 기반 동적 장애물 회피
+
+**결론: 맞고, 이미 사용자가 제안한 것보다 한 단계 더 나아간 3중 구조로 구현되어 있다.**
+
+| 계층 | 역할 | 데이터 소스 | 근거 |
+| :--- | :--- | :--- | :--- |
+| `global_costmap` | 정적 전역 지도(`static_layer`) + 관측된 장애물(`obstacle_layer`)로 전역 경로 계획 | `map` 토픽(사전 구축 맵) + `/scan` | `fori_nav2_params.yaml:104-127` |
+| `local_costmap` | 로봇 주변 3×3m 롤링 윈도우, 실시간 장애물만 반영해 국소 회피 | `/scan`(라이다 실시간) | `fori_nav2_params.yaml:75-102` |
+| **Collision Monitor**(추가 안전계층) | Nav2 코스트맵과 별개로 최종 속도 명령 단계에서 급출현 장애물을 감속(`SlowZone`)·정지(`StopZone`)·예측정지(`FootprintApproach`)시키는 마지막 게이트 | `/scan` | `fori_collision_monitor_params.yaml` |
+
+Collision Monitor는 `terrain_speed_node`가 낸 `/cmd_vel_terrain`(경사 보정까지 끝난 명령)을
+받아 최종 `/cmd_vel`을 낸다 — 즉 실제 파이프라인은 `Nav2 계획 → 경사 보정 → 급출현 장애물
+감속/정지 → 실제 모터`(§7.5 아키텍처 그림 참고) 순서로, 사용자가 제안한 "글로벌은 지도로,
+로컬은 라이다로" 구조를 그대로 따르면서 한 겹의 실시간 안전장치를 더 두고 있다.
+
+**알려진 한계 하나**: Nav2의 `behavior_server`(막힘 시 후진/제자리회전 등 복구 동작)가 내는
+속도 명령은 `nav2_bringup` 내부 리맵 구조상 `velocity_smoother`를 거치지 않아
+`terrain_speed_node`의 경사 보정과 Collision Monitor의 급출현 감속·정지를 **우회**한다
+(`fori_nav2.launch.py:128-137` 주석에 상세 근거). 다만 복구 동작 자체가 저속·단거리로 제한되고
+`serial_bridge_node`의 `target_linear_speed`(0.3m/s) 하드 클램프는 경로와 무관하게 항상
+적용되므로 위험은 제한적이지만, 완전히 무시할 문제는 아니다.
+
+## 7.4 질문에 대한 답
+
+### Q1. 저장된 맵을 제공하고 그 맵으로 Nav2를 진행해도 실시간 자세 추종이 가능한가?
+
+**가능하다.** "SLAM(지도를 만들면서 동시에 자세를 추정)"과 "Localization(이미 있는 지도
+안에서 자세만 추정)"은 다른 문제다. 후자는 지도를 새로 만들 필요가 없어 계산량이 훨씬 가볍고,
+이게 정확히 Nav2의 **AMCL**(Adaptive Monte Carlo Localization)이 하는 일이다. `fori_ws`에는
+이미 이 구조가 설정되어 있다:
+
+* `map_server`가 `pcd_gunsan_output/map.yaml`(사전 구축된 정적 지도)을 서빙
+* `amcl`이 `scan_topic: "scan"`을 구독 — Mid-360의 3D 포인트클라우드를
+  `pointcloud_to_laserscan`으로 2D로 투영한 것(`fori_nav2.launch.py:88-103`)
+* AMCL은 파티클 필터로 이 2D 스캔을 정적 지도와 계속 대조해 `/amcl_pose`(map 프레임 기준 위치)를
+  실시간으로 발행
+
+즉 SLAM처럼 지도를 다시 만들 필요 없이, **저장된 지도 하나만 있으면 실시간 위치 추정이 가능**
+하다는 것이 맞다.
+
+### Q2. 그렇다면 라이다로 주행 중 저장된 맵과 계속 대조하며 자세를 추종하는 것인가?
+
+**맞다 — 그게 AMCL이 하는 정확한 방식(스캔매칭 기반 파티클 필터)이다.** 다만 `fori_ws`의
+**실제 구현에는 중요한 예외가 있고, 이게 지금 이 시스템의 가장 중요한 미해결 갭이다.**
+
+`fori_nav2_params.yaml:25`에서 AMCL의 `tf_broadcast: false`로 꺼져 있다. 이유는 코드 주석에
+명시되어 있다:
+
+> FAST-LIO(LIDAR SLAM)가 `camera_init→aft_mapped→base_link` TF를 이미 발행하고 있는데, AMCL이
+> 표준대로 `map→odom`을 또 발행하면 `odom`의 부모를 두 곳(FAST-LIO의 정적 브릿지 vs AMCL)이
+> 동시에 주장하게 되어 `base_link↔odom` 연결을 tf2가 못 찾는 "unconnected trees" 오류가 난다.
+
+그래서 실제로는 `camera_init`을 `odom`과 `map` 양쪽에 **정적(변화 없는 identity transform)**
+으로 동시에 연결해 놓았다(`static_tf_camera_init_node`, `static_tf_camera_init_to_map_node`,
+둘 다 `fori_nav2.launch.py:48-70`). 결과적으로:
+
+* **실제 항법(Nav2 코스트맵/플래너가 보는 "map" 프레임)은 FAST-LIO가 만드는 연속적인
+  LiDAR-관성 오도메트리(LIO) 그 자체**다 — 이건 저장된 지도와 무관하게, 매 순간 이전 프레임
+  대비 상대 이동만 누적하는 방식이라 시간이 지나면 드리프트가 쌓인다.
+* AMCL은 여전히 돌고 있고 `/amcl_pose`(진짜 저장된 지도와 스캔매칭한 결과)를 정상 발행하지만,
+  이 값은 **`terrain_speed_node`/`parking_controller_node`가 위치 조회용으로 참고만 할 뿐,
+  Nav2의 TF 트리·코스트맵에는 재반영되지 않는다.**
+
+정리하면: **지금은 "저장된 맵과 계속 대조해서 드리프트를 실제로 보정하는" 루프가 완성되어 있지
+않다.** AMCL은 돌고 있지만 보조적 위치 확인 용도이고, 로봇이 실제로 어디 있다고 "믿고" 주행하는
+좌표계는 FAST-LIO의 순수 오도메트리다. 짧은 거리·짧은 시간의 순찰(사람이 대기하며 감독하는
+Phase 4~5 수준)에서는 드리프트가 실용적으로 무시할 만하겠지만, **주행 거리·시간이 늘어날수록
+"로봇이 믿는 위치"와 "저장된 지도 위 실제 위치"가 벌어질 위험**이 있다 — 이건 §7.1에서 확인한
+사용자의 요청(저장된 2.5D 맵을 활용한 자율주행)의 정확도에 직접 영향을 준다. 이 TF 충돌을 풀어
+AMCL의 보정이 실제로 항법 좌표계에 반영되게 하는 것을 §7.5 Phase 1의 최우선 항목으로 둔다.
+
+## 7.5 Phase — 남은 작업 (기존 코드 근거 기반으로 재정의)
+
+이전 버전 문서의 Phase 1~3(인프라 확인/실내 SLAM 검증/야외 검증)은 `fori_ws`에서 이미 그
+이상으로 진행되어 있으므로 폐기한다. 아래는 §7.2~7.4에서 확인한 **구체적 갭**을 기준으로 다시
+짠 순서다. [06번 문서](06-imu-integration-plan.md)와 같은 원칙 — 먼저 관측/검증, 그 다음에만
+주행 제어 개입 — 을 유지한다.
+
+### Phase 1 — 자세 추종 정확도 갭 해소 (최우선, 주행 제어 영향 큼)
+
+* **AMCL↔FAST-LIO TF 통합**: `unconnected trees` 문제를 임시 봉합(정적 aliasing)이 아니라 실제로
+  풀어, AMCL의 스캔매칭 보정이 `map→odom` 형태로든 다른 형태로든 Nav2가 실제로 쓰는 좌표계에
+  반영되게 한다. 후보: (a) FAST-LIO 쪽에서 `aft_mapped`를 `odom`으로 두고 AMCL이 표준대로
+  `map→odom`을 발행하도록 프레임 이름 재배선, (b) `robot_localization`의 `navsat_transform` 유사
+  패턴으로 AMCL 보정치를 주기적으로 오도메트리에 융합. §7.4 Q2에서 확인한 현재 구조(정적
+  aliasing)로는 저장된 지도 대비 실제 위치 보정이 이름만 있고 실질적으로 동작하지 않는다.
+* **스킷스티어 캘리브레이션 이식**: §7.2에서 확인한 `k_skid=0.51`/`effective_w=0.87`(06번 문서
+  Phase 5a 실측값)을 `fori_serial_bridge_cpp`의 회전 킨매틱스에 반영. 지금은 순수 기하학적
+  트랙폭(0.374m)만 쓰고 있어 Nav2가 지시한 회전량과 실제 회전량이 얼마나 다른지 검증되지 않았다.
+* **게이트**: 위 두 항목을 고친 뒤, 로봇을 손으로/저속으로 움직이며 `/amcl_pose`와 로봇의 실제
+  위치(줄자·표식 등 외부 기준)를 비교해 보정이 실제로 드리프트를 줄이는지 확인해야 다음 단계로
+  넘어간다.
+
+### Phase 2 — 맵/웨이포인트 데이터 검증
+
+* **고도맵 ROI 재확인**: `elevation_map.yaml`의 `min_elevation/max_elevation`이 실제 군산
+  부지의 지형 고도차를 다 담고 있는지(§7.1에서 언급한 과거 클리핑 이력) `pcd_to_gridmap_node`의
+  `min_height`/`max_height` 파라미터를 재확인하고 필요하면 넓혀서 재변환한다.
+* **순찰 웨이포인트 설정**: `parking_controller_node`의 `patrol_waypoints`가 현재 빈 배열이다
+  (예전 실내 지도 좌표는 새 군산 부지에 안 맞아 이미 비워둔 상태 — `parking_controller_node.py:47`
+  주석 참고). rviz2로 새 지도를 열어 순찰 좌표를 실측/지정한다.
+* **게이트**: rviz에서 라이다 스캔이 정적 지도와 정렬되어 보이고, 지정한 웨이포인트가 지도상
+  주행 가능 영역(occupancy=free) 안에 있는지 확인.
+
+### Phase 3 — 통제 구역 저속 폐루프 테스트
+
+* 울타리 등으로 막힌 좁은 구역, 저속, 사람이 항시 대기하며 즉시 개입 가능한 상태에서 Phase 1~2
+  적용 후 첫 자율주행 테스트.
+* 오르막/내리막 구간을 포함해 `terrain_speed_node`의 속도 보정이 실제로 등판력 확보/안전한
+  서행으로 이어지는지 확인(§7.4의 게인 파라미터 `uphill_gain=3.0`/`downhill_gain=4.0`이 이
+  로봇 중량·모터 토크에 적절한지는 아직 실측 검증 전).
+* [06번 문서](06-imu-integration-plan.md) Phase 4(전신 슬립 감지)가 이 자율주행 경로 위에서도
+  살아있는지 — 즉 `fori_serial_bridge_cpp`로 옮겨 갔는지, 아니면 아직 `xbox_motor_control.cpp`
+  에만 있는지부터 확인해야 한다. 현재 `serial_bridge_node.cpp`를 보면 슬립 감지·온도 모니터링
+  로직은 이식되지 않은 것으로 보인다 — 이식 여부를 이 Phase에서 명시적으로 판단한다.
+* **게이트**: 짧은 구간 반복 검증이 안정적이어야 범위를 넓힌다.
+
+### Phase 4 — 범위 확장 + 동적 장애물 실지 검증
+
+* 더 길고 개방된 구역, 사람/물체가 실제로 튀어나오는 상황에서 Collision Monitor의
+  StopZone(0.45m)/SlowZone(0.90m)/FootprintApproach(1.5초 예측)가 의도대로 동작하는지 확인.
+* §7.3에서 확인한 `behavior_server` 복구 동작의 안전장치 우회 한계가 실제로 문제되는지(복구가
+  얼마나 자주 트리거되는지) 관찰.
+
+### Phase 5 — (낮은 우선순위, 보류) ros2_control 전환 재검토
+
+§7.2에서 판단했듯 지금은 불필요하다. 다만 이후 표준 ROS2 컨트롤러 생태계(예: 다른 로봇과 코드
+공유, `moveit`류 확장 등) 활용 필요성이 실제로 생기면 그때 재검토한다 — 지금 시점에 선제적으로
+할 이유는 없다.
+
+## 7.6 요약 — 이번 조사에서 바뀐 것
+
+* 이전 문서는 "무엇을 어떻게 새로 만들 것인가"를 물었지만, 실제로는 **이미 만들어져 있었다.**
+  URDF, 2.5D 맵 변환, 경사 기반 속도 조절, Nav2 글로벌/로컬 코스트맵 + Collision Monitor까지
+  전부 `~/fori_ws`에 구현되어 있다.
+* ros2_control은 필요 없다 — 더 가벼운 직접 시리얼 브릿지가 이미 그 역할을 하고 있고, 지금
+  다시 쓰는 건 순수 재작업이다.
+* 가장 중요한 실질적 갭은 두 가지다: **(1) AMCL의 지도 대조 보정이 TF 충돌 회피용 임시방편
+  때문에 실제 항법 좌표계에 반영되지 않고 있다는 것**(§7.4 Q2), **(2) 06번 문서에서 실측
+  캘리브레이션한 스킷스티어 보정값(`k_skid`/`effective_w`)이 ROS2 스택의 킨매틱스에 이식되지
+  않았다는 것**(§7.2). 이 둘이 Phase 1의 핵심이다.

doc/08-improvements-report.md

@@ -0,0 +1,207 @@
+# 08. 로봇 제어 시스템 개선 보고서
+
+> 이 문서는 `feat/radiomaster-rc-control` 브랜치에서 진행한 개선 작업을 정리한 보고서다.
+> 모든 항목은 실제 로봇 실주행 로그로 검증을 거쳤으며, 정량적 결과는 실측 CSV 로그 분석
+> 기반이다. 관련 상세 계획/근거는 [06-imu-integration-plan.md](06-imu-integration-plan.md),
+> [joystick.md](joystick.md)에 별도로 정리되어 있다.
+
+## 개요
+
+기존 Xbox 컨트롤러(SDL2) 기반 조종을 RadioMaster Pocket + XR1 RC 수신기로 교체하면서 시작해,
+IMU(WitMotion HWT905) 도입을 통해 로봇이 "지령한 대로 실제로 움직이고 있는지"를 처음으로
+확인·보정할 수 있게 만들었다. 그 결과 제자리 회전 성능이 지령 대비 실측 기준 **44~58% →
+93.6%**로 크게 개선되었고, 직진 헤딩 자동 유지, 전신 슬립 감지, 모터 과열 조기 감지 등의
+안전/편의 기능이 추가되었다.
+
+## 1. RC 조종 시스템 전환
+
+* Xbox/SDL2 조이스틱을 RadioMaster Pocket + XR1 수신기(CRSF 프로토콜, 420000bps)로 교체.
+* 실측 하드웨어 캘리브레이션(CH1 조향/CH2 전후진/CH5 브레이크/CH6 속도모드) 및 Fail-safe
+  타임아웃 적용.
+
+**문제**: 실측 중 채널 값이 간헐적으로 엉뚱한 값으로 튀는 현상 발견(예: 스틱을 살짝만
+움직였는데 최대 후진값이 순간적으로 찍힘). CRSF 프로토콜은 표준상 CRC-8 체크섬으로 손상된
+프레임을 걸러낼 수 있는데, 이 특정 수신기 하드웨어에서는 표준 CRC-8(DVB-S2) 및 가능한
+변형(다항식/초기값/반전 조합 전수조사, 보율 오차 가설 등)을 모두 실측 데이터로 대조해봐도
+일치율이 최대 33%에 그쳐 **이 하드웨어에서는 CRC 검증 자체가 신뢰할 수 없다**는 결론을 실험적으로
+확정.
+
+**적용 방식**: CRC 대신 3단계 방어를 조합했다.
+1. **범위 검증**: 캘리브레이션으로 확인된 정상 범위를 벗어나는 값은 무조건 거부.
+2. **슬루레이트(변화율) 제한**: 한 틱 사이에 물리적으로 불가능할 만큼 큰 변화는 손상 프레임으로
+   간주해 거부하되, 진짜 빠른 스틱 조작까지 막지 않도록 N틱 연속 거부되면 강제로 재동기화하는
+   예외 처리를 둠(그렇지 않으면 채널이 영구히 멈추는 결함이 있었음 — 발견 후 수정).
+3. **중앙값(median-of-3) 필터**: 최근 3개 값의 중앙값을 취해 단발성 튐값의 영향을 한 번 더
+   줄임.
+* udev 규칙으로 `/dev/ttyMOTOR`/`/dev/ttyRC`/`/dev/ttyIMU` 고정 심볼릭 링크 적용 — USB 재연결
+  시 포트 번호가 매번 바뀌던 문제 해결.
+
+## 2. 주행 제어 안정성 개선
+
+* **브레이크**: 기존엔 스로틀을 중립으로 되돌려 자연 정지할 때도 전자식 브레이크가 자동으로
+  잠겼는데, 이를 CH5 스위치를 눌렀을 때만 잠기도록 변경.
+
+* **가속/감속 — 저크 제한(jerk-limited) S-curve란**: 속도(RPM)를 목표까지 바로 바꾸는 게 아니라,
+  "가속도가 변하는 속도(저크)" 자체에 상한을 둬서 가속도 곡선을 매끄러운 S자로 만드는 방식이다.
+  급격한 가속도 변화는 큰 전류 스파이크(역기전력)로 이어질 수 있어, 감속에는 계속 이 방식을
+  유지한다. 다만 하드웨어 백EMF 보호회로 구성이 완료되면서 **가속은 더 이상 소프트웨어로 완만하게
+  램프업할 필요가 없어져, 조이스틱 목표값에 1:1로 즉시 추종하도록 변경**했다(감속은 기존과 동일).
+
+* **정지 직전 반전 방지**: 위 "즉시가속" 로직은 "목표값이 현재값보다 커지면 무조건 즉시 반영"
+  하는 단순 조건이었는데, 정지 막바지 저속 구간에서 RC 노이즈로 목표값이 순간적으로 반대 부호로
+  흔들리면 이 조건이 그대로 성립해버려 "전진→후진→전진"처럼 튀는 현상이 있었다. **적용 방식**:
+  조건을 "같은 방향으로 더 빨라지는 경우"로 한정하고, 부호가 바뀌는 경우(RC 노이즈든 실제
+  방향 전환 지시든)는 전부 저크 제한 감속 경로로 보내 0을 관통할 때도 부드럽게 처리하도록 수정.
+
+## 3. IMU 통합 — 오도메트리 (Phase 1~2)
+
+* WitMotion HWT905-RS232 IMU를 별도 USB 포트로 연동, 전원 인가 시점의 방향을 정면(0°) 기준으로
+  저장.
+
+**적용 방식 — 데드 레커닝(dead reckoning)**: GPS 없이 "지금까지 어느 방향으로 얼마나 움직였는지"를
+누적 적분해서 현재 위치(x, y)를 추정하는 고전적 방식이다. 매 제어 주기(틱)마다 (1) 바퀴 인코더
+피드백으로 계산한 이동 속도와 (2) 그 순간의 헤딩(진행 방향)을 곱해 이동한 x, y 변위를 누적한다.
+정확도는 전적으로 **헤딩을 얼마나 정확히 아는가**에 달려 있다 — 헤딩이 조금만 틀려도 매 틱
+누적되며 오차가 기하급수적으로 벌어지기 때문에, 이번 작업에서 가장 공들인 부분이 헤딩 소스
+선정이었다.
+
+* 바퀴 엔코더 기반 위치 적분(x, y)과 회전율(wheel_omega) 대 IMU 실측 회전율(imu_omega) 비교
+  로직 추가.
+* **헤딩 소스 결정 — 실측 검증으로 설계를 뒤집은 사례**: 처음엔 상식적으로 "IMU 자체 내장
+  융합 yaw(가속도+자이로를 자체적으로 결합해 계산한 값)가 원시 자이로를 그냥 적분하는 것보다
+  안정적일 것"이라 예상하고 그렇게 구현했다. 그런데 **검증 방법으로 "제자리로 돌아오는
+  루프백 주행"을 설계**해서(직선/원 등 정해진 경로로 나간 뒤 출발점으로 복귀시켜, 오차가
+  없다면 최종 위치가 (0,0)에 가까워야 한다는 원리) 실측해보니 시작점 대비 **20.8m나 이탈**하는
+  심각한 오차가 나왔다. 원인을 더 파보니 IMU가 자체적으로 계산해 내놓는 융합 yaw가 **자기
+  자신의 원시 자이로 방향과도 1초 평균 기준 72%밖에 일치하지 않음**을 발견했다(저가 IMU가
+  모터 전류로 인한 자기장 간섭 등으로 내부 지자기 보정이 흔들리는 것으로 추정). **이후 헤딩
+  소스를 바퀴 인코더로 추정한 회전율과 원시 자이로 값의 평균(단순 상보 융합)으로 교체** —
+  이 두 신호는 서로 독립적인 센서(엔코더 vs IMU)인데도 부호/크기 일치율이 99.3%로 매우
+  높아 신뢰할 수 있음을 먼저 확인한 뒤 채택했다.
+* **재검증**: 동일한 루프백 테스트를 다시 수행해 이탈 거리가 **20.8m → 0.55m**로 개선됨을
+  확인(22m 주행 기준 약 2.5% 오차) — 같은 검증 방법을 "전/후 비교"에도 그대로 재사용해
+  설계 변경의 효과를 정량적으로 입증했다.
+
+## 4. 직진 헤딩 자동 유지 (Phase 3)
+
+* 조향이 중립이고 실제로 주행 중이며 라이다 회피가 개입하지 않을 때, 진입 시점의 헤딩을
+  목표로 고정하고 **PI 제어**로 좌우 편향을 자동 보정.
+
+**적용 방식 — PI 제어**: 목표 헤딩과 현재 헤딩의 차이(오차)를 계속 관찰하면서, (1) **P(비례)항**
+— 오차가 클수록 더 크게 보정하고, (2) **I(적분)항** — 작은 오차가 오래 지속되면(예: 좌우 노면
+마찰 차이로 인한 지속적 편향) 그것까지 누적해서 보정을 강화하는 두 요소를 더해 좌우 바퀴 속도에
+미세한 트림(보정값)을 얹는 방식이다. 트림 값에는 상한을 둬서 사용자의 의도적 조향 입력을 절대
+압도하지 않도록 제한했고, 적분항에도 별도 상한(anti-windup)을 둬 트림이 무한정 커지는 것을 방지.
+
+* 실주행 검증(다양한 노면, 총 20분 이상): 평균 오차 0.48~1.24°, 최대 오차 10° 이내, 트림
+  값은 상한(±0.15 rad/s)의 최대 38%까지만 사용 — 포화·진동 없이 안정적으로 수렴.
+
+## 5. 전신 슬립 감지 (Phase 4)
+
+**적용 방식 — 독립 신호 교차검증**: 기존 걸림/들뜸 판정은 "지령 RPM 대비 실제 RPM"과
+"전류"만으로 개별 바퀴 상태를 추정하는데, 이 두 값 모두 바퀴 자체(엔코더/전류 센서)에서
+나오는 신호라 만약 4바퀴가 동시에(예: 빙판·젖은 잔디) 헛돌면 "다들 정상적으로 지령을
+따라가는 것처럼" 보여 잡아내지 못하는 사각지대가 있다. 그래서 **바퀴 시스템과 완전히
+독립적인 IMU(관성 센서)로 실제 차체가 얼마나 돌고 있는지를 직접 재서 지령값과 비교**하는
+이중 안전망을 추가했다: 지령 회전율과 IMU 실측 회전율의 비율이 임계값 밑으로 일정 시간
+지속되면 "전신 슬립"으로 판정해 라이다 회피의 감속 패턴과 동일한 방식으로 속도를 일시적으로
+낮춘다. 제자리 회전은 원래도 지령보다 덜 도는 게 정상(스크럽 마찰)이라, 오탐을 피하기 위해
+"거의 안 도는" 극단적인 경우만 잡히도록 문턱값을 보수적으로 설정.
+
+* 18분 실주행 중 11회, 0.4%의 틱에서만 발동 — 오탐 거의 없음. 가장 긴 발동 사례(1.68초)를
+  분석한 결과, 고속 주행 중 급선회 시 관성으로 인한 언더스티어(로봇이 거의 안 도는 상황)를
+  정확히 감지해 속도를 절반으로 낮췄고, 그 직후 실제로 회전이 정상적으로 회복되는 것을 확인
+  — 설계 의도대로 작동.
+
+## 6. 킨매틱스 실측 캘리브레이션 — 제자리 회전 성능 대폭 개선 (Phase 5a)
+
+### 6.1 원인 진단
+
+제자리 회전이 지령한 만큼 돌지 않는 문제의 원인을 로그로 추적한 결과, 두 가지가 확인됨:
+
+1. 회전 시 반력 토크로 인한 대각선 방향 하중 이동(FL+RR 또는 FR+RL 바퀴 쌍이 동시에
+   가벼워짐)이 정상적인 물리 현상인데, 기존 "들뜸(바퀴가 지면에서 떨어짐)" 판정 로직이 이를
+   오탐하여 목표 속도를 0으로 꺼버려 4륜 중 사실상 2륜만 구동되는 상황이 전체 회전 틱의
+   **38%**에서 발생.
+2. 회전 반경 계산에 쓰이는 트랙폭 계수(`k_skid`, `effective_w`)가 기하학적 공식값으로,
+   실제 스킷스티어 특유의 스크럽 마찰(바퀴가 옆으로 긁히며 도는 저항)을 반영하지 못해
+   지령보다 작은 차동 속도를 걸고 있었음.
+
+### 6.2 조치 및 결과
+
+* 제자리 회전 중에는 "들뜸" 판정으로 목표 속도를 꺾는 개입을 비활성화(판정 표시는 유지).
+
+**적용 방식 — 실측 역산(파라미터 피팅)**: 기존 `k_skid`/`effective_w`는 로봇의 트랙폭·휠베이스
+치수만으로 계산한 순수 기하학적 공식값이었는데, 실제 지면에서는 바퀴가 옆으로 긁히는 저항
+때문에 이 공식이 가정하는 것보다 더 큰 차동을 걸어줘야 같은 회전 속도가 나온다. 이를 정량화하기
+위해 매 틱 로그에 이미 기록되고 있던 "바퀴 인코더로 계산한 회전율"과 "IMU로 실측한 실제
+회전율"을 이용해, 공식을 거꾸로 풀어서(예: `유효계수 = 바퀴 기반 회전율 추정치 × 기존계수 ÷
+IMU 실측 회전율`) **틱마다 "실제로 맞았어야 할 계수값"을 역산**했다. 이렇게 나온 수천 개의
+값 중 이상치 영향을 줄이기 위해 평균이 아닌 **중앙값**을 대표값으로 채택했고, 서로 다른 날
+수집한 **3차례의 독립적인 실주행 로그**에서 반복 계산해도 값이 거의 같게 수렴하는 것을
+확인해 신뢰도를 높였다. 회전 반경별로 값을 나눠 보면 완만한 커브(0.44)와 급한 커브(0.53)
+사이에서 계수가 변하는 경향도 확인했는데, 정적 상수 하나로는 이 편차를 완전히 없앨 수
+없어 실용적인 절충값(중앙값)을 채택 — 이걸 실시간으로 자동 보정하는 방식(Phase 5b)도
+검토했으나, 슬립이 발생하는 바로 그 순간에 추정 신호 자체가 같이 왜곡되는 구조적 문제가
+있어 득보다 실이 크다고 판단해 보류했다. 아래 값으로 최종 수렴:
+
+| 파라미터 | 기존(기하학적 공식) | 실측 캘리브레이션 |
+| :--- | :--- | :--- |
+| `k_skid` (커브 선회) | 0.406 | **0.51** |
+| `effective_w` (제자리 회전) | 0.684 | **0.87** |
+
+* **최종 성능**: 제자리 회전 시 지령 대비 실제 회전율(IMU 실측 기준)이 **44~58% → 93.6%**로
+  개선. 완전히 100%에 도달하지 못하는 잔여 오차는 스킷스티어 구조 특유의 물리적 한계(4륜
+  전부가 옆으로 긁히며 돌아야 하는 구조상 슬립 없이는 방향 전환이 불가능)로 판단되며, 현재
+  수준이 이 하드웨어에서 실용적으로 기대 가능한 한계에 근접한 것으로 평가.
+* 실시간 온라인 적응형 보정(Phase 5b)은 추가 정확도 이득 대비 구현/검증 리스크가 크다고
+  판단해 보류 결정(근거는 06번 문서 참고).
+
+## 7. 모터/드라이버 온도 모니터링
+
+* 기존엔 사용하지 않던 드라이버 레지스터(모터 온도 `0x20A4`, 드라이버 온도 `0x20B0`)를 읽어
+  HUD/CSV에 실시간 노출.
+
+**적용 방식**: 드라이버와의 통신은 매 제어 주기(약 27Hz)마다 이루어지는데, 통신 1회를 늘릴
+때마다 제어 루프 지연이 커진다. 모터 온도 레지스터(`0x20A4`)는 마침 기존에 읽고 있던
+레지스터 범위(`0x20A5`~) 바로 앞이라, **읽는 레지스터 개수만 하나 늘려서 통신 횟수 추가 없이**
+얻어냈다. 반면 드라이버 자체 온도(`0x20B0`)는 기존 읽기 범위와 떨어져 있어 합치면 통신이
+늘어나는데, 온도는 물리적으로 초 단위로 천천히 변하는 값이라 매 틱 갱신할 필요가 없다는 점을
+이용해 **약 1.1초에 한 번만 별도로 폴링**하는 저빈도 방식을 택했다 — 정확도 손실 없이 통신
+비용을 최소화한 설계.
+* 60℃ 이상 시 HUD에 경고 표시.
+
+## 8. 진단만 완료, 조치는 보류 중인 이슈
+
+* **급정지 시 미세한 요잉(회전) 현상**: 처음엔 "조이스틱을 놓을 때 튕기는 물리적 반동" 가설을
+  세우고 로그의 RC 원시값 변화를 직접 확인했으나, 해당 구간에서 스틱 값은 매끄럽게 중립으로
+  복귀했을 뿐 반대 방향으로 튀는 값은 전혀 없어 이 가설은 기각했다. 대신 정지 이벤트마다
+  전후 바퀴 명령/피드백/상태 플래그를 시간순으로 대조해본 결과 두 가지 원인을 확인:
+  (1) 거친 노면에서 정지 직전 특정 바퀴가 순간적으로 "들뜸" 판정되어 다른 바퀴와 감속 타이밍이
+  어긋나는 것, (2) 드라이버 자체의 속도 제어 루프가 급정지 시 0을 살짝 지나쳤다 돌아오는
+  오버슈트(4바퀴가 0을 지나치는 타이밍이 서로 달라 그 사이 짧게 회전 모멘트가 생김). 감속
+  속도를 낮추면 완화될 여지가 있으나 "즉각 정지" 체감이 희생되는 트레이드오프가 있어 적용
+  여부는 보류 중.
+
+## 9. 자동화 및 진단 인프라
+
+* 매 주행마다 `logs/` 폴더에 타임스탬프 CSV 자동 저장(수동으로 `--log` 지정할 필요 없음) —
+  RC/IMU/모터 피드백/오도메트리/헤딩홀드/슬립감지/온도까지 전 항목 기록, 이번 보고서의 모든
+  정량적 검증이 이 로그를 근거로 이루어짐.
+
+## 10. 향후 계획
+
+라이다(Livox Mid-360S) 기반 SLAM/자율주행 확장 계획은
+[07-lidar-slam-plan.md](07-lidar-slam-plan.md)에 별도 정리되어 있음(아직 미착수).
+
+## 종합 결과 요약
+
+| 항목 | 개선 전 | 개선 후 |
+| :--- | :--- | :--- |
+| 조종기 | Xbox/SDL2 (유선) | RadioMaster Pocket + XR1 (무선 RC) |
+| 브레이크 | 자연 정지 시에도 자동 잠김 | CH5로만 제어 |
+| 헤딩 인지 | 없음 | 오도메트리(x,y,θ) + 자동 직진 유지 |
+| 제자리 회전 정확도(지령 대비) | 44~58% | 93.6% |
+| 슬립 감지 | 개별 바퀴 전류 기반만 존재 | + IMU 기반 전신 슬립 이중 감지 |
+| 모터 상태 가시성 | 전류만 | + 온도(모터/드라이버) 실시간 감시 |
+| 주행 기록 | 수동 `--log` 필요 | 매 주행 자동 저장 |

doc/README.md

@@ -25,7 +25,8 @@
 | [03-system-architecture.md](03-system-architecture.md) | 로봇 물리 제원, 2드라이버×4모터 매핑, 스키드 스티어 운동학 |
 | [04-software-control.md](04-software-control.md) | `xbox_motor_control.cpp` 제어 루프/클래스 구조 분석 |
 | [05-error-troubleshooting.md](05-error-troubleshooting.md) | 알람 에러코드 표, 휠 이상(들뜸/걸림) 진단, 전류 한계 튜닝, 트러블슈팅 |
-| [06-imu-integration-plan.md](06-imu-integration-plan.md) | **(계획안, 미구현)** IMU 도입을 통한 제어 고도화 조사 및 단계별 계획 — Husky/논문 비교 |
+| [06-imu-integration-plan.md](06-imu-integration-plan.md) | IMU 도입을 통한 제어 고도화 조사 및 단계별 계획 — Husky/논문 비교 (Phase 1~5a 구현 완료) |
+| [07-lidar-slam-plan.md](07-lidar-slam-plan.md) | **(계획안, 미구현)** Livox Mid-360S 기반 LiDAR SLAM 도입 계획 (Phase 6) |
 
 06번 문서에 대한 Q&A 기록은 [`../QnA/`](../QnA/) 폴더에 별도로 누적된다 (예: [`q1.md`](../QnA/q1.md) / [`a1.md`](../QnA/a1.md)).
 

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