28.7.2.3. `update()` 내 핵심 수학적 궤적 생성 로직 작성 및 `_trajectory_setpoint` 버퍼 출력 바인딩

28.7.2.3. update() 내 핵심 수학적 궤적 생성 로직 작성 및 _trajectory_setpoint 버퍼 출력 바인딩

FlightTaskCustom 클래스의 모든 준비 작업(초기화 및 센서 폴링)이 끝났다면, 이제 PX4 커스텀 비행 모드의 진정한 존재 의의라 할 수 있는 update() 가상 함수 내부를 구현할 차례다.

이 함수는 PX4 시스템의 심장 박동(통상 비행 로직 루프인 250Hz)에 맞춰 1초에 수백 번씩 메인 스레드(Main Thread)의 시간 조각을 할당받아 실행된다. 본 절에서는 이 고주파 루프 안에서 지켜야 할 제어 공학적 코딩 패턴과, 만들어낸 수학적 결과물을 코어 제어기(mc_pos_control)로 넘겨주는 최종 관문인 _trajectory_setpoint 버퍼의 바인딩 규칙을 다룬다.

1. update() 함수의 제어 공학적 구현 패턴

비행 제어 로직을 짤 때 평범한 소프트웨어 엔지니어링과 가장 크게 다른 점은, 루프 안에서 과거의 데이터를 기억(Stateful)하여 적분(Integral)이나 미분(Derivative) 등의 시간 오프셋 연산을 직접 수행해야 할 때가 많다는 점이다.

1.1 델타 타임(\Delta t) 기반의 상태 적분 로직 구현 방안

PX4의 루프 타임은 대체로 일정하지만, OS 스케줄링에 따라 미세한 지터(Jitter)가 발생한다. 따라서 속도를 적분하여 위치를 구하거나, 시간에 따른 감쇠(Decay) 필터를 짤 때는 항상 이번 루프와 지난 루프 사이의 실제 측정된 시간 차이 달타 초(\Delta t)를 사용해야 한다.
FlightTask 부모 클래스는 이 귀중한 \Delta t 값을 _deltatime이라는 보호된(Protected) 실수(float) 변수로 실시간 갱신해 주고 있다.

bool FlightTaskCustom::update()
{
	// 1. 매 루프 시작 시 반드시 궤적 버퍼를 빈 상태로 초기화한다.
	_trajectory_setpoint.clear();

	// 2. 외부 조종기 조작량(스틱 입력값: -1.0 ~ 1.0)을 물리 속도로 변환 (m/s)
	// 예: 최대 속도가 5m/s 인 커스텀 모드
	const float target_speed_x = _stick_input_x * 5.0f;
    
	// 3. 제어 모델링(물리 연산): 로우패스 펄터(Low-pass Filter) 적용 예시
    // _custom_filter_state는 헤더파일에 정의된 이전 루프의 속도 저장용 멤버 변수
	const float alpha = 0.1f; // 필터 시상수(Time Constant) 비례 계수
	_custom_filter_state = _custom_filter_state * (1.0f - alpha) + (target_speed_x * alpha);

	// 4. [선택사항] 델타타임(_deltatime)을 이용한 가상 위치(Virtual Position) 추종 생성
	// 매 루프마다 현재 목표 속도에 시간을 곱해 가상의 참조 위치(Reference Position)를 전진시킴
	// _target_position(0) += _custom_filter_state * _deltatime;

    // ... (최종 버퍼 바인딩으로 이어짐) ...
}

2. _trajectory_setpoint 버퍼 출력 바인딩 규칙

물리적 연산이 끝났다면 그 데이터를 하위 mc_pos_control이 맛깔나게 씹어 삼킬 수 있도록 예쁜 접시(_trajectory_setpoint 구조체)에 담아 주어야 한다. 이 바인딩 과정에서 가장 잦은 치명적 에러가 발생한다.

2.1 NAN(Not A Number) 프로토콜의 엄격한 준수

메모장을 비우는 _trajectory_setpoint.clear() 함수가 실행되면, 구조체 안의 모든 위치(Position), 속도(Velocity), 가속도(Acceleration), 저크(Jerk) 배열 공간은 숫자 형태를 띠지 않는 쓰레기값, 즉 NAN(Not a Number) 으로 가득 채워진다.

PX4 코어 제어기의 원칙은 다음과 같다.
“버퍼에 값이 들어오면 추종하고, NAN 이면 해당 계층의 제어 연산은 무시(Bypass)한다.”

따라서 개발자는 자신이 명확하게 책임질 수 있는 차원 및 미분 계층에만 실수(Float) 값을 덮어써야 한다.
예를 들어 XY 평면은 위치(Position)로 고정하여 호버링시키고, Z축 고도는 조종사의 스틱에 비례하여 속도(Velocity)로 승강시키는 로직이라면 제어 계층을 세밀하게 분리해서 박아넣어야 한다.

    // 5. 궤적 세트포인트(_trajectory_setpoint) 바인딩 
    
    // (A) XY 평면 제어: 위치(Position) 고정 모드
    // 앞선 로직에서 보관해둔 _target_position을 대입. 
    // 이때 XY 평면 속도/가속도/저크는 기본값인 NAN으로 남겨둔다.
    _trajectory_setpoint.position[0] = _target_position(0);
    _trajectory_setpoint.position[1] = _target_position(1);
    
    // (B) Z 고도 제어: 속도(Velocity) 제어 모드
    // 예를 들어 상승/하강 속도를 -2.0m/s로 지시하고 싶다면:
    // Z축 위치값은 덮어쓰지 않고 NAN으로 둔 상태에서, Z 속도 배열에만 값을 넣는다.
    _trajectory_setpoint.velocity[2] = -2.0f; 
    
    // (C) 헤딩(Yaw) 제어: 각속도 제어
    // 기체의 머리 방향을 바꾸는 속도를 Radian/s 로 지시
    _trajectory_setpoint.yawspeed = 0.5f; // 초당 0.5라디안 선회

	// 모든 바인딩이 안전하게 끝났음을 알리며 루프 정상 종료
	return true; 
}

2.2 가속도 및 저크(Jerk) 바인딩을 이용한 궤적 스무딩(Smoothing)

커스텀 모드의 비행 품질(Flight Quality)을 ArduPilot 이상으로 매끄럽게(Premium) 만들려면 위치와 속도뿐만 아니라 가속도(Acceleration)나 저크(Jerk, 가속도의 미분) 배터까지 계산하여 밀어 넣는 편이 좋다.
mc_pos_control은 다단 제어 루프를 돌기 때문에 피드포워드(Feedforward) 값을 가속도 버퍼(_trajectory_setpoint.acceleration[i])에 실어주면 모터가 목표를 향해 치고 나가는 반응성(Latency)이 비약적으로 향상되고, 물리적인 기체의 충격을 방지할 수 있다.