-
Notifications
You must be signed in to change notification settings - Fork 0
/
Hexagon(Mission Planner).py
157 lines (116 loc) · 5.58 KB
/
Hexagon(Mission Planner).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
############### PACKAGES
############################################################
from __future__ import print_function
from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command
import time
import math
from pymavlink import mavutil
#############################################################
############## CONNECTION
#############################################################
connection_string = 'udp:127.0.0.1:14551'
# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)
#############################################################
def get_location_metres(original_location, dNorth, dEast):
'''
Parameters -
original position - LocationGlobal() of refrence location
dNorth - meters moved in north
dEast - meters moved in east
Return - LocationGlobal of desired point.
'''
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, 10)
############# DEFINING POINTS OF HEXAGON
##############################################################
aSize = 38
h = (math.sqrt(3)/2)*aSize
home = get_location_metres(vehicle.location.global_frame, 0, 0)
point1 = get_location_metres(vehicle.location.global_frame, 0, aSize)
point2 = get_location_metres(vehicle.location.global_frame, h, aSize/2)
point3 = get_location_metres(vehicle.location.global_frame, h, -aSize/2)
point4 = get_location_metres(vehicle.location.global_frame, 0, -aSize)
point5 = get_location_metres(vehicle.location.global_frame, -h, -aSize/2)
point6 = get_location_metres(vehicle.location.global_frame, -h, aSize/2)
##############################################################
def add_mission():
'''
Defining WayPoints on Mission Planner
'''
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.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.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))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 11))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point2.lat, point2.lon, 12))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point3.lat, point3.lon, 13))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point5.lat, point5.lon, 14))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point6.lat, point6.lon, 14))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 14))
print(" Upload new commands to vehicle")
cmds.upload()
time.sleep(5)
def arm_and_takeoff(aTargetAltitude):
'''
Parameters -
aTargetAltitude - Desired Altitude
Take off The drone in Mission Planner upto Provided Altitude
'''
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)
############### MAIN CODE ##################
#####################################################
print('Create a new mission (for current location)')
add_mission() # creating way points on Mission Planner
arm_and_takeoff(10) # take off the copter
print("Starting mission")
vehicle.simple_goto(point1, groundspeed=10)
time.sleep(18)
vehicle.simple_goto(point2, groundspeed=10)
time.sleep(18)
vehicle.simple_goto(point3, groundspeed=10)
time.sleep(18)
vehicle.simple_goto(point4, groundspeed=10)
time.sleep(18)
vehicle.simple_goto(point5, groundspeed=10)
time.sleep(18)
vehicle.simple_goto(point6, groundspeed=10)
time.sleep(18)
vehicle.simple_goto(point1, groundspeed=10)
time.sleep(18)
print('Return to launch')
vehicle.simple_goto(home, groundspeed=10)
time.sleep(20)
#Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()
###################################################
###################################################