From 1aa40fbececeadc6ee708e42f921c9c1dd710dfe Mon Sep 17 00:00:00 2001 From: __ Date: Tue, 14 May 2024 14:50:30 +0200 Subject: [PATCH] Cleanup --- .gitignore | 3 + .../artificial_horizon.cpython-311.pyc | Bin 0 -> 7556 bytes src/artificial_horizon.py | 118 ++++++++ src/camera.py | 42 +++ src/controller.py | 25 ++ src/main.py | 258 ++++++++++++++++++ src/test_artificial_horizon.py | 60 ++++ 7 files changed, 506 insertions(+) create mode 100644 .gitignore create mode 100644 src/__pycache__/artificial_horizon.cpython-311.pyc create mode 100644 src/artificial_horizon.py create mode 100644 src/camera.py create mode 100644 src/controller.py create mode 100644 src/main.py create mode 100644 src/test_artificial_horizon.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ed87a00 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +venv/* +tello-venv/* + diff --git a/src/__pycache__/artificial_horizon.cpython-311.pyc b/src/__pycache__/artificial_horizon.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..60b3213904836611a0c6c31aa7c040021756cb5d GIT binary patch literal 7556 zcmc&(U2Gf25xygN1Wa6ER7H>nKD3bG1VQtX&g}7D zQDPbex+LemnfqpEcJFTYcKMOrZegGtxN~s!QX|8Bfl9G(1@Ledz!=FBoPFEF&0n%u5W>-(ra2wvJ)$!cQx=;c&O8Ff$zGalkWH^Ja{Moo0NdJaVUe ztZF6!IS>p5qS0W<9e}f-Jp3Ji_ZWehVF@#%dyNtFf`PElf|Vj&3F%A7P(oY@84;N) zKBR%Lf}ZH$r-z>bejNOa@Z*Vj)8dCO(9n|>g#*n{ z(u|-LuRUDtY32?)cqC-2=4*oNrOnsC zVS>py8&_j#zFl&IT7Lrcx-?;R72!=72yV5QyCyi;`jLy*Pz!w{>3qz{^k%4y%wuG{ zzpE}#(Kv{)%gtU%^KFkAoZf`B!7UhPP7trn$+6~|mTP8Tbpr7;-}RUY&4x}!G0K1z$b_Hf7aPv z6-7IWP88iJdO_S_Xdd(-l5i-Z0tcXI2i)I@iqE#ym6%LS-ki*t9f|Xa^Ec1uEY8G4 zV&dk+wxcm|F>&$c#avT++JN7Tp*?4JC#DipH>Y-Z#?zPU?#cD^=Dhv61HN3JcgN}A zdUqHQ+pS$Y94bZ_Kz2-scm`}{L>8vgvtvbMWBLYn?1&spN3Z7YV%#k|4QRuPDujpS zI(C?nFtS`5ss#aRJE{c%YB#C{0ct<01p#UosC3|1Ux0p9WY2+lJMF6oZvV>^MR`b`N%j37Rr1H6IL!7 z;W%Gv&&RszSCKeOqw`oBE$fCNsytS|*2Hx(K7@rLW5R_(^olM9*Mk^hU?E&}q}!0& z-BNyWn4&AX@3ihCSmN6$6cHdW5(z{a3`r0ex4giEAWW+*IX-S72AE@#3pI zaOm7sYCOzU9u<9DcC7KR$Zh3w^Q9{hw-P>XBj&iBSmF+1RgAx3KG4OT#1?lEyJDhR z198ON#HsL9Yb36C6KPP)RBI;gcnfJ%EL3YHP4PC;9B(Hr@ea}&?<8&UF47+FmT@#I zlzDtPphaOx2kBJWN!M+-7w+QoNej#D$?qmTN?RTLUgA+Y>)`hhZ(aO;GEf(P9~rER zzn>hai|-=`>*60GhwI`$L5|eLA0kiI#Xm}p)x|$fo~n!gG&xZhf0&%Ci+_rY)Wv^> zJX;rklss1#|1^2NF8&xfQy2dQ@?u^5adNgU{!8RsUHq37JvkqPF?3c(CMbpxWmczj zJo*@*a9Ryy68sQD4dlWj)KOf6EnNE##uYR=*e2^c}u1Ys^=w z-xyfBJomji<`C0u65xUtF0l!vct z`H#?KczqfzVhBmc&|*F_cKP!7r7K|8=Wzzi-$d~b5McNY@s}u&?9*#&k5hmp6ksUf zDg)+RG*DbN=*sICXacUcBwX<1zk>n>N<>5PaLE5s!*+1a z|B7O16zx$BpGLf-;W5N-X!r%hS2Rrd&uEzP(EvG6`xj-qG@@awM{A)@!g@p&k}B}U zP7Px{Vz-8oFAi&%@=s|P>lddrO!==-EMA90{*Q1HuGS62TN<7KEKO?o0^$oA_Egxt zWqbZ>I9=f9zd@|=^M9?de_mn#ZH4{AvR(YJqW<5P>lZ(-u>Y;X{!NAbKNWT_T%%(C z1BeUxU#PHOEZW7x=y?Lt5dQ=yAIF5~6~P5g%ndtnbd$~_h6!xZ$q^|JG~C_Og%?#W zCP!C|4l@k{N*95u| zrRr9W(mgr=$LNlo%4u7Qdf04Kb=RY+9(Dj!!)yfdv!L`xVc$_u&GMWWd^;>eqpA(| zi2O1{^e;lw_@bsdwEc_H8B1ALZBTuMPf=5?*P~^#si3Gf$f0cJi<-(^l_JYw)o?Ym zB&fy}+zeb+x#c-g26b^sT7*RPl0c$3MtOd)XuEL6rA63$6=H3>*O}UxVW=qjFPIGy z%zc;pT|U+Q$<>eN(yrl*YdGr~PI5U{)4P^?u7NGrK*lwgbqyxD`*zo_n^T6=()+gc zXr}Gp#;cizp=`rY#{Oj1{$x@|_w(*Ky<1Li#yOC64kQhli#|o(x2#WR+74_S%``lb zZFnMMKa#Z{0T+jRb^e~iv*qw)9Nw(Mo7BVhLxUUQYiIhm9Q_%`zN}+kQlGOotj^rC z_ix$zGxmL1`@WL@z?Nem;~30329x@)U5u+S>)f9-M8kKa%A~()`GM-b(FnuE#c? z{?wb{N3;BBnjbBD%xrXQzV_*v41YSypHB0qiyrmbuWXpBA{6VXwg2xUKWe6XN8bBP zu@#RRqh9|1FY=d#J0rM$Y=iyqMCvCS)0^JScm6QAapBVwpB>BaXS4j-G=H|(qO0pi z-=AF%0`Jmh-<{Vtr)zjxORQ_-sgvt(!QTL>gX^;!;sZTvKJ^s?0(R5Pr@rR#Mn%%d z`cHqH^hh-guTN|?<(hidjT>eRcO1hV$8g7Cv&!aNbN}LG^5lJ+b9ExcrEY9=<=VPZ z^BaSkK49BNfo&fJc0oWh?5=8N|HcY3N02#!%n_}vmUQF(jphtLl;wxg{1A4Ob#3hD zj#WpJezdJ#owmE@<0KD^f>Iv}FABO#t03Bk0{@oK_G9-WLV@poQT+QXQTpuYSgsRt zs1n8nmTTQ%zF%q;V|6~0^cEacF1)m`xFX@JlWHyf2`EG*Jy^8eV46a8THo@|YTMAy zNk0P~&d$*fKyFOH!nW#e@c?aKp?IJN-mP|x W`6l}g%R=+MUp}Drul6`g8~ None: + self.drone = drone + + def update(self): + image = np.zeros((400, 750, 3), dtype=np.uint8) + pitch = self.drone.get_pitch() + y1 = int(round((-5.6 * pitch + 200), 0)) + roll = self.drone.get_roll() + y2 = int(round((8.6 * roll + 375), 0)) + yaw = self.drone.get_yaw() + # speed = round((((self.drone.get_speed_x()) ** (2)) + + # ((self.drone.get_speed_y()) ** (2))) ** (1 / 2), 0) + speed = 0 + alt = self.drone.get_height() + vs = self.drone.get_speed_z() + bat = self.drone.get_battery() + + # yaw = 20 + # speed = 35 + # alt = 3.4 + # vs = 20 + # bat = 15 + + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.6 + thickness = 2 + + yaw_text_pos = (10, 30) + yaw_text = f'YAW: {yaw}' + + speed_text_pos = (10, 197) + speed_text = f'SPEED: {speed}' + + alt_text_pos = (660, 197) + alt_text = f'ALT: {alt}' + + vs_text_pos = (660, 167) + vs_text = f'VS: {vs}' + + bat_text_pos = (620, 30) + bat_text = f'BATTERY: {bat}' + + brown = (5, 129, 223) + blue = (255, 229, 50) + yellow = (0, 255, 255) + white = (255, 255, 255) + purple = (255, 0, 255) + red = (0, 0, 255) + + # SKY/GROUND + cv2.rectangle(image, (0, 0), (750, 200), blue, -1) + cv2.rectangle(image, (0, 200), (750, 400), brown, -1) + + # PITCH/UP + cv2.rectangle(image, (275, 171), (475, 173), white, -1) + cv2.rectangle(image, (225, 143), (525, 145), white, -1) + cv2.rectangle(image, (275, 115), (475, 117), white, -1) + cv2.rectangle(image, (225, 87), (525, 89), white, -1) + cv2.rectangle(image, (275, 59), (475, 61), white, -1) + + # PITCH/DOWN + cv2.rectangle(image, (275, 227), (475, 229), white, -1) + cv2.rectangle(image, (225, 255), (525, 257), white, -1) + cv2.rectangle(image, (275, 283), (475, 285), white, -1) + cv2.rectangle(image, (225, 311), (525, 313), white, -1) + cv2.rectangle(image, (275, 339), (475, 341), white, -1) + + # ROLL/MID + cv2.rectangle(image, (374, 190), (376, 210), white, -1) + + # ROLL/LEFT + cv2.rectangle(image, (331, 190), (333, 210), white, -1) + cv2.rectangle(image, (288, 190), (290, 210), white, -1) + cv2.rectangle(image, (245, 190), (247, 210), white, -1) + cv2.rectangle(image, (202, 190), (204, 210), white, -1) + cv2.rectangle(image, (159, 190), (161, 210), white, -1) + + # ROLL/RIGHT + cv2.rectangle(image, (417, 190), (419, 210), white, -1) + cv2.rectangle(image, (460, 190), (462, 210), white, -1) + cv2.rectangle(image, (503, 190), (505, 210), white, -1) + cv2.rectangle(image, (546, 190), (548, 210), white, -1) + cv2.rectangle(image, (589, 190), (591, 210), white, -1) + + cv2.rectangle(image, (300, y1 - 5), (450, y1 + 5), yellow, -1) + cv2.rectangle(image, (y2 - 5, 125), (y2 + 5, 275), yellow, -1) + + # TEXT/YAW + cv2.putText(image, yaw_text, yaw_text_pos, + font, font_scale, purple, thickness) + + # SPEED/TEXT + cv2.putText(image, speed_text, speed_text_pos, + font, font_scale, purple, thickness) + + # ALT/TEXT + cv2.putText(image, alt_text, alt_text_pos, + font, font_scale, purple, thickness) + + # VS/TEXT + cv2.putText(image, vs_text, vs_text_pos, font, + font_scale, purple, thickness) + + # BAT/TEXT + if bat > 20: + cv2.putText(image, bat_text, bat_text_pos, + font, font_scale, purple, thickness) + else: + cv2.putText(image, bat_text, bat_text_pos, + font, font_scale, red, thickness) + + cv2.imshow('attitude indicator', image) + cv2.waitKey(1) diff --git a/src/camera.py b/src/camera.py new file mode 100644 index 0000000..db75872 --- /dev/null +++ b/src/camera.py @@ -0,0 +1,42 @@ +from dataclasses import dataclass +import numpy as np +import cv2 +from pyzbar.pyzbar import decode + + +@dataclass +class BarcodeData: + rect: np.ndarray + polygon: np.ndarray + data: str + height_left: float + height_right: float + + +class Camera: + + def update_camera(self, frame, barcode_data): + cap = frame + + cap = cv2.resize(cap, (1280, 720)) + cap = cv2.cvtColor(cap, cv2.COLOR_BGR2RGB) + + pts = np.array([barcode_data.polygon], np.int32) + pts = pts.reshape((-1, 1, 2)) + cv2.polylines(cap, [pts], True, (255, 0, 255), 5) + + cv2.imshow('frame', cap) + cv2.waitKey(1) + + def decode_qr(self, frame): + cap = frame + + barcode_data = BarcodeData + for barcode in decode(cap): + barcode_data.rect = barcode.rect + barcode_data.polygon = barcode.polygon + barcode_data.data = barcode.data.decode('utf-8') + barcode_data.height_left = (barcode.polygon[0][1] - barcode.polygon[3][1]) + barcode_data.height_right = (barcode.polygon[1][1] - barcode.polygon[2][1]) + + return barcode_data diff --git a/src/controller.py b/src/controller.py new file mode 100644 index 0000000..373d76d --- /dev/null +++ b/src/controller.py @@ -0,0 +1,25 @@ + + +class Controller: + + def __init__(self, drone, move_speed): + self.move_speed = move_speed + self.drone = drone + + def rc_control(self, lr=0, fb=0, up=0, y=0): + self.drone.send_rc_control(left_right_velocity=lr, + forward_backward_velocity=fb, + up_down_velocity=up, + yaw_velocity=y) + + def right(self): + self.rc_control(lr=self.move_speed) + + def left(self): + self.rc_control(lr=-self.move_speed) + + def forward(self): + self.rc_control(fb=self.move_speed) + + def backward(self): + self.rc_control(fb=-self.move_speed) diff --git a/src/main.py b/src/main.py new file mode 100644 index 0000000..e926eae --- /dev/null +++ b/src/main.py @@ -0,0 +1,258 @@ +from threading import Thread, Event +import time +from djitellopy import Tello +import keyboard + +from artificial_horizon import ArtificialHorizon +from camera import Camera + + +class TelloDrone: + + def force_emergency_stop(self): + self.drone.emergency() + self.stop_controller.set() + + class TelloKillSwitch(Thread): + + def __init__(self, tc_handler): + Thread.__init__(self) + self.tc_handler = tc_handler + + def run(self): + keyboard.wait('space') + self.tc_handler.force_emergency_stop() + + class Threading(Thread): + interval = 1.0 + running = None + func = None + + def __init__(self, interval, event, func): + Thread.__init__(self) + self.running = event + self.interval = interval + self.func = func + + def run(self): + while not self.running.wait(self.interval): + self.func() + + def battery_temp_check(self): + print(f"\nBATTERY: {self.drone.get_battery()}") + print(f"TEMPERATURE: {self.drone.get_temperature()}\n") + + def attitude_indicator(self): + self.artificial_horizon.update() + + def show_camera(self): + frame = self.drone.get_frame_read().frame + barcode_data = self.camera.decode_qr(frame) + self.barcode_rect = barcode_data.rect + self.barcode_polygon = barcode_data.polygon + self.barcode_data = barcode_data.data + self.height_14 = barcode_data.height_left + self.height_23 = barcode_data.height_right + + self.camera.update_camera(frame, barcode_data) + + def follow_qr(self): + + if self.barcode_data == 'right': + self.right_90 = True + self.left_90 = False + elif self.barcode_data == 'left': + self.left_90 = True + self.right_90 = False + # 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 + + 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 mission_func(self): + self.drone.takeoff() + self.rc_control(0, 0, 0, 0) + time.sleep(5) + cnt = 0 + + while cnt < 450: + # 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: + self.rc_control(0, 0, 0, -10) + print("left") + elif self.drone_right: + print("right") + self.rc_control(0, 0, 0, 10) + elif self.drone_rotate_stop: + print("straight") + if self.roll_left: + self.rc_control(-15, 0, 0, 0) + print("Turlaj sie w lewo") + elif self.roll_right: + self.rc_control(15, 0, 0, 0) + print("Turlaj sie w prawo") + elif self.roll_ok: + print("Turlaj dropsa") + if self.drone_up: + print("up") + self.rc_control(0, 0, 15, 0) + elif self.drone_down: + print("Hubert") + self.rc_control(0, 0, (-15), 0) + elif self.drone_straight: + print("straight as an arrow") + width = self.barcode_rect[2] + height = self.barcode_rect[3] + if width > 250 or height > 210: + self.drone.land() + # self.rc_control(0, 0, 0, 0) + # print("Jesteśmy totalnie zajebiści ale chóbert nie") + # print(self.barcode_data) + # if self.left_90 == True: + # self.drone.rotate_counter_clockwise(90) + # print("90 left") + # self.left_90 = False + # elif self.right_90 == True: + # self.drone.rotate_clockwise(90) + # print("90 right") + # self.right_90 = False + 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.drone.land() + + def main(self): + # KILLSWITCH + self.kill_switch = self.TelloKillSwitch(self) + self.kill_switch.start() + self.stop_controller = Event() + + # START TELLO + self.drone = Tello() + self.drone.connect() + + # CAMERA + self.drone.streamon() + self.camera = Camera() + camera_show = self.Threading( + 0.001, self.stop_controller, self.show_camera) + camera_show.start() + + # ATTITUDE INDICATOR + self.artificial_horizon = ArtificialHorizon(self.drone) + attitude_indicator = self.Threading( + 0.001, self.stop_controller, self.attitude_indicator) + attitude_indicator.start() + + # BATTERY + battery_check = self.Threading( + 25, self.stop_controller, self.battery_temp_check) + battery_check.start() + + # FOLLOWQR + follow_qr = self.Threading(0.3, self.stop_controller, self.follow_qr) + follow_qr.start() + + def __init__(self): + 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.barcode_data = '' + self.height_14 = 0 + self.height_23 = 0 + self.right_90 = False + self.left_90 = False + self.roll_left = False + self.roll_right = False + self.roll_ok = False + + self.main() + + self.mission_func() + + self.drone.streamoff() + self.drone.end() + + +if __name__ == "__main__": + td = TelloDrone() diff --git a/src/test_artificial_horizon.py b/src/test_artificial_horizon.py new file mode 100644 index 0000000..c76ccc9 --- /dev/null +++ b/src/test_artificial_horizon.py @@ -0,0 +1,60 @@ +from dataclasses import dataclass +from artificial_horizon import ArtificialHorizon +import time + + +@dataclass +class drone_data: + pitch: float + yaw: float + roll: float + + altitude: float + vertical_speed: float + battery: float + + def __init__( + self, + pitch: float = 0, + yaw: float = 0, + roll: float = 0, + altitude: float = 0, + vertical_speed: float = 0, + battery: float = 0 + ) -> None: + self.pitch = pitch + self.yaw = yaw + self.roll = roll + self.altitude = altitude + self.vertical_speed = vertical_speed + self.battery = battery + + def get_pitch(self): + return self.pitch + + def get_yaw(self): + return self.yaw + + def get_roll(self): + return self.roll + + def get_height(self): + return self.altitude + + def get_speed_z(self): + return self.vertical_speed + + def get_battery(self): + return self.battery + + +def test(): + data = drone_data() + ah = ArtificialHorizon(data) + ah.update() + + time.sleep(5) + + +if __name__ == "__main__": + test()