28.7.2.2. initialize(), updateInitialize() 구현을 통한 커스텀 센서 데이터 수신 및 상태 검증

커스텀 FlightTask 클래스의 뼈대를 세웠다면, 이제 그 객체가 생명을 얻고 동작하기 위한 사전 준비 작업(Pre-condition Setup) 에 해당하는 함수들을 오버라이딩(Overriding)해야 한다.

PX4의 FlightTask 객체 생명주기(Lifecycle) 내에는 크게 두 가지의 초기화/검증 함수가 존재한다. 첫 번째는 모드 진입 시 단 한 번 실행되는 initialize()이며, 두 번째는 매 주기(Tick)마다 메인 로직 구동 직전에 실행되는 updateInitialize()이다. 본 절에서는 이 두 함수 내부에서 커스텀 센서 데이터를 uORB로 구독(Subscribe)하고 시스템 상태를 검증하는 패턴을 다룬다.

1. initialize(): 모드 진입점에서의 단발성 검증 및 세팅

initialize() 함수는 조종기나 GCS 명령에 의해 기체가 이 비행 모드로 갓 스위칭(Switching)되었을 때 최초 1회 단발성으로 호출되는 가상 함수다.
이 함수는 bool 값을 반환하며, 만약 false를 반환하면 Commander는 스위칭을 거부하고 기체를 안전 모드(예: Loiter 또는 Position)로 튕겨내 버린다.

따라서 커스텀 로직을 구동하기 위한 ’필수 전제 조건’이 만족되었는지를 검사하는 수문장 역할을 이곳에서 수행해야 한다.

1.1 initialize() 오버라이딩 패턴

bool FlightTaskCustom::initialize()
{
	// 1. 부모 클래스(Core)의 초기화를 먼저 반드시 수행
	bool ret = FlightTask::initialize();
	
	if (!ret) {
		return false; // 부모 초기화 실패 시 즉시 거부
	}

	// 2. 커스텀 비행을 위한 선행 조건(Pre-condition) 검사
	// 예: 로컬 위치 추정치(Local Position)가 유효한지 검사
	if (!_sub_local_position.get().xy_valid || !_sub_local_position.get().z_valid) {
		PX4_ERR("CustomTask: Local position invalid, refusing to switch!");
		return false;
	}

	// 3. 내부 상태 변수(State Variable) 초기화
	// 모드 진입 시 기체의 폭주를 막기 위해 현재 상태값을 초기 좌표로 스냅(Snap)
	_target_position = matrix::Vector3f(_sub_local_position.get().x, 
	                                    _sub_local_position.get().y, 
	                                    _sub_local_position.get().z);
	_custom_filter_state = 0.f;

	return true; // 최종 승인
}

2. updateInitialize(): 매 주기(Iterative) 센서 폴링 및 검증

모드에 성공적으로 진입했다 하더라도 안심할 수 없다. 비행 도중 센서 케이블이 뽑히거나 데이터가 멈추는 에러는 언제든 발생할 수 있다.
updateInitialize() 함수는 최종 궤적을 뿜어내는 update() 함수가 호출되기 아주 직전, 매 루프(약 250Hz, 4ms)마다 호출되어 최신 센서 데이터를 갱신하고 상태를 재검증하는 역할을 한다.

2.1 uORB 데이터 수신(Subscription) 및 복사

비행 로직 안에서 외부 센서(예: 특수 라이다, 카메라 비전 데이터 등)가 필요하다면, 이 함수 내에서 최신 uORB 메시지를 긁어와 클래스 멤버 변수에 캐싱(Caching)해 두어야 한다.

bool FlightTaskCustom::updateInitialize()
{
	// 1. 부모 클래스의 통상적인 주기적 업데이트 실행
	bool ret = FlightTask::updateInitialize();

	// 부모의 업데이트 과정에서 치명적 에러가 터졌다면 루프 스킵
	if (!ret) {
		return false;
	}

	// 2. 커스텀 센서 데이터 업데이트 (uORB Polling)
	// _sub_obstacle_distance는 헤더에 선언된 uORB::Subscription 객체
	if (_sub_obstacle_distance.updated()) {
		// 외부 데이터를 읽어와서 로컬 구조체(_obstacle_data)에 복사
		_sub_obstacle_distance.copy(&_obstacle_data);
		
		// 데이터가 도착한 타임스탬프를 기억 (타임아웃 계산 용도)
		_last_sensor_update_time = hrt_absolute_time();
	}

	// 3. 센서 타임아웃(Timeout) 및 무결성 실시간 감시 방어 로직
	// 500ms(500,000us) 이상 센서 데이터가 안 들어오면 비행 로직을 블록(Block)시킴
	const hrt_abstime time_now = hrt_absolute_time();
	if (time_now - _last_sensor_update_time > 500000) {
		_sensor_valid = false;
        // PX4_WARN은 너무 자주 쏘면 안되므로 콘솔 출력 억제 로직(Throttle) 필요
	} else {
		_sensor_valid = true;
	}

	// 4. 조종사의 스틱(RC Input) 데이터 읽기
	// 수동 개입이 필요한 커스텀 모드라면 이 시점에 맵핑된 스틱 입력을 파싱한다.
	_stick_input_x = _sticks.getPositionExpo()(0);
	_stick_input_y = _sticks.getPositionExpo()(1);

	// 여기서 true를 반환해야 비로소 update() 본연의 궤적 생성 함수로 진입함
	return true; 
}

3. 요약: 아키텍처 생명주기(Lifecycle) 분리의 아름다움

이렇듯 PX4 FlightTask 프레임워크는 초기화(initialize) \rightarrow 시스템 건전성 확보 및 센서 갱신(updateInitialize) \rightarrow 물리 궤적 연산(update) 이라는 세 파트로 강력하게 나뉘어 있다.

이는 C++ 메인 update() 루프 안에 수백 줄에 걸친 센서 널 체크(Null Check), 타임아웃 검사, if-else 블록이 섞여 들어가서 핵심 수학 공식(Math formula)이 눈에 보이지 않게 되는 대참사를 막아준다.
개발자는 updateInitialize()에서 “지금 센서 상태가 정상이니 써도 좋다(_sensor_valid == true)“고 깔끔하게 정리해 놓은 변수만 믿고, 다음 챕터에서 다룰 update() 함수 안에서는 순수하게 뉴턴 역학과 칼만 알고리즘(Algorithm)에만 집중할 수 있게 되는 것이다.