Skip to content
Snippets Groups Projects
Commit 7573be4c authored by Zsedrovits Tamás's avatar Zsedrovits Tamás
Browse files

v1.0

parent a67815a2
No related branches found
No related tags found
No related merge requests found
......@@ -16,7 +16,8 @@ from dbr import *
# Speed of the drone
S = 100
# Step size
STEP = 100
STEP = 40
RSTEP = 16
# Frames per second of the pygame window display
# A low number also results in input lag, as input information is processed once per frame.
FPS = 120
......@@ -33,7 +34,7 @@ class FrontEnd():
"""
class CommandThread(threading.Thread):
""" Class tu send commands to Tello."""
""" Class to send commands to Tello."""
def __init__(self,command_queue,tello,updatefunction,battery_queue):
threading.Thread.__init__(self)
self.commands_to_run = command_queue
......@@ -44,33 +45,27 @@ class FrontEnd():
""" Main thread."""
while True:
command = self.commands_to_run.get(block=True, timeout=None)
try:
if command == "e": # forward
while not self.tello.move_forward(STEP):
time.sleep(1)
self.tello.move_forward(STEP)
elif command == "h": # back
while not self.tello.move_back(STEP):
time.sleep(1)
self.tello.move_back(STEP)
elif command == "b": # left
while not self.tello.move_left(STEP):
time.sleep(1)
self.tello.move_left(STEP)
elif command == "j": # right
while not self.tello.move_right(STEP):
time.sleep(1)
self.tello.move_right(STEP)
elif command == "f": # up
while not self.tello.move_up(int(STEP/4)):
time.sleep(1)
self.tello.move_up(int(STEP/2))
elif command == "l": # down
while not self.tello.move_down(int(STEP/4)):
time.sleep(1)
self.tello.move_down(int(STEP/2))
elif command == "ccw": # turn left
while not self.tello.rotate_counter_clockwise(15):
time.sleep(1)
self.tello.rotate_counter_clockwise(RSTEP)
elif command == "cw": # turn right
while not self.tello.rotate_clockwise(15):
time.sleep(1)
self.tello.rotate_clockwise(RSTEP)
elif command == "rc":
self.updatefunction()
except Exception:
pass
battery_info = self.tello.get_battery()
try:
text = "Battery: {}%".format(battery_info.rstrip())
......@@ -102,6 +97,7 @@ class FrontEnd():
self.send_rc_control = False
self.battery_text = ""
self.read_mission_pad = False
# create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
......@@ -315,7 +311,7 @@ class FrontEnd():
self.tello.streamoff()
self.tello.streamon()
self.tello.send_control_command("mon")
# self.tello.send_control_command("mon")
try:
battery_info = self.tello.get_battery()
......@@ -372,12 +368,16 @@ class FrontEnd():
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
if self.read_mission_pad:
print(self.tello.get_mission_pad_id())
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
try:
self.reader.stop_video_mode()
self.tello.end()
except Exception:
pass
def keydown(self, key):
""" Update velocities based on key pressed
......@@ -401,15 +401,18 @@ class FrontEnd():
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S
self.command_queue.put("rc",block=True, timeout=None)
self.command_queue.put("rc")
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
try:
if key == pygame.K_SPACE: # toggle rc and step control
self.send_rc_control = not self.send_rc_control
elif key == pygame.K_p: # read mission pad id
self.read_mission_pad = not self.read_mission_pad
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
elif key == pygame.K_l: # land
......@@ -425,24 +428,26 @@ class FrontEnd():
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
self.command_queue.put("rc",block=True, timeout=None)
self.command_queue.put("rc")
else:
if key == pygame.K_UP: # forward
self.command_queue.put("e",block=True, timeout=None)
self.command_queue.put("e")
elif key == pygame.K_DOWN: # back
self.command_queue.put("h",block=True, timeout=None)
self.command_queue.put("h")
elif key == pygame.K_LEFT: # left
self.command_queue.put("b",block=True, timeout=None)
self.command_queue.put("b")
elif key == pygame.K_RIGHT: # right
self.command_queue.put("j",block=True, timeout=None)
self.command_queue.put("j")
elif key == pygame.K_w: # up
self.command_queue.put("f",block=True, timeout=None)
self.command_queue.put("f")
elif key == pygame.K_s: # down
self.command_queue.put("l",block=True, timeout=None)
self.command_queue.put("l")
elif key == pygame.K_d: # turn right
self.command_queue.put("cw",block=True, timeout=None)
self.command_queue.put("cw")
elif key == pygame.K_a: # turn left
self.command_queue.put("ccw",block=True, timeout=None)
self.command_queue.put("ccw")
except Exception:
pass
def update(self):
""" Update routine. Send velocities to Tello."""
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment