Home Drone Programming - How to Control Drone with Keyboard using DroneKit-Python?
Post
Cancel

Drone Programming - How to Control Drone with Keyboard using DroneKit-Python?

According to the Federal Aviation Administration (FAA), around 866,076 Drones Registered in the United States till November 2021, of which 337,832 are commercial drones. The flight of a drone is either controlled autonomously by computers in the vehicle or remotely controlled by a pilot on the ground.

Drone Programming Image Credits: Pixabay

If you’re using the DIY drone or community-supported flight controllers like pixhawk or navio, you can use DroneKit-Python to control your drone.

The DroneKit-Python is an Application Programming Interface (API) that allows developers to create Python apps that communicate with vehicles over MAVLink protocol.1 According to Pepy, DroneKit-Python was downloaded more than 463,900+ times.2 Downloads

How to Detect Keypresses using Python?

For controlling a drone using your computer’s keyboard, firstly, you need to understand how to detect keypresses using python.

Drone Programming Image Credits: Pixabay

Few available packages help in achieving the action of recording or detecting the keypress using python, like:

Note

In this article, we will be using sshkeyboard. As compared to pyinput and keyboard, the sshkeyboard has some advantages.3

Advertisement

Installing sshkeyboard

You can able to install sshkeyboard on your device easily using pip. For installing sshkeyboard on your device, execute the following commands on your terminal:

1
$ pip install sshkeyboard

You can also open the terminal window using the keyboard shortcut Ctrl + Alt + T

Using sshkeyboard

After installing sshkeyboard on your device, you can able to import the package in your python script using the import sshkeyboard.

The following is the python script named keypress.py. This script will help you understand how the sshkeyboard package works:

1
2
3
4
5
6
7
8
9
10
from sshkeyboard import listen_keyboard

def press(key):
    print(f"'{key}' is pressed")

def main():
    listen_keyboard(on_press=press)

if __name__ == "__main__":
    main()

Create a python script named keypress.py on your device and copy the above code to that script and then save it.

Execution

You can able to execute the script keypress.py by executing the following command on your terminal:

1
$ python keypress.py

Output

Detecting Keypresses using Python Detecting Keypresses using Python

In the above script, the listen_keyboard() function is responsible for logging the keypresses, and it’ll be running continuously as a separate thread. The press(key) is passed as an argument for the listen_keyboard(on_press=press), and the purpose of the press() function here is to print the pressed keys from the keyboard.

Note

By default, pressing the ESC key will stop the execution of the script.

Controlling a Drone using Keyboard

From the above section, I hope you can able to understand how to detect the keypresses using python. To control a drone using python, you can use the dronekit-python package on your scripts.

Note

I’ve already written an article titled, How to Control a Drone Using Python?

If you’re a beginner, kindly have a look at that article before jumping into this topic because it’ll give you a better understanding of the technological things involved in drone programming.

For controlling a drone using keyboard, you need select a set of keys to perform a set of tasks for every keypress. In simple words, we need to map the keypresses to drone movements and actions.

The python script used in this article performs the following task when you press the listed keys on your keyboard:

KeysDescription
tTakeoff
lChanges the vehicle mode to LAND
gChanges the vehicle mode to GUIDED
rChanges the vehicle mode to RTL
Vehicle to the Left
Vehicle to the Right
Moves the Vehicle Forward
Moves the Vehicle Backward
SpacebarIncreases vehicle altitude by 5 meters from the current altitude
TabDecreases vehicle altitude by 5 meters from the current altitude

For example, after the execution of the script, if you press t on your keyboard, the drone will take off from the ground to 5m altitude. Similarly, if you press l, then the drone will land on the same location.

Advertisement

Python Program to Control a Drone using Keyboard

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/control-drone-with-keyboard

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 keycontrol.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
#!/usr/bin/env python

#...........................................................................
# Author:  Saiffullah Sabir Mohamed
# Github:  https://github.com/TechnicalVillager
# Website: http://technicalvillager.github.io/
# Source: https://github.com/TechnicalVillager/control-drone-with-keyboard/
#...........................................................................

# Import Necessary Packages
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time, math
from sshkeyboard import listen_keyboard

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
        time.sleep(1)


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 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  Latitude
        3.  distance            -   Distance from the home location
        4.  bearing             -   Bearing angle from the home location

    """

    #Radius of earth in metres
    R = 6371e3

    rlat1 = homeLattitude * (math.pi/180) 
    rlon1 = 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 control(value):

    """
    
    This function call the respective functions based on received arguments.

        t             -       Take-Off
        l             -       Land
        g             -       Guided Mode
        r             -       RTL Mode
        up, down,
        right, left   -       This will call the navigation() function 

    Inputs:
        1.  value         -   ['space', 'tab', 't', 'l', 'g', 'r', 'up', 'down', 'right', 'left']

    """

    allowed_keys = ['space', 'tab', 't', 'l', 'g', 'r', 'up', 'down', 'right', 'left']

    if value in allowed_keys:

        if value == 'space':
            change_alt(step = "INC")

        if value == 'tab':
            change_alt(step = "DEC")

        if value == 't':
            if int(vehicle.location.global_relative_frame.alt) <= 5:
                basic_takeoff(altitude = 5)

        if value == 'l':
            change_mode(mode = "LAND")

        if value == 'g':
            change_mode(mode = "GUIDED")

        if value == 'r':
            change_mode(mode = "RTL")

        if value in allowed_keys[-4:]:
            navigation(value = value)

    else:
        print("Enter a valid Key!!!")

def navigation(value):

    """
    
    This function moves the vehicle to front, back, right, left
    based on the input argument.

        UP       -   Moves the Vehicle to Forward
        DOWN     -   Moves the Vehicle to Backward
        RIGHT    -   Moves the Vehicle to Right
        LEFT     -   Moves the Vehicle to Left

    Inputs:
        1.  value         -   [right, left, up, down]

    """

    # Vehicle Location
    angle = int(vehicle.heading)
    loc   = (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, vehicle.location.global_relative_frame.alt)

    # Default Distance in meters
    default_distance = 5

    if value == 'up':
        front = angle + 0
        new_loc = destination_location(homeLattitude = loc[0], homeLongitude = loc[1], distance = default_distance, bearing = front)
        send_to(new_loc[0], new_loc[1], loc[2])

    if value == 'down':
        back = angle + 180
        new_loc = destination_location(homeLattitude = loc[0], homeLongitude = loc[1], distance = default_distance, bearing = back)
        send_to(new_loc[0], new_loc[1], loc[2])

    if value == 'right':
        right = angle + 90
        new_loc = destination_location(homeLattitude = loc[0], homeLongitude = loc[1], distance = default_distance, bearing = right)
        send_to(new_loc[0], new_loc[1], loc[2])

    if value == 'left':
        left = angle -90
        new_loc = destination_location(homeLattitude = loc[0], homeLongitude = loc[1], distance = default_distance, bearing = left)
        send_to(new_loc[0], new_loc[1], loc[2])

def press(key):

    """
    
    This function prints the keybooard presses and calls the control()
    function.

    Inputs:
        1.  key         -   Pressed keyboard Key

    """

    print(f"'{key}' is pressed")

    # Sending Control Inputs
    control(value = key)

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

    # Listen Keyboard Keys
    listen_keyboard(on_press=press)

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 keycontrol.py, execute the following command on your terminal:

1
$ python keycontrol.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

As descriptions in each function of the above program are self-explanatory, instead of explaining them again, I’ll add some suggestions that will be helpful to update the code based on your requirements:

How to Change the Take Off Altitude?

To change the default Take-Off altitude in the above script, change altitude = 5 in line number 203 to your desired take-off altitude. For Example, to change the default take-off value to 10 meters:

1
basic_takeoff(altitude = 10)

How to Change the Default Distance?

Similarly, for changing the default vehicle movement distance (default is 5 meters), change default_distance = 5 to your desired value on line number 243. For Example, to change the default distance value to 10 meters:

1
default_distance = 10

Altitude Control

On pressing space and tab, the altitude of the vehicle increases 5 meters from the current altitude or decreases 5 meters from the current altitude, respectively. To change this threshold, change the changed_altitude = [(actual_altitude + 5), (actual_altitude - 5)] in line number 91. For Example, to change the threshold to 8 meters:

1
changed_altitude = [(actual_altitude + 8), (actual_altitude - 8)]

Tada, now you can able to control your drone using your keyboard!!!

Conclusion

In this article, I’ve tried to explain how to control a drone with the keyboard 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

  1. Official Documentation, DroneKit-Python. 

  2. PePy

  3. How it works, Official Documentation, sshkeyboard. 

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 Taxi - Flying Cabs For Hire

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