28.7.2. 2단계: `FlightTaskCustom` 파생 클래스 템플릿 작성 및 물리 모델링

PX4-Autopilot의 커스텀 비행 모드 확장 파이프라인 중 두 번째 단계는 물리 제어 로직의 뇌(Brain) 를 깎아내는 과정이다. 앞선 1단계에서 시스템에게 새로운 NAVIGATION_STATE의 존재를 알렸다면, 이제 그 상태로 진입했을 때 기체가 물리적으로 “어디로, 어떻게” 움직여야 할지를 C++ 코드로 수학적으로 증명해 주어야 한다.

이러한 로컬 궤적 제어기(Local Trajectory Controller) 역할을 담당하는 것이 바로 FlightTask 프레임워크 이다. 본 절에서는 기존 코어 시스템(mc_pos_control 등)을 건드리지 않고, 객체 지향적 상속(Inheritance)을 통해 커스텀 비행 제어 클래스를 구축하는 표준 설계 템플릿(Template)을 다룬다.

1. `FlightTask` 아키텍처의 철학과 추상화(Abstraction)

과거의 비행 제어기 소스는 비행 상태 분기(switch-case)문 안에 수천 줄의 제어 로직이 뒤엉켜 있는 스파게티 코드(Spaghetti Code) 형태를 띠었다. 이는 모드를 하나 추가할 때마다 코어 위치 제어기가 무거워지고 버그가 전이되는 치명적인 구조적 결함을 안고 있었다.

이에 PX4 코어 팀은 위치 제어 루프(Position Control Loop)와 궤적 생성 루프(Trajectory Generation Loop)를 완벽하게 분리하는 FlightTask 패턴을 도입했다.

graph TD;
    A[조종기/GCS Setpoint 입력] --> B(FlightModeManager);
    B --> C{현재 모드(Navigation State) 파악};
    C -- 모드 A --> D[FlightTaskManual];
    C -- 모드 B --> E[FlightTaskAutoMapper];
    C -- 커스텀 모드 --> F[FlightTaskCustom (우리가 만들 뇌)];
    
    D --> G;
    E --> G;
    F --> G((_trajectory_setpoint 버퍼));
    
    G --> H[mc_pos_control (코어 위치 제어기)];
    H --> I[모터/서보 제어 신호];

비결합성(Decoupling): 개발자는 오로지 FlightTaskCustom이라는 격리된 클래스 내부에서만 코딩한다. 여기서 생성된 최종 궤적 목표값(XYZ 좌표, 속도, 가속도 배열)을 _trajectory_setpoint라는 통일된 규격의 전역 버퍼에 던져주기만 하면 끝난다.
하위 종속성 위임: 이 궤적을 받아 실제로 드론의 모터 RPM을 어떻게 쪼개서 기체를 뒤집을 것인지(PID 연산 및 Attitude Control)는 하위의 검증된 코어 스택(mc_pos_control)이 온전히 알아서 처리한다.

2. 파생 클래스(Derived Class) 템플릿의 기본 구조

새로운 커스텀 비행 Task를 만들기 위해서는 부모 클래스인 FlightTask를 그대로 상속(Inheritance)받아 껍데기(Header)와 알맹이(Implementation)를 구현해야 한다.

2.1 헤더 파일 (`FlightTaskCustom.hpp`) 템플릿

객체 지향 설계에 따라 최소한으로 덮어써야(Override) 하는 필수 가상 함수(Virtual Function) 3개부터 선언한다.

#pragma once
#include "FlightTask.hpp"

// 상위 클래스 FlightTask를 public으로 상속
class FlightTaskCustom : public FlightTask
{
public:
	FlightTaskCustom() = default;
	virtual ~FlightTaskCustom() = default;

	// 1. Task가 최초 진입할 때 1회 호출 (초기화)
	bool initialize() override;

	// 2. 매 주기(Tick, 약 250Hz)마다 호출되며, 핵심 물리 연산 수행
	bool update() override;

	// 3. Task가 종료되거나 다른 모드로 넘어갈 때 1회 호출
	void empty() override;

private:
	// 커스텀 센서 데이터 구독을 위한 uORB Subscriber 선언 예시
	// uORB::Subscription _sub_obstacle_distance{ORB_ID(obstacle_distance)};
    
    // 내부적으로 사용할 물리 변수 (속도 추종 필터 등)
    float _target_speed_z{0.f};
};

2.2 구현 파일 (`FlightTaskCustom.cpp`) 템플릿 플로우

이 클래스가 궤적 버퍼에 어떻게 값을 집어넣는지 update() 함수의 표준 구조를 확인해야 한다.

#include "FlightTaskCustom.hpp"

bool FlightTaskCustom::initialize()
{
	bool ret = FlightTask::initialize();
	// [여기에 커스텀 모드 시작 시 필요한 센서 캘리브레이션 트리거 등을 작성]
	return ret; // true를 반환해야만 Commander가 모드 스위칭을 허락함
}

bool FlightTaskCustom::update()
{
	// 1. 기존 _trajectory_setpoint 버퍼를 가장 먼저 비워준다 (초기화)
	_trajectory_setpoint.clear();

	// 2. [가장 중요한 비즈니스 로직 작성 공간]
	// 예: 라이다 센서 값에 반비례하여 Z축 속도를 깎아버리는 로직
	// float current_dist = read_lidar();
	// _target_speed_z = math::constrain((current_dist - 2.0f) * gain, -5.0f, 5.0f);

	// 3. 연산된 목표값을 _trajectory_setpoint 구조체에 명시적으로 대입
	// 주의: 지정하지 않은 배열 공간은 무한대(NAN) 처리되어 하위 PID가 무시함
	_trajectory_setpoint.velocity[2] = _target_speed_z;
    
    // XY 평면은 제자리 호버링(속도 0) 유지
    _trajectory_setpoint.velocity[0] = 0.f;
    _trajectory_setpoint.velocity[1] = 0.f;

	return true;
}

3. 물리 모델링 제약 사항 요약 (Safety Bounding)

비록 샌드박스 격리 구역에서 코딩한다 하더라도, 파생 클래스 템플릿 내에서 지켜야 할 철칙이 존재한다.

Non-Blocking 함수 체계: update() 함수는 mc_pos_control의 메인 루프에서 직접 호출되므로, while(1) { sleep() } 같은 스레드 블로킹(Blocking) 코드를 절대 삽입해서는 안 된다. 시스템 심박수(Hz)를 떨어뜨려 워치독(Watchdog)이 재부팅을 때리게 만든다. 모든 센서 데이터는 폴링(Polling)이나 비동기 콜백(Callback)으로 순식간에 읽고 넘겨야 한다.
NAN(Not a Number) 프로토콜 준수: _trajectory_setpoint.position에 값을 넣으면서 velocity에도 값을 넣으면, 펌웨어 버전에 따라 하위 계층에서 위치 PID 우선인지 속도 PID 우선인지 충돌이 발생할 수 있다. PX4의 표준 관례는, 자신이 제어하지 않을 차원(Dimension)의 변수는 반드시 명시적으로 NAN으로 비워두는 것 이다. 그래야 하위 코어 제어기가 충돌 없이 종속 제어 루프를 스킵(Skip)할 수 있다.

28.7.2. 2단계: FlightTaskCustom 파생 클래스 템플릿 작성 및 물리 모델링

1. FlightTask 아키텍처의 철학과 추상화(Abstraction)