-
Notifications
You must be signed in to change notification settings - Fork 0
/
swarm(Triangle Leader).py
137 lines (105 loc) · 5.04 KB
/
swarm(Triangle Leader).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
from __future__ import print_function
from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command
import time
import math
from pymavlink import mavutil
connection_string_one = 'udp:127.0.0.1:14553'
connection_string_two = 'udp:127.0.0.1:14554'
connection_string_three = 'udp:127.0.0.1:14555'
sitl = None
# Connect to the Vehicle
print('Connecting to first vehicle on: %s' % connection_string_one)
vehicle1 = connect(connection_string_one, wait_ready=True)
print('Connecting to second vehicle on: %s' % connection_string_two)
vehicle2 = connect(connection_string_two, wait_ready=True)
# print('Connecting to third vehicle on: %s' % connection_string_three)
# vehicle3 = connect(connection_string_three, wait_ready=True)
def get_location_metres(original_location, dNorth, dEast, alt):
earth_radius=6378137.0 #Radius of "spherical" earth
#Coordinate offsets in radians
dLat = dNorth/earth_radius
dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))
#New position in decimal degrees
newlat = original_location.lat + (dLat * 180/math.pi)
newlon = original_location.lon + (dLon * 180/math.pi)
return LocationGlobal(newlat, newlon, alt)
aSize = 40
print('========= Defining points ==========')
theta = math.pi/3
home = get_location_metres(vehicle1.location.global_frame, 0, 0, 10)
pt1 = get_location_metres(vehicle1.location.global_frame, 0, aSize, 10)
pt2 = get_location_metres(vehicle1.location.global_frame, (aSize*(math.sin(theta))), aSize/2, 10)
print('===== Defining points completed ====')
def add_mission():
cmds_1 = vehicle1.commands
print(" Clear any existing commands")
cmds_1.clear()
print(" Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds_1.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))
for i in point1 :
cmds_1.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, i.lat, i.lon, 1))
for i in point2 :
cmds_1.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, i.lat, i.lon, 1))
print(" Upload new commands to vehicle")
cmds_1.upload()
cmds_2 = vehicle2.commands
print(" Clear any existing commands")
cmds_2.clear()
print(" Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds_2.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))
for i in point1 :
cmds_2.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, i.lat, i.lon, 1))
for i in point2 :
cmds_2.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, i.lat, i.lon, 1))
print(" Upload new commands to vehicle")
cmds_2.upload()
time.sleep(5)
def arm_and_takeoff(aTargetAltitude, vehicle):
print("Basic pre-arm checks")
while not vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
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)
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Trigger just below target alt.
print("Reached target altitude")
break
time.sleep(1)
print('Create a new mission (for current location)')
# add_mission()
arm_and_takeoff(10, vehicle2)
vehicle2.simple_goto(get_location_metres(vehicle1.location.global_frame, 0, -40, 10), groundspeed=20)
time.sleep(15)
arm_and_takeoff(10, vehicle1)
vehicle1.simple_goto(pt1, groundspeed=10)
for i in range(8) :
p = get_location_metres(vehicle1.location.global_relative_frame, 0, -40, 10)
vehicle2.simple_goto(p, groundspeed=20)
time.sleep(2)
vehicle1.simple_goto(pt2, groundspeed=10)
for i in range(8) :
p = get_location_metres(vehicle1.location.global_relative_frame, 0, -40, 10)
vehicle2.simple_goto(p, groundspeed=20)
time.sleep(2)
# arm_and_takeoff(10, vehicle2)
print('Return to launch')
vehicle1.simple_goto(home, groundspeed=10)
vehicle2.simple_goto(home, groundspeed=10)
time.sleep(20)
#Close vehicle object before exiting script
print("Close vehicle object")
vehicle1.close()
vehicle2.close()
# vehicle3.close()