# Controlling Copter via dronekit
###### tags: `Drone` `python`
[TOC]
## Import Require Modules
```python=
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) : 起飛並將無人機飛行到指定的高度（以米為單位)。
```python=
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)
```
```python=30
# Initialize the takeoff sequence to 20m
arm_and_takeoff(20)
```
:::info
* 此功能只能用於Copter。
* 必須是GUIDED模式。
* 當達到正確的高度時沒有通知機制，如果另一個命令在該點之前到達（例如simple_goto()），它將立刻改為運行simple_goto()指令。因此需要阻塞程式，比如利用while True:，確認到達目標才繼續下一個指令。
:::
### Without GPS
```python=
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)
```
```python=43
## Takeoff to ten meters.
arm_and_takeoff_nogps(10)
```
## Go to designated location
simple_goto（位置，空速=無，地速=無）
轉到指定的全局位置（LocationGlobal或LocationGlobalRelative）。
當到達目標位置時沒有通知機制，如果另一個命令在該點之前到達將立即執行。
您可以選擇設置所需的空速或地速（這與設置airspeed或相同 groundspeed）。如果這些值未設置或兩者都設置，車輛將確定使用什麼速度。
:::info
* 必須是GUIDED模式，若當前不是GUIDED模式使用此指令將會強制轉換成GUIDED模式。
* 當到達目標位置時沒有通知機制，如果另一個命令在到達目的地前接收則將立即執行新命令。阻塞程序可以使用goto(如下)。
:::
### NED
Control the Copter by using (North, East, Down).
:::info
* 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
```python=
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
```python=
goto_position_target_local_ned(0, 0, -5)
```
Goto North 5 meters and Altitude 5 meters，絕對座標(5,0)
```python=
goto_position_target_local_ned(5,0,-5)
```
Goto North 5 meters South 5 meters and Altitude 5 meters，絕對座標(5,-5)
```python=
goto_position_target_local_ned(5,-5,-5)
```
:::warning
此NED所用的是絕對座標，並不是相對座標，假設無人機到座標北5公尺東0公尺高度5公尺(5,0,-5)，接著再給無人機(5,0,-5)是不會往北5公尺的，會停在原地。若要再往北5公尺則需要(10,0,-5)。
:::
#### MAV_FRAME_LOCAL_OFFSET_NED
```python=
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
```python=
goto_position_target_local_ned(0, 0, -5)
```
Goto North 5 meters and Altitude 5 meters，相對座標(5,0)
```python=
goto_position_target_local_ned(5,0,-5)
```
Goto North 5 meters South 5 meters and Altitude 5 meters，相對座標(5,-5)
```python=
goto_position_target_local_ned(5,-5,-5)
```
:::warning
此用法是無人機自己為原點，座標為相對座標。
:::
### LocationGlobal
class dronekit.LocationGlobal(lat，lon，alt=None)
緯度和經度是相對於WGS84坐標系的**海拔高度與平均海平面** (MSL) 相關。
例如，海拔高度為 30 米：
```python=
LocationGlobal(-34.364114, 149.166022, 30)
```
```python=
# 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 米：
```python=
LocationGlobalRelative(-34.364114, 149.166022, 30)
```
```python=
# 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
前往目的地，計算當前位置與目的地位置的距離來阻塞程序。
```python=
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)
```
```python=
goto(newlat, newlon)
```
### Message Factory
```python=
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
```python=
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)，將無人機轉向。
```python=
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)
```
:::info
* is_relative = 1 代表相對於行駛方向的偏航。
* is_relative = 0 代表絕對角度東西南北，0是北方，90是東方，180是南方。
* direction = 1 ， 順時針旋轉
* direction = -1 ，逆時針旋轉
* heading 只有正的角度值
:::
```python=
print("Yaw 225 absolute")
condition_yaw(225)
```
順時針旋轉
```python=
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90,relative=True)
```
逆時針旋轉
```python=
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90,-1,relative=True)
```
:::warning
一定要先有“**移動**”才能執行偏航指令否則會被忽略，若是要起飛後原地偏航須先給指令移動到原地的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
Sign in with Wallet
Connect another wallet