Programming a drone is not that hard as you think. Also, the utilization of drones in various industries is favorably increasing day by day. There are a lot of resources and guides available online for programming your drone in your most comfortable programming languages like C, C++, Python, etc.
Drone programming is not always about autonomous takeoff and landings. There will be a few cases where you need to perform some tasks using drones that need to either satisfy your requirements or your customer requirements.
Video Credits: Dhulkarnayn, Elucidate Drones
For example, if you want to program your drone to fly in a triangular or square trajectory. Firstly, you need to concentrate on how this process can be achieved mathematically before thinking about how to program this process.
Since the flight of drones is entirely based on Global Position System (GPS) when it comes to outdoor navigation, to be honest, there is a lot of mathematics involved in the guidance and navigation part.
How to Program your Drone to Fly in a Square Trajectory?
For programming your drone to follow a square trajectory, we need to have some understanding of the geometry of the square. Square is a regular quadrilateral, which means that it has four equal sides and four equal angles.
Image Credits: Dhulkarnayn, Elucidate Drones
Some of the properties of square includes1:
- All four angles of a square are equal (
angle = 90°
) - All four sides of a square are equal
- The opposite sides of a square are both parallel and equal in length
- The diagonals of a square bisect each other and meet at 90°
Note
The python script used in this article is written based on the geometric properties of a square.
First, you need to generate a square path and convert that path’s local coordinates into geographical coordinates (latitudes and longitudes). Once after retrieving the geographic coordinates, all you need to do is, guide the drone to those locations sequentially.
Advertisement
As you know, all four angles of a square are equal to 90°, and all four sides are equal. Based on the given side length in meters, you can calculate the geographic location of the next point by incrementing the angle to 90° from the current heading angle of the vehicle.
Repeating the above step four times yields all the geographic locations of a square trajectory.
Python Program - Square Mission using DroneKit-Python
You can directly download (clone) the following script to your device by executing the following git
command on your terminal:
1
$ git clone https://github.com/Dhulkarnayn/square-mission-dronekit-python
Note
If
git
is not already installed on your device means, execute the following command on your terminal to installgit
:
1 2 $ sudo apt-get update $ sudo apt-get install git
Or, if you want to copy and paste the script, you can copy the following script and paste it to a file named square_mission.py
on your device.
Code
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
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
#!/usr/bin/env python
#..................................................................................
# Author : Saiffullah Sabir Mohamed
# Github : https://github.com/TechnicalVillager
# Website : http://technicalvillager.github.io/
# Source : https://github.com/TechnicalVillager/square-mission-dronekit-python/
#..................................................................................
# Import Necessary Packages
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time, math
def basic_takeoff(altitude):
"""
This function take-off the vehicle from the ground to the desired
altitude by using dronekit's simple_takeoff() function.
Inputs:
1. altitude - TakeOff Altitude
"""
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
time.sleep(2)
vehicle.simple_takeoff(altitude)
while True:
print("Reached Height = ", vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt >= (altitude - 1.5):
break
def change_mode(mode):
"""
This function will change the mode of the Vehicle.
Inputs:
1. mode - Vehicle's Mode
"""
vehicle.mode = VehicleMode(mode)
def send_to(latitude, longitude, altitude):
"""
This function will send the drone to desired location, when the
vehicle is in GUIDED mode.
Inputs:
1. latitude - Destination location's Latitude
2. longitude - Destination location's Longitude
3. altitude - Vehicle's flight Altitude
"""
if vehicle.mode.name == "GUIDED":
location = LocationGlobalRelative(latitude, longitude, float(altitude))
vehicle.simple_goto(location)
time.sleep(1)
def change_alt(step):
"""
This function will increase or decrease the altitude
of the vehicle based on the input.
Inputs:
1. step - Increase 5 meters of altitude from
current altitude when INC is passed as argument.
- Decrease 5 meters of altitude from
current altitude when DEC is passed as argument.
"""
actual_altitude = int(vehicle.location.global_relative_frame.alt)
changed_altitude = [(actual_altitude + 5), (actual_altitude - 5)]
if step == "INC":
if changed_altitude[0] <= 50:
send_to(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, changed_altitude[0])
else:
print("Vehicle Reached Maximum Altitude!!!")
if step == "DEC":
if changed_altitude[1] >= 5:
send_to(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, changed_altitude[1])
else:
print("Vehicle Reached Minimum Altitude!!!")
def distance_calculation(homeLattitude, homeLongitude, destinationLattitude, destinationLongitude):
"""
This function returns the distance between two geographiclocations using
the haversine formula.
Inputs:
1. homeLattitude - Home or Current Location's Latitude
2. homeLongitude - Home or Current Location's Longitude
3. destinationLattitude - Destination Location's Latitude
4. destinationLongitude - Destination Location's Longitude
"""
# Radius of earth in metres
R = 6371e3
rlat1, rlon1 = homeLattitude * (math.pi/180), homeLongitude * (math.pi/180)
rlat2, rlon2 = destinationLattitude * (math.pi/180), destinationLongitude * (math.pi/180)
dlat = (destinationLattitude - homeLattitude) * (math.pi/180)
dlon = (destinationLongitude - homeLongitude) * (math.pi/180)
# Haversine formula to find distance
a = (math.sin(dlat/2) * math.sin(dlat/2)) + (math.cos(rlat1) * math.cos(rlat2) * (math.sin(dlon/2) * math.sin(dlon/2)))
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
# Distance (in meters)
distance = R * c
return distance
def destination_location(homeLattitude, homeLongitude, distance, bearing):
"""
This function returns the latitude and longitude of the
destination location, when distance and bearing is provided.
Inputs:
1. homeLattitude - Home or Current Location's Latitude
2. homeLongitude - Home or Current Location's Longitude
3. distance - Distance from the home location
4. bearing - Bearing angle from the home location
"""
# Radius of earth in metres
R = 6371e3
rlat1, rlon1 = homeLattitude * (math.pi/180), homeLongitude * (math.pi/180)
d = distance
#Converting bearing to radians
bearing = bearing * (math.pi/180)
rlat2 = math.asin((math.sin(rlat1) * math.cos(d/R)) + (math.cos(rlat1) * math.sin(d/R) * math.cos(bearing)))
rlon2 = rlon1 + math.atan2((math.sin(bearing) * math.sin(d/R) * math.cos(rlat1)) , (math.cos(d/R) - (math.sin(rlat1) * math.sin(rlat2))))
#Converting to degrees
rlat2 = rlat2 * (180/math.pi)
rlon2 = rlon2 * (180/math.pi)
# Lat and Long as an Array
location = [rlat2, rlon2]
return location
def square_calculation(side_length):
"""
This function will generate the geographical coordinates (latitudes & longitudes)
of the square path with the given side length. The origin or reference location
for the generation of the square trajectory is the vehicle's current location.
Inputs:
1. side_length - Side length of the square
"""
# Vehicle's heading and current location
angle = int(vehicle.heading)
loc = (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, vehicle.location.global_relative_frame.alt)
# Declaring a array variable to store
# the geogrpahical location of square points
final_location = []
for count in range(4):
new_loc = destination_location(homeLattitude = loc[0], homeLongitude = loc[1], distance = side_length, bearing = angle)
final_location.append((new_loc[0], new_loc[1], loc[2]))
loc = (new_loc[0], new_loc[1], loc[2])
# Incrementing heading angle
angle += 90
return final_location
def square_mission(side_length):
"""
This function retrieves the square coordinates from the square_calculation()
function and guides the vehicle to the retrieved points.
Inputs:
1. side_length - Side length of the square
"""
# Retrieving the array of the locations of the square path
locations = square_calculation(side_length = side_length)
for location in locations:
# Send vehicle to the destination
send_to(latitude = location[0], longitude = location[1], altitude = location[2])
while True:
# Distance between the current location of the vehicle and the destination
distance = distance_calculation(homeLattitude = vehicle.location.global_frame.lat,
homeLongitude = vehicle.location.global_frame.lon,
destinationLattitude = location[0],
destinationLongitude = location[1])
if distance <= 1.8:
break
time.sleep(2)
def main():
# Declaring Vehicle as global variable
global vehicle
# Connecting the Vehicle
vehicle = connect('udpin:127.0.0.1:14551', baud=115200)
# Setting the Heading angle constant throughout flight
if vehicle.parameters['WP_YAW_BEHAVIOR'] != 0:
vehicle.parameters['WP_YAW_BEHAVIOR'] = 0
print("Changed the Vehicle's WP_YAW_BEHAVIOR parameter")
while True:
# Getting Input from User
value = input("Enter your Input:\n").upper()
if value == 'TAKEOFF':
basic_takeoff(altitude = 5)
if value == 'LAND':
change_mode(mode = value)
if value == 'INC' or 'DEC':
change_alt(step = value)
if value == 'SQUARE':
side = int(input("Enter Side Length of the Square Path (in meters):\n"))
square_mission(side_length = side)
if __name__ == "__main__":
main()
Source: Link
Note
WP_YAW_BEHAVIOR
is a parameter that determines how the autopilot controls the yaw during missions and Return To Launch (RTL).As the method used in the above code is completely based on the heading angle, the yaw behavior of the drone is set to
0
(Never Change Yaw). You can change theWP_YAW_BEHAVIOR
parameter for other missions based on your needs by referring to the following table:
Value Meaning 0 Never change yaw 1 Face next waypoint 2 Face next waypoint except RTL 3 Face along GPS course
Advertisement
Execution
To run the script square_mission.py
, execute the following command on your terminal:
1
$ python square_mission.py
Output
Video Credits: Dhulkarnayn, Elucidate Drones
Note
Instead of using an actual drone, I’ve used ArduPilot’s Software In The Loop (SITL) simulation and Mission Planner as Ground Control Station (GCS) software. Mission Planner is one of the MAVLink supported Ground Control Station (GCS) software that runs natively on the Windows operating system. Also, you can able run Mission Planner on Linux and Mac OS using Mono.
Code Explanation
The python script square_mission.py
helps to control any MAVLink supported drone to fly in a square trajectory of a given side length. After the execution of square_mission.py
, try entering any of the following inputs:
Input | Description |
---|---|
takeoff | Takeoff |
land | Land |
inc | Increases the current altitude |
dec | Decreases the current altitude |
square | Starts the square mission |
Note
The main requirement for executing the above script is
dronekit
. You can installdronekit
on your device by executing the following command on your terminal if it’s not installed already2:
1 $ sudo pip install dronekitKindly have a look at the following article if there are some issues in installing
dronekit
on your device:
As all functions in the above program are self-descriptive, I’ll try to explain the following core functions:
Distance Calculation - distance_calculation()
The distance_calculation()
function calculates the distance between two geographical locations using the haversine formula. For example, if you know the geographical coordinates (latitudes
and longitudes
) of two locations, you can able to calculate the distance between those locations easily using this function.
The distance_calculation()
function returns the distance between two locations in meters and has the following parameters as input:
Parameters/Arguments | Meaning |
---|---|
homeLattitude | Home Location’s Latitude |
homeLongitude | Home Location’s Longitude |
destinationLattitude | Destination Location’s Latitude |
destinationLongitude | Destination Location’s Longitude |
Destination Location - destination_location()
The destination_location()
function returns the geographical coordinate (latitude
and longitude
) of the destination by considering the input home location as a reference.
The location of the destination is determined using distance and bearing values from the home location.
Note
The major difference between
distance_calculation()
function anddestination_location()
function is that the former function returns the distance between two locations, and the latter function returns the location based on the distance and bearing.
The following are the list of input arguments/parameters in the destination_location()
function:
Parameters/Arguments | Meaning |
---|---|
homeLattitude | Home Location’s Latitude |
homeLongitude | Home Location’s Longitude |
distance | Distance from the home location |
bearing | Bearing angle from the home location |
Advertisement
Square Calculation - square_calculation()
This function will generate the geographical coordinates of the square trajectory with the given side length. The square_calculation()
function generates the square trajectory by invoking the destination_location()
function in loop which repeats for four times.
The loop has four iterations because there are four sides in the square, and the side length is equal for all sides. Changing the bearing angle to 90° degrees from the current heading leads to the formation of a square trajectory for a given side length.
All the vertices or points are converted into geographical coordinates like this, and the four vertices of square trajectory are stored in an array.
For example, I’ve added print statements in the square_calculation()
function for printing the vehicle’s current location and geographic location of the square’s vertices. Also, for your reference, I am adding the terminal output with the Mission Planner’s path trace below.
Image Credits: Dhulkarnayn, Elucidate Drones
Square Mission - square_mission()
The square_mission()
function retrieves the array of locations from the square_calculation()
function and then guides the vehicle to each location present in the array sequentially.
The while
loop in this function continuously checks the distance between the vehicle’s current location and the next location until the distance approximately becomes 0.
Conclusion
In this article, I’ve tried to explain how to program your drone to fly in a square trajectory using dronekit-python. I’m expectantly waiting for your valuable feedback and suggestions regarding this article.
At last, Sharing is Caring, feel free to share with your friends if you’ve liked this article. Thank you!
References
Official Documentation, DroneKit-Python. ↩