azmo/fori_zltech_motor_test
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add doc

Browse Source
This commit is contained in:
robin
2026-08-19 13:56:22 +09:00
parent bb5a0adc0c
commit 6be30eb929
4 changed files with 263 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xbox_motor_control.cpp
+161 -87
View File
@@ -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 { // <asm/termbits.h>의 커널 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<uint8_t> 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<speed_t>(cfg_.baud);
tio.c_ospeed = static_cast<speed_t>(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<int, int> 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<size_t>(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<uint32_t>(frame_buf_[i + 3 + byte_idx])
<< bit_count;
bit_count += 8;
++byte_idx;
}
int raw_val = static_cast<int>(bit_accum & 0x7FFU);
bit_accum >>= 11;
bit_count -= 11;
int us_val = static_cast<int>(
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<std::mutex> 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<size_t>(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<float>(rc_cfg.throttle_center_lo - t) /
static_cast<float>(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
v_x = -(static_cast<float>(rc_cfg.throttle_center_lo - t) /
static_cast<float>(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
v_max_now;
} else {
v_x = -(static_cast<float>(t - rc_cfg.throttle_center_hi) /
static_cast<float>(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) *
v_x = (static_cast<float>(t - rc_cfg.throttle_center_hi) /
static_cast<float>(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);