27.2.2.3. 센서 폴링 레이트 통제를 위한 구독 콜백(SubscriptionCallback) 활용

27.2.2.3. 센서 폴링 레이트 통제를 위한 구독 콜백(SubscriptionCallback) 활용

이전 절에서 EKF2 모듈이 WorkItem 스케줄러를 통해 이벤트 드리븐(Event-driven) 방식으로 깨어난다는 사실을 확인했다. 그렇다면 구체적으로 ’어떤 이벤트’가 ’어떤 주기’로 이 거대한 EKF 클래스의 루프를 트리거(Trigger)하는 것일까?

이러한 고주파 센서 갱신 이벤트의 심장 박동(Heartbeat)을 조율하는 핵심 uORB 추상화 클래스가 바로 uORB::SubscriptionCallback 이다. 본 절에서는 EKF2 래퍼 모듈이 서로 다른 주기를 가진 동기/비동기 센서들을 어떻게 이 콜백 큐스레드 하나로 통제하는지 분석한다.

1. 프라이머리 센서(Primary Sensor)와 EKF 루프 레이트

24상태 확장 칼만 필터(EKF)의 수학 공식은 ’가속도(Accelerometer)’와 ‘자이로(Gyroscope)’ 값을 적분하여 시스템 상태 행렬(State Matrix)을 전파(Propagation)하는 예측(Prediction) 단계를 가장 높은 빈도로 수행한다.

따라서 PX4 EKF2 모듈의 스케줄링 레이트(Loop Rate)는 전적으로 이 IMU 데이터의 발행 주기에 종속되어야 한다. 이를 위해 EKF2 클래스는 수많은 센서 То픽들 중에서 오직 하나의 마스터 토픽에만 SubscriptionCallback을 걸어 둔다.

// src/modules/ekf2/EKF2.hpp 의 구독 콜백 선언 부위
#include <uORB/SubscriptionCallback.hpp>

class EKF2 : public px4::WorkItem {
    // ...
private:
    // EKF2 모듈을 깨우는 유일한 알람시계 역할 (프라이머리 센서)
    uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
    
    // 일반적인 비동기(Asynchronous) 구독 토픽들 (스케줄러를 깨우지 않음)
    uORB::SubscriptionInterval _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
    uORB::SubscriptionInterval _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
};

1.1 SubscriptionCallbackWorkItem 의 작동 원리

위 코드의 물리적 의미는 명확하다. 오직 sensor_selection(또는 최신 PX4의 vehicle_imu) 토픽 배열에 새로운 자이로/가속도 데이터가 uORB 네트워크를 타고 도달했을 때만, 커널 스케줄러가 EKF2의 Run() 콜백을 호출해 동면(Sleep)을 깬다.

이 프라이머리 센서의 주기가 250Hz라면 EKF2 루프도 250Hz로 돌아가고, 400Hz라면 400Hz로 돌아가게 된다.

2. 폴링 레이트(Polling Rate) 통제와 SubscriptionInterval

모든 센서가 400Hz로 들어온다면 편하겠지만, 실제 드론의 센서 생태계는 매우 비동기적(Asynchronous)이다. GPS는 보통 5~10Hz로 매우 느리게 들어오고, 기압계는 50Hz, 지자기 센서는 20Hz마다 들어온다.

2.1 EKF 업데이트 병목 방지

만약 이 모든 비동기 센서 토픽마다 콜백 트리거(Callback Trigger)를 걸어둔다면 어떻게 될까?
EKF 스레드는 IMU가 들어오지 않았는데도 GPS 때문에 깨어나고, 기압계 때문에 깨어나는 식의 불필요한 공회전(Busy Waiting)을 반복하게 되어 CPU 점유율이 폭발적으로 상승할 것이다.

2.2 수동적 구독(Passive Subscription) 패턴

이를 방지하기 위해 EKF2 래퍼는 IMU 이외의 모든 보조 센서 토픽(GPS, Baro, Mag, Vision)을 uORB::SubscriptionInterval 객체로 선언해 둔다. 이 객체는 토픽이 갱신되어도 절대 스케줄러를 깨우지 않는다.

대신, EKF2의 Run() 루프가 IMU 데이터에 의해 어차피 깨어났을 때(초당 수백 번), 아주 가벼운 updated() 불리언(Boolean) 검사 함수를 호출하여 “혹시 내가 잠든 지난 스텝 사이에 GPS 새 데이터 가 들어온게 있니?” 라고 폴링(Polling)만 수행한다.

// EKF2::Run() 내부의 센서 수집 및 퓨전 플로우
void EKF2::Run() {
    // 1. 트리거가 된 IMU 데이터 수집 (필수)
    imuSample imu;
    if (_sensor_selection_sub.update(&imu)) {
        _ekf.setIMUData(imu);
    }

    // 2. 비동기 센서 데이터 수동적 확인 (선택적)
    gpsSample gps;
    if (_vehicle_gps_position_sub.updated()) {  // 새 게 들어왔는지 불리언 판별
        _vehicle_gps_position_sub.copy(&gps);
        _ekf.setGpsData(gps);
    }
    
    // 3. 필터 연산 수행 
    _ekf.update(); 
}

3. 요약: 조화로운 다중 레이트(Multi-Rate) 시스템의 구현

결론적으로 PX4의 EKF2 래퍼 클래스는 지휘자(IMU) 에 맞춰 오케스트라의 박자를 고정하고, 각 기기(GPS, Mag, Baro) 의 비동기적 연주 데이터는 버퍼에 담아두었다가 지휘봉 패턴 루프에 맞춰 끄집어내는 정교한 다중 레이트 스케줄링 구조를 가지고 있다.

이러한 SubscriptionCallback 기반의 프라이머리 락(Primary Lock) 메커니즘을 통해, EKF 필터는 자신의 위치 추정 적분기(Integrator) 스텝을 하드웨어 IMU 클럭에 오차 없이 일치시키면서도, GPS 같은 지연(Delay)이 큰 센서는 언제든 여유롭게 융합할 수 있는 소프트웨어적 아키텍처 토대를 완성했다.

다음 절에서는 이렇게 수집된 다양한 포맷의 센서 uORB 메시지들이 C++ 수학 라이브러리에 주입되기 직전에 어떠한 직렬화(Serialization) 변환 과정을 거치는지 그 구체적인 브릿지(Bridge) 메커니즘을 확인하겠다.