Cleanup

.gitignore

@@ -0,0 +1,3 @@
+venv/*
+tello-venv/*
+

src/artificial_horizon.py

@@ -0,0 +1,118 @@
+import cv2
+import numpy as np
+
+
+class ArtificialHorizon:
+    def __init__(self, drone) -> 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)

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

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)

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

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