라즈베리파이로 드론 조종하기

이전 글에서 라즈베리파이와 픽스호크를 연결하는 법에 대해서 다루었다. 그렇다면 이번엔 라즈베리 파이를 통해서 어떠한 방식으로 실질적으로 드론을 조종할 수 있는지 다뤄보겠다.

 

 

https://github.com/dronekit/dronekit-python

GitHub - dronekit/dronekit-python: DroneKit-Python library for communicating with Drones via MAVLink.

 

 

전체적인 예제 코드는 위 git dronekit 공식 문서에서 따왔다.

 

 

우선 라즈베리파이로 드론을 제어하기 위해서 dronekit 라이브러리를 활용해서 다룰 것이다.

 

 

해당 글을 읽기전이나 읽은 후에 위 git example 코드를 잘 해석해 보면 이런 저런 코드를 다룰 수 있을 것이다.

 

 

from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative
from pymavlink import mavutil # Needed for command message definitions
import time
import math

# Set up option parsing to get connection string
import argparse






def connectMyCopter():
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()
    
    connection_string = args.connect
    baud_rate = 921600
    
    vehicle = connect(connection_string,baud=baud_rate,wait_ready=True)
    return vehicle

vehicle = connectMyCopter()


# Start SITL if no connection string specified


def arm_and_takeoff_nogps(aTargetAltitude):
    

    ##### CONSTANTS #####
    DEFAULT_TAKEOFF_THRUST = 0.7
    SMOOTH_TAKEOFF_THRUST = 0.6

    print("Basic pre-arm checks")
    # Don't let the user try to arm until autopilot is ready
    # If you need to disable the arming check,
    # just comment it with your own responsibility.
    #while vehicle.is_armable:
     #   print(" Waiting for vehicle to initialise...")
      #  time.sleep(1)


    print("Arming motors")
    # Copter should arm in GUIDED_NOGPS mode
    vehicle.mode = VehicleMode("GUIDED_NOGPS")
    vehicle.armed = True

    while not vehicle.armed:
        print(" Waiting for arming...")
        vehicle.armed = True
        time.sleep(1)

    print("Taking off!")

    thrust = DEFAULT_TAKEOFF_THRUST
    while True:
        current_altitude = vehicle.location.global_relative_frame.alt
        print(" Altitude: %f  Desired: %f" %
              (current_altitude, aTargetAltitude))
        print(vehicle.mode)
        if current_altitude >= aTargetAltitude*0.95: # Trigger just below target alt.
            print("Reached target altitude")
            break
        elif current_altitude >= aTargetAltitude*0.6:
            thrust = SMOOTH_TAKEOFF_THRUST
        set_attitude(thrust = thrust)
        time.sleep(0.2)

def send_attitude_target(roll_angle = 0.0, pitch_angle = 0.0,
                         yaw_angle = None, yaw_rate = 0.0, use_yaw_rate = False,
                         thrust = 0.5):
    
    if yaw_angle is None:
        # this value may be unused by the vehicle, depending on use_yaw_rate
        yaw_angle = vehicle.attitude.yaw
    # Thrust >  0.5: Ascend
    # Thrust == 0.5: Hold the altitude
    # Thrust <  0.5: Descend
    msg = vehicle.message_factory.set_attitude_target_encode(
        0, # time_boot_ms
        1, # Target system
        1, # Target component
        0b00000000 if use_yaw_rate else 0b00000100,
        to_quaternion(roll_angle, pitch_angle, yaw_angle), # Quaternion
        0, # Body roll rate in radian
        0, # Body pitch rate in radian
        math.radians(yaw_rate), # Body yaw rate in radian/second
        thrust  # Thrust
    )
    vehicle.send_mavlink(msg)

def set_attitude(roll_angle = 0.0, pitch_angle = 0.0,
                 yaw_angle = None, yaw_rate = 0.0, use_yaw_rate = False,
                 thrust = 0.5, duration = 0):
   
    send_attitude_target(roll_angle, pitch_angle,
                         yaw_angle, yaw_rate, False,
                         thrust)
    start = time.time()
    while time.time() - start < duration:
        send_attitude_target(roll_angle, pitch_angle,
                             yaw_angle, yaw_rate, False,
                             thrust)
        time.sleep(0.1)
    # Reset attitude, or it will persist for 1s more due to the timeout
    send_attitude_target(0, 0,
                         0, 0, True,
                         thrust)

def to_quaternion(roll = 0.0, pitch = 0.0, yaw = 0.0):
    
    t0 = math.cos(math.radians(yaw * 0.5))
    t1 = math.sin(math.radians(yaw * 0.5))
    t2 = math.cos(math.radians(roll * 0.5))
    t3 = math.sin(math.radians(roll * 0.5))
    t4 = math.cos(math.radians(pitch * 0.5))
    t5 = math.sin(math.radians(pitch * 0.5))

    w = t0 * t2 * t4 + t1 * t3 * t5
    x = t0 * t3 * t4 - t1 * t2 * t5
    y = t0 * t2 * t5 + t1 * t3 * t4
    z = t1 * t2 * t4 - t0 * t3 * t5

    return [w, x, y, z]

# Take off 2.5m in GUIDED_NOGPS mode.
arm_and_takeoff_nogps(2.5)

# Hold the position for 3 seconds.
print("Hold position for 3 seconds")
set_attitude(duration = 1)

# Uncomment the lines below for testing roll angle and yaw rate.
# Make sure that there is enough space for testing this.

# set_attitude(roll_angle = 1, thrust = 0.5, duration = 3)
# set_attitude(yaw_rate = 30, thrust = 0.5, duration = 3)

# Move the drone forward and backward.
# Note that it will be in front of original position due to inertia.
print("Move forward")
set_attitude(pitch_angle = -10, thrust = 0.5, duration = 1.21)

print("Move backward")
set_attitude(pitch_angle = 10, thrust = 0.5, duration = 1)


print("Setting LAND mode...")
vehicle.mode = VehicleMode("LAND")
time.sleep(1)

# Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()

# Shut down simulator if it was started.
if sitl is not None:
    sitl.stop()

print("Completed")

 

 

위 코드는 git의 set_attitude_tartget 을 살짝 변형한 코드이다.

 

 

픽스호크에는 여러가지 모드들이 있다.

 

https://www.evergray.kr/%ED%94%BD%EC%8A%A4%ED%98%B8%ED%81%ACpixhawk-%EB%B9%84%ED%96%89-%EB%AA%A8%EB%93%9C-%EC%9D%BC%EB%9E%8C%ED%91%9C/

픽스호크(Pixhawk) 비행 모드 일람표 | 世路少知音

 

위에서 정보들을 확인할 수 있다. 

 

이 중에서 우리가 봐야할 모드들은

 

GUIDED, GUIDED_NOGPS, ALTHOLD, LAND

 

네가지가 있다. 현재 필자의 드론은 ALTHOLD 모드로 수동조종을 한다. 고도유지가 쉽기 때문. 그리고 LAND모드로 조작을 하면 자동으로 착륙이 된다. 수동으로 착륙하려면 굉장히 어렵지만 (드론이 뒤집어 진다던지) 쉽게 해낼 수 있다.

 

무튼 중요한건 드론을 라즈베리파이로 조종하는 것이다. 이를 위해선 결국 수동이 아닌 '자동모드'로 드론을 제어할 줄 알아야 한다.

 

이때 사용되는 것이 GUIDED,GUIDED_NOGPS모드이다. 두 모드 다 명령을 받아서 픽스호크가 움직일 수 있는 모드이지만 GUIDED모드는 gps가 꼭 연결되어 있어야한다. 보통 픽스호크를 활용해서 드론을 만들면 gps모듈을 같이 장착할텐데 이 모듈이 정상동작하는 야외에서 잘 동작하는 모드이다.

 

반면 NOGPS모드는 gps없이도 자율조종을 할 수 있는 모드로 간단한 이륙,하강,앞뒤의 전진을 수행할 수 있으나 gps가 없기 때문에 바람을 고려하지 못한다. ( 더 찾아보면 있는거 같긴 하다 )

 

작년에 No_GPS로 구동되는 위 코드로 실행했을때는 잘 동작하였던 코드가 No_GPS모드로 바꾸려 하면 

 

'No such mode'

 

라는 에러메시지를 내보내며 변경이 되지 않았다. 외국 사이트를 뒤져보니 이런저런 업그레이드가 되면서 모드가 사라져서 못찾는거 같기도 하고... 암튼 안된다. 하지만 GUIDED모드로도 위 코드는 정상동작된다. (대신 실내에서는 테스트를 할 수 없다.)

 

 

또한 Connect하는 함수도 조금 다르다. 예제 코드와는 다르게 baud rate를 별도로 설정해서 넣어줬는데 통신 속도를 맞춰줘야 잘 돌아간다.

 

 

 

또한 위 코드를 그냥 파이썬 모듈로 돌리면 동작하지 않고, 꼭 터미널을 열어서 

 

python 파일이름.py --connect /dev/ttyAMA0

 

혹은

 

python 파일이름.py --connect /dev/serial0

 

 

 

처럼 시리얼 포트와 함께 실행해줘야 한다.