28.5.2.1 위치 제어기(`mc_pos_control` 등) 최상단에서 플래그를 해석하여 특정 PID 연산을 스킵(Skip)하는 최적화

수십에서 수백 헤르츠(Hz)로 동작하는 비행 제어 루프에서 불필요한 연산을 줄이는 것은 시스템의 응답성과 배터리 효율, 그리고 무엇보다도 실시간성(Real-time Constraints)을 보장하는 데 매우 중요하다. PX4-Autopilot의 멀티로터 위치 제어 모듈인 mc_pos_control 은 vehicle_control_mode 토픽의 플래그 조합을 단방향으로 전달만 하는 것이 아니라, 내부적으로 이 플래그들을 가장 먼저 해독하여 자신이 담당하는 특정 계층의 PID 연산을 과감하게 생략(Skip)해버리는 지능형 최적화(Optimization) 기법을 내장하고 있다.

본 절에서는 단일 컨트롤러 모듈 내에서 어떻게 논리적 플래그가 물리적 연산 주기를 절단(Truncate)하는지 소스 코드 레벨의 아키텍처를 분석한다.

1. 개요: `mc_pos_control` 모듈의 구조적 책임

mc_pos_control 모듈은 이름이 ’위치 제어기’임에도 불구하고, 실제로는 두 가지의 중대한 제어 계층을 동시에 내포하고 있다.

Position Controller (위치 기반 P 제어): 위치 오차를 타겟 속도 벡터로 변환
Velocity Controller (속도 기반 PID 제어): 타겟 속도를 타겟 가속도 및 자세각(Attitude) 지령으로 변환

이 두 컨트롤러 계층은 연속적으로 실행될 수도 있고, 상황에 따라 어느 하나만 실행될 수도 있다. 따라서 이 모듈의 최상단 실행 함수(Run() 또는 update())에서는 현재 요구되는 비행 모드가 무엇인지 알리는 플래그(Flag) 비트맵을 필터링하는 과정이 최우선적으로 전개된다.

2. 플래그 기반 연산 스킵(Skip) 메커니즘

2.1 Early Return에 의한 위치(Position) 연산 우회

만약 기체가 ’Altitude Mode(고도 유지)’로 비행 중이라면, 수동 스틱 입력이 자세 제어기로 직접 들어가므로 수평 위치 유지 로직은 필요가 없다. 이 상황에서 vehicle_control_mode의 flag_control_position_enabled는 false가 된다.

mc_pos_control 메인 루프의 코드는 대략 다음과 같은 절단면을 갖는다.

// 1. 위치 제어 플래그 검사
if (!_control_mode.flag_control_position_enabled) {
    // 1-a. 위치 제어가 불필요하므로 관련 P-Controller 연산을 스킵.
    // 위치 적분기(Integrator)에 쌓인 오차 상태를 초기화(Reset)하여 
    // 나중에 다시 켜졌을 때 급가속(Bump)되는 것을 막는다.
    _control.resetPosition();
    
} else {
    // 1-b. 플래그가 참일 때만 삼각함수와 행렬 연산이 섞인 
    // 막대한 위치 비례 제어 루틴을 수행한다.
    _control.updatePositionController();
}

위 로직 덕분에, CPU는 updatePositionController() 내부에 존재하는 수많은 로컬 좌표 변환 연산 블록을 단 한 줄의 if 분기로 완전히 건너뛰어(Skip) 명령 주기를 절약하게 된다.

2.2 독립적 속도(Velocity) 연산 진입 허가

위치 연산 로직을 스킵하고 난 뒤, 코드는 바로 이어지는 속도 제어 루프 플래그를 검사한다.
Position이 꺼져있어도 Velocity 플래그는 단독으로 켜져 있을 수 있기 때문(예: 외부 오프보드 속도 제어 모드)이다.

// 2. 속도 제어 플래그 검사
if (_control_mode.flag_control_velocity_enabled) {
    // 2-a. 속도 제어 루프 실행 (PID 및 저크 제한 알고리즘 포함)
    _control.updateVelocityController();
    
    // 2-b. 생성된 가속도 벡터를 기체 기울기 각도(Attitude Setpoint)로 역산 변환
    publish_attitude_setpoint();
    
} else {
    // 속도 플래그마저 꺼져있다면, PID 적분기 리셋 및 Idle 대기
    _control.resetVelocity();
}

이처럼 하나의 실행 루프(Main Task Loop)를 구성하는 각 수학적 서브 블록 상단에 플래그 문지기(Gate Check)를 세워둠으로써, 모듈 하나로 여러 가지의 스위칭 상태 기계(State Machine)를 자유롭게 넘나들 수 있는 효율성이 확보된다.

3. 적분기 안티 와인드업(Anti-Windup)과 범프리스(Bumpless) 트랜스퍼 처리

단순히 if 문으로 계산만 건너뛰게 만들면 부작용이 발생한다. 제어 루프가 스킵된 상태에서 기체가 강한 바람을 맞아 밀려났다고 상상해보자. 속도 제어 알고리즘의 적분기(I-Term, $\int e dt$ )는 꺼져있는 동안 오차가 무한히 누적되어(Windup), 플래그가 다시 켜지는 그 즉시 기체를 미친 듯이 뒤집어 버릴 수 있다.

때문에 “스킵(Skip)” 동작의 본질은 “아무것도 안 함“이 아니라 “상태 초기화 및 섀도우 트래킹(Shadow Tracking)” 이다.

리셋 로직 (resetPosition(), resetVelocity() 호출):
제어기가 우회될 때마다 호출되는 리셋 함수 내부에서는 PID 적분치( $I$ )와 미분치( $D$ )를 강제로 0으로 소거한다.
현재 상태와 목표 동기화:
플래그가 꺼져 진행되는 수동 모드 비행 중에, mc_pos_control은 계산은 스킵하지만 현재 기체의 위치와 속도를 내부 가상 타겟 (_pos_sp, _vel_sp)으로 덮어쓴다.
이후 조종사가 포지션 스위치를 켜서 플래그가 true가 되는 정확히 그 순간의 타겟과 기체의 현재 상태 간의 오차가 0인 상태로 제어 연산이 ‘소프트 록(Soft Lock)’ 모드로 부드럽게 재개된다. 이를 제어 공학에서 범프리스 트랜스퍼(Bumpless Transfer) 튜닝이라고 부른다.

4. ArduPilot과의 최적화 방식 비교 분석

PX4 (단일 프로세스 파이프라인): mc_pos_control이라는 큰 줄기 안에서 플래그 상태에 따라 필요한 수식 계산만 ‘켜고 끄는’ 방식으로 진행된다. C++ 코드의 if 분기 처리로 인한 브랜치 프레딕터(Branch Predictor) 비용만 조금 발생할 뿐, 메모리 캐시 최적화에 유리하다.
ArduPilot (모드별 독립 인스턴스 지향): 각 비행 모드(mode_loiter.cpp, mode_althold.cpp)가 pos_control 객체의 특정 메서드만 의도적으로 골라 호출한다. 계산을 스킵한다기보다는 해당 모드가 “내가 쓸 함수만 부른다“는 방식이므로 스킵의 분기 검사가 각 상위 도메인에 흩어져 있는 차이가 있다. 범프리스 처리는 모드 초기화 루틴(init() 함수) 단에서 이루어진다.

5. 결론: 플래그 지향 설계의 철학

PX4의 mc_pos_control 모듈 내부에 삽입된 플래그 기반 스킵 최적화 루틴은, 겉으로 보기엔 단순한 제어문(if-else)의 집합에 불과하지만, 그 이면에 깔린 철학은 “제어 연산의 생명주기(Lifecycle) 제어를 uORB 플래그 스펙트럼 하나에 완벽히 종속시키겠다” 는 결의를 담고 있다.

이 설계를 통해 PX4는 저성능 임베디드 칩셋 안에서도 무거운 삼각함수, 행렬 제어 오버헤드를 상황에 맞게 쳐내면서도(Pruning), 모드 스위칭 시점에 발생할 수 있는 동역학적 충격을 완벽히 흡수하는 안정된 다단 제어 아키텍처를 보유하게 되었다.

28.5.2.1 위치 제어기(mc_pos_control 등) 최상단에서 플래그를 해석하여 특정 PID 연산을 스킵(Skip)하는 최적화

1. 개요: mc_pos_control 모듈의 구조적 책임