diff --git a/doc/joystick.md b/doc/joystick.md
index adf81cf..31f3301 100644
--- a/doc/joystick.md
+++ b/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`: 좌측 끝
• `1480 ~ 1500`: 중앙 (데드존)
• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1480~1500`) 적용
• 좌회전(-) / 우회전(+) 정규화 |
-| **CH2** | 피치/스로틀 스틱 | • `1000`: 위 끝 (전진)
• `1500`: 중앙 (정지)
• `2000`: 아래 끝 (후진) | **전후진 (Throttle / Linear X)** | • `1000` 방향으로 이동 시 전진
• `2000` 방향으로 이동 시 후진
• CH7에서 결정된 최대 속도 기준으로 선형 매핑 |
-| **CH5** | 2단 스위치 | • `1011`: 미작동 (OFF)
• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)
• `1011` 수신 시 일반 주행 허용 |
-| **CH7** | 2단 스위치 | • `1988`: 위쪽 누름
• `1011`: 아래쪽 누름 | **속도 모드 선택 (Speed Mode)** | • `1988` (위쪽): 저속 모드 (최대 **0.3 m/s**)
• `1011` (아래쪽): 고속 모드 (최대 **0.6 m/s**) |
+| **CH1** | 롤/조향 스틱 | • `1005`: 좌측 끝
• `1481 ~ 1503`: 중앙 (데드존)
• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1481~1503`) 적용
• 좌회전(-) / 우회전(+) 정규화 |
+| **CH2** | 피치/스로틀 스틱 | • `1005`: 최대 후진
• `1500`: 중앙 (정지)
• `2000`: 최대 전진 | **전후진 (Throttle / Linear X)** | • 값이 높을수록 전진, 낮을수록 후진 (CH1과 반대 방향)
• CH6에서 결정된 최대 속도 기준으로 선형 매핑 |
+| **CH5** | 2단 스위치 | • `1012`: 미작동 (OFF)
• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)
• `1012` 수신 시 일반 주행 허용 |
+| **CH6** | 2단 스위치 | • `1832`: 저속 위치
• `1012~1090`: 고속 위치 | **속도 모드 선택 (Speed Mode)** | • `1832`(≥1500 부근): 저속 모드 (최대 **0.3 m/s**)
• `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}$$
---
diff --git a/mac/mac_code b/mac/mac_code
new file mode 100644
index 0000000..6805b4e
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp
index 7a38ade..2b4c7d8 100644
--- a/xbox_motor_control.cpp
+++ b/xbox_motor_control.cpp
@@ -604,28 +604,50 @@ LidarObstacleDetector *LidarObstacleDetector::instance_ = nullptr;
// -----------------------------------------------------------------------------
// RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈
-// doc/joystick.md 기능 명세서 기반: 텍스트 라인 "CH1:1500,CH2:1500,..." 포맷을
-// 파싱해 채널별 raw 값(1000~2000)을 제공한다.
+// 실기 캡처로 확인한 실제 프로토콜: CRSF(Crossfire), 420000bps, 8N1.
+// 프레임: [0]=Sync(0xC8) [1]=Length [2]=FrameType [3..]=Payload [-1]=CRC8
+// FrameType 0x16(RC_CHANNELS_PACKED)의 22바이트 payload에 16채널이 11비트씩
+// 리틀엔디안으로 패킹되어 있다 (raw 172~1811 -> 1000~2000us로 선형 변환).
+// 참고: mac/mac_code (맥에서 정상 동작 확인된 레퍼런스 파이썬 스크립트)와
+// 동일한 파싱 방식을 따른다 — 실측 결과 CRC8이 표준 DVB-S2(poly 0xD5) 값과
+// 맞지 않아(송신기별 구현 차이로 추정) 레퍼런스와 동일하게 CRC는 검증하지
+// 않고 Sync+Length 기반 프레이밍만 신뢰한다.
// -----------------------------------------------------------------------------
+#ifndef BOTHER
+#define BOTHER 0010000 // 커널 termbits.h: CBAUDEX와 동일값, 임의 보드레이트 지정용
+#endif
+struct termios2 { // 의 커널 ABI와 동일 레이아웃 (glibc의
+ // struct termios와 이름 충돌을 피하기 위해 직접 선언).
+ // 420000bps처럼 표준 B-상수가 없는 보드레이트를
+ // TCSETS2/BOTHER ioctl로 설정하기 위해 필요하다.
+ tcflag_t c_iflag, c_oflag, c_cflag, c_lflag;
+ cc_t c_line;
+ cc_t c_cc[19];
+ speed_t c_ispeed, c_ospeed;
+};
+
struct RcConfig {
std::string port = "/dev/ttyUSB1";
- int baud = 115200;
+ int baud = 420000; // CRSF 표준 보드레이트
int ch_steer = 1; // CH1: 좌우 조향
int ch_throttle = 2; // CH2: 전후진
int ch_brake = 5; // CH5: 비상 브레이크
- int ch_speed = 7; // CH7: 속도 모드
+ int ch_speed = 6; // CH6: 속도 모드
- int steer_min = 1001, steer_center_lo = 1480, steer_center_hi = 1500,
+ // 실기 캡처로 확인한 실측값 (좌 1005 ~ 중 1481~1503 ~ 우 2000)
+ int steer_min = 1005, steer_center_lo = 1481, steer_center_hi = 1503,
steer_max = 2000;
- int throttle_min = 1000, throttle_center_lo = 1480, throttle_center_hi = 1520,
+ // 실측값: 후진끝 1005 ~ 중 1500 ~ 전진끝 2000 (CH2는 값이 높을수록 전진 —
+ // CH1과 반대 방향이라 아래 3.3 매핑 로직도 그에 맞춰 부호가 반대다)
+ int throttle_min = 1005, throttle_center_lo = 1480, throttle_center_hi = 1520,
throttle_max = 2000;
int brake_threshold = 1500; // 이상이면 브레이크 ON
int speed_threshold = 1500; // 이상이면 저속 모드
- float v_max_low = 0.3f; // m/s (CH7 저속 모드)
- float v_max_high = 0.6f; // m/s (CH7 고속 모드)
+ float v_max_low = 0.3f; // m/s (CH6 저속 모드)
+ float v_max_high = 1.5f; // m/s (CH6 고속 모드)
int failsafe_timeout_ms = 200; // 이 시간 이상 신호 미수신 시 비상정지
};
@@ -679,7 +701,11 @@ private:
std::chrono::steady_clock::time_point last_rx_time_{};
bool port_open_ = false;
- std::string line_buf_;
+ std::vector frame_buf_;
+
+ static constexpr uint8_t CRSF_SYNC = 0xC8;
+ static constexpr uint8_t CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16;
+ static constexpr size_t CRSF_MAX_FRAME_LEN = 64; // 스펙상 프레임은 훨씬 작음
bool openPort() {
fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
@@ -688,46 +714,40 @@ private:
fcntl(fd_, F_SETFL, O_NONBLOCK);
- struct termios options;
- tcgetattr(fd_, &options);
-
- speed_t speed = B115200;
- switch (cfg_.baud) {
- case 9600:
- speed = B9600;
- break;
- case 19200:
- speed = B19200;
- break;
- case 38400:
- speed = B38400;
- break;
- case 57600:
- speed = B57600;
- break;
- default:
- speed = B115200;
- break;
+ // CRSF 표준 420000bps는 termios의 표준 B-상수 목록에 없어 TCSETS2 +
+ // BOTHER + c_ispeed/c_ospeed(임의 정수 보드레이트) ioctl로 설정한다.
+ struct termios2 tio;
+ if (ioctl(fd_, TCGETS2, &tio) < 0) {
+ close(fd_);
+ fd_ = -1;
+ return false;
}
- cfsetispeed(&options, speed);
- cfsetospeed(&options, speed);
+ tio.c_cflag &= ~CBAUD;
+ tio.c_cflag |= BOTHER;
+ tio.c_ispeed = static_cast(cfg_.baud);
+ tio.c_ospeed = static_cast(cfg_.baud);
- options.c_cflag &= ~PARENB;
- options.c_cflag &= ~CSTOPB;
- options.c_cflag &= ~CSIZE;
- options.c_cflag |= CS8;
- options.c_cflag &= ~CRTSCTS;
- options.c_cflag |= CREAD | CLOCAL;
+ tio.c_cflag &= ~PARENB;
+ tio.c_cflag &= ~CSTOPB;
+ tio.c_cflag &= ~CSIZE;
+ tio.c_cflag |= CS8;
+ tio.c_cflag &= ~CRTSCTS;
+ tio.c_cflag |= CREAD | CLOCAL;
- // 텍스트 라인 프로토콜이므로 canonical 모드(라인 단위)로 사용
- options.c_lflag |= ICANON;
- options.c_lflag &= ~(ECHO | ECHOE | ISIG);
- options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
- ISTRIP | INLCR | IGNCR | ICRNL);
- options.c_oflag &= ~OPOST;
+ // 바이너리 프로토콜이므로 raw 모드(비정규/논캐노니컬)로 읽는다.
+ tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | IEXTEN);
+ tio.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
+ ISTRIP | INLCR | IGNCR | ICRNL);
+ tio.c_oflag &= ~OPOST;
+ tio.c_cc[VMIN] = 0;
+ tio.c_cc[VTIME] = 0;
- tcsetattr(fd_, TCSANOW, &options);
+ if (ioctl(fd_, TCSETS2, &tio) < 0) {
+ close(fd_);
+ fd_ = -1;
+ return false;
+ }
tcflush(fd_, TCIOFLUSH);
return true;
}
@@ -741,38 +761,64 @@ private:
port_open_ = false;
}
- void parseLine(const std::string &line) {
+ // 축적된 바이트 버퍼에서 CRSF 프레임을 최대한 뽑아낸다. Sync 바이트를
+ // 찾고, 선언된 Length만큼 버퍼에 데이터가 쌓일 때까지 기다린 뒤
+ // RC_CHANNELS_PACKED(0x16) 프레임이면 16채널(11비트씩)을 언패킹한다.
+ // (CRC8은 이 송신기에서 표준 DVB-S2 값과 맞지 않아 검증하지 않음 — 실기
+ // 캡처로 확인된 mac/mac_code 레퍼런스와 동일한 방식.)
+ void parseCrsfBuffer() {
std::map parsed;
- size_t pos = 0;
- while (pos <= line.size()) {
- size_t comma = line.find(',', pos);
- std::string tok = line.substr(
- pos, comma == std::string::npos ? std::string::npos : comma - pos);
+ size_t i = 0;
+ while (i < frame_buf_.size()) {
+ if (frame_buf_[i] != CRSF_SYNC) {
+ ++i;
+ continue;
+ }
+ if (i + 1 >= frame_buf_.size())
+ break; // length 바이트 대기
- size_t b = tok.find_first_not_of(" \t\r\n");
- size_t e = tok.find_last_not_of(" \t\r\n");
- if (b != std::string::npos) {
- tok = tok.substr(b, e - b + 1);
- if (tok.size() >= 4 && (tok[0] == 'C' || tok[0] == 'c') &&
- (tok[1] == 'H' || tok[1] == 'h')) {
- size_t colon = tok.find(':');
- if (colon != std::string::npos) {
- try {
- int ch_num = std::stoi(tok.substr(2, colon - 2));
- int val = std::stoi(tok.substr(colon + 1));
- parsed[ch_num] = val;
- } catch (...) {
- // 손상된 토큰은 무시
- }
+ uint8_t length = frame_buf_[i + 1];
+ size_t total_len = static_cast(length) + 2;
+ if (length < 2 || total_len > CRSF_MAX_FRAME_LEN) {
+ // Sync처럼 보이는 노이즈 바이트: 1바이트만 버리고 재동기화
+ ++i;
+ continue;
+ }
+ if (i + total_len > frame_buf_.size())
+ break; // 프레임 전체 수신 대기
+
+ uint8_t frame_type = frame_buf_[i + 2];
+ if (frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED && length >= 23) {
+ // payload = frame_buf_[i+3 .. i+total_len-2] (CRC 바이트 제외 22바이트
+ // = 176비트 = 16채널×11비트). uint64_t 하나에 통째로 담으면 64비트를
+ // 넘는 시프트가 발생해 정의되지 않은 동작(UB)이 되므로, 32비트 롤링
+ // 비트 누산기로 바이트를 하나씩 채워가며 11비트씩 뽑아낸다.
+ uint32_t bit_accum = 0;
+ int bit_count = 0;
+ int byte_idx = 0;
+ for (int ch = 0; ch < 16; ++ch) {
+ while (bit_count < 11) {
+ bit_accum |= static_cast(frame_buf_[i + 3 + byte_idx])
+ << bit_count;
+ bit_count += 8;
+ ++byte_idx;
}
+ int raw_val = static_cast(bit_accum & 0x7FFU);
+ bit_accum >>= 11;
+ bit_count -= 11;
+ int us_val = static_cast(
+ std::lround((raw_val - 172) * 1000.0 / 1639.0 + 1000.0));
+ parsed[ch + 1] = us_val; // CH1..CH16 (1-based)
}
}
-
- if (comma == std::string::npos)
- break;
- pos = comma + 1;
+ i += total_len;
}
+ // 처리하지 못한 잔여 바이트(다음 프레임 일부)만 남기고 버퍼 정리
+ frame_buf_.erase(frame_buf_.begin(), frame_buf_.begin() + i);
+ if (frame_buf_.size() > CRSF_MAX_FRAME_LEN * 4) // 비정상 누적 방지
+ frame_buf_.clear();
+
if (!parsed.empty()) {
std::lock_guard lock(mutex_);
for (auto &kv : parsed)
@@ -812,15 +858,8 @@ private:
ssize_t n = read(fd_, buf, sizeof(buf));
if (n > 0) {
- line_buf_.append(buf, static_cast(n));
- size_t nl;
- while ((nl = line_buf_.find('\n')) != std::string::npos) {
- std::string line = line_buf_.substr(0, nl);
- line_buf_.erase(0, nl + 1);
- parseLine(line);
- }
- if (line_buf_.size() > 4096) // 개행 없는 쓰레기 데이터 누적 방지
- line_buf_.clear();
+ frame_buf_.insert(frame_buf_.end(), buf, buf + n);
+ parseCrsfBuffer();
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n";
closePort();
@@ -1052,6 +1091,7 @@ int main(int argc, char **argv) {
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,"
"cmd_fl,cmd_fr,cmd_rl,cmd_rr,"
"fb_fl,fb_fr,fb_rl,fb_rr,"
"amp_fl,amp_fr,amp_rl,amp_rr,"
@@ -1077,6 +1117,18 @@ int main(int argc, char **argv) {
}
}
+ // 모터(--port)와 RC 수신기(--rc_port)가 같은 물리 포트를 가리키면 두
+ // 스레드가 같은 장치의 read()를 두고 경쟁해 RC 라인 파싱이 깨진다.
+ // 흔한 실수(예: 모터 미연결 상태에서 RC 수신기가 ttyUSB0에 꽂혀있는데
+ // --rc_port를 기본값 ttyUSB1로 둔 채 실행)를 바로 알아차리도록 경고한다.
+ if (port1 == rc_cfg.port) {
+ std::cout << "\n[경고] --port(모터, " << port1 << ")와 --rc_port(RC 수신기, "
+ << rc_cfg.port
+ << ")가 동일한 포트로 설정되어 있습니다. 두 기능이 같은 장치를 "
+ "동시에 읽으면 RC 데이터 파싱이 깨질 수 있으니 서로 다른 "
+ "포트로 지정하세요.\n\n";
+ }
+
RcReceiver rc(rc_cfg);
rc.start();
std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 ("
@@ -1221,6 +1273,14 @@ int main(int argc, char **argv) {
rc.getChannel(rc_cfg.ch_brake, raw_brake);
rc.getChannel(rc_cfg.ch_speed, raw_speed_mode);
+ // CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
+ // 직접 눈으로 검증할 수 있도록 HUD/CSV에 그대로 노출).
+ int raw_ch[9]; // 인덱스 1..8 사용
+ for (int ch = 1; ch <= 8; ++ch) {
+ if (!rc.getChannel(ch, raw_ch[ch]))
+ raw_ch[ch] = 0; // 아직 수신 못한 채널은 0으로 표시
+ }
+
if (!rc_ok && prev_rc_ok) {
std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms
<< "ms 이상 미수신) -> 비상 정지.\n";
@@ -1230,7 +1290,7 @@ int main(int argc, char **argv) {
// 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리
bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold);
- // 3.2 속도 제한 설정 (CH7)
+ // 3.2 속도 제한 설정 (CH6)
float v_max_now = (raw_speed_mode >= rc_cfg.speed_threshold)
? rc_cfg.v_max_low
: rc_cfg.v_max_high;
@@ -1239,17 +1299,18 @@ int main(int argc, char **argv) {
float omega = 0.0f; // 각속도 rad/s
if (!brake_on) {
- // 3.3 전후진 속도 계산 (CH2)
+ // 3.3 전후진 속도 계산 (CH2) — 실측 결과 raw 값이 높을수록 전진(+),
+ // 낮을수록 후진(-)이라 CH1(조향)과는 반대 부호 방향이다.
int t = std::clamp(raw_throttle, rc_cfg.throttle_min, rc_cfg.throttle_max);
if (t >= rc_cfg.throttle_center_lo && t <= rc_cfg.throttle_center_hi) {
v_x = 0.0f;
} else if (t < rc_cfg.throttle_center_lo) {
- v_x = (static_cast(rc_cfg.throttle_center_lo - t) /
- static_cast(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
+ v_x = -(static_cast(rc_cfg.throttle_center_lo - t) /
+ static_cast(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
v_max_now;
} else {
- v_x = -(static_cast(t - rc_cfg.throttle_center_hi) /
- static_cast(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) *
+ v_x = (static_cast(t - rc_cfg.throttle_center_hi) /
+ static_cast(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) *
v_max_now;
}
@@ -1619,7 +1680,10 @@ int main(int argc, char **argv) {
log_stream << t_ms << ',' << state << ',' << raw_steer << ','
<< raw_throttle << ',' << (rc_ok ? 1 : 0) << ','
<< (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ','
- << omega << ',' << cmd_fl << ',' << cmd_fr << ',' << cmd_rl
+ << 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] << ','
+ << 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]
@@ -1629,9 +1693,19 @@ int main(int argc, char **argv) {
<< dist_axle << ',' << comm_ms << '\n';
}
- printf("\r\033[K[%-8s] RC:%s%s AXLE:%5.3fm | %-28s | I(A):FL%+4.1f FR%+4.1f "
- "RL%+4.1f RR%+4.1f | WHL(FL/FR/RL/RR):%s%s | %4.1fms",
+ // RC 조종기 입력값 피드백: 실기 조작 중 CH1~CH8 raw 값(CH5=브레이크 값
+ // 직접 확인용 포함)과 그로부터 계산된 선속도/각속도 지령을 실시간으로
+ // 확인할 수 있도록 HUD에 표시한다.
+ const char *speed_mode_str =
+ (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 | "
+ "I(A):FL%+4.1f FR%+4.1f RL%+4.1f RR%+4.1f | "
+ "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,
dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
rl_amp, rr_amp, wheel_status_str,
fault_active ? " [!!알람!!]" : "", comm_ms);
diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp
index 59e3167..b0d4d9b 100755
Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ