diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp index 057584f..4af3b35 100644 --- a/xbox_motor_control.cpp +++ b/xbox_motor_control.cpp @@ -652,6 +652,13 @@ struct RcConfig { // 손상된 프레임으로 간주해 버린다. CH5/CH6(스위치)는 원래 한 번에 크게 // 뛰는 게 정상이라 이 제한을 적용하지 않는다. int max_slew_per_tick = 200; + // 슬루레이트 거부가 연속으로 이 틱 수 이상 이어지면(=한 번의 손상 + // 프레임이 아니라 실제로 스틱이 그만큼 빠르게/멀리 움직인 상황으로 + // 판단) 그냥 최신값을 그대로 받아들여 재동기화한다. 이 escape hatch가 + // 없으면 range는 정상인데 slew만 거부된 값 하나가 last_raw_*를 그 + // 자리에 영원히 고정시켜버려서, 이후 들어오는 진짜 값들도 그 고정된 + // 기준과 계속 200 이상 차이나 버려 채널이 그 값에 영구히 멈춰버린다. + int max_reject_streak = 3; float v_max_low = 0.3f; // m/s (CH6 저속 모드) float v_max_high = 1.5f; // m/s (CH6 고속 모드) @@ -1068,6 +1075,8 @@ int main(int argc, char **argv) { rc_cfg.speed_threshold = std::stoi(argv[++i]); else if (arg == "--rc_max_slew" && i + 1 < argc) rc_cfg.max_slew_per_tick = std::stoi(argv[++i]); + else if (arg == "--rc_max_reject_streak" && i + 1 < argc) + rc_cfg.max_reject_streak = std::stoi(argv[++i]); else if (arg == "--rc_vmax_low" && i + 1 < argc) rc_cfg.v_max_low = std::stof(argv[++i]); else if (arg == "--rc_vmax_high" && i + 1 < argc) @@ -1309,11 +1318,16 @@ int main(int argc, char **argv) { // 물리적 타당성 검사로 걸러낸다: // 1) 캘리브레이션 범위(steer_min~max, throttle_min~max) 밖의 값 거부 // 2) 슬루레이트: 한 틱(~10ms) 사이에 사람이 스틱으로 낼 수 없을 만큼 - // 크게 튀는 값 거부 (예: 1227 -> 1800 같은 순간 점프) - // 두 경우 모두 그 틱은 버리고 마지막으로 유효했던 값을 그대로 사용한다. + // 크게 튀는 값 거부 (예: 1227 -> 1800 같은 순간 점프). 단, max_reject_streak + // 틱 연속으로 거부되면(=한 번의 손상 프레임이 아니라 실제 상황) 강제 + // 수락해 재동기화한다 — 그렇지 않으면 last_raw_*가 그 자리에 영구히 + // 고정되어, 이후 들어오는 진짜 값들도 계속 그 고정된 기준과 크게 + // 차이나 버려 채널이 응답 없이 멈춰버린다. + // 범위 밖 값은 항상 거부, 그 틱은 마지막으로 유효했던 값을 그대로 사용한다. int last_raw_steer = rc_cfg.steer_center_lo; int last_raw_throttle = 1500; bool steer_initialized = false, throttle_initialized = false; + int steer_reject_streak = 0, throttle_reject_streak = 0; while (g_running) { // --------------------------------------------------------------------- @@ -1329,26 +1343,41 @@ int main(int argc, char **argv) { bool steer_range_ok = raw_steer >= rc_cfg.steer_min && raw_steer <= rc_cfg.steer_max; - bool steer_slew_ok = !steer_initialized || - std::abs(raw_steer - last_raw_steer) <= - rc_cfg.max_slew_per_tick; - if (steer_range_ok && steer_slew_ok) { - last_raw_steer = raw_steer; - steer_initialized = true; + if (!steer_range_ok) { + raw_steer = last_raw_steer; // 캘리브레이션 범위 밖 -> 항상 거부 } else { - raw_steer = last_raw_steer; // 범위 밖 또는 비정상 점프 -> 손상된 프레임으로 간주 + bool steer_slew_ok = !steer_initialized || + std::abs(raw_steer - last_raw_steer) <= + rc_cfg.max_slew_per_tick; + // slew 거부가 max_reject_streak틱 연속되면 진짜 빠른 스틱 조작으로 + // 보고 강제로 재동기화한다 (아래 주석 참고: 그렇지 않으면 last_raw_*가 + // 영구히 고정되어 채널이 멈춰버림). + if (steer_slew_ok || steer_reject_streak >= rc_cfg.max_reject_streak) { + last_raw_steer = raw_steer; + steer_initialized = true; + steer_reject_streak = 0; + } else { + raw_steer = last_raw_steer; + ++steer_reject_streak; + } } bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min && raw_throttle <= rc_cfg.throttle_max; - bool throttle_slew_ok = !throttle_initialized || - std::abs(raw_throttle - last_raw_throttle) <= - rc_cfg.max_slew_per_tick; - if (throttle_range_ok && throttle_slew_ok) { - last_raw_throttle = raw_throttle; - throttle_initialized = true; - } else { + if (!throttle_range_ok) { raw_throttle = last_raw_throttle; + } else { + bool throttle_slew_ok = !throttle_initialized || + std::abs(raw_throttle - last_raw_throttle) <= + rc_cfg.max_slew_per_tick; + if (throttle_slew_ok || throttle_reject_streak >= rc_cfg.max_reject_streak) { + last_raw_throttle = raw_throttle; + throttle_initialized = true; + throttle_reject_streak = 0; + } else { + raw_throttle = last_raw_throttle; + ++throttle_reject_streak; + } } // CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로 @@ -1392,18 +1421,22 @@ int main(int argc, char **argv) { v_max_now; } - // 3.4 좌우 조향 각속도 계산 (CH1) + // 3.4 좌우 조향 각속도 계산 (CH1). omega 부호 규약(코드 전역 공통, + // 자동회피 로직의 "+omega=좌회전/-omega=우회전" 주석과 일치): 스틱을 + // 왼쪽(raw 낮음)으로 밀면 +omega(좌회전), 오른쪽(raw 높음)이면 + // -omega(우회전)여야 한다. 예전엔 부호가 반대로 들어가 있어서 스틱 + // 방향과 실제 회전 방향이 뒤바뀌어 있었다. int s = std::clamp(raw_steer, rc_cfg.steer_min, rc_cfg.steer_max); if (s >= rc_cfg.steer_center_lo && s <= rc_cfg.steer_center_hi) { omega = 0.0f; } else if (s < rc_cfg.steer_center_lo) { - omega = -(static_cast(rc_cfg.steer_center_lo - s) / - static_cast(rc_cfg.steer_center_lo - rc_cfg.steer_min)) * + omega = (static_cast(rc_cfg.steer_center_lo - s) / + static_cast(rc_cfg.steer_center_lo - rc_cfg.steer_min)) * omega_max; } else { - omega = (static_cast(s - rc_cfg.steer_center_hi) / - static_cast(rc_cfg.steer_max - rc_cfg.steer_center_hi)) * - omega_max; + omega = -(static_cast(s - rc_cfg.steer_center_hi) / + static_cast(rc_cfg.steer_max - rc_cfg.steer_center_hi)) * + omega_max; } } diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp index aa2fe86..1d5e7e7 100755 Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ