Fix RC steering sign inversion and throttle/steer slew-filter livelock
Steering (CH1): omega was computed with the opposite sign from the convention used everywhere else in this file (+omega=left, -omega=right, per the LiDAR avoidance comments and the prior Xbox joystick code), so pushing the stick left steered the robot right and vice versa. Flipped the sign at the single point omega is derived from the stick so curve-turn and spin-turn both inherit the fix. Throttle/steer anti-corruption filter: once a reading was rejected for exceeding max_slew_per_tick, last_raw_throttle/last_raw_steer never updated, so every subsequent real reading kept failing the same slew check against that now-permanently-stale reference — a livelock. Symptom: CH2 showing neutral (1500) on the HUD while the robot kept driving forward regardless of stick input. Added a max_reject_streak escape hatch: after a few consecutive rejections in a row, treat it as a real fast stick movement rather than a one-off corrupted frame and resync to the latest value. Verified working on hardware. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
This commit is contained in:
+41
-8
@@ -652,6 +652,13 @@ struct RcConfig {
|
|||||||
// 손상된 프레임으로 간주해 버린다. CH5/CH6(스위치)는 원래 한 번에 크게
|
// 손상된 프레임으로 간주해 버린다. CH5/CH6(스위치)는 원래 한 번에 크게
|
||||||
// 뛰는 게 정상이라 이 제한을 적용하지 않는다.
|
// 뛰는 게 정상이라 이 제한을 적용하지 않는다.
|
||||||
int max_slew_per_tick = 200;
|
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_low = 0.3f; // m/s (CH6 저속 모드)
|
||||||
float v_max_high = 1.5f; // 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]);
|
rc_cfg.speed_threshold = std::stoi(argv[++i]);
|
||||||
else if (arg == "--rc_max_slew" && i + 1 < argc)
|
else if (arg == "--rc_max_slew" && i + 1 < argc)
|
||||||
rc_cfg.max_slew_per_tick = std::stoi(argv[++i]);
|
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)
|
else if (arg == "--rc_vmax_low" && i + 1 < argc)
|
||||||
rc_cfg.v_max_low = std::stof(argv[++i]);
|
rc_cfg.v_max_low = std::stof(argv[++i]);
|
||||||
else if (arg == "--rc_vmax_high" && i + 1 < argc)
|
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) 밖의 값 거부
|
// 1) 캘리브레이션 범위(steer_min~max, throttle_min~max) 밖의 값 거부
|
||||||
// 2) 슬루레이트: 한 틱(~10ms) 사이에 사람이 스틱으로 낼 수 없을 만큼
|
// 2) 슬루레이트: 한 틱(~10ms) 사이에 사람이 스틱으로 낼 수 없을 만큼
|
||||||
// 크게 튀는 값 거부 (예: 1227 -> 1800 같은 순간 점프)
|
// 크게 튀는 값 거부 (예: 1227 -> 1800 같은 순간 점프). 단, max_reject_streak
|
||||||
// 두 경우 모두 그 틱은 버리고 마지막으로 유효했던 값을 그대로 사용한다.
|
// 틱 연속으로 거부되면(=한 번의 손상 프레임이 아니라 실제 상황) 강제
|
||||||
|
// 수락해 재동기화한다 — 그렇지 않으면 last_raw_*가 그 자리에 영구히
|
||||||
|
// 고정되어, 이후 들어오는 진짜 값들도 계속 그 고정된 기준과 크게
|
||||||
|
// 차이나 버려 채널이 응답 없이 멈춰버린다.
|
||||||
|
// 범위 밖 값은 항상 거부, 그 틱은 마지막으로 유효했던 값을 그대로 사용한다.
|
||||||
int last_raw_steer = rc_cfg.steer_center_lo;
|
int last_raw_steer = rc_cfg.steer_center_lo;
|
||||||
int last_raw_throttle = 1500;
|
int last_raw_throttle = 1500;
|
||||||
bool steer_initialized = false, throttle_initialized = false;
|
bool steer_initialized = false, throttle_initialized = false;
|
||||||
|
int steer_reject_streak = 0, throttle_reject_streak = 0;
|
||||||
|
|
||||||
while (g_running) {
|
while (g_running) {
|
||||||
// ---------------------------------------------------------------------
|
// ---------------------------------------------------------------------
|
||||||
@@ -1329,26 +1343,41 @@ int main(int argc, char **argv) {
|
|||||||
|
|
||||||
bool steer_range_ok =
|
bool steer_range_ok =
|
||||||
raw_steer >= rc_cfg.steer_min && raw_steer <= rc_cfg.steer_max;
|
raw_steer >= rc_cfg.steer_min && raw_steer <= rc_cfg.steer_max;
|
||||||
|
if (!steer_range_ok) {
|
||||||
|
raw_steer = last_raw_steer; // 캘리브레이션 범위 밖 -> 항상 거부
|
||||||
|
} else {
|
||||||
bool steer_slew_ok = !steer_initialized ||
|
bool steer_slew_ok = !steer_initialized ||
|
||||||
std::abs(raw_steer - last_raw_steer) <=
|
std::abs(raw_steer - last_raw_steer) <=
|
||||||
rc_cfg.max_slew_per_tick;
|
rc_cfg.max_slew_per_tick;
|
||||||
if (steer_range_ok && steer_slew_ok) {
|
// slew 거부가 max_reject_streak틱 연속되면 진짜 빠른 스틱 조작으로
|
||||||
|
// 보고 강제로 재동기화한다 (아래 주석 참고: 그렇지 않으면 last_raw_*가
|
||||||
|
// 영구히 고정되어 채널이 멈춰버림).
|
||||||
|
if (steer_slew_ok || steer_reject_streak >= rc_cfg.max_reject_streak) {
|
||||||
last_raw_steer = raw_steer;
|
last_raw_steer = raw_steer;
|
||||||
steer_initialized = true;
|
steer_initialized = true;
|
||||||
|
steer_reject_streak = 0;
|
||||||
} else {
|
} else {
|
||||||
raw_steer = last_raw_steer; // 범위 밖 또는 비정상 점프 -> 손상된 프레임으로 간주
|
raw_steer = last_raw_steer;
|
||||||
|
++steer_reject_streak;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min &&
|
bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min &&
|
||||||
raw_throttle <= rc_cfg.throttle_max;
|
raw_throttle <= rc_cfg.throttle_max;
|
||||||
|
if (!throttle_range_ok) {
|
||||||
|
raw_throttle = last_raw_throttle;
|
||||||
|
} else {
|
||||||
bool throttle_slew_ok = !throttle_initialized ||
|
bool throttle_slew_ok = !throttle_initialized ||
|
||||||
std::abs(raw_throttle - last_raw_throttle) <=
|
std::abs(raw_throttle - last_raw_throttle) <=
|
||||||
rc_cfg.max_slew_per_tick;
|
rc_cfg.max_slew_per_tick;
|
||||||
if (throttle_range_ok && throttle_slew_ok) {
|
if (throttle_slew_ok || throttle_reject_streak >= rc_cfg.max_reject_streak) {
|
||||||
last_raw_throttle = raw_throttle;
|
last_raw_throttle = raw_throttle;
|
||||||
throttle_initialized = true;
|
throttle_initialized = true;
|
||||||
|
throttle_reject_streak = 0;
|
||||||
} else {
|
} else {
|
||||||
raw_throttle = last_raw_throttle;
|
raw_throttle = last_raw_throttle;
|
||||||
|
++throttle_reject_streak;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
|
// CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
|
||||||
@@ -1392,16 +1421,20 @@ int main(int argc, char **argv) {
|
|||||||
v_max_now;
|
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);
|
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) {
|
if (s >= rc_cfg.steer_center_lo && s <= rc_cfg.steer_center_hi) {
|
||||||
omega = 0.0f;
|
omega = 0.0f;
|
||||||
} else if (s < rc_cfg.steer_center_lo) {
|
} else if (s < rc_cfg.steer_center_lo) {
|
||||||
omega = -(static_cast<float>(rc_cfg.steer_center_lo - s) /
|
omega = (static_cast<float>(rc_cfg.steer_center_lo - s) /
|
||||||
static_cast<float>(rc_cfg.steer_center_lo - rc_cfg.steer_min)) *
|
static_cast<float>(rc_cfg.steer_center_lo - rc_cfg.steer_min)) *
|
||||||
omega_max;
|
omega_max;
|
||||||
} else {
|
} else {
|
||||||
omega = (static_cast<float>(s - rc_cfg.steer_center_hi) /
|
omega = -(static_cast<float>(s - rc_cfg.steer_center_hi) /
|
||||||
static_cast<float>(rc_cfg.steer_max - rc_cfg.steer_center_hi)) *
|
static_cast<float>(rc_cfg.steer_max - rc_cfg.steer_center_hi)) *
|
||||||
omega_max;
|
omega_max;
|
||||||
}
|
}
|
||||||
|
|||||||
Binary file not shown.
Reference in New Issue
Block a user