Hi everyone,
I am fiddling around with video and with the gamepad and I am slowly getting there. My script is in a state which is usable. You may get some ideas from it and I am looking forward for hints.
I started from the Tello3 demo and enlarged it step by step. I work with Python3 under Windows.
With all the official apps I got glitches in the video. It means that movements are shown jerky. This disturbs me a lot. I tried a range extender which seems to make it better but not satisfactory. In my script I just do basic stuff with opencv and there are no glitches. Very fine, so far.
Just adding video and flying Tello by typing commands like "forward 100" is not satisfactory, so I added joystick support. I am testing with a rather inexpensive gamepad (STRIKE NX) and the inputs library. Gamesir t1d won't connect to the PC.
If no gamepad is found, the gamepad task just terminates and you can fly Tello by typing commands.
Room for improvement:
Okay, and here is the script, have some fun with it!
I am fiddling around with video and with the gamepad and I am slowly getting there. My script is in a state which is usable. You may get some ideas from it and I am looking forward for hints.
I started from the Tello3 demo and enlarged it step by step. I work with Python3 under Windows.
With all the official apps I got glitches in the video. It means that movements are shown jerky. This disturbs me a lot. I tried a range extender which seems to make it better but not satisfactory. In my script I just do basic stuff with opencv and there are no glitches. Very fine, so far.
Just adding video and flying Tello by typing commands like "forward 100" is not satisfactory, so I added joystick support. I am testing with a rather inexpensive gamepad (STRIKE NX) and the inputs library. Gamesir t1d won't connect to the PC.
If no gamepad is found, the gamepad task just terminates and you can fly Tello by typing commands.
Room for improvement:
- Communication does not start reliably. If you see an "OK", it is OK. If not, quit and restart.
- Tons of messages from opencv/ffmpeg indicating that there are no (or not enough) key frames. These blue and red ones. Maybe there is a way to increase a timeout in opencv.
- Both input() and inputs.get_gamepad() are blocking. When you quit the program via keyboard you need to move a joystick. Vice versa, when you quit the program via joystick button, you need to press enter.
- Video can be started and stopped only once, then the video task terminates
- I never learned object oriented programming and it looks like I never will.
- If you don't enter a command for 15 seconds (not very likely), Tello will land automatically.
Okay, and here is the script, have some fun with it!
Python:
import threading
import socket
import sys
import time
import os
import inputs # for gamepad
# import opencv 4.2.0
import cv2
#-----------------------------------------------------------------------------------
def recv():
global TimeSent
global Ready
global Running
global DataDecoded
print ("Tello recv task started")
count = 0
while Running:
RecvError = False
try:
data, server = sock.recvfrom(1518)
except Exception as e:
RecvError = True
if (str(e) == 'timed out'):
print (".", end = "") # python2 users, please remove this line
pass
else:
print ('\n------------------- Exception: ' + str(e) + '\n')
break
if (not RecvError):
DataDecoded = data.decode(encoding="utf-8")
print(DataDecoded)
Ready = True
print ("recv ended")
#--------------------------------------------------------------
def rcCommand (RcArray):
'''create a command like rc 100 100 100 100 from an array of 4 integers'''
RcCommand = 'rc'
for Count in range (0,4):
if (RcArray[Count] > 100):
RcArray[Count] = 100
if (RcArray[Count] < -100):
RcArray[Count] = -100
RcCommand = RcCommand + ' ' + str(RcArray[Count])
# print (RcCommand)
return (RcCommand)
#-----------------------------------------------------------------------------------
def pad():
global Running
global RunningVideo
global Rc
global sock
global Ready
pads = inputs.devices.gamepads
if (len(pads) > 0):
print ("found gamepad" + str(pads[0]))
while (Running):
msgPad = ''
events = inputs.get_gamepad()
for event in events:
# print(event.ev_type, event.code, event.state)
msgPad = "" # preset
if (event.ev_type == "Absolute"):
# print (event.code)
stickVal = int(int (event.state) / 327) # / 32767 * 100
if (event.code == "ABS_X"):
Rc[3] = stickVal
elif (event.code == "ABS_Y"):
Rc[2] = stickVal
elif (event.code == "ABS_RX"):
Rc[0] = stickVal
elif (event.code == "ABS_RY"):
Rc[1] = stickVal
msgPad = rcCommand (Rc)
elif (str(event.ev_type) == "Key"):
if (event.code == "BTN_NORTH"):
if (str(event.state) == "1"):
msgPad = "takeoff"
Ready = False
elif (event.code == "BTN_SOUTH"):
if (str(event.state) == "1"):
msgPad = "land"
Ready = False
elif (event.code == "BTN_WEST"):
if (str(event.state) == "1"):
pass
### something to do with this button
elif (event.code == "BTN_EAST"):
if (str(event.state) == "1"):
pass
### something to do with this button
elif (event.code == "BTN_TL"):
if (str(event.state) == "1"):
RunningVideo = True
print ("starting video")
elif (event.code == "BTN_TR"):
if (str(event.state) == "1"):
RunningVideo = False
print ("stopping video")
elif (event.code == "BTN_START"):
if (str(event.state) == "1"):
msgPad = "rc -100 -100 -100 100"
print ("starting motors")
elif (event.code == "BTN_SELECT"):
if (str(event.state) == "1"):
msgPad = "emergency"
Running = False
if (msgPad != ""):
if not 'rc' in msgPad:
print (msgPad)
msgPad = msgPad.encode(encoding="utf-8")
sent = sock.sendto(msgPad, tello_address)
else:
print ("No gamepad found")
print ("pad task ended")
#-----------------------------------------------------------------------------------
def video():
global out
global displayVideo
global writeVideo
global RunningVideo
global Running
print ("video task started")
# wait for frame
ret = False
# scale down
scale = 1
# wait until video is started by user
while Running and not RunningVideo:
time.sleep (1)
if not Running:
print ("video task ended")
return
time.sleep(3) # give tello some time to start video stream
print ("starting video window")
telloVideo = cv2.VideoCapture("udp://@0.0.0.0:11111")
if (telloVideo.isOpened() == False):
print("Unable to read camera feed")
else:
time.sleep(3) # give ffmpeg some time to analyze video stream
frame_width = int(telloVideo.get(cv2.CAP_PROP_FRAME_WIDTH))
frame_height = int(telloVideo.get(cv2.CAP_PROP_FRAME_HEIGHT))
frame_fps = telloVideo.get(cv2.CAP_PROP_FPS)
print (frame_width, ' x ', frame_height, ' ', frame_fps, 'fps' )
if writeVideo:
# find a filename which doesn't yet exist
found = False
count = 0
while (not found):
filenameToWrite = 'telloVideo' + str(count).zfill(3) + '.avi'
# filenameToWrite = os.path.join (pathToWrite, filenameToWrite)
if (os.path.exists(filenameToWrite)):
count = count + 1
else:
found = True
fourcc = cv2.VideoWriter_fourcc(*'XVID')
# fourcc = cv2.VideoWriter_fourcc(*'MJPG')
out = cv2.VideoWriter(filenameToWrite, fourcc, frame_fps, (frame_width,frame_height))
videoInterval = 1 / frame_fps
timeNextFrame = time.time() + videoInterval
timeStart = time.time()
numFramesReceived = 0
numFramesWritten = 0
numGlitches = 0
while Running and RunningVideo:
# Capture frame-by-frame
ret, frame = telloVideo.read()
if(ret):
# Our operations on the frame come here
numFramesReceived = numFramesReceived + 1
if writeVideo:
out.write(frame)
numFramesWritten = numFramesWritten + 1
timeNextFrame = timeNextFrame + videoInterval
if (time.time() > timeNextFrame):
numGlitches = numGlitches + 1
# Display the resulting frame
if (displayVideo):
if (scale != 1):
height , width , layers = frame.shape
new_h=int(height/scale)
new_w=int(width/scale)
frame = cv2.resize(frame, (new_w, new_h)) # <- resize for improved performance
cv2.imshow('Tello',frame)
cv2.waitKey(1) # this is essential, otherwise cv2 won't show anything!
timeVideo = time.time() - timeStart
# When everything is done, clean up
telloVideo.release()
out.release()
cv2.destroyAllWindows()
# do some statistics
print ("time: ", timeVideo)
print (numFramesReceived, "frames received, ", numFramesReceived/timeVideo, " fps")
print (numFramesWritten, "frames written", numFramesWritten/timeVideo, " fps")
print (numGlitches, " glitches")
print ("video task ended")
#-----------------------------------------------------------------------------------
host = ''
port = 9000
locaddr = (host,port)
# Create a UDP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.settimeout (1)
tello_address = ('192.168.10.1', 8889)
sock.bind(locaddr)
TimeSend = 0
Ready = True
Rc = [0,0,0,0]
print ('\r\n\r\nTello Python3 Demo.\r\n')
print ('Tello: command takeoff land flip forward back left right \r\n up down cw ccw speed speed? battery?\r\n')
print ('end -- quit demo.\r\n')
Running = True
RunningVideo = False
DataDecoded = ""
out = None
displayVideo = True
writeVideo = True
#recvThread create
recvThread = threading.Thread(target=recv)
recvThread.start()
#videoThread create
videoThread = threading.Thread(target=video)
videoThread.start()
# padThread create
padThread = threading.Thread(target=pad)
padThread.start()
msg = 'command'
while Running:
if (msg == ''):
msg = input(">")
if 'video' in msg:
RunningVideo = True;
msg = 'streamon'
elif 'start' in msg:
msg = "rc -100 -100 -100 100" # start motors
elif 'end' in msg:
print ('...')
Running = False
RunningVideo = False
msg = ''
if (msg != ''):
# Send data
msg = msg.encode(encoding="utf-8")
sent = sock.sendto(msg, tello_address)
TimeSent = time.time()
print (str(sent) + ' bytes sent')
msg = ''
TimeShutdown = 2
print ("Will shut down in " + str(TimeShutdown) + " seconds")
time.sleep (TimeShutdown) # give recv task some time to end
sock.close()
