Home Drone Programming - How to Program your Drone to Fly in a Triangular Path using DroneKit-Python?
Post
Cancel

Drone Programming - How to Program your Drone to Fly in a Triangular Path using DroneKit-Python?

The utilization of drones in various industries is favorably increasing day by day. Initially, most of the Unmanned Aerial Vehicles (UAVs) were designed solely for military purposes, but now they are serving many applications.

Programming a drone is not that hard as you think. It’s much easier to program a drone if you’re already aware of the programming language that the drone’s flight controller supports.

Also, 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.

Video Credits: Dhulkarnayn, Elucidate Drones

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.

Advertisement

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 Triangular Path?

For programming your drone to follow a triangular path, we need to have some understanding of the geometry of the triangle. The triangle here refers to an equilateral triangle.

Equilateral Triangle - Representing Side Length Image Credits: Dhulkarnayn, Elucidate Drones

Properties of Equilateral Triangle

Some of the properties of equilateral triangle includes1:

  • All three sides have the same length in an equilateral triangle
  • All three interior angles of an equilateral triangle are equal (angle = 60°)
  • The altitudes, the angle bisectors, the perpendicular bisectors, and the medians to each side coincide

Relationship between the Interior and Exterior Angles of a Triangle

The exterior angle of a triangle is the angle that is formed between one of the sides of a triangle and its adjacent extended side.

Equilateral Triangle - Representing Interior and Exterior Angles Image Credits: Dhulkarnayn, Elucidate Drones

As the interior angle and its corresponding exterior angle are supplementary in a triangle, we can able to express the relationship between them as follows2:

Interior angle + Exterior angle = 180°

From the above relationship, we can able to calculate the exterior angle by,

Exterior angle = 180° - Interior angle

From the properties of the equilateral triangle,

Interior angle = 60°

Therefore, the Exterior angle of an equilateral triangle is equal to 120°.

Exterior angle = 120°

Note

As the triangular path calculation is based on the exterior angles and side length of an equilateral triangle, the above relationship is explained.

Programming a Drone to Fly in a Triangular Path

For Programming your drone to follow a triangular path: Firstly, you need to generate a triangular trajectory and convert that path’s local coordinates into geographical coordinates (latitudes and longitudes). There are several ways to generate a triangular path. The triangular path here refers to the equilateral triangle’s vertices with respect to a given reference point or origin.

A simple way to generate the triangular path is, calculating the next geographic location by considering the bearing angle as exterior angle (120°) and distance value to side length.

Repeating the above step thrice yields all the geographic locations of a triangular trajectory.

Advertisement

Once after retrieving the geographic coordinates, all you need to do is, guide the drone to those locations sequentially.

Python Program - Triangular 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/triangular-mission-dronekit-python

Note

If git is not already installed on your device means, execute the following command on your terminal to install git:

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 triangular_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
266
267
268
269
270
271
272
273
#!/usr/bin/env python

#..................................................................................
# Author  :  Saiffullah Sabir Mohamed
# Github  :  https://github.com/TechnicalVillager
# Website :  http://technicalvillager.github.io/
# Source  :  https://github.com/TechnicalVillager/triangular-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 triangle_calculation(side_length):

    """

    This function will generate the geographical coordinates (latitudes & longitudes)
    of the triangular (Equilateral Triangle) path with the given side length. The origin or
    reference location for the generation of the triangular trajectory is the vehicle's current location.

    Inputs:
        1.  side_length         -   Side length of the Equilateral Triangle

    """

    # 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)

    # Offset Angle
    offset_angle   =  90

    # Decrementing offset angle in the vehicle's heading angle to form the 
    # triangle direction with respect to the vehicle's heading angle.
    angle         -=  offset_angle

    # Declaring a array variable to store
    # the geogrpahical location of triangular points
    final_location =  []

    for count in range(3):
        # Incrementing heading angle
        # Exterior angle of equilateral triangle = 120 degrees
        angle  += 120

        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])

    return final_location

def triangular_mission(side_length):

    """

    This function retrieves the triangle coordinates from the triangle_calculation()
    function and guides the vehicle to the retrieved points.

    Inputs:
        1.  side_length         -   Side length of the equilateral triangle

    """

    # Retrieving the array of the locations of the triangular path
    locations  =  triangle_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 == 'TRIANGLE':
            side = int(input("Enter Side Length of the Triangular Path (in meters):\n"))
            triangular_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 the WP_YAW_BEHAVIOR parameter for other missions based on your needs by referring to the following table:

ValueMeaning
0Never change yaw
1Face next waypoint
2Face next waypoint except RTL
3Face along GPS course

Advertisement

Execution

To run the script triangular_mission.py, execute the following command on your terminal:

1
$ python triangular_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 triangular_mission.py helps to control any MAVLink supported drone to fly in a triangular path. The shape of the generated path resembles an Equilateral triangle. After the execution of triangular_mission.py, try entering any of the following inputs:

InputDescription
takeoffTakeoff
landLand
incIncreases the current altitude
decDecreases the current altitude
triangleStarts the triangular mission

Note

The main requirement for executing the above script is dronekit. You can install dronekit on your device by executing the following command on your terminal if it’s not installed already3:

1
$ sudo pip install dronekit

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/ArgumentsMeaning
homeLattitudeHome Location’s Latitude
homeLongitudeHome Location’s Longitude
destinationLattitudeDestination Location’s Latitude
destinationLongitudeDestination 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 and destination_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/ArgumentsMeaning
homeLattitudeHome Location’s Latitude
homeLongitudeHome Location’s Longitude
distanceDistance from the home location
bearingBearing angle from the home location

Advertisement

Triangle Calculation - triangle_calculation()

This function will generate the geographical coordinates of the triangular path with the given side length. The calculation of the triangular path’s vertices is done by considering the exterior angle as bearing and side length as the distance for each side.

The triangle_calculation() function generates the triangular path by invoking the destination_location() function in loop thrice.

All the vertices or points are converted into geographical coordinates like this, and the three vertices of the triangular path are stored in an array.

The offset of 90° is decremented in the original heading angle to make the generated path form in the side of the vehicle’s heading. If this offset bias is not included, then the generated path will not be on the side of the heading. For avoiding this issue, the true north needs to be considered as a factor.

Note

True north (also called geodetic north or geographic north) is the direction along Earth’s surface towards the geographic North Pole or True North Pole.4

The following figure depicts the output setting the offset value as zero or without adding/reducing the offset values.

Triangular Mission - Without Offset Image Credits: Dhulkarnayn, Elucidate Drones

Triangular Mission - triangular_mission()

The triangular_mission() function retrieves the array of locations from the triangle_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 triangular path 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

Advertisement

This post is licensed under CC BY-SA 4.0 by the author.

Advertisement

Contents

Advertisement

Dhulkarnayn - Elucidate Drones

Dhulkarnayn

I am 25 years old drone developer, holds a postgraduate degree in Avionics. I've worked on a few complex projects like drone swarms, drone light shows, autonomous landing of drones using computer-vision algorithms, etc. Since childhood, I'm much passionate about electronics, aerospace & engineering.

Please consider supporting this project!

If this article has been of help to you, and you feel generous at the moment, don’t hesitate to buy me a coffee. ☕

Buy me a coffee

Drone Programming - How to Program your Drone to Fly in a Square Trajectory using DroneKit-Python?

[SOLVED] - AttributeError: module 'collections' has no attribute 'MutableMapping' - DroneKit-Python