29.5.3.1. 각속도 센서 지연 최소화를 위한 고속 폴링(High-speed Polling) 및 노치 필터(Notch Filter) 연동부

아크로(Acro) 모드와 같이 기체의 한계 성능을 끌어내는 제어 환경에서는, 제어기의 명령이 모터에 도달하여 추력을 내기까지 걸리는 시간적 지연(Latency)뿐만 아니라, 자이로스코프(Gyroscope) 센서가 기체의 회전을 감지하여 제어기로 데이터를 넘겨주는 센서 데이터 파이프라인의 지연(Sensor Latency)을 극소화하는 것이 생명이다.

본 절에서는 PX4의 mc_rate_control 모듈 내에서 자이로 데이터를 어떻게 고속으로 폴링(High-speed Polling)하며, 모터 진동에 의한 고주파 악성 노이즈를 제어 지연 없이 걸러내기 위해 동적 노치 필터(Dynamic Notch Filter)를 어떻게 연동하는지 소스 코드 레벨에서 심층 분석한다.

1. uORB 폴링 설계 메커니즘

PX4의 모듈들은 uORB(Micro Object Request Broker) 메시징 시스템을 기반으로 데이터를 주고받는다. Rate Controller는 기체 제어 루프 중 가장 빠른 속도로 구동되어야 하므로 일반적인 콜백(Callback)이나 주기적 서브스크립션 방식을 넘어, 자이로스코프 드라이버가 데이터를 생산하는 즉시 스레드를 깨우는(Wake-up) 직접적인 폴링(px4_poll) 방식을 사용한다.

// src/modules/mc_rate_control/RateControlMain.cpp 의 워크 큐(Work Queue) 실행 구조

void RateControl::Run()
{
    // 자이로스코프 데이터(vehicle_angular_velocity)가 들어올 때까지 태스크 휴면(Blocking)
    // 폴링 구조를 통해 자이로 드라이버에서 인터럽트(ISR) 단계 직후에 곧바로 제어 루프를 진입시킴
    px4_pollfd_struct_t fds[1];
    fds[0].fd     = _v_angular_velocity_sub.get();
    fds[0].events = POLLIN;

    int ret = px4_poll(fds, 1, _polling_timeout_ms);

    if (ret > 0 && (fds[0].revents & POLLIN)) {
        // 드디어 최신 각속도 데이터 센싱 완료, 본 연산 진입
        vehicle_angular_velocity_s angular_velocity;
        _v_angular_velocity_sub.copy(&angular_velocity);
        
        // ... (필터 체인 및 PID 제어 업데이트 로직 호출) ...
    }
}

이 고속 폴링 루프 덕분에, ICM-20689나 BMI088과 같은 최신 IMU 센서들이 SPI 버스를 타고 1000Hz(1ms 주기) 이상의 속도로 데이터를 뽑아낼 때, CPU는 대기 시간(Overhead) 없이 센서 데이터 생성 빈도와 1:1로 동기화되어 Rate Control 연산을 수행할 수 있다.

2. 동적 노치 필터(Notch Filter)의 통합 아키텍처

기체의 프로펠러와 모터가 회전할 때 필연적으로 특정 주파수 대역(RPM 대역)에서 강한 기계적 공진(Resonance) 노이즈가 발생한다. 이는 자이로 데이터에 섞여 들어오며, 이 노이즈가 그대로 Rate Controller의 미분항(D-Term)으로 들어가게 되면 모터가 미친 듯이 떨리는 진동 발산(Oscillation Runaway)과 모터 과열 현상을 유발시킨다.

PX4에서는 이러한 현상을 억제하면서도 제어 지연을 막기 위해 노치 필터(Notch Filter) 를 제어 스택 초입부에 연동시켰다. 단순 저주파 통과 필터(Low-Pass Filter)는 컷오프(Cut-off) 주파수를 너무 낮추면 물리적인 신호 지연(Phase Shift)이 심해져 아크로 성능이 급락하지만, 노치 필터는 문제가 되는 특정 주파수(예: 150Hz) 부근의 치명적인 노이즈만 아주 좁은 대역폭(Q-factor)으로 도려내기 때문에 원본 센서의 위상(Phase) 지연을 최소화한다.

// 필터 적용 메커니즘 예시 코드

void RateControl::update_filters(const vehicle_angular_velocity_s &rates)
{
    // RPM 센서나 D-shot 텔레메트리 등에서 측정한 모터 회전 주파수를 기반으로
    // 노치 필터의 중심 주파수(Center Frequency)를 동적으로 업데이트
    if (_esc_status_sub.updated()) {
        float dynamic_freq = calculate_dominant_rpm_frequency();
        _notch_filter.setParameters(dynamic_freq, _notch_bandwidth);
    }

    // 자이로 원시 데이터에서 모터 공진 주파수만 도려내는 필터링 연산 수행
    _rates_filtered.xyz(0) = _notch_filter.apply(rates.xyz[0]);
    _rates_filtered.xyz(1) = _notch_filter.apply(rates.xyz[1]);
    _rates_filtered.xyz(2) = _notch_filter.apply(rates.xyz[2]);
}

위의 로직을 통해 구현되는 동적 노치 필터(Dynamic Notch Filter) 체계는 Ardupilot에서 ’Harmonic Notch Filter’라 부르는 체계와 궤를 같이한다. PX4는 CAN 기반의 D-Shot 텔레메트리나 ESC 피드백을 통해 실시간으로 변하는 모터의 RPM 1차 트렌드와 배파(Harmonic) 주파수를 계산하고, 이 값을 uORB (esc_status) 토픽을 통해 받아들여 노치 필터의 핀포인트(Pinpoint) 주파수 타겟을 끊임없이 재조정한다.

3. 시스템 최적화 결론

결론적으로, PX4의 Rate Control 루프는 ’px4_poll’을 통한 이벤트 록스텝(Event Lock-step) 1:1 동기화 메커니즘 으로 물리적 버스 레이턴시를 삭제하고, 동적 노치 필터 를 통해 신호 왜곡 및 위상 지연 없이 악성 모터 노이즈를 제거해낸다. 이 두 시스템의 완벽한 앙상블 덕분에 조종사는 아크로 모드에서 D 게인을 극한까지 높일 수 있으며, 스틱을 조작하는 순간 기척의 지연 없이 기동하는 이른바 “락-인(Locked-In)” 감각을 온전히 느낄 수 있게 된다.