Exemple #1
0
def main():
    print("Initialising joystick...")
    joystick = Joystick()
    joystick.initJoystick()
    if (joystick.initalised):
        print("Setting up pygame window...")
        pygame.init()

        screen = pygame.display.set_mode((960, 720))
        pygame.display.set_caption("OpenCV camera stream on Pygame")
        screen.fill([0, 0, 0])
        noConnImage = pygame.image.load(r'.\No_Connection_Error.png')
        screen.blit(noConnImage, (0, 0))
        print("Initialising connection to drone...")
        drone = Tello()
        # Send command message to initiate the drone
        drone.send_command("command")
        # Enable the stream
        drone.send_command("streamon")
        drone.receive_telemetry()

        stabilized = True
        takeoff = False

        startTime = time.time()

        # Start video capture
        drone.get_video()

        while True:
            drone_feed = pygame.surfarray.make_surface(drone.get_frame())
            #Rotate image - Issue with OpenCV
            drone_feed = pygame.transform.rotate(drone_feed, 270)
            drone_feed = pygame.transform.flip(drone_feed, True, False)
            screen.blit(drone_feed, (0, 0))

            currentTime = time.time()
            eventType = None
            axisX = None  # Axis 0 = X
            axisY = None  # Axis 1 = Y
            axisZ = None  # Axis 2 = Rotational Z
            height = None  # Axis 3 = Height
            for event in pygame.event.get():
                axisX = joystick.getDevice().get_axis(0)
                axisY = joystick.getDevice().get_axis(1)
                axisZ = joystick.getDevice().get_axis(2)
                height = joystick.getDevice().get_axis(3)
                try:
                    if (axisX != None or axisY != None or axisZ != None):
                        if (axisX == None):
                            axisX = 0
                        if (axisY == None):
                            axisY = 0
                        if (axisZ == None):
                            height = 0
                        if ((currentTime - startTime) >= DEBOUNCE):
                            startTime = time.time()
                            print("rc " + str(int(axisX * 100)) + " " +
                                  str(int(-1 * axisY * 100)) + " " +
                                  str(int(-1 * height * 100)) + " " +
                                  str(int(axisZ * 100)))
                            drone.send_command("rc " + str(int(axisX * 100)) +
                                               " " +
                                               str(int(-1 * axisY * 100)) +
                                               " " +
                                               str(int(-1 * height * 100)) +
                                               " " + str(int(axisZ * 100)))
                            stabilized = False
                    else:
                        if (not stabilized):
                            drone.send_command("rc 0 0 0 0")
                            stabilized = True
                    if (joystick.getDevice().get_button(0) == 1):
                        print("LAND")
                        drone.send_command("land")
                    if (joystick.getDevice().get_button(1) == 1):
                        print("TAKEOFF")
                        drone.send_command("takeoff")

                except KeyboardInterrupt:
                    # print ('\n . . .\n')
                    drone.send_command("land")
                    break
            pygame.display.update()
    else:
        print('Joystick failed to initalise... Exiting...')
        exit(1)