教育用小型ドローンTELLOを用いた追従機能の開発

ー初めての投稿ー
初めまして，現在大学四年生です。
私は，TELLOを用いて追従機能の開発（といってもそんなに立派なものではありませんが...）をしています。
担当の教授の専門分野が電子回路系で，プログラムのことを聞けずお手上げ状態です。
至らぬ点もございますが，どうかお助けください。

ー開発環境ー
---windows10
---python

ープログラミングの中身ー

import socket
import threading
import cv2
import numpy as np

event = threading.Event()

face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

cap = cv2.VideoCapture(0)

width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))

X = width//2
Y = height//3

W = width//2
H = height//2

x = width//2
y = height//3

w = width//2
h = height//2

gain_vx = 3
gain_vy = 3

gain_bf = 0.0022

gain_x_1 = 0.15
gain_y_1 = 0.1

gain_x_2 = 0.0003
gain_y_2 = 0.00005

dx = X - x
dy = Y - y

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
sock.bind(("", 9000))

tello_address = ("192.168.10.1", 8889)

def send_command(msg):
print(msg)
msg = msg.encode(encoding="utf-8")
sock.sendto(msg, tello_address)
event.wait()
event.clear()

def recv():
while True:
try:
data, server = sock.recvfrom(1518)
print(data.decode(encoding="utf-8"))
event.set()
except Exception:
break
recvThread = threading.Thread(target=recv)
recvThread.start()

def tracking(dx,dy,vx,vy):

if 5 < dx <= 150:
    print('right')
    drone.right(gain_x_1*dx + gain_vx*vx)

if -150 <= dx < -5:
    print('left')
    drone.left(- gain_x_1*dx - gain_vx*vx)

if dx > 150:
    print('right')
    drone.right(gain_x_2*dx*dx + gain_vx*vx/2)

if dx < -150:
    print('left')
    drone.left(gain_x_2*dx*dx - gain_vx*vx/2)

if dy > 5:
    print('down')
    drone.down(gain_y_2*dy*dy + gain_vy*vy/2)

if dy < 5:
    print('up')
    drone.up(gain_y_2*dy*dy + gain_vy*vy/2)

else:
    print("___ERROR___")

def main():
try:
send_command("command")
send_command("streamon")
send_command("takeoff")

    cap = cv2.VideoCapture("udp://0.0.0.0:11111")

    while True:
        ret, img = cap.read()
        cv2.rectangle(img, (X, Y), (X+W, Y+H), color=(0,0,255),thickness= 4)


        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        faces = face_cascade.detectMultiScale(gray, 1.3, 5)
        for (x,y,w,h) in faces:
            cv2.rectangle(img,(x,y),(x+w,y+h),(255,0,0),2)
            roi_gray = gray[y:y+h, x:x+w]
            roi_color = img[y:y+h, x:x+w]
        cv2.imshow('img', img)

        tracking(dx,dy,vx,vy)


        if cv2.waitKey(1) & 0xFF == ord('q'):
            break




except Exception as ex:
    print (ex)

finally:
    cap.release()
    cv2.destroyAllWindows()
    send_command("streamoff")
    sock.close()
    print("--- END ---")

if name == "main":
main()

ーうまくいかない点ー
①フレームスキップを入れるとしたらどこが適切か
➁そもそもフレームスキップ云々以前の問題なのか

よろしくお願いいたします。

ー参考にさせていただいたサイトー
https://atsblog.org/tello_tracking_camshift/
http://totech.hateblo.jp/entry/2017/10/22/100726