robin
9a97cf9022
IMU integration (WitMotion HWT905-RS232, doc/06-imu-integration-plan.md): - Phase 1: raw accel/gyro/angle observation, power-on-relative yaw offset - Phase 2: x,y odometry + wheel-vs-IMU omega residual, using a self-fused heading (wheel encoder omega + raw gyro_z average) instead of the IMU's own onboard fused yaw, which field logs showed disagreeing with its own raw gyro sign ~30% of the time during turns - Phase 3: straight-line heading-hold PI trim, active only when steering is centered and no lidar dodge is in progress, capped and field-tuned - Phase 4: whole-body slip detection (commanded vs IMU-measured omega ratio) that temporarily scales down v_x/omega, independent of the per-wheel current-based diagnostics - Phase 5a: k_skid/effective_w replaced with values fitted from real wheel-vs-IMU logs (0.406->0.51, 0.684->0.87) instead of the geometric formula, which field data showed under-driving every turn RC receiver: switched from ttyUSB* guessing to udev-stable device names (ttyMOTOR/ttyRC/ttyIMU), wired through run_4wd.sh and set_low_latency.sh. Motor driver: read motor/driver temperature registers (0x20A4/0x20B0) for proactive overheat visibility in the HUD/CSV. Control fixes: - Airborne-wheel zeroing no longer applies during spin turns, where reaction-torque-driven diagonal load transfer was misclassified as wheels leaving the ground and cutting spin torque in half - jerkLimitedStep's instant-acceleration path now only fires for same-direction speed increases; sign reversals (RC noise/deadzone jitter near a stop, or a genuine direction change) go through the jerk-limited path instead of snapping across zero Auto CSV logging (logs/, gitignored) extended with encoder ticks, IMU, odometry, heading-hold, slip, and temperature columns for field analysis. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
2442 lines
103 KiB
C++
2442 lines
103 KiB
C++
#include <algorithm>
|
||
#include <array>
|
||
#include <atomic>
|
||
#include <cerrno>
|
||
#include <chrono>
|
||
#include <cmath>
|
||
#include <csignal>
|
||
#include <ctime>
|
||
#include <fcntl.h>
|
||
#include <filesystem>
|
||
#include <fstream>
|
||
#include <iostream>
|
||
#include <map>
|
||
#include <mutex>
|
||
#include <string>
|
||
#include <sys/ioctl.h>
|
||
#include <termios.h>
|
||
#include <thread>
|
||
#include <unistd.h>
|
||
#include <vector>
|
||
|
||
#include "livox_lidar_api.h"
|
||
#include "livox_lidar_def.h"
|
||
|
||
// Global flag for signal handling
|
||
volatile std::sig_atomic_t g_running = 1;
|
||
|
||
void signalHandler(int signum) {
|
||
(void)signum;
|
||
g_running = 0;
|
||
}
|
||
|
||
// Modbus RTU CRC16 calculation
|
||
uint16_t calcCRC16(const uint8_t *data, size_t len) {
|
||
uint16_t crc = 0xFFFF;
|
||
for (size_t i = 0; i < len; ++i) {
|
||
crc ^= data[i];
|
||
for (int j = 0; j < 8; ++j) {
|
||
if (crc & 0x0001) {
|
||
crc >>= 1;
|
||
crc ^= 0xA001;
|
||
} else {
|
||
crc >>= 1;
|
||
}
|
||
}
|
||
}
|
||
return crc;
|
||
}
|
||
|
||
class SerialPort {
|
||
private:
|
||
int fd_;
|
||
std::string port_name_;
|
||
|
||
public:
|
||
SerialPort() : fd_(-1) {}
|
||
~SerialPort() { closePort(); }
|
||
|
||
bool openPort(const std::string &port_name, int baudrate = 115200) {
|
||
port_name_ = port_name;
|
||
fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
||
if (fd_ < 0) {
|
||
std::cerr << "[오류] C++ 포트 열기 실패: " << port_name << std::endl;
|
||
return false;
|
||
}
|
||
|
||
fcntl(fd_, F_SETFL, 0);
|
||
|
||
struct termios options;
|
||
tcgetattr(fd_, &options);
|
||
|
||
speed_t speed = B115200;
|
||
switch (baudrate) {
|
||
case 9600:
|
||
speed = B9600;
|
||
break;
|
||
case 19200:
|
||
speed = B19200;
|
||
break;
|
||
case 38400:
|
||
speed = B38400;
|
||
break;
|
||
case 57600:
|
||
speed = B57600;
|
||
break;
|
||
default:
|
||
speed = B115200;
|
||
break;
|
||
}
|
||
|
||
cfsetispeed(&options, speed);
|
||
cfsetospeed(&options, speed);
|
||
|
||
options.c_cflag &= ~PARENB; // No parity
|
||
options.c_cflag &= ~CSTOPB; // 1 stop bit
|
||
options.c_cflag &= ~CSIZE;
|
||
options.c_cflag |= CS8; // 8 data bits
|
||
options.c_cflag &= ~CRTSCTS; // No hardware flow control
|
||
options.c_cflag |= CREAD | CLOCAL;
|
||
|
||
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||
options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
|
||
ISTRIP | INLCR | IGNCR | ICRNL);
|
||
options.c_oflag &= ~OPOST;
|
||
|
||
options.c_cc[VMIN] = 0;
|
||
options.c_cc[VTIME] = 1; // 0.1s timeout
|
||
|
||
tcsetattr(fd_, TCSANOW, &options);
|
||
tcflush(fd_, TCIOFLUSH);
|
||
return true;
|
||
}
|
||
|
||
void closePort() {
|
||
if (fd_ >= 0) {
|
||
close(fd_);
|
||
fd_ = -1;
|
||
}
|
||
}
|
||
|
||
bool writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
|
||
uint8_t pkt[8];
|
||
pkt[0] = slave;
|
||
pkt[1] = 0x06;
|
||
pkt[2] = (reg >> 8) & 0xFF;
|
||
pkt[3] = reg & 0xFF;
|
||
pkt[4] = (val >> 8) & 0xFF;
|
||
pkt[5] = val & 0xFF;
|
||
|
||
uint16_t crc = calcCRC16(pkt, 6);
|
||
pkt[6] = crc & 0xFF;
|
||
pkt[7] = (crc >> 8) & 0xFF;
|
||
|
||
tcflush(fd_, TCIOFLUSH);
|
||
ssize_t written = write(fd_, pkt, 8);
|
||
if (written != 8)
|
||
return false;
|
||
|
||
if (slave == 0)
|
||
return true; // Broadcast
|
||
|
||
uint8_t rx_buf[8];
|
||
ssize_t rx_bytes = 0;
|
||
auto start = std::chrono::steady_clock::now();
|
||
while (rx_bytes < 8) {
|
||
ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
|
||
if (res > 0)
|
||
rx_bytes += res;
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
|
||
.count() > 50)
|
||
break;
|
||
std::this_thread::sleep_for(std::chrono::microseconds(500));
|
||
}
|
||
if (rx_bytes != 8)
|
||
return false;
|
||
|
||
// CRC 검증: 손상된 응답을 정상으로 오인하지 않도록 확인
|
||
uint16_t resp_crc = calcCRC16(rx_buf, 6);
|
||
uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) |
|
||
(static_cast<uint16_t>(rx_buf[7]) << 8);
|
||
return resp_crc == recv_crc && rx_buf[0] == slave;
|
||
}
|
||
|
||
bool writeRegs(uint8_t slave, uint16_t reg,
|
||
const std::vector<uint16_t> &vals) {
|
||
size_t count = vals.size();
|
||
size_t pkt_len = 7 + count * 2 + 2;
|
||
std::vector<uint8_t> pkt(pkt_len);
|
||
|
||
pkt[0] = slave;
|
||
pkt[1] = 0x10;
|
||
pkt[2] = (reg >> 8) & 0xFF;
|
||
pkt[3] = reg & 0xFF;
|
||
pkt[4] = (count >> 8) & 0xFF;
|
||
pkt[5] = count & 0xFF;
|
||
pkt[6] = static_cast<uint8_t>(count * 2);
|
||
|
||
for (size_t i = 0; i < count; ++i) {
|
||
pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
|
||
pkt[8 + i * 2] = vals[i] & 0xFF;
|
||
}
|
||
|
||
uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
|
||
pkt[pkt_len - 2] = crc & 0xFF;
|
||
pkt[pkt_len - 1] = (crc >> 8) & 0xFF;
|
||
|
||
tcflush(fd_, TCIOFLUSH);
|
||
ssize_t written = write(fd_, pkt.data(), pkt_len);
|
||
if (written != static_cast<ssize_t>(pkt_len))
|
||
return false;
|
||
|
||
if (slave == 0)
|
||
return true;
|
||
|
||
uint8_t rx_buf[8];
|
||
ssize_t rx_bytes = 0;
|
||
auto start = std::chrono::steady_clock::now();
|
||
while (rx_bytes < 8) {
|
||
ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
|
||
if (res > 0)
|
||
rx_bytes += res;
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
|
||
.count() > 50)
|
||
break;
|
||
std::this_thread::sleep_for(std::chrono::microseconds(500));
|
||
}
|
||
if (rx_bytes != 8)
|
||
return false;
|
||
|
||
uint16_t resp_crc = calcCRC16(rx_buf, 6);
|
||
uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) |
|
||
(static_cast<uint16_t>(rx_buf[7]) << 8);
|
||
return resp_crc == recv_crc && rx_buf[0] == slave;
|
||
}
|
||
|
||
bool readRegs(uint8_t slave, uint16_t reg, uint16_t count,
|
||
std::vector<uint16_t> &out_vals) {
|
||
uint8_t pkt[8];
|
||
pkt[0] = slave;
|
||
pkt[1] = 0x03;
|
||
pkt[2] = (reg >> 8) & 0xFF;
|
||
pkt[3] = reg & 0xFF;
|
||
pkt[4] = (count >> 8) & 0xFF;
|
||
pkt[5] = count & 0xFF;
|
||
|
||
uint16_t crc = calcCRC16(pkt, 6);
|
||
pkt[6] = crc & 0xFF;
|
||
pkt[7] = (crc >> 8) & 0xFF;
|
||
|
||
tcflush(fd_, TCIOFLUSH);
|
||
if (write(fd_, pkt, 8) != 8)
|
||
return false;
|
||
|
||
size_t expected_bytes = 5 + count * 2;
|
||
std::vector<uint8_t> rx_buf(expected_bytes);
|
||
size_t rx_bytes = 0;
|
||
|
||
auto start = std::chrono::steady_clock::now();
|
||
while (rx_bytes < expected_bytes) {
|
||
ssize_t res =
|
||
read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
|
||
if (res > 0)
|
||
rx_bytes += res;
|
||
auto now = std::chrono::steady_clock::now();
|
||
// 15ms -> 25ms: 100Hz(10ms 주기) 루프 내에서 응답을 안정적으로 받기 위한
|
||
// 여유 확보
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
|
||
.count() > 25)
|
||
break;
|
||
std::this_thread::sleep_for(std::chrono::microseconds(100));
|
||
}
|
||
|
||
if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
|
||
// CRC 검증 추가: 노이즈로 손상된 응답을 정상 데이터로 오인하지 않도록
|
||
// 방지
|
||
uint16_t resp_crc = calcCRC16(rx_buf.data(), expected_bytes - 2);
|
||
uint16_t recv_crc =
|
||
static_cast<uint16_t>(rx_buf[expected_bytes - 2]) |
|
||
(static_cast<uint16_t>(rx_buf[expected_bytes - 1]) << 8);
|
||
if (resp_crc != recv_crc)
|
||
return false;
|
||
|
||
out_vals.resize(count);
|
||
for (uint16_t i = 0; i < count; ++i) {
|
||
out_vals[i] =
|
||
(static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
|
||
}
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
};
|
||
|
||
class MotorDriver {
|
||
private:
|
||
SerialPort *port_;
|
||
uint8_t slave_id_;
|
||
|
||
public:
|
||
MotorDriver(SerialPort *port, uint8_t slave_id)
|
||
: port_(port), slave_id_(slave_id) {}
|
||
|
||
bool initDriver(uint16_t acl_ms = 500, uint16_t dcl_ms = 500) {
|
||
port_->writeReg(slave_id_, 0x200E, 0x0006); // Alarm clear
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
port_->writeReg(slave_id_, 0x200E, 0x0007); // Disable
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
port_->writeReg(slave_id_, 0x200D, 3); // Velocity mode
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
|
||
port_->writeRegs(slave_id_, 0x2080, {acl_ms, acl_ms}); // Accel
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
port_->writeRegs(slave_id_, 0x2082, {dcl_ms, dcl_ms}); // Decel
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
|
||
port_->writeReg(slave_id_, 0x200E, 0x0008); // Enable
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
setBrakes(true);
|
||
return true;
|
||
}
|
||
|
||
void setBrakes(bool lock) {
|
||
uint16_t val = lock ? 1 : 0;
|
||
port_->writeReg(slave_id_, 0x201A, val);
|
||
port_->writeReg(slave_id_, 0x201B, val);
|
||
}
|
||
|
||
void setRPMs(float l_rpm, float r_rpm) {
|
||
int16_t l_val = static_cast<int16_t>(std::clamp(l_rpm, -3000.0f, 3000.0f));
|
||
int16_t r_val = static_cast<int16_t>(std::clamp(r_rpm, -3000.0f, 3000.0f));
|
||
port_->writeRegs(
|
||
slave_id_, 0x2088,
|
||
{static_cast<uint16_t>(l_val), static_cast<uint16_t>(r_val)});
|
||
}
|
||
|
||
bool readFeedback(float &l_fb, float &r_fb, int32_t &l_tick,
|
||
int32_t &r_tick, float &l_torque_a, float &r_torque_a,
|
||
uint16_t &err_l, uint16_t &err_r, int &l_temp_c,
|
||
int &r_temp_c) {
|
||
std::vector<uint16_t> regs;
|
||
// 0x20A4~0x20AE 11레지스터 연속 읽기: 모터온도(0x20A4)부터 에러코드
|
||
// (0x20A5/0x20A6), 포지션 틱, RPM 피드백, 실제 토크(전류)까지 한 번의
|
||
// 통신으로 확보. 0x20A4는 기존 읽기 범위 바로 앞이라 추가 통신 없이
|
||
// 온도까지 같이 받아온다(과열 사전감지용, doc/02 §읽기전용상태 참고).
|
||
// 무부하(공중에 뜬) 바퀴는 전류가 급격히 낮아지므로 슬립 진단에 사용,
|
||
// 에러코드는 과전류/과부하 등 드라이버 알람 발생 시 원인 진단에 사용.
|
||
if (port_->readRegs(slave_id_, 0x20A4, 11, regs)) {
|
||
l_temp_c = static_cast<int8_t>((regs[0] >> 8) & 0xFF);
|
||
r_temp_c = static_cast<int8_t>(regs[0] & 0xFF);
|
||
|
||
err_l = regs[1];
|
||
err_r = regs[2];
|
||
|
||
// 수정: uint32_t -> int32_t 캐스팅은 2의 보수 표현에서 안전하게 부호가
|
||
// 재해석됨. 기존의 "val - 0x100000000ULL" 방식은 uint64_t 승격 후
|
||
// int32_t로 축소 캐스팅하는 구현정의 동작(현실적으로는 대부분 동작하지만
|
||
// 명확성/이식성이 떨어짐)이라 제거함.
|
||
uint32_t val_l = (static_cast<uint32_t>(regs[3]) << 16) | regs[4];
|
||
l_tick = static_cast<int32_t>(val_l);
|
||
|
||
uint32_t val_r = (static_cast<uint32_t>(regs[5]) << 16) | regs[6];
|
||
r_tick = static_cast<int32_t>(val_r);
|
||
|
||
int16_t vl = static_cast<int16_t>(regs[7]);
|
||
int16_t vr = static_cast<int16_t>(regs[8]);
|
||
l_fb = static_cast<float>(vl) * 0.1f; // 0.1RPM 단위 -> RPM
|
||
r_fb = static_cast<float>(vr) * 0.1f;
|
||
|
||
int16_t tl = static_cast<int16_t>(regs[9]);
|
||
int16_t tr = static_cast<int16_t>(regs[10]);
|
||
l_torque_a = static_cast<float>(tl) * 0.1f; // 0.1A 단위 -> A
|
||
r_torque_a = static_cast<float>(tr) * 0.1f;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// 드라이버 자체 온도(0x20B0, 0.1℃). 열 시정수가 초 단위로 느려서 매 틱
|
||
// 읽을 필요가 없어 별도 저빈도 폴링용으로 분리(readFeedback 연속범위와
|
||
// 떨어져 있어 합치면 통신 1회가 늘어남).
|
||
bool readDriverTemp(float &temp_c) {
|
||
std::vector<uint16_t> regs;
|
||
if (!port_->readRegs(slave_id_, 0x20B0, 1, regs))
|
||
return false;
|
||
temp_c = static_cast<int16_t>(regs[0]) * 0.1f;
|
||
return true;
|
||
}
|
||
|
||
// 정격전류(0x2033/0x2063)·최대전류(0x2034/0x2064) 실측 조회. 채널당
|
||
// 2레지스터씩 떨어져 있어 L/R 두 번 읽음(설정값이라 시작 시 1회만 조회).
|
||
bool readCurrentLimits(uint16_t &rated_l, uint16_t &max_l, uint16_t &rated_r,
|
||
uint16_t &max_r) {
|
||
std::vector<uint16_t> regs_l, regs_r;
|
||
if (!port_->readRegs(slave_id_, 0x2033, 2, regs_l))
|
||
return false;
|
||
if (!port_->readRegs(slave_id_, 0x2063, 2, regs_r))
|
||
return false;
|
||
rated_l = regs_l[0];
|
||
max_l = regs_l[1];
|
||
rated_r = regs_r[0];
|
||
max_r = regs_r[1];
|
||
return true;
|
||
}
|
||
|
||
// 최대전류(0x2034/0x2064)를 좌/우 동일 값으로 설정. 단위 0.1A.
|
||
void setMaxCurrent(uint16_t max_current_01a) {
|
||
port_->writeReg(slave_id_, 0x2034, max_current_01a);
|
||
port_->writeReg(slave_id_, 0x2064, max_current_01a);
|
||
}
|
||
};
|
||
|
||
// 드라이버 에러코드(0x20A5/0x20A6) 비트마스크를 사람이 읽을 수 있는 설명으로 변환
|
||
std::string decodeDriverError(uint16_t code) {
|
||
if (code == 0)
|
||
return "";
|
||
std::string s;
|
||
auto add = [&](uint16_t bit, const char *name) {
|
||
if (code & bit) {
|
||
if (!s.empty())
|
||
s += "+";
|
||
s += name;
|
||
}
|
||
};
|
||
add(0x0001, "과전압");
|
||
add(0x0002, "저전압");
|
||
add(0x0004, "과전류");
|
||
add(0x0008, "과부하");
|
||
add(0x0010, "전류이상(예약)");
|
||
add(0x0020, "엔코더오차");
|
||
add(0x0040, "속도이상(예약)");
|
||
add(0x0080, "기준전압오류");
|
||
add(0x0100, "EEPROM오류");
|
||
add(0x0200, "홀센서오류");
|
||
add(0x0400, "모터과열");
|
||
add(0x0800, "엔코더오류");
|
||
add(0x2000, "속도설정오류");
|
||
if (s.empty()) {
|
||
char buf[32];
|
||
snprintf(buf, sizeof(buf), "알수없음(0x%04X)", code);
|
||
s = buf;
|
||
}
|
||
return s;
|
||
}
|
||
|
||
// -----------------------------------------------------------------------------
|
||
// Livox Mid-360S Single LiDAR Obstacle Detection & Avoidance Class
|
||
// -----------------------------------------------------------------------------
|
||
struct ObstacleStatus {
|
||
float min_dist_front = 999.0f; // m
|
||
float min_dist_rear = 999.0f; // m
|
||
float min_dist_left = 999.0f; // m
|
||
float min_dist_right = 999.0f; // m
|
||
bool connected = false;
|
||
std::chrono::steady_clock::time_point last_update;
|
||
};
|
||
|
||
class LidarObstacleDetector {
|
||
private:
|
||
std::string config_path_;
|
||
std::atomic<bool> initialized_{false};
|
||
std::mutex status_mutex_;
|
||
ObstacleStatus status_;
|
||
|
||
// Filtering & ROI parameters
|
||
float min_z_ = -0.40f; // m (Z axis relative to LiDAR after tilt rotation)
|
||
float max_z_ = 0.80f; // m
|
||
float min_r_ = 0.15f; // m (blind spot / chassis)
|
||
float max_r_ = 3.50f; // m (max range)
|
||
float robot_half_width_ =
|
||
0.255f; // m (robot width 0.410m / 2 + 0.05m margin = 0.255m)
|
||
float pitch_deg_ = 17.0f; // deg (LiDAR downward tilt angle)
|
||
float lidar_height_ = 0.460f; // m (LiDAR height above ground)
|
||
float lidar_x_offset_ =
|
||
0.250f; // m (LiDAR forward offset 250mm from robot center)
|
||
|
||
float cos_pitch_ = std::cos(17.0f * 3.14159265f / 180.0f);
|
||
float sin_pitch_ = std::sin(17.0f * 3.14159265f / 180.0f);
|
||
|
||
public:
|
||
static LidarObstacleDetector *instance_;
|
||
|
||
LidarObstacleDetector() { instance_ = this; }
|
||
|
||
~LidarObstacleDetector() { stop(); }
|
||
|
||
void setParams(float min_z, float max_z, float robot_half_width,
|
||
float pitch_deg = 17.0f, float lidar_height = 0.460f,
|
||
float lidar_x_offset = 0.250f) {
|
||
min_z_ = min_z;
|
||
max_z_ = max_z;
|
||
robot_half_width_ = robot_half_width;
|
||
pitch_deg_ = pitch_deg;
|
||
lidar_height_ = lidar_height;
|
||
lidar_x_offset_ = lidar_x_offset;
|
||
|
||
float pitch_rad = pitch_deg_ * 3.14159265358979323846f / 180.0f;
|
||
cos_pitch_ = std::cos(pitch_rad);
|
||
sin_pitch_ = std::sin(pitch_rad);
|
||
}
|
||
|
||
bool init(const std::string &config_path) {
|
||
config_path_ = config_path;
|
||
if (!LivoxLidarSdkInit(config_path_.c_str())) {
|
||
std::cerr << "[경고] Livox Mid-360S SDK2 초기화 실패 (" << config_path_
|
||
<< ")\n";
|
||
return false;
|
||
}
|
||
|
||
SetLivoxLidarPointCloudCallBack(PointCloudCallbackStatic, nullptr);
|
||
SetLivoxLidarInfoChangeCallback(InfoChangeCallbackStatic, nullptr);
|
||
|
||
initialized_ = true;
|
||
std::cout << "[정보] Livox Mid-360S 라이다 수신 시작 (설정: "
|
||
<< config_path_ << ", 피치 기울임: " << pitch_deg_
|
||
<< "도, 설치높이: " << lidar_height_ * 1000.0f
|
||
<< "mm, 전방 오프셋: " << lidar_x_offset_ * 1000.0f << "mm)\n";
|
||
return true;
|
||
}
|
||
|
||
void stop() {
|
||
if (initialized_) {
|
||
LivoxLidarSdkUninit();
|
||
initialized_ = false;
|
||
std::cout << "[정보] Livox Mid-360S 라이다 수신기 종료.\n";
|
||
}
|
||
}
|
||
|
||
static void PointCloudCallbackStatic(const uint32_t handle,
|
||
const uint8_t dev_type,
|
||
LivoxLidarEthernetPacket *data,
|
||
void *client_data) {
|
||
(void)dev_type;
|
||
(void)client_data;
|
||
if (instance_)
|
||
instance_->handlePointCloud(handle, data);
|
||
}
|
||
|
||
static void InfoChangeCallbackStatic(const uint32_t handle,
|
||
const LivoxLidarInfo *info,
|
||
void *client_data) {
|
||
(void)client_data;
|
||
if (!info)
|
||
return;
|
||
std::cout << "\n[라이다 연결 완료] Handle: " << handle
|
||
<< ", SN: " << info->sn << ", IP: " << info->lidar_ip
|
||
<< std::endl;
|
||
SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr);
|
||
}
|
||
|
||
void handlePointCloud(uint32_t handle, LivoxLidarEthernetPacket *packet) {
|
||
(void)handle;
|
||
if (!packet)
|
||
return;
|
||
|
||
float curr_front = 999.0f;
|
||
float curr_rear = 999.0f;
|
||
float curr_left = 999.0f;
|
||
float curr_right = 999.0f;
|
||
|
||
auto parsePoint = [&](float raw_x, float raw_y, float raw_z) {
|
||
// Apply 17 degree pitch rotation (LiDAR tilted downward towards ground)
|
||
float x = raw_x * cos_pitch_ + raw_z * sin_pitch_;
|
||
float y = raw_y;
|
||
float z = -raw_x * sin_pitch_ + raw_z * cos_pitch_;
|
||
|
||
if (z < min_z_ || z > max_z_)
|
||
return; // Filter ground floor and ceiling
|
||
float r = std::sqrt(x * x + y * y);
|
||
if (r < min_r_ || r > max_r_)
|
||
return;
|
||
|
||
// Coordinate Frame: +X Forward, +Y Left, +Z Up
|
||
// 1. Front Obstacle Zone
|
||
if (x > 0.05f && std::abs(y) <= robot_half_width_) {
|
||
if (x < curr_front)
|
||
curr_front = x;
|
||
}
|
||
// 2. Rear Obstacle Zone
|
||
else if (x < -0.05f && std::abs(y) <= robot_half_width_) {
|
||
if (-x < curr_rear)
|
||
curr_rear = -x;
|
||
}
|
||
|
||
// 3. Side Obstacle Zones (Front Lookahead window: X in [0.05m, 1.8m])
|
||
if (x > 0.05f && x < 1.8f) {
|
||
if (y > 0.10f && y < 1.2f) { // Left Side
|
||
float dist = std::sqrt(x * x + y * y);
|
||
if (dist < curr_left)
|
||
curr_left = dist;
|
||
} else if (y < -0.10f && y > -1.2f) { // Right Side
|
||
float dist = std::sqrt(x * x + y * y);
|
||
if (dist < curr_right)
|
||
curr_right = dist;
|
||
}
|
||
}
|
||
};
|
||
|
||
if (packet->data_type == kLivoxLidarCartesianCoordinateHighData) {
|
||
LivoxLidarCartesianHighRawPoint *pts =
|
||
(LivoxLidarCartesianHighRawPoint *)packet->data;
|
||
for (uint32_t i = 0; i < packet->dot_num; ++i) {
|
||
parsePoint(pts[i].x / 1000.0f, pts[i].y / 1000.0f, pts[i].z / 1000.0f);
|
||
}
|
||
} else if (packet->data_type == kLivoxLidarCartesianCoordinateLowData) {
|
||
LivoxLidarCartesianLowRawPoint *pts =
|
||
(LivoxLidarCartesianLowRawPoint *)packet->data;
|
||
for (uint32_t i = 0; i < packet->dot_num; ++i) {
|
||
parsePoint(pts[i].x / 100.0f, pts[i].y / 100.0f, pts[i].z / 100.0f);
|
||
}
|
||
}
|
||
|
||
std::lock_guard<std::mutex> lock(status_mutex_);
|
||
auto updateEMA = [](float &old_val, float new_val) {
|
||
if (new_val < old_val) {
|
||
old_val = new_val; // Fast response to approaching obstacles
|
||
} else {
|
||
old_val = old_val * 0.70f + new_val * 0.30f; // Smooth recovery
|
||
}
|
||
};
|
||
|
||
updateEMA(status_.min_dist_front, curr_front);
|
||
updateEMA(status_.min_dist_rear, curr_rear);
|
||
updateEMA(status_.min_dist_left, curr_left);
|
||
updateEMA(status_.min_dist_right, curr_right);
|
||
status_.connected = true;
|
||
status_.last_update = std::chrono::steady_clock::now();
|
||
}
|
||
|
||
ObstacleStatus getStatus() {
|
||
std::lock_guard<std::mutex> lock(status_mutex_);
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - status_.last_update)
|
||
.count() > 800) {
|
||
status_.connected = false;
|
||
}
|
||
return status_;
|
||
}
|
||
};
|
||
|
||
LidarObstacleDetector *LidarObstacleDetector::instance_ = nullptr;
|
||
|
||
// -----------------------------------------------------------------------------
|
||
// RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈
|
||
// 실기 캡처로 확인한 실제 프로토콜: 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 {
|
||
// udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된
|
||
// 심볼릭 링크. ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름은 고정.
|
||
std::string port = "/dev/ttyRC";
|
||
int baud = 420000; // CRSF 표준 보드레이트
|
||
|
||
int ch_steer = 1; // CH1: 좌우 조향
|
||
int ch_throttle = 2; // CH2: 전후진
|
||
int ch_brake = 5; // CH5: 비상 브레이크
|
||
int ch_speed = 6; // CH6: 속도 모드
|
||
|
||
// 실기 캡처로 확인한 실측값 (좌 1005 ~ 중 1481~1503 ~ 우 2000)
|
||
int steer_min = 1005, steer_center_lo = 1481, steer_center_hi = 1503,
|
||
steer_max = 2000;
|
||
// 실측값: 후진끝 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; // 이상이면 저속 모드
|
||
|
||
// CH1/CH2(연속 아날로그 스틱) 슬루레이트 한계: 메인 루프 한 틱(~10ms)
|
||
// 사이에 raw 값이 이 이상 튀면 사람이 스틱으로 낼 수 없는 변화로 보고
|
||
// 손상된 프레임으로 간주해 버린다. 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 고속 모드)
|
||
|
||
int failsafe_timeout_ms = 200; // 이 시간 이상 신호 미수신 시 비상정지
|
||
};
|
||
|
||
class RcReceiver {
|
||
public:
|
||
explicit RcReceiver(const RcConfig &cfg) : cfg_(cfg) {}
|
||
~RcReceiver() { stop(); }
|
||
|
||
void start() {
|
||
running_ = true;
|
||
thread_ = std::thread(&RcReceiver::run, this);
|
||
}
|
||
|
||
void stop() {
|
||
running_ = false;
|
||
if (thread_.joinable())
|
||
thread_.join();
|
||
closePort();
|
||
}
|
||
|
||
// 채널 raw 값 조회. 아직 한 번도 수신하지 못했으면 false.
|
||
bool getChannel(int ch, int &out_val) {
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
auto it = channels_.find(ch);
|
||
if (it == channels_.end())
|
||
return false;
|
||
out_val = it->second;
|
||
return true;
|
||
}
|
||
|
||
// 포트가 열려 있고, failsafe_timeout_ms 이내에 유효한 패킷을 수신했는가.
|
||
bool isConnected() {
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
if (!port_open_ || channels_.empty())
|
||
return false;
|
||
auto now = std::chrono::steady_clock::now();
|
||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - last_rx_time_)
|
||
.count() <= cfg_.failsafe_timeout_ms;
|
||
}
|
||
|
||
private:
|
||
RcConfig cfg_;
|
||
std::atomic<bool> running_{false};
|
||
std::thread thread_;
|
||
int fd_ = -1;
|
||
|
||
std::mutex mutex_;
|
||
std::map<int, int> channels_;
|
||
std::chrono::steady_clock::time_point last_rx_time_{};
|
||
bool port_open_ = false;
|
||
|
||
std::vector<uint8_t> frame_buf_;
|
||
|
||
// 채널별 미디언(median-of-3) 필터 히스토리. 이 송신기는 CRC/체크섬이
|
||
// 표준 방식으로 검증되지 않아(전수조사로 확인) 단발성 손상 프레임이
|
||
// 섞여 들어올 수 있는데, 최근 3개 raw 값 중 가운데 값을 채택하면 3개
|
||
// 중 하나가 손상돼도 자동으로 걸러진다. 스위치 채널도 전환 시 최대
|
||
// 1프레임(~4ms)만 지연될 뿐이라 안전하게 전 채널에 적용 가능하다.
|
||
std::map<int, std::array<int, 2>> raw_history_;
|
||
std::map<int, int> history_count_;
|
||
|
||
static int median3(int a, int b, int c) {
|
||
if ((a <= b && b <= c) || (c <= b && b <= a))
|
||
return b;
|
||
if ((b <= a && a <= c) || (c <= a && a <= b))
|
||
return a;
|
||
return c;
|
||
}
|
||
|
||
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);
|
||
if (fd_ < 0)
|
||
return false;
|
||
|
||
fcntl(fd_, F_SETFL, O_NONBLOCK);
|
||
|
||
// 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;
|
||
}
|
||
|
||
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);
|
||
|
||
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;
|
||
|
||
// 바이너리 프로토콜이므로 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;
|
||
|
||
if (ioctl(fd_, TCSETS2, &tio) < 0) {
|
||
close(fd_);
|
||
fd_ = -1;
|
||
return false;
|
||
}
|
||
tcflush(fd_, TCIOFLUSH);
|
||
return true;
|
||
}
|
||
|
||
void closePort() {
|
||
if (fd_ >= 0) {
|
||
close(fd_);
|
||
fd_ = -1;
|
||
}
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
port_open_ = false;
|
||
}
|
||
|
||
// 축적된 바이트 버퍼에서 CRSF 프레임을 최대한 뽑아낸다. Sync 바이트를
|
||
// 찾고, 선언된 Length만큼 버퍼에 데이터가 쌓일 때까지 기다린 뒤
|
||
// RC_CHANNELS_PACKED(0x16) 프레임이면 16채널(11비트씩)을 언패킹한다.
|
||
// (CRC8은 이 송신기에서 표준 DVB-S2 값과 맞지 않아 검증하지 않음 — 실기
|
||
// 캡처로 확인된 mac/mac_code 레퍼런스와 동일한 방식.)
|
||
void parseCrsfBuffer() {
|
||
std::map<int, int> parsed;
|
||
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 바이트 대기
|
||
|
||
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));
|
||
|
||
// 미디언-3 필터: 최근 2개 raw 값 + 이번 값 중 가운데 값을 채택.
|
||
// 히스토리는 항상 raw 값으로 갱신한다(필터링된 결과가 아니라).
|
||
int ch_num = ch + 1;
|
||
std::array<int, 2> &hist = raw_history_[ch_num];
|
||
int &count = history_count_[ch_num];
|
||
int filtered = us_val;
|
||
if (count >= 2) {
|
||
filtered = median3(hist[0], hist[1], us_val);
|
||
} else {
|
||
++count; // 워밍업 구간(처음 2개)은 그대로 통과
|
||
}
|
||
hist[0] = hist[1];
|
||
hist[1] = us_val;
|
||
parsed[ch_num] = filtered; // CH1..CH16 (1-based)
|
||
}
|
||
}
|
||
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)
|
||
channels_[kv.first] = kv.second;
|
||
last_rx_time_ = std::chrono::steady_clock::now();
|
||
}
|
||
}
|
||
|
||
void run() {
|
||
// now() - 1001ms로 초기화: time_point::min()을 쓰면 첫 비교에서
|
||
// "now - sentinel"이 부호있는 정수 오버플로우(UB)를 일으켜 -O3에서
|
||
// 재연결 시도 자체가 영구히 스킵될 수 있다.
|
||
auto last_open_attempt =
|
||
std::chrono::steady_clock::now() - std::chrono::milliseconds(1001);
|
||
char buf[256];
|
||
|
||
while (running_) {
|
||
if (fd_ < 0) {
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - last_open_attempt)
|
||
.count() > 1000) {
|
||
last_open_attempt = now;
|
||
if (openPort()) {
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
port_open_ = true;
|
||
std::cout << "\n[정보] RC 수신기 포트(" << cfg_.port
|
||
<< ") 연결 성공.\n";
|
||
} else {
|
||
std::cerr << "\n[경고] RC 수신기 포트(" << cfg_.port
|
||
<< ") 열기 실패. 1초 후 재시도.\n";
|
||
}
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||
continue;
|
||
}
|
||
|
||
ssize_t n = read(fd_, buf, sizeof(buf));
|
||
if (n > 0) {
|
||
frame_buf_.insert(frame_buf_.end(), buf, buf + n);
|
||
parseCrsfBuffer();
|
||
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
|
||
std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n";
|
||
closePort();
|
||
} else {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||
}
|
||
}
|
||
}
|
||
};
|
||
|
||
// -----------------------------------------------------------------------------
|
||
// WitMotion HWT905-RS232 IMU 리더 (doc/06-imu-integration-plan.md Phase 1)
|
||
// RS485/Modbus 변형과 달리 이 RS232 변형은 액티브 푸시 방식 바이너리 프로토콜을
|
||
// 쓴다: [0]=0x55 헤더, [1]=타입(0x51 가속도/0x52 자이로/0x53 각도/0x54 지자기),
|
||
// [2..9]=int16×4(LE, XYZ+예약), [10]=체크섬(0~9바이트 합의 하위 8비트).
|
||
// imu/imu_test에서 실기로 검증된 프로토콜/스케일을 그대로 이식했다.
|
||
// Phase 1 범위: 순수 관측만 한다 — 제어 루프에는 절대 개입하지 않는다.
|
||
// -----------------------------------------------------------------------------
|
||
struct ImuConfig {
|
||
// udev 규칙으로 고정된 심볼릭 링크 (모터=/dev/ttyMOTOR, RC=/dev/ttyRC와 별도)
|
||
std::string port = "/dev/ttyIMU";
|
||
int baud = 9600; // HWT905-232 공장 출하 기본값(실기 확인됨)
|
||
int failsafe_timeout_ms = 500; // 이 시간 이상 미수신 시 미연결로 표시
|
||
};
|
||
|
||
class ImuReader {
|
||
public:
|
||
struct Snapshot {
|
||
double accel[3] = {0, 0, 0}; // g (x,y,z)
|
||
double gyro[3] = {0, 0, 0}; // deg/s (x,y,z)
|
||
double angle[3] = {0, 0, 0}; // deg (roll, pitch, yaw)
|
||
bool valid = false;
|
||
};
|
||
|
||
explicit ImuReader(const ImuConfig &cfg) : cfg_(cfg) {}
|
||
~ImuReader() { stop(); }
|
||
|
||
void start() {
|
||
running_ = true;
|
||
thread_ = std::thread(&ImuReader::run, this);
|
||
}
|
||
|
||
void stop() {
|
||
running_ = false;
|
||
if (thread_.joinable())
|
||
thread_.join();
|
||
closePort();
|
||
}
|
||
|
||
bool isConnected() {
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
if (!port_open_ || !has_angle_)
|
||
return false;
|
||
auto now = std::chrono::steady_clock::now();
|
||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - last_rx_time_)
|
||
.count() <= cfg_.failsafe_timeout_ms;
|
||
}
|
||
|
||
Snapshot getSnapshot() {
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
Snapshot s;
|
||
for (int i = 0; i < 3; ++i) {
|
||
s.accel[i] = accel_[i];
|
||
s.gyro[i] = gyro_[i];
|
||
s.angle[i] = angle_[i];
|
||
}
|
||
s.valid = isConnectedLocked();
|
||
return s;
|
||
}
|
||
|
||
private:
|
||
ImuConfig cfg_;
|
||
std::atomic<bool> running_{false};
|
||
std::thread thread_;
|
||
int fd_ = -1;
|
||
|
||
std::mutex mutex_;
|
||
double accel_[3] = {0, 0, 0};
|
||
double gyro_[3] = {0, 0, 0};
|
||
double angle_[3] = {0, 0, 0};
|
||
bool has_accel_ = false, has_gyro_ = false, has_angle_ = false;
|
||
std::chrono::steady_clock::time_point last_rx_time_{};
|
||
bool port_open_ = false;
|
||
|
||
static constexpr uint8_t kFrameHeader = 0x55;
|
||
static constexpr uint8_t kTypeAccel = 0x51;
|
||
static constexpr uint8_t kTypeGyro = 0x52;
|
||
static constexpr uint8_t kTypeAngle = 0x53;
|
||
|
||
bool isConnectedLocked() const {
|
||
if (!port_open_ || !has_angle_)
|
||
return false;
|
||
auto now = std::chrono::steady_clock::now();
|
||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - last_rx_time_)
|
||
.count() <= cfg_.failsafe_timeout_ms;
|
||
}
|
||
|
||
static 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));
|
||
}
|
||
|
||
static speed_t baudToSpeed(int baud) {
|
||
switch (baud) {
|
||
case 4800:
|
||
return B4800;
|
||
case 19200:
|
||
return B19200;
|
||
case 38400:
|
||
return B38400;
|
||
case 57600:
|
||
return B57600;
|
||
case 115200:
|
||
return B115200;
|
||
case 230400:
|
||
return B230400;
|
||
default:
|
||
return B9600;
|
||
}
|
||
}
|
||
|
||
bool openPort() {
|
||
fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
||
if (fd_ < 0)
|
||
return false;
|
||
fcntl(fd_, F_SETFL, 0); // 블로킹 읽기로 전환
|
||
|
||
struct termios options;
|
||
if (tcgetattr(fd_, &options) != 0) {
|
||
close(fd_);
|
||
fd_ = -1;
|
||
return false;
|
||
}
|
||
|
||
speed_t speed = baudToSpeed(cfg_.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.5초 바이트 간 타임아웃 (종료 감지용)
|
||
|
||
tcflush(fd_, TCIFLUSH);
|
||
if (tcsetattr(fd_, TCSANOW, &options) != 0) {
|
||
close(fd_);
|
||
fd_ = -1;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void closePort() {
|
||
if (fd_ >= 0) {
|
||
close(fd_);
|
||
fd_ = -1;
|
||
}
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
port_open_ = false;
|
||
}
|
||
|
||
// 검증된 11바이트 프레임 하나를 상태에 반영. 각도 프레임 완료 시 true.
|
||
bool applyFrame(const uint8_t *frame) {
|
||
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]);
|
||
accel_[i] = raw / 32768.0 * 16.0; // g
|
||
}
|
||
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]);
|
||
gyro_[i] = raw / 32768.0 * 2000.0; // deg/s
|
||
}
|
||
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]);
|
||
angle_[i] = raw / 32768.0 * 180.0; // deg
|
||
}
|
||
has_angle_ = true;
|
||
return true;
|
||
default:
|
||
return false; // 시간/쿼터니언/GPS 등: Phase 1에서는 무시
|
||
}
|
||
}
|
||
|
||
void run() {
|
||
auto last_open_attempt =
|
||
std::chrono::steady_clock::now() - std::chrono::milliseconds(1001);
|
||
uint8_t buf[11];
|
||
size_t buf_len = 0;
|
||
|
||
while (running_) {
|
||
if (fd_ < 0) {
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - last_open_attempt)
|
||
.count() > 1000) {
|
||
last_open_attempt = now;
|
||
if (openPort()) {
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
port_open_ = true;
|
||
std::cout << "\n[정보] IMU 포트(" << cfg_.port << ") 연결 성공.\n";
|
||
} else {
|
||
std::cerr << "\n[경고] IMU 포트(" << cfg_.port
|
||
<< ") 열기 실패. 1초 후 재시도.\n";
|
||
}
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||
buf_len = 0;
|
||
continue;
|
||
}
|
||
|
||
uint8_t byte;
|
||
ssize_t n = read(fd_, &byte, 1);
|
||
if (n <= 0) {
|
||
if (n < 0 && errno != EAGAIN && errno != EINTR) {
|
||
std::cerr << "\n[경고] IMU 포트 통신 오류. 재연결 시도...\n";
|
||
closePort();
|
||
buf_len = 0;
|
||
}
|
||
continue; // n==0: VTIME 타임아웃, running_ 재확인 후 계속 대기
|
||
}
|
||
|
||
if (buf_len == 0) {
|
||
if (byte != kFrameHeader)
|
||
continue; // 헤더 대기하며 재동기화
|
||
buf[buf_len++] = byte;
|
||
continue;
|
||
}
|
||
|
||
buf[buf_len++] = byte;
|
||
if (buf_len < 11)
|
||
continue;
|
||
|
||
uint8_t sum = 0;
|
||
for (int i = 0; i < 10; ++i)
|
||
sum += buf[i];
|
||
if (sum != buf[10]) {
|
||
buf_len = 0; // 체크섬 불일치: 프레임 폐기 후 재동기화
|
||
continue;
|
||
}
|
||
|
||
bool angle_done;
|
||
{
|
||
std::lock_guard<std::mutex> lock(mutex_);
|
||
angle_done = applyFrame(buf);
|
||
if (angle_done)
|
||
last_rx_time_ = std::chrono::steady_clock::now();
|
||
}
|
||
buf_len = 0;
|
||
(void)angle_done;
|
||
}
|
||
}
|
||
};
|
||
|
||
int main(int argc, char **argv) {
|
||
std::signal(SIGINT, signalHandler);
|
||
std::signal(SIGTERM, signalHandler);
|
||
|
||
// udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된
|
||
// 심볼릭 링크. ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름은 고정.
|
||
std::string port1 = "/dev/ttyMOTOR";
|
||
std::string port2 = "";
|
||
uint8_t id1 = 1;
|
||
uint8_t id2 = 2;
|
||
bool bcast_mode = false;
|
||
RcConfig rc_cfg;
|
||
float max_spin_v = 0.30f; // m/s
|
||
float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름
|
||
float k_skid_override = -1.0f; // 0 이상이면 Phase 5a 기본 캘리브레이션값을 대체
|
||
float effective_w_override = -1.0f; // 0 이상이면 Phase 5a 기본 캘리브레이션값을 대체
|
||
// 가속은 역기전력 보호회로가 하드웨어로 구성되어 목표값에 1:1로 즉시
|
||
// 추종하므로 별도의 가속 한계값이 없다. decel_rate/jerk_rate(및
|
||
// spin_jerk_rate)는 감속에만 적용된다.
|
||
float decel_rate = 180.0f; // RPM/s (최대 감속도 한계)
|
||
float jerk_rate = 600.0f; // RPM/s^2 (직진/커브 선회 감속 저크 한계)
|
||
float spin_jerk_rate = 220.0f; // RPM/s^2 (제자리 회전 감속 저크 한계)
|
||
float spin_arc_bias = 0.12f; // 0~1 (제자리 회전 시 섞을 최소 회전반경용
|
||
// 미세 전진 성분 비율. ICR을 로봇 중심에서
|
||
// 살짝 벗어나게 해 스크럽 마찰 저항을 줄임)
|
||
float bumper_offset = 0.045f; // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리
|
||
|
||
// Phase 3 — 직진 헤딩 홀드 PI 트림 (doc/06-imu-integration-plan.md §Phase 3).
|
||
// 조향 중립 + 라이다 회피 미개입 + 실제 주행 중일 때만 개입해, 진입 순간의
|
||
// fused_yaw_deg를 목표로 고정하고 그로부터 벗어난 만큼을 PI로 보정한다.
|
||
bool heading_hold_enabled = true;
|
||
float heading_kp = 0.6f; // rad/s per rad 오차
|
||
float heading_ki = 0.15f; // rad/s per (rad·s) 적분 오차
|
||
float heading_max_trim = 0.15f; // rad/s (트림 상한 — 의도적 조향을 압도하지 않도록)
|
||
|
||
// Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (doc §Phase 4). 지령
|
||
// omega 대비 바퀴 피드백과 무관한 IMU 원시 gyro_z 비율이 임계값 밑으로
|
||
// 지속되면 "전신 슬립"(빙판/젖은 잔디 등 4륜 동시 헛돎)으로 판정해 라이다
|
||
// 회피의 speed_scale과 같은 패턴으로 속도를 일시적으로 낮춘다. 제자리
|
||
// 회전은 반력 토크로 인한 정상적인 스크럽 손실(지령 대비 40~60% 미달도
|
||
// 흔함)이 있어 문턱값을 낮게(=크게 미달일 때만) 잡아 정상 회전을 오탐하지
|
||
// 않게 한다.
|
||
bool whole_slip_enabled = true;
|
||
float whole_slip_min_omega = 0.15f; // rad/s (이 미만 지령은 판정 보류)
|
||
float whole_slip_ratio_threshold = 0.15f; // |imu_omega|/|omega| 가 이 밑이면 슬립 후보
|
||
int whole_slip_debounce_ticks = 15; // 틱 (100Hz 기준 150ms) 연속돼야 확정
|
||
float whole_slip_speed_scale = 0.5f; // 슬립 확정 시 v_x/omega에 곱하는 배율
|
||
|
||
// 바퀴별 이상(들뜸/걸림) 판정 파라미터: 속도 폐루프 특성상 "지령 대비 실제
|
||
// 속도" 하나만으로는 무부하(들뜸) 판정이 안 되므로, 동료 바퀴 대비 전류
|
||
// 편차와 함께 두 축으로 판정한다.
|
||
float min_active_rpm = 3.0f; // RPM (이 미만이면 판정 보류: 정지 취급)
|
||
float airborne_current_ratio = 0.35f; // 동료 바퀴 중앙값 전류 대비 이 비율
|
||
// 미만이면 무부하(들뜸)로 판정
|
||
float stall_vel_ratio = 0.5f; // 실제속도/지령속도가 이 비율 미만이면
|
||
// 속도 지연으로 판단(걸림 후보)
|
||
float stall_current_a = 12.0f; // A (정격 15A 근접) 이상이면서 속도
|
||
// 지연이 동반되면 걸림/과부하로 판정
|
||
int airborne_debounce_ticks = 5; // 틱 (100Hz 기준 50ms) 연속 들뜸 판정
|
||
// 시에만 실제로 목표속도를 낮춤
|
||
// 비어있으면 프로그램 시작 시 logs/drive_YYYYmmdd_HHMMSS.csv로 자동 생성된다
|
||
// (매 주행마다 RC/IMU/모터 로깅을 자동으로 별도 파일에 남기기 위함).
|
||
// --log <파일경로>로 직접 지정하거나 --no_log로 완전히 끌 수 있다.
|
||
std::string log_path = "";
|
||
bool no_log = false;
|
||
// 실측 결과 정격15A/최대30A(공장 기본값) 그대로였음. 정격 위로 5A 여유는
|
||
// 남기되, 걸림/과부하 상황에서 30A까지 밀어붙이며 3초씩 버티다 과부하
|
||
// 알람이 터지는 걸 막기 위해 기본값을 20A로 낮춤. --max_current_a로 조정 가능.
|
||
// 음수를 주면 미변경(공장/기존 설정 유지).
|
||
float max_current_a = 20.0f;
|
||
|
||
// IMU 파라미터 (doc/06-imu-integration-plan.md Phase 1: 순수 관측)
|
||
bool use_imu = true;
|
||
ImuConfig imu_cfg;
|
||
|
||
// LiDAR Parameters & Robot Physical Specs (User Specification)
|
||
bool use_lidar = true;
|
||
std::string lidar_config = "mid360s_config.json";
|
||
float front_stop_dist = 0.35f; // m (전방 완전 정지 거리 0.35m)
|
||
float front_warn_dist = 0.50f; // m (전방 감속/경고 시작 거리 0.50m = 50cm)
|
||
float side_dodge_dist = 0.50f; // m (측면 장애물 우회 감지 거리 0.50m = 50cm)
|
||
float side_stop_dist = 0.20f; // m (측면 한계 접근 거리 0.20m)
|
||
float max_dodge_omega = 0.35f; // rad/s (자동 회피 최대 조향 각속도)
|
||
float robot_width = 0.410f; // m (로봇 가로/좌우 실측 전폭 410mm)
|
||
float robot_length = 0.631f; // m (로봇 세로/전후 실측 전장 631mm)
|
||
float robot_height = 0.286f; // m (로봇 전고 286mm)
|
||
float lidar_height = 0.460f; // m (지면 기준 라이다 높이 460mm)
|
||
float lidar_pitch = 17.0f; // deg (라이다 하향 기울임 17도)
|
||
float lidar_x_offset = 0.250f; // m (로봇 중심 기준 라이다 전방 오프셋 250mm)
|
||
float min_z = -lidar_height + 0.06f; // m (-0.40m: 지면 감지 방지 안전 오프셋)
|
||
float max_z = 0.80f; // m (라이다 기준 상단 높이)
|
||
|
||
for (int i = 1; i < argc; ++i) {
|
||
std::string arg = argv[i];
|
||
if (arg == "--port" && i + 1 < argc)
|
||
port1 = argv[++i];
|
||
else if (arg == "--port1" && i + 1 < argc)
|
||
port1 = argv[++i];
|
||
else if (arg == "--port2" && i + 1 < argc)
|
||
port2 = argv[++i];
|
||
else if (arg == "--id1" && i + 1 < argc)
|
||
id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
|
||
else if (arg == "--id2" && i + 1 < argc)
|
||
id2 = static_cast<uint8_t>(std::stoi(argv[++i]));
|
||
else if (arg == "--k_skid" && i + 1 < argc)
|
||
k_skid_override = std::stof(argv[++i]);
|
||
else if (arg == "--effective_w" && i + 1 < argc)
|
||
effective_w_override = std::stof(argv[++i]);
|
||
else if (arg == "--radius" && i + 1 < argc)
|
||
wheel_radius = std::stof(argv[++i]);
|
||
else if (arg == "--bumper" && i + 1 < argc)
|
||
bumper_offset = std::stof(argv[++i]);
|
||
else if (arg == "--decel" && i + 1 < argc)
|
||
decel_rate = std::stof(argv[++i]);
|
||
else if (arg == "--jerk" && i + 1 < argc)
|
||
jerk_rate = std::stof(argv[++i]);
|
||
else if (arg == "--spin_jerk" && i + 1 < argc)
|
||
spin_jerk_rate = std::stof(argv[++i]);
|
||
else if (arg == "--spin_arc_bias" && i + 1 < argc)
|
||
spin_arc_bias = std::stof(argv[++i]);
|
||
else if (arg == "--min_active_rpm" && i + 1 < argc)
|
||
min_active_rpm = std::stof(argv[++i]);
|
||
else if (arg == "--airborne_current_ratio" && i + 1 < argc)
|
||
airborne_current_ratio = std::stof(argv[++i]);
|
||
else if (arg == "--stall_vel_ratio" && i + 1 < argc)
|
||
stall_vel_ratio = std::stof(argv[++i]);
|
||
else if (arg == "--stall_current_a" && i + 1 < argc)
|
||
stall_current_a = std::stof(argv[++i]);
|
||
else if (arg == "--airborne_debounce_ticks" && i + 1 < argc)
|
||
airborne_debounce_ticks = std::stoi(argv[++i]);
|
||
else if (arg == "--log" && i + 1 < argc)
|
||
log_path = argv[++i];
|
||
else if (arg == "--no_log")
|
||
no_log = true;
|
||
else if (arg == "--max_current_a" && i + 1 < argc)
|
||
max_current_a = std::stof(argv[++i]);
|
||
else if (arg == "--bcast" || arg == "--broadcast")
|
||
bcast_mode = true;
|
||
else if (arg == "--lidar_config" && i + 1 < argc)
|
||
lidar_config = argv[++i];
|
||
else if (arg == "--front_stop" && i + 1 < argc)
|
||
front_stop_dist = std::stof(argv[++i]);
|
||
else if (arg == "--front_warn" && i + 1 < argc)
|
||
front_warn_dist = std::stof(argv[++i]);
|
||
else if (arg == "--side_dodge" && i + 1 < argc)
|
||
side_dodge_dist = std::stof(argv[++i]);
|
||
else if (arg == "--robot_width" && i + 1 < argc)
|
||
robot_width = std::stof(argv[++i]);
|
||
else if (arg == "--lidar_height" && i + 1 < argc)
|
||
lidar_height = std::stof(argv[++i]);
|
||
else if (arg == "--lidar_pitch" && i + 1 < argc)
|
||
lidar_pitch = std::stof(argv[++i]);
|
||
else if (arg == "--lidar_x_offset" && i + 1 < argc)
|
||
lidar_x_offset = std::stof(argv[++i]);
|
||
else if (arg == "--min_z" && i + 1 < argc)
|
||
min_z = std::stof(argv[++i]);
|
||
else if (arg == "--max_z" && i + 1 < argc)
|
||
max_z = std::stof(argv[++i]);
|
||
else if (arg == "--no_lidar")
|
||
use_lidar = false;
|
||
else if (arg == "--imu_port" && i + 1 < argc)
|
||
imu_cfg.port = argv[++i];
|
||
else if (arg == "--imu_baud" && i + 1 < argc)
|
||
imu_cfg.baud = std::stoi(argv[++i]);
|
||
else if (arg == "--no_imu")
|
||
use_imu = false;
|
||
else if (arg == "--rc_port" && i + 1 < argc)
|
||
rc_cfg.port = argv[++i];
|
||
else if (arg == "--rc_baud" && i + 1 < argc)
|
||
rc_cfg.baud = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_ch_steer" && i + 1 < argc)
|
||
rc_cfg.ch_steer = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_ch_throttle" && i + 1 < argc)
|
||
rc_cfg.ch_throttle = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_ch_brake" && i + 1 < argc)
|
||
rc_cfg.ch_brake = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_ch_speed" && i + 1 < argc)
|
||
rc_cfg.ch_speed = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_steer_min" && i + 1 < argc)
|
||
rc_cfg.steer_min = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_steer_center_lo" && i + 1 < argc)
|
||
rc_cfg.steer_center_lo = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_steer_center_hi" && i + 1 < argc)
|
||
rc_cfg.steer_center_hi = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_steer_max" && i + 1 < argc)
|
||
rc_cfg.steer_max = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_throttle_min" && i + 1 < argc)
|
||
rc_cfg.throttle_min = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_throttle_center_lo" && i + 1 < argc)
|
||
rc_cfg.throttle_center_lo = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_throttle_center_hi" && i + 1 < argc)
|
||
rc_cfg.throttle_center_hi = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_throttle_max" && i + 1 < argc)
|
||
rc_cfg.throttle_max = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_brake_threshold" && i + 1 < argc)
|
||
rc_cfg.brake_threshold = std::stoi(argv[++i]);
|
||
else if (arg == "--rc_speed_threshold" && i + 1 < argc)
|
||
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)
|
||
rc_cfg.v_max_high = std::stof(argv[++i]);
|
||
else if (arg == "--rc_failsafe_ms" && i + 1 < argc)
|
||
rc_cfg.failsafe_timeout_ms = std::stoi(argv[++i]);
|
||
else if (arg == "--max_spin_v" && i + 1 < argc)
|
||
max_spin_v = std::stof(argv[++i]);
|
||
else if (arg == "--no_heading_hold")
|
||
heading_hold_enabled = false;
|
||
else if (arg == "--heading_kp" && i + 1 < argc)
|
||
heading_kp = std::stof(argv[++i]);
|
||
else if (arg == "--heading_ki" && i + 1 < argc)
|
||
heading_ki = std::stof(argv[++i]);
|
||
else if (arg == "--heading_max_trim" && i + 1 < argc)
|
||
heading_max_trim = std::stof(argv[++i]);
|
||
else if (arg == "--no_whole_slip")
|
||
whole_slip_enabled = false;
|
||
else if (arg == "--whole_slip_min_omega" && i + 1 < argc)
|
||
whole_slip_min_omega = std::stof(argv[++i]);
|
||
else if (arg == "--whole_slip_ratio" && i + 1 < argc)
|
||
whole_slip_ratio_threshold = std::stof(argv[++i]);
|
||
else if (arg == "--whole_slip_debounce_ticks" && i + 1 < argc)
|
||
whole_slip_debounce_ticks = std::stoi(argv[++i]);
|
||
else if (arg == "--whole_slip_speed_scale" && i + 1 < argc)
|
||
whole_slip_speed_scale = std::stof(argv[++i]);
|
||
}
|
||
|
||
// --log를 직접 지정하지 않았으면 이 저장소(프로젝트 폴더) 안의 logs/에
|
||
// 타임스탬프 파일명으로 자동 생성한다 — 매 주행마다 RC/IMU/모터 로깅을
|
||
// 빠짐없이 남기기 위함. --no_log로 완전히 끌 수 있다.
|
||
if (log_path.empty() && !no_log) {
|
||
std::filesystem::create_directories("logs");
|
||
std::time_t now_c = std::time(nullptr);
|
||
char ts_buf[32];
|
||
std::strftime(ts_buf, sizeof(ts_buf), "%Y%m%d_%H%M%S", std::localtime(&now_c));
|
||
log_path = std::string("logs/drive_") + ts_buf + ".csv";
|
||
}
|
||
|
||
constexpr float PI_VAL = 3.14159265358979323846f;
|
||
float rpm_per_ms = 60.0f / (2.0f * PI_VAL * wheel_radius);
|
||
|
||
// effective_w — Phase 5a 정적 캘리브레이션. 기하학적 공식값(대각선 길이,
|
||
// 0.684)은 제자리 회전 실측 대비 작았다. 전용 회전 로그 2개(무결점
|
||
// 405틱, 두 로그 개별 중앙값 0.855/0.904로 서로 일치)에서 역산한
|
||
// 중앙값 0.87을 대신 사용한다. --effective_w로 재조정 가능.
|
||
float effective_w = (effective_w_override >= 0.0f) ? effective_w_override : 0.87f;
|
||
// k_skid — Phase 5a 정적 캘리브레이션(doc/06-imu-integration-plan.md §5a):
|
||
// 기하학적 공식값(트랙폭/휠베이스로만 계산, 0.406)은 실측 대비 계속
|
||
// 작게 나왔다(회전이 지령보다 항상 덜 도는 원인 중 하나). 두 차례
|
||
// 실주행 로그(커브 선회 중 무결점 총 5,150틱)에서 바퀴 실측 속도차 대
|
||
// IMU 실측 회전율로 역산한 중앙값이 0.51 적용 전 0.510, 적용 후에도
|
||
// 0.517로 수렴해 그대로 0.51을 유지한다. 회전 반경에 따라
|
||
// 0.44(완만한 커브)~0.53(급한 커브)로 흔들리는 편이라 0.51은 그
|
||
// 절충값 — 반경별로 정밀하게 맞추려면 Phase 5b(온라인 ICR 추정)가
|
||
// 필요하다. --k_skid로 재조정 가능.
|
||
float k_skid = (k_skid_override >= 0.0f) ? k_skid_override : 0.51f;
|
||
|
||
std::cout << "\n============================================================="
|
||
"=====\n";
|
||
std::cout
|
||
<< " [C++17 4WD ZLAC8015D + Mid-360S 라이다 정밀 장애물 회피 시스템]\n";
|
||
std::cout << " - 로봇 스펙: 가로(전폭) " << robot_width * 1000.0f
|
||
<< "mm | 세로(전장) " << robot_length * 1000.0f << "mm | 높이 "
|
||
<< robot_height * 1000.0f << "mm\n";
|
||
std::cout << " - 라이다 설치: 지면 높이 " << lidar_height * 1000.0f
|
||
<< "mm | 피치 경사각 " << lidar_pitch << "도 (하향) | 전방 오프셋 "
|
||
<< lidar_x_offset * 1000.0f << "mm\n";
|
||
std::cout << " - 필터링 범위: 높이 Z(" << min_z << "m ~ " << max_z
|
||
<< "m) | 회피 정밀 전폭 " << (robot_width + 0.10f) * 1000.0f
|
||
<< "mm\n";
|
||
std::cout << " - 최고 속도: 저속 " << rc_cfg.v_max_low << "m/s / 고속 "
|
||
<< rc_cfg.v_max_high << "m/s (CH" << rc_cfg.ch_speed
|
||
<< " 전환) | 가속: 즉시 추종(1:1) | 감속 Jerk: " << jerk_rate
|
||
<< "RPM/s² (제자리회전: " << spin_jerk_rate
|
||
<< "RPM/s²) | 회전 곡선혼합: " << spin_arc_bias * 100.0f << "%\n";
|
||
std::cout << " - RC 수신기: " << rc_cfg.port << " @" << rc_cfg.baud
|
||
<< "bps | 조향 CH" << rc_cfg.ch_steer << " | 스로틀 CH"
|
||
<< rc_cfg.ch_throttle << " | 브레이크 CH" << rc_cfg.ch_brake
|
||
<< " | 속도모드 CH" << rc_cfg.ch_speed
|
||
<< " | Fail-safe: " << rc_cfg.failsafe_timeout_ms << "ms\n";
|
||
std::cout << " - 휠 이상판정: 무부하(들뜸) 전류비 " << airborne_current_ratio
|
||
<< " | 걸림 속도비 " << stall_vel_ratio << " / 전류 "
|
||
<< stall_current_a << "A 이상\n";
|
||
std::cout << " - 라이다 장애물 회피 (LiDAR Dodge): "
|
||
<< (use_lidar ? "활성화 (Mid-360S)" : "비활성화") << "\n";
|
||
std::cout << " - 전방 정지 거리: " << front_stop_dist
|
||
<< "m | 전방 경고: " << front_warn_dist
|
||
<< "m | 측면 회피: " << side_dodge_dist << "m\n";
|
||
std::cout
|
||
<< "==================================================================\n";
|
||
|
||
// CSV 로깅: 기본적으로 매 주행마다 logs/에 자동 저장된다(위 auto-path 로직).
|
||
// RC 채널, IMU(자이로/각도), 모터 지령/피드백/전류/휠상태/엔코더 raw tick까지
|
||
// 매 틱 전부 기록한다. 실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로,
|
||
// 문제가 된 구간(턱 넘는 지점 등)이나 IMU-엔코더 융합용 데이터를 나중에 잘라서
|
||
// 분석하기 위한 용도.
|
||
std::ofstream log_stream;
|
||
if (!log_path.empty()) {
|
||
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,"
|
||
"imu_ok,imu_gyro_z,imu_roll,imu_pitch,imu_yaw,imu_yaw_rel,"
|
||
"cmd_fl,cmd_fr,cmd_rl,cmd_rr,"
|
||
"fb_fl,fb_fr,fb_rl,fb_rr,"
|
||
"amp_fl,amp_fr,amp_rl,amp_rr,"
|
||
"temp_fl,temp_fr,temp_rl,temp_rr,temp_drv_front,temp_drv_rear,"
|
||
"stat_fl,stat_fr,stat_rl,stat_rr,"
|
||
"err_fl,err_fr,err_rl,err_rr,"
|
||
"tick_fl,tick_fr,tick_rl,tick_rr,"
|
||
"wheel_omega,imu_omega,omega_residual,fused_yaw,odom_x,odom_y,"
|
||
"heading_hold,heading_target,heading_trim,whole_slip,"
|
||
"dist_axle,comm_ms\n";
|
||
std::cout << " - CSV 로그 기록: " << log_path << "\n";
|
||
} else {
|
||
std::cerr << "[경고] 로그 파일 열기 실패: " << log_path << "\n";
|
||
}
|
||
}
|
||
auto log_start_time = std::chrono::steady_clock::now();
|
||
|
||
// Initialize LiDAR Detector
|
||
LidarObstacleDetector lidar_detector;
|
||
if (use_lidar) {
|
||
// Robot half width tolerance = robot_width / 2 + 5cm margin
|
||
lidar_detector.setParams(min_z, max_z, robot_width / 2.0f + 0.05f,
|
||
lidar_pitch, lidar_height, lidar_x_offset);
|
||
if (!lidar_detector.init(lidar_config)) {
|
||
std::cout << "[경고] 라이다 연결 실패. 장애물 감지 없이 조이스틱 모드로 "
|
||
"실행합니다.\n";
|
||
}
|
||
}
|
||
|
||
// 모터(--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";
|
||
}
|
||
if (use_imu && (imu_cfg.port == port1 || imu_cfg.port == rc_cfg.port)) {
|
||
std::cout << "\n[경고] --imu_port(" << imu_cfg.port
|
||
<< ")가 모터 또는 RC 수신기 포트와 동일합니다. 서로 다른 "
|
||
"포트로 지정하세요.\n\n";
|
||
}
|
||
|
||
RcReceiver rc(rc_cfg);
|
||
rc.start();
|
||
std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 ("
|
||
<< rc_cfg.port << "). 신호 수신 전까지는 안전을 위해 브레이크가 "
|
||
"걸린 상태로 대기합니다.\n";
|
||
|
||
ImuReader imu(imu_cfg);
|
||
if (use_imu) {
|
||
imu.start();
|
||
std::cout << "[정보] IMU(HWT905-RS232) 리더 시작 (" << imu_cfg.port << " @"
|
||
<< imu_cfg.baud << "bps). Phase 1: 관측 전용, 제어에는 개입하지 "
|
||
"않습니다.\n";
|
||
}
|
||
|
||
SerialPort sp1, sp2;
|
||
if (!sp1.openPort(port1))
|
||
return 1;
|
||
std::cout << "[정보] RS485 포트1 (" << port1 << ") 오픈 성공.\n";
|
||
|
||
SerialPort *sp2_ptr = &sp1;
|
||
if (!port2.empty()) {
|
||
if (sp2.openPort(port2)) {
|
||
sp2_ptr = &sp2;
|
||
std::cout << "[정보] RS485 포트2 (" << port2 << ") 오픈 성공.\n";
|
||
}
|
||
}
|
||
|
||
MotorDriver driver_front(&sp1, bcast_mode ? 0 : id1);
|
||
driver_front.initDriver(150, 150);
|
||
|
||
MotorDriver *driver_rear_ptr = nullptr;
|
||
MotorDriver driver_rear(sp2_ptr, bcast_mode ? 0 : id2);
|
||
if (!bcast_mode) {
|
||
driver_rear.initDriver(150, 150);
|
||
driver_rear_ptr = &driver_rear;
|
||
}
|
||
|
||
// 현재 드라이버에 실제로 설정된 정격/최대전류를 읽어와서 보여준다
|
||
// (문서 기본값이 아니라 실제 하드웨어에 저장된 값을 확인하기 위함).
|
||
// --max_current_a가 지정되면 4바퀴 최대전류를 그 값으로 낮춘다.
|
||
if (!bcast_mode) {
|
||
uint16_t f_rated_l, f_max_l, f_rated_r, f_max_r;
|
||
if (driver_front.readCurrentLimits(f_rated_l, f_max_l, f_rated_r,
|
||
f_max_r)) {
|
||
std::cout << " - 전방 드라이버 전류설정: 정격 L" << f_rated_l * 0.1f
|
||
<< "A/R" << f_rated_r * 0.1f << "A | 최대 L" << f_max_l * 0.1f
|
||
<< "A/R" << f_max_r * 0.1f << "A\n";
|
||
} else {
|
||
std::cout << "[경고] 전방 드라이버 전류설정 조회 실패\n";
|
||
}
|
||
if (driver_rear_ptr) {
|
||
uint16_t r_rated_l, r_max_l, r_rated_r, r_max_r;
|
||
if (driver_rear_ptr->readCurrentLimits(r_rated_l, r_max_l, r_rated_r,
|
||
r_max_r)) {
|
||
std::cout << " - 후방 드라이버 전류설정: 정격 L" << r_rated_l * 0.1f
|
||
<< "A/R" << r_rated_r * 0.1f << "A | 최대 L"
|
||
<< r_max_l * 0.1f << "A/R" << r_max_r * 0.1f << "A\n";
|
||
} else {
|
||
std::cout << "[경고] 후방 드라이버 전류설정 조회 실패\n";
|
||
}
|
||
}
|
||
|
||
if (max_current_a >= 0.0f) {
|
||
uint16_t new_max_01a = static_cast<uint16_t>(max_current_a * 10.0f);
|
||
driver_front.setMaxCurrent(new_max_01a);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setMaxCurrent(new_max_01a);
|
||
std::cout << " - 최대전류를 " << max_current_a << "A로 낮춤(4바퀴 동일)\n";
|
||
}
|
||
}
|
||
|
||
std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: Ctrl+C, "
|
||
"라이다 회피 " << (use_lidar ? "활성화" : "비활성화")
|
||
<< " 상태로 시작 - 변경하려면 --no_lidar 옵션을 사용하세요)\n\n";
|
||
|
||
float target_fl = 0.0f, target_fr = 0.0f;
|
||
float target_rl = 0.0f, target_rr = 0.0f;
|
||
|
||
float cmd_fl = 0.0f, cmd_fr = 0.0f;
|
||
float cmd_rl = 0.0f, cmd_rr = 0.0f;
|
||
// 각 바퀴의 현재 가속도 상태(저크 제한 프로파일용)
|
||
float accel_fl = 0.0f, accel_fr = 0.0f;
|
||
float accel_rl = 0.0f, accel_rr = 0.0f;
|
||
|
||
const float loop_hz = 100.0f;
|
||
const float dt = 1.0f / loop_hz;
|
||
|
||
// 가속: 역기전력 보호회로가 하드웨어로 구성되어 더 이상 소프트웨어로
|
||
// 완만하게 램프업할 필요가 없어져, 조이스틱 목표값에 1:1로 즉시 추종한다.
|
||
// 감속: 기존과 동일하게 저크 제한(jerk-limited) S-curve 유지 — 가속도
|
||
// 자체를 매 틱 jerk_limit 만큼만 변화시켜 서서히 목표 가속도에 도달하게
|
||
// 해서, 급감속 시 타이어 스크럽/전복 위험을 줄인다.
|
||
auto jerkLimitedStep = [dt](float current_vel, float target_vel,
|
||
float ¤t_accel, float max_decel_mag,
|
||
float jerk_limit) {
|
||
float diff = target_vel - current_vel;
|
||
if (std::abs(diff) < 0.01f) {
|
||
current_accel = 0.0f;
|
||
return target_vel;
|
||
}
|
||
|
||
// 같은 방향으로 더 빨라지는 경우만 즉시 스냅한다. 부호가 바뀌는 역전은
|
||
// (현재속도가 0이 아닌데 목표가 반대부호로 바뀌는 경우) 저크 제한 감속
|
||
// 경로로 보낸다 — 정지 직전 저속 구간에서 RC/조향 노이즈만으로 목표가
|
||
// 순간 반대부호로 흔들려도 그대로 즉시 스냅되면 "전진→후진→전진"처럼
|
||
// 튀는 현상이 생기고, 실제 반전 요청이라도 0을 관통하는 순간 스냅은
|
||
// 역기전력 스파이크가 오히려 일반 가속보다 더 크다.
|
||
bool same_direction =
|
||
(current_vel == 0.0f) || (target_vel * current_vel > 0.0f);
|
||
bool is_accelerating =
|
||
same_direction && (std::abs(target_vel) > std::abs(current_vel));
|
||
if (is_accelerating) {
|
||
current_accel = 0.0f;
|
||
return target_vel;
|
||
}
|
||
|
||
// 이번 틱에 diff를 없애는데 필요한 가속도(부호 포함)를 물리적 한계로 클램프
|
||
float desired_accel = std::clamp(diff / dt, -max_decel_mag, max_decel_mag);
|
||
|
||
float max_delta_accel = jerk_limit * dt;
|
||
current_accel = std::clamp(desired_accel, current_accel - max_delta_accel,
|
||
current_accel + max_delta_accel);
|
||
|
||
float next_vel = current_vel + current_accel * dt;
|
||
bool overshoot =
|
||
(diff > 0.0f) ? (next_vel >= target_vel) : (next_vel <= target_vel);
|
||
if (overshoot) {
|
||
current_accel = 0.0f;
|
||
return target_vel;
|
||
}
|
||
return next_vel;
|
||
};
|
||
|
||
std::string state = "STOPPED";
|
||
bool trip_initialized = false;
|
||
int32_t start_fl = 0, start_fr = 0, start_rl = 0, start_rr = 0;
|
||
|
||
// 바퀴별 이상 상태(직전 틱 피드백 기반 판정, 1틱=10ms 지연으로 반영)
|
||
bool airborne_fl = false, airborne_fr = false;
|
||
bool airborne_rl = false, airborne_rr = false;
|
||
int air_count_fl = 0, air_count_fr = 0, air_count_rl = 0, air_count_rr = 0;
|
||
// 착지(airborne 해제) 직후 한 틱 동안: 차체는 이미 그 속도로 움직이고
|
||
// 있는데 cmd가 뒤늦게 target을 쫓아가면 그 사이 바퀴가 지면 이동속도를
|
||
// 못 따라가 끌리며 긁히는 문제가 있어 즉시 target으로 스냅시켜 지연을
|
||
// 없앤다.
|
||
bool just_landed_fl = false, just_landed_fr = false;
|
||
bool just_landed_rl = false, just_landed_rr = false;
|
||
char wheel_status_str[5] = "OOOO";
|
||
bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부
|
||
// 드라이버 온도(0x20B0)는 열 시정수가 초 단위로 느려 매 틱 통신할
|
||
// 필요가 없다 — 30틱(실측 ~27Hz 기준 약 1.1초)마다만 갱신.
|
||
float front_driver_temp_c = 0.0f, rear_driver_temp_c = 0.0f;
|
||
int driver_temp_poll_counter = 0;
|
||
constexpr int kDriverTempPollTicks = 30;
|
||
// 하드웨어 스펙(doc/01) 동작온도 상한 50°C, 드라이버 자체 과열 보호
|
||
// 임계값 기본 80°C 대비 여유를 두고 조기 경보를 띄우는 기준.
|
||
constexpr int kOverheatWarnC = 60;
|
||
bool prev_brake_on = false; // 브레이크(CH5/Fail-safe) 직전 틱 상태
|
||
bool prev_rc_ok = false; // RC 연결 직전 틱 상태 (Fail-safe 로그용)
|
||
float omega_max = (effective_w > 0.0f) ? (max_spin_v / (effective_w / 2.0f))
|
||
: 0.0f; // CH1 조향 최대 각속도
|
||
// CRC 미검증으로 인해 손상된 CRSF 프레임이 드물게 통과할 수 있다.
|
||
// (실측 결과 이 송신기는 표준 CRC8 변형 어떤 것과도 안 맞아 — poly
|
||
// 전수조사+여러 바이트 범위로 확인해도 일치율이 최대 33% 수준이라
|
||
// CRC 자체를 신뢰할 수 없다고 판단, 검증 시도를 포기함.) 대신 두 가지
|
||
// 물리적 타당성 검사로 걸러낸다:
|
||
// 1) 캘리브레이션 범위(steer_min~max, throttle_min~max) 밖의 값 거부
|
||
// 2) 슬루레이트: 한 틱(~10ms) 사이에 사람이 스틱으로 낼 수 없을 만큼
|
||
// 크게 튀는 값 거부 (예: 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;
|
||
|
||
// IMU Phase 1: 순수 관측용 표시 오프셋. 로봇 전원이 켜질 때(또는 이
|
||
// 프로그램이 시작될 때) IMU가 처음 보고하는 절대 yaw를 "정면(0°)"
|
||
// 기준으로 저장해, 화면에는 그 시점 대비 상대 yaw를 보여준다. 이건
|
||
// 순수 표시용 계산일 뿐 제어 로직에는 전혀 관여하지 않는다 — 헤딩을
|
||
// 실제로 잠그거나 보정하는 건 Phase 3의 몫이다.
|
||
double imu_yaw_offset = 0.0;
|
||
bool imu_yaw_offset_set = false;
|
||
|
||
// Phase 2 — 헤딩 적분 + x,y 오도메트리 (여전히 순수 관측, 제어 미개입).
|
||
// 처음엔 IMU 내장 AHRS가 계산해 주는 imu_yaw_rel을 θ로 그대로 썼으나,
|
||
// 실측 루프백(한 바퀴 돌아 제자리 복귀) 주행 로그로 검증한 결과 그 값이
|
||
// 같은 장치의 원시 자이로(gyro_z)와도 1초 평균 구간 기준 부호 일치율이
|
||
// 72%에 불과함이 드러났다(회전 중 모터 전류로 인한 지자기 간섭 등으로
|
||
// 추정 — 저가 AHRS의 흔한 증상). 실제로 그 루프백 로그에서 odom이 시작
|
||
// 위치로 전혀 돌아오지 못했다. 반면 바퀴 인코더 기반 wheel_omega와 원시
|
||
// gyro_z는 부호 일치율 99.3%로 서로 강하게 신뢰할 수 있음을 같은 로그에서
|
||
// 확인했다. 그래서 θ는 IMU 융합 yaw 대신 wheel_omega와 원시 gyro_z의
|
||
// 평균을 자체 적분한 fused_yaw_deg를 쓴다(IMU 미연결 시엔 wheel_omega만
|
||
// 사용). IMU 융합 yaw(imu_yaw_rel)는 비교용으로 HUD/CSV에 계속 남긴다.
|
||
double odom_x = 0.0, odom_y = 0.0; // m, Phase 1 오프셋 설정 시점 기준
|
||
double fused_yaw_deg = 0.0; // deg, wheel_omega+gyro_z 자체 적분 헤딩
|
||
auto last_odom_time = std::chrono::steady_clock::now();
|
||
|
||
// Phase 3 — 헤딩 홀드 PI 상태. 적분 windup 방지용 상한은 트림 상한을
|
||
// Ki로 나눠 "적분항 단독으로도 트림 상한을 넘지 않는" 값으로 고정한다.
|
||
bool heading_hold_active_prev = false;
|
||
double heading_target_deg = 0.0;
|
||
double heading_error_integral = 0.0;
|
||
const float heading_integral_limit =
|
||
(heading_ki > 1e-6f) ? (heading_max_trim / heading_ki) : 0.0f;
|
||
|
||
// Phase 4 — 전신 슬립 디바운스 카운터
|
||
int whole_slip_count = 0;
|
||
|
||
while (g_running) {
|
||
// ---------------------------------------------------------------------
|
||
// RC 수신기 채널 읽기 (doc/joystick.md §2, §3 기능 명세서 기준)
|
||
// ---------------------------------------------------------------------
|
||
int raw_steer = rc_cfg.steer_center_lo, raw_throttle = 1500;
|
||
int raw_brake = 1011, raw_speed_mode = 1011;
|
||
bool rc_ok = rc.isConnected();
|
||
rc.getChannel(rc_cfg.ch_steer, raw_steer);
|
||
rc.getChannel(rc_cfg.ch_throttle, raw_throttle);
|
||
rc.getChannel(rc_cfg.ch_brake, raw_brake);
|
||
rc.getChannel(rc_cfg.ch_speed, raw_speed_mode);
|
||
|
||
bool steer_range_ok =
|
||
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 ||
|
||
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;
|
||
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=브레이크 값을 실시간으로
|
||
// 직접 눈으로 검증할 수 있도록 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으로 표시
|
||
}
|
||
|
||
// IMU Phase 1: 순수 관측. 제어 로직에는 관여하지 않는다.
|
||
ImuReader::Snapshot imu_snap = use_imu ? imu.getSnapshot() : ImuReader::Snapshot{};
|
||
if (imu_snap.valid && !imu_yaw_offset_set) {
|
||
imu_yaw_offset = imu_snap.angle[2];
|
||
imu_yaw_offset_set = true;
|
||
std::cout << "\n[정보] IMU 정면 기준 설정: 시작 시점 yaw " << imu_yaw_offset
|
||
<< "도를 0도(정면)로 저장했습니다.\n";
|
||
}
|
||
double imu_yaw_rel = 0.0;
|
||
if (imu_yaw_offset_set) {
|
||
imu_yaw_rel = imu_snap.angle[2] - imu_yaw_offset;
|
||
// [-180, 180) 범위로 정규화
|
||
while (imu_yaw_rel > 180.0) imu_yaw_rel -= 360.0;
|
||
while (imu_yaw_rel < -180.0) imu_yaw_rel += 360.0;
|
||
}
|
||
// 바퀴 피드백과 무관한 IMU 원시 요레이트. Phase 2(휠-IMU 융합 헤딩)와
|
||
// Phase 4(지령-IMU 전신 슬립 감지, 명령 결정 이전에 필요)가 공유한다.
|
||
float imu_omega_rad = imu_snap.gyro[2] * (PI_VAL / 180.0f); // rad/s
|
||
|
||
if (!rc_ok && prev_rc_ok) {
|
||
std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms
|
||
<< "ms 이상 미수신) -> 비상 정지.\n";
|
||
}
|
||
prev_rc_ok = rc_ok;
|
||
|
||
// 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리
|
||
bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold);
|
||
|
||
// 3.2 속도 제한 설정 (CH6)
|
||
float v_max_now = (raw_speed_mode >= rc_cfg.speed_threshold)
|
||
? rc_cfg.v_max_low
|
||
: rc_cfg.v_max_high;
|
||
|
||
float v_x = 0.0f; // 선속도 m/s
|
||
float omega = 0.0f; // 각속도 rad/s
|
||
bool user_steer_centered = false; // Phase 3 헤딩 홀드 진입 조건용
|
||
|
||
if (!brake_on) {
|
||
// 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_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_max_now;
|
||
}
|
||
|
||
// 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<float>(rc_cfg.steer_center_lo - s) /
|
||
static_cast<float>(rc_cfg.steer_center_lo - rc_cfg.steer_min)) *
|
||
omega_max;
|
||
} else {
|
||
omega = -(static_cast<float>(s - rc_cfg.steer_center_hi) /
|
||
static_cast<float>(rc_cfg.steer_max - rc_cfg.steer_center_hi)) *
|
||
omega_max;
|
||
}
|
||
user_steer_centered = (omega == 0.0f);
|
||
}
|
||
|
||
// ---------------------------------------------------------------------
|
||
// LiDAR Obstacle Avoidance & Stopping Logic Integration
|
||
// (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작)
|
||
// ---------------------------------------------------------------------
|
||
std::string lidar_telemetry = "LIDAR:OFF";
|
||
float avoid_omega_offset = 0.0f; // Phase 3 헤딩 홀드가 라이다 회피와
|
||
// 충돌하지 않도록 블록 밖에서도 확인
|
||
if (use_lidar) {
|
||
ObstacleStatus obs = lidar_detector.getStatus();
|
||
if (obs.connected) {
|
||
float speed_scale = 1.0f;
|
||
|
||
// 1. Forward / Backward Automatic Stopping
|
||
if (v_x > 0.01f) {
|
||
if (obs.min_dist_front < front_stop_dist) {
|
||
speed_scale = 0.0f; // Complete forward stop
|
||
} else if (obs.min_dist_front < front_warn_dist) {
|
||
speed_scale = (obs.min_dist_front - front_stop_dist) /
|
||
(front_warn_dist - front_stop_dist);
|
||
speed_scale = std::clamp(speed_scale, 0.0f, 1.0f);
|
||
}
|
||
} else if (v_x < -0.01f) {
|
||
if (obs.min_dist_rear < front_stop_dist) {
|
||
speed_scale = 0.0f; // Complete backward stop
|
||
}
|
||
}
|
||
|
||
// 2. Side Dodge Steering (전진 중, 사용자 조향 입력이 없을 때만 동작)
|
||
if (v_x > 0.01f && omega == 0.0f) {
|
||
float right_intensity = 0.0f;
|
||
float left_intensity = 0.0f;
|
||
|
||
if (obs.min_dist_right < side_dodge_dist) {
|
||
right_intensity = (side_dodge_dist - obs.min_dist_right) /
|
||
(side_dodge_dist - side_stop_dist);
|
||
right_intensity = std::clamp(right_intensity, 0.0f, 1.0f);
|
||
}
|
||
if (obs.min_dist_left < side_dodge_dist) {
|
||
left_intensity = (side_dodge_dist - obs.min_dist_left) /
|
||
(side_dodge_dist - side_stop_dist);
|
||
left_intensity = std::clamp(left_intensity, 0.0f, 1.0f);
|
||
}
|
||
|
||
// Right obstacle -> Steer Left (+omega)
|
||
// Left obstacle -> Steer Right (-omega)
|
||
avoid_omega_offset =
|
||
(right_intensity - left_intensity) * max_dodge_omega;
|
||
}
|
||
|
||
v_x *= speed_scale;
|
||
|
||
char lbuf[128];
|
||
snprintf(lbuf, sizeof(lbuf), "F:%4.2fm|L:%4.2fm|R:%4.2fm %s",
|
||
obs.min_dist_front > 99.0f ? 9.99f : obs.min_dist_front,
|
||
obs.min_dist_left > 99.0f ? 9.99f : obs.min_dist_left,
|
||
obs.min_dist_right > 99.0f ? 9.99f : obs.min_dist_right,
|
||
avoid_omega_offset > 0.05f
|
||
? "[Dodge L]"
|
||
: (avoid_omega_offset < -0.05f
|
||
? "[Dodge R]"
|
||
: (speed_scale < 0.01f ? "[STOP!]" : "[OK]")));
|
||
lidar_telemetry = lbuf;
|
||
|
||
// Add avoidance offset to user steering
|
||
omega += avoid_omega_offset;
|
||
} else {
|
||
lidar_telemetry = "LIDAR:WAITING";
|
||
}
|
||
}
|
||
|
||
// ---------------------------------------------------------------------
|
||
// Phase 3 — 직진 헤딩 홀드 (doc/06-imu-integration-plan.md §Phase 3).
|
||
// 조향 중립 + 라이다 회피 미개입 + 실제 주행 중일 때만 개입한다. 진입
|
||
// 순간의 fused_yaw_deg를 목표 헤딩으로 고정(lock)하고, 이후 그로부터
|
||
// 벗어난 만큼(오차)을 작은 PI로 보정해 omega에 트림을 더한다. 사용자
|
||
// 조향/정지/라이다 회피가 개입하는 즉시 해제되고 적분항도 리셋된다.
|
||
// Phase 2 검증(실측 루프백 로그)에서 wheel_omega/gyro_z 부호 일치율이
|
||
// 99.3%로 확인된 fused_yaw_deg를 피드백으로 쓴다 — IMU 단독 융합 yaw는
|
||
// 회전 중 신뢰도가 낮아 제어 피드백에서 제외했다(imu_yaw_rel은 HUD/CSV
|
||
// 비교용으로만 남김).
|
||
bool heading_hold_condition = heading_hold_enabled && !brake_on &&
|
||
user_steer_centered &&
|
||
avoid_omega_offset == 0.0f && v_x != 0.0f;
|
||
float heading_trim_omega = 0.0f; // HUD/CSV 노출용
|
||
if (heading_hold_condition) {
|
||
if (!heading_hold_active_prev) {
|
||
heading_target_deg = fused_yaw_deg;
|
||
heading_error_integral = 0.0;
|
||
}
|
||
double heading_error_deg = heading_target_deg - fused_yaw_deg;
|
||
while (heading_error_deg > 180.0) heading_error_deg -= 360.0;
|
||
while (heading_error_deg < -180.0) heading_error_deg += 360.0;
|
||
double heading_error_rad = heading_error_deg * (PI_VAL / 180.0);
|
||
|
||
heading_error_integral += heading_error_rad * dt;
|
||
heading_error_integral = std::clamp(
|
||
heading_error_integral,
|
||
static_cast<double>(-heading_integral_limit),
|
||
static_cast<double>(heading_integral_limit));
|
||
|
||
heading_trim_omega = static_cast<float>(
|
||
heading_kp * heading_error_rad + heading_ki * heading_error_integral);
|
||
heading_trim_omega =
|
||
std::clamp(heading_trim_omega, -heading_max_trim, heading_max_trim);
|
||
omega += heading_trim_omega;
|
||
} else {
|
||
heading_error_integral = 0.0;
|
||
}
|
||
heading_hold_active_prev = heading_hold_condition;
|
||
|
||
// ---------------------------------------------------------------------
|
||
// Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (doc §Phase 4). 지령
|
||
// omega(라이다 회피/헤딩 홀드 트림까지 반영된 최종값) 대비 바퀴 피드백과
|
||
// 완전히 독립적인 IMU 원시 gyro_z의 비율이 문턱값 밑으로
|
||
// whole_slip_debounce_ticks 틱 이상 지속되면 "전신 슬립"으로 확정하고,
|
||
// 라이다 회피의 speed_scale과 같은 패턴으로 v_x/omega를 일시적으로
|
||
// 낮춘다. 문턱값을 낮게(0.15) 잡아 제자리 회전의 정상적인 스크럽 손실
|
||
// (지령 대비 40~60% 미달도 흔함)과 혼동하지 않고 사실상 전혀 못 도는
|
||
// 극단적 슬립만 잡아낸다.
|
||
bool whole_slip_now = false;
|
||
if (whole_slip_enabled && imu_snap.valid &&
|
||
std::abs(omega) > whole_slip_min_omega) {
|
||
float slip_ratio = std::abs(imu_omega_rad) / std::abs(omega);
|
||
whole_slip_now = slip_ratio < whole_slip_ratio_threshold;
|
||
}
|
||
whole_slip_count = whole_slip_now ? whole_slip_count + 1 : 0;
|
||
bool whole_slip_active = whole_slip_count >= whole_slip_debounce_ticks;
|
||
if (whole_slip_active) {
|
||
v_x *= whole_slip_speed_scale;
|
||
omega *= whole_slip_speed_scale;
|
||
}
|
||
|
||
// 제자리 회전 여부: CH2(전후진) 입력이 완전히 중립(v_x == 0.0f)이면서
|
||
// CH1(조향) 입력이 있을 때만 진입한다. v_x는 데드존 처리 시 정확히
|
||
// 0.0f로 설정되므로 등호 비교가 안전하다.
|
||
bool is_spin_turn = (v_x == 0.0f) && (std::abs(omega) > 0.0f);
|
||
|
||
if (is_spin_turn) {
|
||
// 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합 (스크럽
|
||
// 마찰 저항 감소를 위해 ICR을 로봇 중심에서 살짝 벗어나게 함)
|
||
float norm = (omega_max > 0.0f)
|
||
? std::clamp(std::abs(omega) / omega_max, 0.0f, 1.0f)
|
||
: 0.0f;
|
||
float v_bias = norm * max_spin_v * spin_arc_bias;
|
||
float v_l = v_bias - omega * (effective_w / 2.0f);
|
||
float v_r = v_bias + omega * (effective_w / 2.0f);
|
||
|
||
target_fl = v_l * rpm_per_ms;
|
||
target_fr = -v_r * rpm_per_ms;
|
||
target_rl = v_l * rpm_per_ms;
|
||
target_rr = -v_r * rpm_per_ms;
|
||
} else {
|
||
// 직진 및 차동 선회 Kinematics (CH1 조향 + 라이다 회피 오프셋 포함)
|
||
float v_l = v_x - omega * k_skid;
|
||
float v_r = v_x + omega * k_skid;
|
||
target_fl = v_l * rpm_per_ms;
|
||
target_fr = -v_r * rpm_per_ms;
|
||
target_rl = v_l * rpm_per_ms;
|
||
target_rr = -v_r * rpm_per_ms;
|
||
}
|
||
|
||
// 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 0으로 낮춰
|
||
// 헛돌이를 억제한다. 접지력이 회복되어 전류가 정상으로 돌아오면 다음
|
||
// 판정 틱에서 자동으로 해제되어 원래 지령으로 복귀한다.
|
||
// 단, 제자리 회전 중에는 반력 토크로 인한 대각선 하중 이동(FL+RR 또는
|
||
// FR+RL 쌍이 동시에 가벼워짐)이 정상적인 현상인데 이걸 "들뜸"으로 오判定해
|
||
// 목표 속도를 꺼버리면 4륜 중 사실상 2륜만 구동되어 회전력이 반토막
|
||
// 난다(실측 로그에서 전체 회전 틱의 38%가 대각선 쌍 동시 들뜸 판정이었고,
|
||
// 그 결과 실제 회전율이 지령 대비 44~58%에 그쳤음). 그래서 제자리 회전
|
||
// 중에는 판정(HUD 표시용)은 유지하되 속도를 꺾는 개입만 끈다.
|
||
if (!is_spin_turn) {
|
||
if (airborne_fl)
|
||
target_fl = 0.0f;
|
||
if (airborne_fr)
|
||
target_fr = 0.0f;
|
||
if (airborne_rl)
|
||
target_rl = 0.0f;
|
||
if (airborne_rr)
|
||
target_rr = 0.0f;
|
||
}
|
||
|
||
// 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시
|
||
// 0으로 스냅 후 브레이크를 잠근다. 해제되면 STOPPED 상태에서 아래
|
||
// 상태머신이 정상적으로 재가속을 시작한다.
|
||
// 전자식 브레이크(솔레노이드)는 오직 CH5로만 제어한다 — 스로틀을 중립으로
|
||
// 되돌려 자연 정지했을 때는 잠그지 않고, CH5를 눌렀을 때만 잠근다.
|
||
if (brake_on && !prev_brake_on) {
|
||
cmd_fl = cmd_fr = cmd_rl = cmd_rr = 0.0f;
|
||
accel_fl = accel_fr = accel_rl = accel_rr = 0.0f;
|
||
driver_front.setRPMs(0, 0);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setRPMs(0, 0);
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||
driver_front.setBrakes(true);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setBrakes(true);
|
||
state = "STOPPED";
|
||
} else if (!brake_on && prev_brake_on) {
|
||
driver_front.setBrakes(false);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setBrakes(false);
|
||
}
|
||
prev_brake_on = brake_on;
|
||
|
||
if (brake_on) {
|
||
// target_fl..rr은 이미 0(v_x=omega=0)이며 STOPPED 상태를 유지한다.
|
||
} else if (state == "STOPPED") {
|
||
if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
|
||
state = "RUNNING";
|
||
}
|
||
} else if (state == "RUNNING") {
|
||
// 착지 직후: 정지출발용 저크 램프를 건너뛰고 차체가 이미 내고 있는
|
||
// 속도로 즉시 맞춰 지면과의 미끄러짐(끌림)을 최소화한다.
|
||
if (just_landed_fl) {
|
||
cmd_fl = target_fl;
|
||
accel_fl = 0.0f;
|
||
just_landed_fl = false;
|
||
}
|
||
if (just_landed_fr) {
|
||
cmd_fr = target_fr;
|
||
accel_fr = 0.0f;
|
||
just_landed_fr = false;
|
||
}
|
||
if (just_landed_rl) {
|
||
cmd_rl = target_rl;
|
||
accel_rl = 0.0f;
|
||
just_landed_rl = false;
|
||
}
|
||
if (just_landed_rr) {
|
||
cmd_rr = target_rr;
|
||
accel_rr = 0.0f;
|
||
just_landed_rr = false;
|
||
}
|
||
|
||
float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate;
|
||
cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate, jerk_now);
|
||
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now);
|
||
cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now);
|
||
cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, decel_rate, jerk_now);
|
||
|
||
if (std::abs(target_fl) < 0.1f && std::abs(target_fr) < 0.1f &&
|
||
std::abs(cmd_fl) < 0.5f && std::abs(cmd_fr) < 0.5f) {
|
||
state = "STOPPING";
|
||
}
|
||
} else if (state == "STOPPING") {
|
||
float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate;
|
||
cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate, jerk_now);
|
||
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now);
|
||
cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now);
|
||
cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, decel_rate, jerk_now);
|
||
|
||
if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) {
|
||
cmd_fl = 0.0f;
|
||
cmd_fr = 0.0f;
|
||
cmd_rl = 0.0f;
|
||
cmd_rr = 0.0f;
|
||
accel_fl = accel_fr = accel_rl = accel_rr = 0.0f;
|
||
driver_front.setRPMs(0, 0);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setRPMs(0, 0);
|
||
// 스로틀 중립 복귀로 인한 자연 정지: 전자식 브레이크는 잠그지 않는다
|
||
// (CH5를 눌렀을 때만 잠금 — 위 브레이크 최우선 로직 참고).
|
||
state = "STOPPED";
|
||
}
|
||
}
|
||
|
||
auto comm_start = std::chrono::high_resolution_clock::now();
|
||
|
||
if (state != "STOPPED") {
|
||
driver_front.setRPMs(cmd_fl, cmd_fr);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setRPMs(cmd_rl, cmd_rr);
|
||
}
|
||
|
||
float fl_fb = 0, fr_fb = 0, rl_fb = 0, rr_fb = 0;
|
||
float fl_amp = 0, fr_amp = 0, rl_amp = 0, rr_amp = 0;
|
||
int32_t fl_tick = 0, fr_tick = 0, rl_tick = 0, rr_tick = 0;
|
||
uint16_t err_f_l = 0, err_f_r = 0, err_r_l = 0, err_r_r = 0;
|
||
int fl_temp = 0, fr_temp = 0, rl_temp = 0, rr_temp = 0;
|
||
|
||
driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick, fl_amp, fr_amp,
|
||
err_f_l, err_f_r, fl_temp, fr_temp);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick, rl_amp,
|
||
rr_amp, err_r_l, err_r_r, rl_temp, rr_temp);
|
||
|
||
// 드라이버(기판) 온도는 저빈도로만 갱신(위 kDriverTempPollTicks 주석 참고).
|
||
if (++driver_temp_poll_counter >= kDriverTempPollTicks) {
|
||
driver_temp_poll_counter = 0;
|
||
driver_front.readDriverTemp(front_driver_temp_c);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->readDriverTemp(rear_driver_temp_c);
|
||
}
|
||
|
||
// 드라이버 알람(과전류/과부하 등) 감지: 새로 발생한 알람만 콘솔에
|
||
// 한 번 크게 출력한다 (매 틱 갱신되는 HUD 줄과 별개의 고정 줄).
|
||
// 알람이 뜨면 드라이버가 명령을 무시하는 잠금 상태가 되어 소프트웨어
|
||
// 재시작만으로는 안 풀리는 경우가 많다(하드웨어 클리어/재활성화 필요).
|
||
bool fault_now = err_f_l || err_f_r || err_r_l || err_r_r;
|
||
if (fault_now && !fault_active) {
|
||
std::cout << "\n\n[!!! 드라이버 알람 발생 !!!] "
|
||
<< "전방L:" << decodeDriverError(err_f_l)
|
||
<< " 전방R:" << decodeDriverError(err_f_r)
|
||
<< " 후방L:" << decodeDriverError(err_r_l)
|
||
<< " 후방R:" << decodeDriverError(err_r_r)
|
||
<< " -> 모터 정지, 전원 재시작 또는 알람클리어 필요\n\n";
|
||
}
|
||
fault_active = fault_now;
|
||
|
||
auto comm_end = std::chrono::high_resolution_clock::now();
|
||
float comm_ms =
|
||
std::chrono::duration<float, std::milli>(comm_end - comm_start).count();
|
||
|
||
// 바퀴별 이상(들뜸/걸림) 판정: 속도 폐루프 특성상 "지령 대비 실제속도"
|
||
// 만으로는 무부하(들뜸)를 구분할 수 없다 (드라이버가 부하와 무관하게
|
||
// 지령 RPM을 그대로 추종하려 하므로). 그래서 두 축으로 판정한다.
|
||
// - 걸림/과부하: 실제속도가 지령을 크게 못 따라가면서 전류가 높음 (독립 판정)
|
||
// - 무부하/들뜸: "같은 지령속도를 받는 짝(앞뒤 같은 쪽)" 대비 전류가
|
||
// 비정상적으로 낮음. 좌/우를 통째로 비교하면 정상적인 커브 선회에서
|
||
// 지령속도가 원래 다른 좌우 바퀴를 오탐하므로, 반드시 target_fl=target_rl,
|
||
// target_fr=target_rr 로 지령이 항상 같은 같은쪽 앞뒤 쌍끼리만 비교한다.
|
||
auto classifyStall = [&](float cmd_rpm, float actual_rpm,
|
||
float amp) -> bool {
|
||
float cmd_abs = std::abs(cmd_rpm);
|
||
if (cmd_abs < min_active_rpm)
|
||
return false;
|
||
float vel_ratio = std::abs(actual_rpm) / cmd_abs;
|
||
return vel_ratio < stall_vel_ratio && std::abs(amp) >= stall_current_a;
|
||
};
|
||
bool stall_fl = classifyStall(cmd_fl, fl_fb, fl_amp);
|
||
bool stall_fr = classifyStall(cmd_fr, fr_fb, fr_amp);
|
||
bool stall_rl = classifyStall(cmd_rl, rl_fb, rl_amp);
|
||
bool stall_rr = classifyStall(cmd_rr, rr_fb, rr_amp);
|
||
|
||
bool air_now_fl = false, air_now_rl = false;
|
||
bool air_now_fr = false, air_now_rr = false;
|
||
auto classifyAirPair = [&](float cmd_rpm, float amp_a, float amp_b,
|
||
bool &air_a, bool &air_b) {
|
||
if (std::abs(cmd_rpm) < min_active_rpm)
|
||
return; // 정지/저속 중이면 판정 보류
|
||
float a = std::abs(amp_a), b = std::abs(amp_b);
|
||
float hi = std::max(a, b);
|
||
if (hi < 0.5f)
|
||
return; // 둘 다 거의 무전류(관성 주행 등)면 판정 보류
|
||
if (a < hi * airborne_current_ratio)
|
||
air_a = true;
|
||
if (b < hi * airborne_current_ratio)
|
||
air_b = true;
|
||
};
|
||
classifyAirPair(cmd_fl, fl_amp, rl_amp, air_now_fl, air_now_rl);
|
||
classifyAirPair(cmd_fr, fr_amp, rr_amp, air_now_fr, air_now_rr);
|
||
|
||
// 디바운스: 노이즈성 순간 전류 편차로 목표속도가 즉시 0으로 깎이지 않도록
|
||
// 연속 airborne_debounce_ticks 틱 이상 지속될 때만 실제로 개입한다.
|
||
air_count_fl = air_now_fl ? air_count_fl + 1 : 0;
|
||
air_count_fr = air_now_fr ? air_count_fr + 1 : 0;
|
||
air_count_rl = air_now_rl ? air_count_rl + 1 : 0;
|
||
air_count_rr = air_now_rr ? air_count_rr + 1 : 0;
|
||
|
||
bool prev_airborne_fl = airborne_fl;
|
||
bool prev_airborne_fr = airborne_fr;
|
||
bool prev_airborne_rl = airborne_rl;
|
||
bool prev_airborne_rr = airborne_rr;
|
||
|
||
airborne_fl = air_count_fl >= airborne_debounce_ticks;
|
||
airborne_fr = air_count_fr >= airborne_debounce_ticks;
|
||
airborne_rl = air_count_rl >= airborne_debounce_ticks;
|
||
airborne_rr = air_count_rr >= airborne_debounce_ticks;
|
||
|
||
// 방금 착지(뜬 상태 -> 정상)한 바퀴는 다음 틱에 cmd를 target으로 즉시
|
||
// 스냅해 재가속 지연을 없앤다.
|
||
if (prev_airborne_fl && !airborne_fl)
|
||
just_landed_fl = true;
|
||
if (prev_airborne_fr && !airborne_fr)
|
||
just_landed_fr = true;
|
||
if (prev_airborne_rl && !airborne_rl)
|
||
just_landed_rl = true;
|
||
if (prev_airborne_rr && !airborne_rr)
|
||
just_landed_rr = true;
|
||
|
||
auto statusChar = [](bool air, bool stall) {
|
||
return air ? 'A' : (stall ? 'S' : 'O');
|
||
};
|
||
wheel_status_str[0] = statusChar(airborne_fl, stall_fl);
|
||
wheel_status_str[1] = statusChar(airborne_fr, stall_fr);
|
||
wheel_status_str[2] = statusChar(airborne_rl, stall_rl);
|
||
wheel_status_str[3] = statusChar(airborne_rr, stall_rr);
|
||
|
||
float meters_per_tick = (2.0f * PI_VAL * wheel_radius) / 16384.0f;
|
||
if (!trip_initialized && (fl_tick != 0 || fr_tick != 0)) {
|
||
start_fl = fl_tick;
|
||
start_fr = fr_tick;
|
||
start_rl = rl_tick;
|
||
start_rr = rr_tick;
|
||
trip_initialized = true;
|
||
}
|
||
|
||
int32_t delta_fl = std::abs(fl_tick - start_fl);
|
||
int32_t delta_fr = std::abs(fr_tick - start_fr);
|
||
int32_t delta_rl = std::abs(rl_tick - start_rl);
|
||
int32_t delta_rr = std::abs(rr_tick - start_rr);
|
||
|
||
float dist_fl = delta_fl * meters_per_tick;
|
||
float dist_fr = delta_fr * meters_per_tick;
|
||
float dist_rl = delta_rl * meters_per_tick;
|
||
float dist_rr = delta_rr * meters_per_tick;
|
||
|
||
// 중앙값(4개 중 가운데 2개 평균) 기반 주행거리 산출: 웅덩이 등으로 바퀴
|
||
// 하나가 지면에서 떨어져 엔코더만 헛돌 때, 그 이상치 값이 평균(mean)을
|
||
// 오염시키지 않도록 최댓값/최솟값을 자동으로 배제함.
|
||
float dist_sorted[4] = {dist_fl, dist_fr, dist_rl, dist_rr};
|
||
std::sort(dist_sorted, dist_sorted + 4);
|
||
float dist_axle = (dist_sorted[1] + dist_sorted[2]) / 2.0f;
|
||
float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);
|
||
(void)dist_bumper;
|
||
|
||
// Phase 2 — 휠 기반 ω(엔코더 피드백) vs IMU 기반 ω(자이로) 비교 +
|
||
// x,y 오도메트리 적분. 실측 검증(실기 로그) 결과 부호 규약이 일치함:
|
||
// 우회전(+omega)일 때 wheel_omega/imu_gyro_z 둘 다 양수로 나온다.
|
||
// 이 둘의 차이(잔차)가 §6.3-4의 슬립 지표다. 여전히 제어에는 관여하지
|
||
// 않는다 — HUD/CSV 노출까지만.
|
||
auto odom_now = std::chrono::steady_clock::now();
|
||
float odom_dt = std::chrono::duration<float>(odom_now - last_odom_time).count();
|
||
last_odom_time = odom_now;
|
||
|
||
float v_l_meas = ((fl_fb + rl_fb) / 2.0f) / rpm_per_ms; // m/s
|
||
float v_r_meas = -((fr_fb + rr_fb) / 2.0f) / rpm_per_ms; // m/s (부호 규약: fr/rr는 -v_r로 지령됨)
|
||
float v_x_meas = (v_l_meas + v_r_meas) / 2.0f;
|
||
float wheel_omega = (k_skid > 0.0f)
|
||
? (v_r_meas - v_l_meas) / (2.0f * k_skid)
|
||
: 0.0f; // rad/s
|
||
float omega_residual = wheel_omega - imu_omega_rad;
|
||
|
||
// θ 적분원: wheel_omega와 원시 gyro_z의 평균(둘의 부호/크기 일치율이
|
||
// 실측상 99.3%로 높음). IMU가 끊겼을 땐 wheel_omega만으로 대체한다.
|
||
float fused_omega_rad =
|
||
imu_snap.valid ? (wheel_omega + imu_omega_rad) / 2.0f : wheel_omega;
|
||
|
||
if (imu_yaw_offset_set && odom_dt > 0.0f && odom_dt < 0.5f) {
|
||
// odom_dt>=0.5s: Fail-safe 재연결 등으로 틱이 크게 벌어진 비정상
|
||
// 구간은 위치 적분에서 제외해 순간 도약을 막는다.
|
||
fused_yaw_deg += fused_omega_rad * (180.0 / PI_VAL) * odom_dt;
|
||
while (fused_yaw_deg > 180.0) fused_yaw_deg -= 360.0;
|
||
while (fused_yaw_deg < -180.0) fused_yaw_deg += 360.0;
|
||
|
||
double theta_rad = fused_yaw_deg * (PI_VAL / 180.0);
|
||
odom_x += v_x_meas * odom_dt * std::cos(theta_rad);
|
||
odom_y += v_x_meas * odom_dt * std::sin(theta_rad);
|
||
}
|
||
|
||
if (log_stream.is_open()) {
|
||
float t_ms = std::chrono::duration<float, std::milli>(
|
||
std::chrono::steady_clock::now() - log_start_time)
|
||
.count();
|
||
log_stream << t_ms << ',' << state << ',' << raw_steer << ','
|
||
<< raw_throttle << ',' << (rc_ok ? 1 : 0) << ','
|
||
<< (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ','
|
||
<< 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] << ','
|
||
<< (imu_snap.valid ? 1 : 0) << ',' << imu_snap.gyro[2] << ','
|
||
<< imu_snap.angle[0] << ',' << imu_snap.angle[1] << ','
|
||
<< imu_snap.angle[2] << ',' << imu_yaw_rel << ','
|
||
<< cmd_fl << ',' << cmd_fr << ',' << cmd_rl
|
||
<< ',' << cmd_rr << ',' << fl_fb << ',' << fr_fb << ','
|
||
<< rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp
|
||
<< ',' << rl_amp << ',' << rr_amp
|
||
<< ',' << fl_temp << ',' << fr_temp << ',' << rl_temp << ','
|
||
<< rr_temp << ',' << front_driver_temp_c << ','
|
||
<< rear_driver_temp_c << ',' << wheel_status_str[0]
|
||
<< ',' << wheel_status_str[1] << ',' << wheel_status_str[2]
|
||
<< ',' << wheel_status_str[3] << ',' << err_f_l << ','
|
||
<< err_f_r << ',' << err_r_l << ',' << err_r_r << ','
|
||
<< fl_tick << ',' << fr_tick << ',' << rl_tick << ',' << rr_tick
|
||
<< ',' << wheel_omega << ',' << imu_omega_rad << ','
|
||
<< omega_residual << ',' << fused_yaw_deg << ','
|
||
<< odom_x << ',' << odom_y << ','
|
||
<< (heading_hold_condition ? 1 : 0) << ',' << heading_target_deg
|
||
<< ',' << heading_trim_omega << ','
|
||
<< (whole_slip_active ? 1 : 0) << ','
|
||
<< dist_axle << ',' << comm_ms << '\n';
|
||
}
|
||
|
||
// 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 | IMU:%s GZ:%+6.2f YAW(imu):%+6.1f | "
|
||
"WO:%+5.2f IO:%+5.2f YAW(fus):%+6.1f ODO:(%+5.2f,%+5.2f) | "
|
||
"HH:%s TRG:%+6.1f TRM:%+5.2f%s | "
|
||
"AXLE:%5.3fm | %-28s | "
|
||
"I(A):FL%+4.1f FR%+4.1f RL%+4.1f RR%+4.1f | "
|
||
"T(C):FL%3d FR%3d RL%3d RR%3d DRV%4.1f/%4.1f%s | "
|
||
"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,
|
||
imu_snap.valid ? "OK " : "LOST", imu_snap.gyro[2], imu_yaw_rel,
|
||
wheel_omega, imu_omega_rad, fused_yaw_deg, odom_x, odom_y,
|
||
heading_hold_condition ? "ON " : "off", heading_target_deg,
|
||
heading_trim_omega, whole_slip_active ? " [!!전신슬립!!]" : "",
|
||
dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
|
||
rl_amp, rr_amp, fl_temp, fr_temp, rl_temp, rr_temp,
|
||
front_driver_temp_c, rear_driver_temp_c,
|
||
(fl_temp >= kOverheatWarnC || fr_temp >= kOverheatWarnC ||
|
||
rl_temp >= kOverheatWarnC || rr_temp >= kOverheatWarnC ||
|
||
front_driver_temp_c >= kOverheatWarnC ||
|
||
rear_driver_temp_c >= kOverheatWarnC)
|
||
? " [!!고온!!]"
|
||
: "",
|
||
wheel_status_str, fault_active ? " [!!알람!!]" : "", comm_ms);
|
||
fflush(stdout);
|
||
|
||
std::this_thread::sleep_for(
|
||
std::chrono::microseconds(static_cast<int>(dt * 1000000)));
|
||
}
|
||
|
||
std::cout << "\n\n[정보] C++ 4WD 안전 정지 및 브레이크 잠금 처리 중...\n";
|
||
driver_front.setRPMs(0, 0);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setRPMs(0, 0);
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
|
||
driver_front.setBrakes(true);
|
||
if (driver_rear_ptr)
|
||
driver_rear_ptr->setBrakes(true);
|
||
|
||
if (use_lidar)
|
||
lidar_detector.stop();
|
||
|
||
if (log_stream.is_open()) {
|
||
log_stream.close();
|
||
std::cout << "[정보] CSV 로그 저장 완료: " << log_path << "\n";
|
||
}
|
||
|
||
rc.stop();
|
||
if (use_imu)
|
||
imu.stop();
|
||
|
||
std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
|
||
return 0;
|
||
} |