이륙하기(MAV_CMD_NAV_TAKEOFF)

저작권: 쿼드(QUAD) 드론연구소 https://www.youtube.com/@quad-robotics

이번 시간에는 COMMAND_LONG 메시지를 이용하여 드론을 이륙하는 MAV_CMD 명령에 대해 알아 보겠습니다.

드론을 이륙시키기 위해서는 MAV_CMD_NAV_TAKEOFF (22 ) 명령을 사용 합니다.

MAV_CMD_NAV_TAKEOFF 전문

파라메터	설명	단위
1: Pitch	Minimum pitch (if airspeed sensor present), desired pitch without sensor	deg
2	Empty
3	Empty
4: Yaw	Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).	deg
5: Latitude	Latitude
6: Longitude	Longitude
7: Altitude	Altitude	m

비행 모드 변경

이륙에 앞서 비행모드를 수동 모드가 아닌 GUIDED 모드로 변경 하여야 합니다.

비행 모드 변경 역시 MAV_CMD의 MAV_CMD_DO_SET_MODE (176 ) 명령을 사용하여 변경 합니다.

FLTMODE1: Flight Mode 1의 값을 4(GUIDED)로 주어야 합니다.

# change MODE to GUIDED into COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 
                                     4, 0, 0, 0, 0, 0)

이륙 명령

# Takeoff COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 10)

이전 시간에 작성한 arm.py 프로그램에 위의 코드를 추가하고 takeoff.py 이름으로 저장한후 실행해 보겠습니다.

from pymavlink import mavutil
import time

# Start a connection listening on a UDP port
# UDP 14550 for primary GSC
#the_connection = mavutil.mavlink_connection('udpin:localhost:14550')
#for ArduPilot
#the_connection = mavutil.mavlink_connection('tcp:localhost:5763')
#for PX4-SITL
the_connection = mavutil.mavlink_connection('udp:localhost:14540')

# Wait for the first heartbeat 
#   This sets the system and component ID of remote system for the link
the_connection.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (the_connection.target_system, the_connection.target_component))

# change MODE to GUIDED into COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 
                                     4, 0, 0, 0, 0, 0)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

# Arming COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)
# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

time.sleep(3)

target_altitude = 10

# Takeoff COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, target_altitude)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

실행결과는 다음과 같습니다.

$ python3 takeoff.py 
Heartbeat from system (system 1 component 0)
COMMAND_ACK {command : 176, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
COMMAND_ACK {command : 400, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
COMMAND_ACK {command : 22, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}

드론의 비행 모드가 GUIDED 모드로 변경후 정상적으로 10m 고도로이륙한 것을 볼 수 있습니다.

[참고]PX4 플라이트 모드 종류 및 설정

상수값 참고

Manual: base_mode:217, main_mode:1, sub_mode:0
Stabilized: base_mode:209, main_mode:7, sub_mode:0
Acro: base_mode:209, main_mode:5, sub_mode:0
Rattitude: base_mode:193, main_mode:8, sub_mode:0
Altitude: base_mode:193, main_mode:2, sub_mode:0
Offboard: base_mode:209, main_mode:6, sub_mode:0
Position: base_mode:209, main_mode:3, sub_mode:0
Hold: base_mode:217, main_mode:4, sub_mode:3
Missition: base_mode:157, main_mode:4, sub_mode:4
Return: base_mode:157, main_mode:4, sub_mode:5
Follow me: base_mode:157, main_mode:4, sub_mode:8

아래는기체가 일정 고도에 도달했는지 확인하는 방법 입니다.

# 기체의 최대 고도(meter)
target_altitude = 10

# 최대 고도에 도달할 때까지 반복
while True:
    # MSG_ID_GLOBAL_POSITION_INT (33)로부터 GPS 고도 정보 수신
    msg = master.recv_match(type='GLOBAL_POSITION_INT', blocking=True)

    # GPS_AP보다 GLOBAL_POSITION_INT를 이용하는 것을 추천
    # msg = master.recv_match(type='ALTITUDE', blocking=True)

    if not msg:
        continue

    # 기체의 현재 고도(meter)
    current_altitude = msg.relative_alt / 1000.0 # 1cm 단위이므로 1000으로 나누어 meter로 변환

    # 현재 고도 출력
    print(f"Altitude: {current_altitude:.2f}m")

    # 최대 고도 도달 확인
    if current_altitude >= target_altitude:
        print(f"Reached target altitude of {target_altitude}m")
        break

    time.sleep(1)

[참고] PX4 Offboard 플라이트 모드 설정

param1: VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED (1, see commander/commander.cpp)
param2: PX4_CUSTOM_MAIN_MODE_OFFBOARD (6, see commander/px4_custom_mode.h)