28.7.4.1. PX4 펌웨어 내 `mavlink_receiver.cpp`의 `MAV_CMD_DO_SET_MODE` 파서(Parser) 수정 및 매핑

우리가 기체 소스 코드 내부에 기나긴 여정을 거쳐 심어놓은 커스텀 비행 모드(FlightTaskCustom)를 백날 시뮬레이터에서 쳐다본들, 외부 관제 시스템(GCS)에서 호출해 주지 않으면 평생 돌아갈 일이 없는 유령 코드가 된다.

외부에서 PX4의 비행 모드를 바꾸라고 지시하는 가장 보편적인 MAVLink 명령어는 MAV_CMD_DO_SET_MODE (명령어 ID: 176) 이다. 본 절에서는 이 MAVLink 패킷이 안테나를 타고 기체로 들어왔을 때, 이를 낚아채어 우리가 만든 내부 컴포넌트로 연결해 주는 MAVLink Receiver 모듈 수정 방법을 상세히 소개한다.

1. `mavlink_receiver.cpp` 구조 및 커맨드 패서(Command Parser)

PX4 펌웨어 내의 src/modules/mavlink/mavlink_receiver.cpp 파일은 텔레메트리(Telemetry) 포트를 통해 들어오는 모든 잡다한 데이터를 의미 있는 명령이나 파라미터로 디코딩(Decoding)하는 일종의 거대한 번역국이다.

이 파일 내부를 잘 찾아보면, COMMAND_LONG 혹은 COMMAND_INT 형태의 메시지를 까서 그 안의 command 필드 번호를 보고 스위치 분기를 태우는 MavlinkReceiver::handle_message_command_long() 류의 함수가 존재한다. (또는 모드 변경만을 전담하는 handle_command_do_set_mode() 형태의 구조체로 분리되어 있을 수도 있다.)

1.1 `MAV_CMD_DO_SET_MODE` 패킷의 구조 이해

MAVLink 공식 문서에 따르면 이 명령어는 3개의 파라미터를 갖는다.

param1 (base_mode): 기체의 큰 틀의 상태 (예: 커스텀 모드 사용 가능 MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
param2 (custom_mode): 기체별로 독자적으로 정의한 메인 비행 모드 번호 (Main mode)
param3 (custom_sub_mode): 메인 모드의 하위 모드 번호 (Sub mode)

우리는 여기서 param2 (custom_mode) 라는 32비트짜리 정수 공간 중 하나를 우리의 커스텀 모드용 고유 통신 번호로 선점(예: 25번)해야 한다.

2. 수신 파서(Parser) 내 분기(Branch) 추가 로직

수신 함수 내부로 들어가 보면 MAVLink에서 파싱한 custom_mode 번호를 PX4 내부 uORB 통신망(Commander로 보낼)의 상수(nav_state 또는 main_state 매핑 전 명령셋)로 번역해 주는 거대한 switch 문이 존재한다. 바로 이곳을 수정해야 한다.

// src/modules/mavlink/mavlink_receiver.cpp (또는 커맨드 파싱 전용 하위 파일)

void MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
{
	mavlink_command_long_t cmd_mavlink;
	mavlink_msg_command_long_decode(msg, &cmd_mavlink);

	switch (cmd_mavlink.command) {
		// ... 다른 MAV_CMD 분기들 ...

		case MAV_CMD_DO_SET_MODE: {
			// 1. Base Mode 플래그에 커스텀 모드 활성화 비트가 켜져 있는지 확인
			if (static_cast<int>(cmd_mavlink.param1) & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
				
				// 2. param2에서 추출한 숫자를 바탕으로 번역 시작
				uint32_t custom_mode = static_cast<uint32_t>(cmd_mavlink.param2);
				uint32_t custom_sub_mode = static_cast<uint32_t>(cmd_mavlink.param3);

				vehicle_command_s vcmd{};
				vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
				vcmd.param1 = (float)cmd_mavlink.param1; // base_mode 보존

				// 3. 커스텀 모드 스위치 번역 맵
				switch (custom_mode) {
					case PX4_CUSTOM_MAIN_MODE_AUTO:
						// 서브 모드에 따라 Auto (Mission, RTL 등) 세부 매핑
						// ...
						break;

					case PX4_CUSTOM_MAIN_MODE_MANUAL:
						// ...
						break;

					// [NEW] 우리가 선점한 25번이라는 패킷이 들어왔을 때의 행동
					case 25: 
						// 사전에 정의된 (혹은 새롭게 정의한) 커맨더 명령 상수로 치환한다!
						// 즉, "외부에서 25번을 불렀으니, 내부는 CUSTOM_FOO로 알아들어라."
						vcmd.param2 = (float)PX4_CUSTOM_MAIN_MODE_CUSTOM_FOO; 
						vcmd.param3 = 0.0f; // 서브 모드가 없다면 0
						break;

					default:
						PX4_WARN("mavlink: Rejecting unknown custom mode: %d", custom_mode);
						// 알 수 없는 모드면 수신 거부 로직 (ACK 전송 등)
						break;
				}

				// 4. 번역이 끝난 명령을 사내 망(uORB)의 vehicle_command 토픽으로 쏴준다.
				// 이 토픽을 Commander 모듈이 받아들고 3단계에서 구현한 유효성 검사를 시작하게 된다.
				_cmd_pub.publish(vcmd);
			}
			break;
		}
	}
}

2.1 매핑 상수의 중앙 집권화 (관례)

위 코드 예시에서 숫자 25를 하드코딩(Hardcoding)했지만, 실무적으로 이렇게 작성하면 리뷰(Review) 단계에서 반려당한다. PX4_CUSTOM_MAIN_MODE_...와 같은 매크로 상수들이 모여 있는 헤더 파일(src/modules/mavlink/mavlink_messages.hpp 등)에 PX4_CUSTOM_MAIN_MODE_CUSTOM_FOO = 25 와 같이 선언해 두고 끌어와 쓰는 것이 유지보수 측면에서 안전하다.

3. 요약: 프로토콜 경계(Boundary)로서의 역할

이 mavlink_receiver.cpp 파일의 수정은 단순히 케이스(Case) 하나를 추가하는 것 이상의 의미를 갖는다.
외부 시스템(MAVLink라는 공용어)과 내부 시스템(PX4 고유의 uORB 아키텍처)이 만나는 국경선(Border)의 여권을 발명해 내는 작업이다.

이 파서(Parser) 수정이 완벽하게 컴파일되어 펌웨어에 구워졌다면,
이제 QGroundControl 없이도 파이썬(Python)이나 ROS 환경에서 param2=25를 욱여넣은 MAV_CMD_DO_SET_MODE 명령어 한 줄만 발송하면, 드론이 우리가 심어놓은 뇌 구조(FlightTaskCustom)로 즉각 물리적 빙의(Possess)를 시작하게 될 것이다.

28.7.4.1. PX4 펌웨어 내 mavlink_receiver.cpp의 MAV_CMD_DO_SET_MODE 파서(Parser) 수정 및 매핑

1. mavlink_receiver.cpp 구조 및 커맨드 패서(Command Parser)