-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtest_location.py
132 lines (101 loc) · 3.35 KB
/
test_location.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
"""
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
The example demonstrates how to arm and takeoff in Copter and how to navigate to
points using Vehicle.commands.goto.
Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html
"""
import time
from droneapi.lib import VehicleMode, Location
from pymavlink import mavutil
api = local_connect()
vehicle = api.get_vehicles()[0]
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if vehicle.mode.name == "INITIALISING":
print "Waiting for vehicle to initialise"
time.sleep(1)
while vehicle.gps_0.fix_type < 2:
print "Waiting for GPS...:", vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.flush()
while not vehicle.armed and not api.exit:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
vehicle.flush()
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.commands.takeoff will execute immediately).
while not api.exit:
print " Altitude: ", vehicle.location.alt
if vehicle.location.alt>=aTargetAltitude*0.95: #Just below target, in case of undershoot.
print "Reached target altitude"
break;
time.sleep(1)
arm_and_takeoff(3)
'''
location:
1.-35.363554 149.165139
2.-35.362664 149.165693
3. -35.363562 149.166230
4.-35.362664 149.166803
5.-35.363562 149.167339
6. -35.362618 149.167875
7-35.363531 149.168440
'''
print "Going to first point..."
#point1 = Location(-35.363554, 149.165139, 3, is_relative=True)
point1 = Location(30.264233, 120.118813, 3, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
'''
print "Going to second point..."
point2 = Location(-35.362664, 149.165693, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
print "Going to second point..."
point2 = Location(-35.363562, 149.166230, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
print "Going to second point..."
point2 = Location(-35.362664, 149.166803, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
print "Going to second point..."
point2 = Location(-35.363562, 149.167339, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
print "Going to second point..."
point2 = Location(-35.362618, 149.167875, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
print "Going to second point..."
point2 = Location(-35.363531,149.168440, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
'''
print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
vehicle.flush()