Controlling Copter via dronekit

tags: Drone python

• Controlling Copter via dronekit
  • Import Require Modules
  • Connection
  • Takeoff
    • GPS
    • Without GPS
  • Go to designated location
    • NED
    • LocationGlobal
    • LocationGlobalRelative
    • goto Function
    • Message Factory
  • Set attitude
  • Yaw
  • Reference

Import Require Modules

from __future__ import print_function

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

Connection

vehicle = connect('/dev/ttyUSB0', wait_ready=True, baud=57600)

• https://dronekit-python.readthedocs.io/en/latest/guide/connecting_vehicle.html

Takeoff

GPS

simple_takeoff(alt=None) : 起飛並將無人機飛行到指定的高度（以米為單位)。

def arm_and_takeoff(aTargetAltitude):

  print "Basic pre-arm checks"
  # Don't let the user try to arm until autopilot is ready
  while not vehicle.is_armable:
    print " Waiting for vehicle to initialise..."
    time.sleep(1)
        
  print "Arming motors"
  # Copter should arm in GUIDED mode
  vehicle.mode    = VehicleMode("GUIDED")
  vehicle.armed   = True

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

  print "Taking off!"
  vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude

  # Check that vehicle has reached takeoff altitude
  while True:
    print " Altitude: ", vehicle.location.global_relative_frame.alt 
    #Break and return from function just below target altitude.        
    if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: 
      print "Reached target altitude"
      break
    time.sleep(1)

# Initialize the takeoff sequence to 20m
arm_and_takeoff(20)

• 此功能只能用於Copter。
• 必須是GUIDED模式。
• 當達到正確的高度時沒有通知機制，如果另一個命令在該點之前到達（例如simple_goto()），它將立刻改為運行simple_goto()指令。因此需要阻塞程式，比如利用while True:，確認到達目標才繼續下一個指令。

Without GPS

def arm_and_takeoff_nogps(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude without GPS data.
    """

    ##### 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 not 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))
        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)

## Takeoff to ten meters.
arm_and_takeoff_nogps(10)

Go to designated location

simple_goto（位置，空速=無，地速=無）
轉到指定的全局位置（LocationGlobal或LocationGlobalRelative）。

當到達目標位置時沒有通知機制，如果另一個命令在該點之前到達將立即執行。

您可以選擇設置所需的空速或地速（這與設置airspeed或相同 groundspeed）。如果這些值未設置或兩者都設置，車輛將確定使用什麼速度。

• 必須是GUIDED模式，若當前不是GUIDED模式使用此指令將會強制轉換成GUIDED模式。
• 當到達目標位置時沒有通知機制，如果另一個命令在到達目的地前接收則將立即執行新命令。阻塞程序可以使用goto(如下)。

NED

Control the Copter by using (North, East, Down).

• The altitude of Drone positive you have to set the negative value because of the coordinate.
• First need to takeoff.

Set different frames for reference of coordinate.

Frame	relative location	Position orientation	Speed orientation
MAV_FRAME_LOCAL_NED	Home	NED	NED
MAV_FRAME_LOCAL_OFFSET_NED	Vehicle	NED	NED
MAV_FRAME_BODY_NED	Home	NED	FRD
MAV_FRAME_BODY_OFFSET_NED	Vehicle	FRD	FRD

• NED : North East Down
• FRD : Front Right Down

MAV_FRAME_LOCAL_NED

def goto_position_target_local_ned(north, east, down):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame
        0b0000111111111000, # type_mask (only positions enabled)
        north, east, down, # x, y, z positions (or North, East, Down in the MAV_FRAME_BODY_NED frame
        0, 0, 0, # x, y, z velocity in m/s  (not used)
        0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
        0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 
    # send command to vehicle
    vehicle.send_mavlink(msg)

Altitude 5 meters

goto_position_target_local_ned(0, 0, -5)

Goto North 5 meters and Altitude 5 meters，絕對座標(5,0)

goto_position_target_local_ned(5,0,-5)

Goto North 5 meters South 5 meters and Altitude 5 meters，絕對座標(5,-5)

goto_position_target_local_ned(5,-5,-5)

此NED所用的是絕對座標，並不是相對座標，假設無人機到座標北5公尺東0公尺高度5公尺(5,0,-5)，接著再給無人機(5,0,-5)是不會往北5公尺的，會停在原地。若要再往北5公尺則需要(10,0,-5)。

MAV_FRAME_LOCAL_OFFSET_NED

def goto_position_target_local_ned(north, east, down):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED, # Frame
        0b0000111111111000, # type_mask (only positions enabled)
        north, east, down, # x, y, z positions (or North, East, Down in the MAV_FRAME_BODY_NED frame
        0, 0, 0, # x, y, z velocity in m/s  (not used)
        0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
        0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 
    # send command to vehicle
    vehicle.send_mavlink(msg)

Altitude 5 meters

goto_position_target_local_ned(0, 0, -5)

Goto North 5 meters and Altitude 5 meters，相對座標(5,0)

goto_position_target_local_ned(5,0,-5)

Goto North 5 meters South 5 meters and Altitude 5 meters，相對座標(5,-5)

goto_position_target_local_ned(5,-5,-5)

此用法是無人機自己為原點，座標為相對座標。

LocationGlobal

class dronekit.LocationGlobal(lat，lon，alt=None)
緯度和經度是相對於WGS84坐標系的海拔高度與平均海平面 (MSL) 相關。
例如，海拔高度為 30 米：

LocationGlobal(-34.364114, 149.166022, 30)

# Set mode to guided - this is optional as the goto method will change the mode if needed.
vehicle.mode = VehicleMode("GUIDED")

# Set the target location in global-relative frame
a_location = LocationGlobal(-34.364114, 149.166022, 30)
vehicle.simple_goto(a_location)

LocationGlobalRelative

class dronekit.LocationGlobalRelative（lat，lon，alt=None ）
緯度和經度是相對於WGS84坐標系的高度是相對於無人機的起始位置。
例如，LocationGlobalRelative高度高於home位置 30 米：

LocationGlobalRelative(-34.364114, 149.166022, 30)

# Set mode to guided - this is optional as the goto method will change the mode if needed.
vehicle.mode = VehicleMode("GUIDED")

# Set the target location in global-relative frame
a_location = LocationGlobalRelative(-34.364114, 149.166022, 30)
vehicle.simple_goto(a_location)

goto Function

前往目的地，計算當前位置與目的地位置的距離來阻塞程序。

def get_distance_metres(aLocation1, aLocation2):
    """
    Returns the ground distance in metres between two LocationGlobal objects.

    This method is an approximation, and will not be accurate over large distances and close to the 
    earth's poles. It comes from the ArduPilot test code: 
    https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py
    """
    dlat = aLocation2.lat - aLocation1.lat
    dlong = aLocation2.lon - aLocation1.lon
    return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5


def goto(newlat, newlon, gotoFunction=vehicle.simple_goto):
    currentLocation=vehicle.location.global_relative_frame
    targetLocation=LocationGlobalRelative(newlat, newlon, currentLocation.alt)
    targetDistance=get_distance_metres(currentLocation, targetLocation)
    gotoFunction(targetLocation)

    while vehicle.mode.name=="GUIDED": #Stop action if we are no longer in guided mode.
        remainingDistance=get_distance_metres(vehicle.location.global_frame, targetLocation)
        print("Distance to target:{}".format(remainingDistance))
        if remainingDistance<=targetDistance*0.01: #Just below target, in case of undershoot.
            print "Reached target"
            break;
        time.sleep(2)

goto(newlat, newlon)

Message Factory

def goto_position_target_global_int(aLocation):
    """
    Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified LocationGlobal.

    For more information see: https://pixhawk.ethz.ch/mavlink/#SET_POSITION_TARGET_GLOBAL_INT

    See the above link for information on the type_mask (0=enable, 1=ignore). 
    At time of writing, acceleration and yaw bits are ignored.
    """
    msg = vehicle.message_factory.set_position_target_global_int_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
        0b0000111111111000, # type_mask (only speeds enabled)
        aLocation.lat*1e7, # lat_int - X Position in WGS84 frame in 1e7 * meters
        aLocation.lon*1e7, # lon_int - Y Position in WGS84 frame in 1e7 * meters
        aLocation.alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
        0, # X velocity in NED frame in m/s
        0, # Y velocity in NED frame in m/s
        0, # Z velocity in NED frame in m/s
        0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
        0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 
    # send command to vehicle
    vehicle.send_mavlink(msg)

• https://dronekit-python.readthedocs.io/en/stable/examples/guided-set-speed-yaw-demo.html#guided-example-source-code

Set attitude

設定姿態，我們需要用到歐拉角轉換四元數，來表示三維空間中無人機的姿態 (Roll, Pitch, Yaw)。

• Thrust > 0.5: Ascend
• Thrust = 0.5: Hold the altitude
• Thrust < 0.5: Descend

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

    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):
    """
    Convert degrees to quaternions
    """
    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]

Here is introduction of Eular Engle and Quaternions.

• https://hackmd.io/@Wei-Lin/Eular_quaternions

Yaw

設置偏航(Yaw)，將無人機轉向。

def condition_yaw(heading,direction=1, relative=False):
    if relative:
        is_relative = 1 #yaw relative to direction of travel
    else:
        is_relative = 0 #yaw is an absolute angle
    # create the CONDITION_YAW command using command_long_encode()
    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
        0, #confirmation
        heading,    # param 1, yaw in degrees
        0,          # param 2, yaw speed deg/s
        direction,          # param 3, direction -1 ccw, 1 cw
        is_relative, # param 4, relative offset 1, absolute angle 0
        0, 0, 0)    # param 5 ~ 7 not used
    # send command to vehicle
    vehicle.send_mavlink(msg)

• is_relative = 1 代表相對於行駛方向的偏航。
• is_relative = 0 代表絕對角度東西南北，0是北方，90是東方，180是南方。
• direction = 1 ， 順時針旋轉
• direction = -1 ，逆時針旋轉
• heading 只有正的角度值

print("Yaw 225 absolute")
condition_yaw(225)

順時針旋轉

print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90,relative=True)

逆時針旋轉

print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90,-1,relative=True)

一定要先有“移動”才能執行偏航指令否則會被忽略，若是要起飛後原地偏航須先給指令移動到原地的GPS位置。

Reference

• https://github.com/dronekit/dronekit-python/blob/master/examples/set_attitude_target/set_attitude_target.py
• https://github.com/dronekit/dronekit-python/blob/master/examples/guided_set_speed_yaw/guided_set_speed_yaw.py
• https://dronekit-python.readthedocs.io/en/latest/guide/copter/guided_mode.html#guided-mode-copter-useful-conversion-functions
• https://dronekit-python.readthedocs.io/en/latest/examples/guided-set-speed-yaw-demo.html#guided-example-source-code