315 lines
11 KiB
Python
315 lines
11 KiB
Python
import threading
|
|
import time
|
|
from djitellopy import Tello
|
|
import csv
|
|
import matplotlib.pyplot as plt
|
|
import cv2
|
|
from pyzbar.pyzbar import decode
|
|
import numpy as np
|
|
|
|
class TelloDrone():
|
|
drone = Tello()
|
|
start = time.time()
|
|
global is_flying
|
|
global has_landed
|
|
global barcode_rect
|
|
global drone_left
|
|
global drone_right
|
|
global drone_straight
|
|
global drone_up
|
|
global drone_down
|
|
global drone_rotate_stop
|
|
|
|
global roll_right
|
|
global roll_left
|
|
global roll_ok
|
|
global barcode_polygon
|
|
|
|
global height_14
|
|
global height_23
|
|
|
|
def __init__(self):
|
|
self.drone.connect()
|
|
self.is_flying = False
|
|
self.has_landed = False
|
|
self.barcode_rect = [0,0,0,0]
|
|
self.drone_left = False
|
|
self.drone_right = False
|
|
self.drone_straight = False
|
|
self.drone_up = False
|
|
self.drone_down = False
|
|
self.drone_rotate_stop = False
|
|
|
|
self.height_14 = 0
|
|
self.height_23 = 0
|
|
|
|
self.roll_left = False
|
|
self.roll_right = False
|
|
self.roll_ok = False
|
|
|
|
def killswitch(self):
|
|
while not self.has_landed:
|
|
if self.is_flying:
|
|
input()
|
|
self.rc_control(0, 0, 0, 0)
|
|
self.drone.emergency()
|
|
|
|
def battery_temp_check(self):
|
|
print(f"\nBATTERY: {self.drone.get_battery()}")
|
|
print(f"TEMPERATURE: {self.drone.get_temperature()}\n")
|
|
|
|
def graphs(self):
|
|
|
|
x = []
|
|
pitch = []
|
|
yaw = []
|
|
roll = []
|
|
|
|
plt.plot(x, pitch, label='pitch')
|
|
plt.plot(x, yaw, label='yaw')
|
|
plt.plot(x, roll, label='roll')
|
|
|
|
cnt = 0
|
|
is_alive = True
|
|
|
|
while not self.has_landed:
|
|
|
|
x.append(cnt)
|
|
pitch.append(self.drone.get_pitch())
|
|
yaw.append(self.drone.get_yaw())
|
|
roll.append(self.drone.get_roll())
|
|
|
|
plt.gca().lines[0].set_xdata(x)
|
|
plt.gca().lines[0].set_ydata(pitch)
|
|
plt.gca().relim()
|
|
plt.gca().autoscale_view()
|
|
plt.pause(0.1)
|
|
|
|
plt.gca().lines[1].set_xdata(x)
|
|
plt.gca().lines[1].set_ydata(yaw)
|
|
plt.gca().relim()
|
|
plt.gca().autoscale_view()
|
|
plt.pause(0.1)
|
|
|
|
plt.gca().lines[2].set_xdata(x)
|
|
plt.gca().lines[2].set_ydata(roll)
|
|
plt.gca().relim()
|
|
plt.gca().autoscale_view()
|
|
plt.pause(0.1)
|
|
|
|
cnt += 1
|
|
|
|
if cnt > 20:
|
|
# cnt = 0
|
|
pitch = pitch[1:]
|
|
yaw = yaw[1:]
|
|
roll = roll[1:]
|
|
x = x[1:]
|
|
# plt.pause(2.0)
|
|
# is_alive = False
|
|
|
|
def flight_data(self):
|
|
while not self.has_landed:
|
|
# print(self.is_flying)
|
|
if self.is_flying == True:
|
|
file = open(f"flight_data.csv", "w")
|
|
r = csv.writer(file, delimiter=";")
|
|
r.writerow(["BATTERY",'FLIGHT_TIME',"ROLL","PITCH","YAW","VEL_X","VEL_Y","VEL_Z","ACC_X","ACC_Y","ACC_Z"])
|
|
while self.is_flying:
|
|
battery = self.drone.get_battery()
|
|
flight_time = self.drone.get_flight_time()
|
|
roll = self.drone.get_roll()
|
|
pitch = self.drone.get_pitch()
|
|
yaw = self.drone.get_yaw()
|
|
vel_x = self.drone.get_speed_x()
|
|
vel_y = self.drone.get_speed_y()
|
|
vel_z = self.drone.get_speed_z()
|
|
acc_x = self.drone.get_acceleration_x()
|
|
acc_y = self.drone.get_acceleration_y()
|
|
acc_z = self.drone.get_acceleration_z()
|
|
r.writerow([battery, flight_time, roll, pitch,yaw,vel_x,vel_y,vel_z,acc_x,acc_y,acc_z])
|
|
time.sleep(0.1)
|
|
file.close()
|
|
|
|
def show_camera(self):
|
|
self.drone.streamon()
|
|
|
|
while not self.has_landed:
|
|
cap = self.drone.get_frame_read().frame
|
|
cap = cv2.resize(cap, (1280, 720))
|
|
cap = cv2.cvtColor(cap, cv2.COLOR_BGR2RGB)
|
|
for barcode in decode(cap):
|
|
self.barcode_rect = barcode.rect
|
|
self.barcode_polygon = barcode.polygon
|
|
|
|
|
|
self.height_14 = (barcode.polygon[0][1] - barcode.polygon[3][1])
|
|
self.height_23 = (barcode.polygon[1][1] - barcode.polygon[2][1])
|
|
|
|
# print(f"Height 1-4: {self.height_14}")
|
|
# print(f"Height 2-3: {self.height_23}")
|
|
# print(f"Height difference: {self.height_14 - self.height_23}")
|
|
|
|
pts = np.array([barcode.polygon], np.int32)
|
|
pts = pts.reshape((-1,1,2))
|
|
cv2.polylines(cap,[pts],True,(255,0,255),5)
|
|
# print(barcode.polygon)
|
|
cv2.imshow('frame', cap)
|
|
cv2.waitKey(1)
|
|
self.drone.streamoff()
|
|
|
|
def follow_qr(self):
|
|
|
|
while not self.has_landed:
|
|
#print(self.barcode_rect)
|
|
if self.is_flying:
|
|
# print("działa w chuj")
|
|
left = self.barcode_rect[0]
|
|
top = self.barcode_rect[1]
|
|
width = self.barcode_rect[2]
|
|
height = self.barcode_rect[3]
|
|
width_dif = (1280-width)/2
|
|
height_dif = (720-height)/2
|
|
|
|
height_difference = self.height_14 - self.height_23
|
|
#print(f"Height difference: {height_difference}")
|
|
if height_difference < -6:
|
|
self.roll_left = True
|
|
self.roll_right = False
|
|
self.roll_ok = False
|
|
print("Turlaj w lewo")
|
|
elif height_difference > 6:
|
|
self.roll_left = False
|
|
self.roll_right = True
|
|
self.roll_ok = False
|
|
print("Turlaj w prawo")
|
|
elif height_difference > -6 and height_difference < 6:
|
|
self.roll_left = False
|
|
self.roll_right = False
|
|
self.roll_ok = True
|
|
print("Roll ok")
|
|
|
|
# print(self.barcode_rect)
|
|
if left != 0:
|
|
if left > (width_dif + 50 + width):
|
|
# print("DRONE RIGHT")
|
|
self.drone_left = False
|
|
self.drone_right = True
|
|
self.drone_rotate_stop = False
|
|
self.drone_up = False
|
|
self.drone_down = False
|
|
elif left < width_dif - 50:
|
|
# print("DRONE LEFT")
|
|
self.drone_left = True
|
|
self.drone_right = False
|
|
self.drone_rotate_stop = False
|
|
self.drone_up = False
|
|
self.drone_down = False
|
|
elif left > width_dif - 50 and left < (width_dif + 50 + width):
|
|
# print("DRONE ROTATION STOP")
|
|
self.drone_left = False
|
|
self.drone_right = False
|
|
self.drone_rotate_stop = True
|
|
|
|
if top != 0:
|
|
if top > (height_dif + 100 + height):
|
|
self.drone_down = True
|
|
self.drone_up = False
|
|
self.drone_straight = False
|
|
if top < height_dif - 100:
|
|
self.drone_down = False
|
|
self.drone_up = True
|
|
self.drone_straight = False
|
|
if top > (height_dif - 100) and top < (height_dif + 100 + height):
|
|
self.drone_down = False
|
|
self.drone_up = False
|
|
self.drone_straight = True
|
|
#print(self.barcode_rect)
|
|
time.sleep(0.3)
|
|
|
|
def rc_control(self, lr, fb, up, y):
|
|
self.drone.send_rc_control(left_right_velocity=lr, forward_backward_velocity=fb,
|
|
up_down_velocity=up, yaw_velocity=y)
|
|
|
|
def takeoff_func(self):
|
|
self.is_flying = True
|
|
self.drone.takeoff()
|
|
|
|
def landing_func(self):
|
|
self.drone.land()
|
|
time.sleep(3)
|
|
self.is_flying = False
|
|
self.has_landed = True
|
|
|
|
def mission_func(self):
|
|
self.takeoff_func()
|
|
print(f"IS FLYING: {self.is_flying} ")
|
|
print(f"HAS LANDED: {self.has_landed} ")
|
|
time.sleep(5)
|
|
cnt = 0
|
|
while cnt < 250:
|
|
#print(self.roll_left, self.roll_right, self.roll_ok)
|
|
if self.barcode_rect == [0, 0, 0, 0]:
|
|
self.rc_control(0,0,0,20)
|
|
print("noqr")
|
|
|
|
|
|
else:
|
|
if self.drone_left == True:
|
|
self.rc_control(0, 0, 0, -10)
|
|
print("left")
|
|
elif self.drone_right == True:
|
|
print("right")
|
|
self.rc_control(0, 0, 0, 10)
|
|
elif self.drone_rotate_stop == True:
|
|
print("straight")
|
|
if self.roll_left == True:
|
|
self.rc_control(-10, 0, 0, 0)
|
|
print("Turlaj sie w lewo")
|
|
elif self.roll_right == True:
|
|
self.rc_control(10, 0, 0, 0)
|
|
print("Turlaj sie w prawo")
|
|
elif self.roll_ok == True:
|
|
print("Turlaj dropsa")
|
|
if self.drone_up == True:
|
|
print("up")
|
|
self.rc_control(0,0,15,0)
|
|
elif self.drone_down == True:
|
|
print("Hubert")
|
|
self.rc_control(0,0,(-15),0)
|
|
elif self.drone_straight == True:
|
|
print("straight as an arrow")
|
|
width = self.barcode_rect[2]
|
|
height = self.barcode_rect[3]
|
|
if width > 250 or height > 210:
|
|
self.rc_control(0, 0, 0, 0)
|
|
print("Jesteśmy totalnie zajebiści ale chóbert nie")
|
|
else:
|
|
self.rc_control(0,20,0,0)
|
|
|
|
time.sleep(0.2)
|
|
cnt += 1
|
|
#print(cnt)
|
|
|
|
self.rc_control(0,0,0,0)
|
|
time.sleep(5)
|
|
self.landing_func()
|
|
print(f"IS FLYING: {self.is_flying} ")
|
|
print(f"HAS LANDED: {self.has_landed} ")
|
|
self.drone.end()
|
|
|
|
def main():
|
|
drone = TelloDrone()
|
|
drone.battery_temp_check()
|
|
threading.Thread(target=drone.killswitch).start()
|
|
# threading.Thread(target=drone.graphs).start()
|
|
threading.Thread(target=drone.show_camera).start()
|
|
# threading.Thread(target=drone.flight_data).start()
|
|
threading.Thread(target=drone.follow_qr).start()
|
|
threading.Thread (target=drone.mission_func).start()
|
|
print("\nBateria po misji: \n")
|
|
drone.battery_temp_check()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|