Follow mission pad for Tello EDU and Tello Talent

follow_mission_pad.py

# Imports
from djitellopy import Tello
import threading
import time
import math

# Maximum speed sent to send_rc_control
MAX_SPEED = 1.5

# If the distance to the target is less than the minimum then just set to zero to keep Tello close
MIN_DISTANCE = 10

MAX_DISTANCE = 100

# Create and connect
tello = Tello()
tello.connect()

# Turn on mission pad recognition
tello.enable_mission_pads()

#tello.set_mission_pad_detection_direction(0)  # forward detection only

# Downward mission pad detection
tello.set_mission_pad_detection_direction(0)

# Print the current battery
print(f'Battery: {tello.get_battery()}')

# Takeoff
tello.takeoff()

# Store this so we don't spam the board with LED commands
previous_pad_id = -1

# After 30s we'll land
count = 300

# Handles control logic
def get_state():

    # So we can store the pad_id so we only detect a change
    global previous_pad_id
    global count

    while True:

        # Look for a mission pad
        pad_id = tello.get_mission_pad_id()

        # Pad detected
        if 1 <= pad_id <= 8:

            # Detect the pad id and position of Tello relative to pad center
            pad_x_dist = tello.get_mission_pad_distance_x()
            pad_y_dist = tello.get_mission_pad_distance_y()
            pad_z_dist = tello.get_mission_pad_distance_z()

            # Display which pad is detected - added this logic so we don't spam the board
            if pad_id != previous_pad_id:
                tello.send_expansion_command(f'mled s b {pad_id}')
                tello.send_expansion_command(f'led 0 255 0')
                previous_pad_id = pad_id


            # Distance from drone to pad in 2d space, we'll handle altitude differently
            distance_to_pad_center = math.sqrt(pad_x_dist ** 2 + pad_y_dist ** 2)

            # Set the x/y speeds
            f_b_speed = int((MAX_SPEED * pad_x_dist) / 2) * -1 # Need to invert this
            l_r_speed = int((MAX_SPEED * pad_y_dist) / 2)
            u_d_speed = 0

            # Try to maintain a reasonable altitude above the pad
            if pad_z_dist < 80:
                u_d_speed = 30
            elif pad_z_dist > 120:
                u_d_speed = -30

            # If we're in the distance range then don't move
            # If we can't find the pad then stop moving
            if abs(distance_to_pad_center) <= MIN_DISTANCE or abs(distance_to_pad_center) > MAX_DISTANCE: #pad_id == -1:
                f_b_speed = 0
                l_r_speed = 0
                u_d_speed = 0

            # For debugging purposes
            print(f'pad: {pad_id}, dist center: {distance_to_pad_center}, x dist: {pad_x_dist}, y dist: {pad_y_dist}, z dist: {pad_z_dist}')

            # Send the control inputs to Tello
            tello.send_rc_control(l_r_speed, f_b_speed, u_d_speed, 0)

        # No pad found
        else:

            # Visual indicator that no pad is found
            #tello.send_expansion_command(f'mled s r X')
            #tello.send_expansion_command(f'led 255 0 0')

            # Display which pad is detected - added this logic so we don't spam the board
            if pad_id != previous_pad_id:
                tello.send_expansion_command(f'mled s r X')
                tello.send_expansion_command(f'led 255 0 0')
                previous_pad_id = pad_id
            
            # Set control inputs to zero
            tello.send_rc_control(0, 0, 0, 0)

        time.sleep(0.1)

state_thread = threading.Thread(target=get_state)
state_thread.daemon = True
state_thread.start()

# So we can kill the script with ctrl-c
while True:
    time.sleep(1)