DroneSwarmDronekit/SimpleTrajectory.py at master · Matth10/DroneSwarmDronekit · GitHub

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
181
""""
This script allow the copter to armed, take off and navigate using GPS location point (3) and then he returns home

"""
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time
import math


#Set up option parsing to get connection string
import argparse

from pymavlink import mavutil

parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.')
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)

print "Launch location"
home_location = vehicle.location.global_relative_frame

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)

    #Arming the drone
    print "Arming Motors"
    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) #Taking off to the target Altitude

    while True:
        print "Location : %s" % vehicle.location.global_relative_frame.alt
        if vehicle.location.global_relative_frame.alt >= aTargetAltitude*0.95:
            print "reached target altitude"
            break
        time.sleep(1)

def get_distance_metres(aLocation1, aLocation2):
     dlat = aLocation2.lat - aLocation1.lat
     dlong = aLocation2.lon - aLocation1.lon
     return math.sqrt((dlat * dlat) + (dlong * dlong)) * 1.113195e5

def goto_position_target_global_int(aLocation):
    """
    Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified location.
    """
    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)

#Take off
arm_and_takeoff(20)

#Set default airspeed
vehicle.airspeed = 10

#Set the first point (-35.361879, 149.165155)
a_location = LocationGlobalRelative(-35.361879, 149.165155, 30)
goto_position_target_global_int(a_location)


while vehicle.mode.name == "GUIDED" :
    print "distance to target : %s" % get_distance_metres(vehicle.location.global_relative_frame, a_location)
    if get_distance_metres(vehicle.location.global_relative_frame, a_location) < 0.2:
        break

print "-----------------------------------------------------------"
print "GPS lat :%s" % vehicle.location.global_relative_frame.lat + " // GPS lat theoretic : %s" % a_location.lat
print "GPS lon :%s" % vehicle.location.global_relative_frame.lon + " // GPS lat theoretic : %s" % a_location.lon
print "GPS alt :%s" % vehicle.location.global_relative_frame.alt + " // GPS lat theoretic : %s" % a_location.alt
print "distance between : %s" % get_distance_metres(vehicle.location.global_relative_frame, a_location)
print "destination reached"
print "-------------------------------------------------------------"
time.sleep(1)

#Set the second point (-35.362216, 149.165970)
b_location = LocationGlobalRelative(-35.362216, 149.165970, 30)
goto_position_target_global_int(b_location)


while vehicle.mode.name == "GUIDED" :
    print "distance to target : %s" % get_distance_metres(vehicle.location.global_relative_frame, b_location)
    if get_distance_metres(vehicle.location.global_relative_frame, b_location) < 0.2:
        break

print "-----------------------------------------------------------"
print "GPS lat :%s" % vehicle.location.global_relative_frame.lat + " // GPS lat theoretic : %s" % b_location.lat
print "GPS lon :%s" % vehicle.location.global_relative_frame.lon + " // GPS lon theoretic : %s" % b_location.lon
print "GPS alt :%s" % vehicle.location.global_relative_frame.alt + " // GPS alt theoretic : %s" % b_location.alt
print "distance between : %s" % get_distance_metres(vehicle.location.global_relative_frame, b_location)
print "destination reached"
print "-------------------------------------------------------------"
time.sleep(1)

#Set the third point (-35.362445, 149.166203)
c_location = LocationGlobalRelative(-35.362445, 149.166203, 30)
goto_position_target_global_int(c_location)


while vehicle.mode.name == "GUIDED" :
    print "distance to target3 : %s" %get_distance_metres(vehicle.location.global_relative_frame, c_location)
    if get_distance_metres(vehicle.location.global_relative_frame, c_location) < 0.2:
        break

print "-----------------------------------------------------------"
print "GPS lat :%s" % vehicle.location.global_relative_frame.lat + " // GPS lat theoretic : %s" % c_location.lat
print "GPS lon :%s" % vehicle.location.global_relative_frame.lon + " // GPS lat theoretic : %s" % c_location.lon
print "GPS alt :%s" % vehicle.location.global_relative_frame.alt + " // GPS lat theoretic : %s" % c_location.alt
print "distance between : %s" % get_distance_metres(vehicle.location.global_relative_frame, c_location)
print "destination reached"
print "-----------------------------------------------------------"
time.sleep(1)

#Come back home
vehicle.mode = VehicleMode("RTL")
while vehicle.mode.name == "RTL":
    print "distance to target : %s" % get_distance_metres(vehicle.location.global_relative_frame, home_location)
    if get_distance_metres(vehicle.location.global_relative_frame, home_location) < 0.2:
        break

print "-----------------------------------------------------------"
print "GPS lat :%s" % vehicle.location.global_relative_frame.lat + " // GPS lat theoretic : %s" % home_location.alt
print "GPS lon :%s" % vehicle.location.global_relative_frame.lon + " // GPS lon theoretic : %s" % home_location.lon
print "GPS alt :%s" % vehicle.location.global_relative_frame.alt + " // GPS alt theoretic : %s" % home_location.alt
print "destination reached"
print "-----------------------------------------------------------"

print "landing the drone"
vehicle.mode = VehicleMode("LAND")

#disarmed the copter
vehicle.armed = False

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

#Shut down simulator
sitl.stop()
print "Completed"