-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathguided_nogps(1).py
180 lines (144 loc) · 6.76 KB
/
guided_nogps(1).py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
change_attitude.py: (Copter Only)
This example shows how to move/direct Copter and send commands in GUIDED_NOGPS mode using DroneKit Python.
Caution: A lot of unexpected behaviors may occur in GUIDED_NOGPS mode.
Always watch the drone movement, and make sure that you are in dangerless environment.
Land the drone as soon as possible when it shows any unexpected behavior.
Tested in Python 2.7.10
"""
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
parser = argparse.ArgumentParser(description='Control Copter and send commands in GUIDED mode ')
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()
connection_string = args.connect
sitl = None
# Start SITL if no connection string specified
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)
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.
if vehicle.mode.name == "INITIALISING":
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...")
time.sleep(1)
print("Taking off!")
thrust = DEFAULT_TAKEOFF_THRUST
while True:
current_altitude = vehicle.location.global_relative_frame.alt
print(" Altitude: %s" % current_altitude)
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 set_attitude(roll_angle = 0.0, pitch_angle = 0.0, yaw_rate = 0.0, thrust = 0.5, duration = 0):
"""
Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
velocity persists until it is canceled. The code below should work on either version
(sending the message multiple times does not cause problems).
"""
"""
The roll and pitch rate cannot be controllbed with rate in radian in AC3.4.4 or earlier,
so you must use quaternion to control the pitch and roll for those vehicles.
"""
# Thrust > 0.5: Ascend
# Thrust == 0.5: Hold the altitude
# Thrust < 0.5: Descend
msg = vehicle.message_factory.set_attitude_target_encode(
0,
0,
# Target system
0,
# Target component
0b00000000,
# Type mask: bit 1 is LSB
to_quaternion(roll_angle, pitch_angle),
# Quaternion
0,
# Body roll rate in radian
0,
# Body pitch rate in radian
math.radians(yaw_rate),
# Body yaw rate in radian
thrust)
# Thrust
vehicle.send_mavlink(msg)
if duration != 0:
# Divide the duration into the frational and integer parts
modf = math.modf(duration)
# Sleep for the fractional part
time.sleep(modf[0])
# Send command to vehicle on 1 Hz cycle
for x in range(0,int(modf[1])):
time.sleep(1)
vehicle.send_mavlink(msg)
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]
# Take off 2.5m in GUIDED_NOGPS mode.
arm_and_takeoff_nogps(0.75)
# Hold the position for 3 seconds.
print("Hold position for 3 seconds")
set_attitude(duration = 7)
# 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 = 1, thrust = 0.5, duration = 3.21)
print("Move backward")
set_attitude(pitch_angle = -1, thrust = 0.5, duration = 3)
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")