tello.connect()
print('Hello Tello!')
tello.takeoff()
tello.streamon()
frame_read = tello.get_frame_read()
cv2.imwrite("picture.png", frame_read.frame)
tello.move_back(20)
frame_read = tello.get_frame_read()
cv2.imwrite("picture2.png", frame_read.frame)
tello.move_forward(20)
tello.land()
tello.streamoff()
battery_data = tello.get_battery()
print('Battery level: ' + battery_data + '%')
barometer_data = tello.get_barometer()
print('Barometer: ' + barometer_data + ' cm')
temperature = tello.get_temperature()
print('Temperature: ' + temperature + ' °C')
class TelloSelfControl(object):
def __init__(self):
self.tello = Tello()
self.is_flying = False
self.frame_read = None
self.cap = None
self.faces = None
self.displayMsg = ''
self.drone_speed = (0, 0, 0, 0)
self.vDistance = (0, 0, 0, 0)
self.windowSize = None
self.windowCenter = None
self.objectCenter = None
self.isOverride = False
self.velocity_left_right = 0
self.velocity_up_down = 0
self.velocity_forward_backward = 0
self.velocity_yaw = 0
self.nFace = (0, 0, 0, 0)
self.last_seen = (0, 0, 0, 0)
self.last_last_seen = (0, 0, 0, 0)
self.valid_lock = False
self.valid_lock_count = 0
self.is_face_lost = False
self.imgCount = 0
def run(self):
# Tello setup
if use_drone:
self.frame_read = self.drone_setup()
else:
self.cap = self.video_capture_setup()
if not self.cap.isOpened:
throw_error('Error opening video capture')
# init cv2 window
cv.namedWindow('Tracker')
# run frames
while True:
# reset
self.isOverride = False
self.displayMsg = ''
self.velocity_left_right = 0
self.velocity_up_down = 0
self.velocity_forward_backward = 0
self.velocity_yaw = 0
# img count for status data form tello
if self.imgCount == 100:
if use_drone:
self.status_update()
self.imgCount = 0
else:
self.imgCount += 1
if use_drone:
if self.frame_read.stopped:
self.frame_read.stop()
self.tello.streamoff()
throw_error('stream turned off')
break
# get single frame
frame = self.frame_read.frame
else:
ret, frame = self.cap.read()
if frame is None:
throw_error('No captured frame -- Break')
break
# get window properties
self.windowSize = cv.getWindowImageRect('Tracker')
self.windowCenter = (self.windowSize[2] // 2,
self.windowSize[3] // 2)
time.sleep(1 / FPS)
# face detection
self.faces = self.face_detection(frame)
# Overridecontrols
if use_drone:
self.override_detection()
# automation controls
if not self.isOverride and not manual_flight:
self.auto_control()
# send rc controls
if use_drone:
self.tello.send_rc_control(self.velocity_left_right,
self.velocity_forward_backward,
self.velocity_up_down,
self.velocity_yaw)
# show frame in window
self.display(frame)
if self.nFace is not self.last_seen:
self.last_last_seen = self.last_seen
self.last_seen = self.nFace
# Exit key esc
key = cv.waitKey(10)
if key == 27:
self.tello.end()
throw_error('Close application')
# drone setup
def drone_setup(self):
if not self.tello.connect():
throw_error('Could not connect to Tello')
if not self.tello.streamon():
throw_error('Could not load stream')
print(self.tello.get_battery()[:2])
return self.tello.get_frame_read()
def video_capture_setup(self):
return cv.VideoCapture(args.camera)
# frame detection
def face_detection(self, frame):
gray_frame = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
gray_frame = cv.equalizeHist(gray_frame)
objects = face_cascade.detectMultiScale(gray_frame)
return objects
# frame visualsation
def display(self, frame):
self.drone_speed = (self.velocity_yaw, self.velocity_up_down,
self.velocity_forward_backward)
if not None:
for (x, y, w, h) in self.faces:
frame = cv.circle(frame, (x + w // 2, y + h // 2), 10,
(78, 214, 99), 2) # center face
frame = cv.rectangle(frame, (x, y), (x + w, y + h),
(78, 214, 99), 2) # face
frame = cv.putText(frame, 'Width: ' + str(w), LEFT1, FONT,
FONTSCALE, FONTCOLOR, LINETYPE)
frame = cv.putText(frame, 'Height: {0}'.format(str(h)), LEFT2,
FONT, FONTSCALE, FONTCOLOR, LINETYPE)
frame = cv.putText(frame, self.displayMsg, LEFT3, FONT, FONTSCALE,
FONTCOLOR, LINETYPE) # data message
frame = cv.putText(frame, str(self.vDistance), TOPLEFT, FONT,
FONTSCALE, FONTCOLOR,
LINETYPE) # distance vektor to center
frame = cv.putText(frame, str(self.drone_speed), (10, 100), FONT,
FONTSCALE, FONTCOLOR,
LINETYPE) # speed send to drone
frame = cv.circle(frame, self.windowCenter, 10, (225, 15, 47),
2) # center window
frame = cv.rectangle(frame, (self.windowCenter[0] - TARGET_SIZE // 2,
self.windowCenter[1] - TARGET_SIZE // 2),
(self.windowCenter[0] + TARGET_SIZE // 2,
self.windowCenter[1] + TARGET_SIZE // 2),
(225, 15, 47), 2) # distance area
frame = cv.rectangle(
frame,
(self.windowCenter[0] - tx // 2, self.windowCenter[1] - ty // 2),
(self.windowCenter[0] + tx // 2, self.windowCenter[1] + ty // 2),
(255, 191, 0), 2) # tolerance area
(x, y, w, h) = self.last_last_seen
frame = cv.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 238),
2) # last last seen face position
(x, y, w, h) = self.last_seen
frame = cv.rectangle(frame, (x, y), (x + w, y + h), (255, 191, 0),
2) # last last seen face position
cv.imshow('Tracker', frame)
# drone autocontrol
def auto_control(self):
if isinstance(self.faces, tuple):
self.face_lost()
return
# we validate which face has the highest chance to be the same face as the last seen face the valid_value
# describes the diffrence between detected and last seen face. the face with the lowest valid_value will be
# selected
valid_value = math.inf
self.nFace = (0, 0, 0, 0)
for face in self.faces:
vD = self.get_dist_vector(face)
value = abs(vD[0]) + abs(vD[1]) + abs(vD[2])
if value < valid_value:
self.nFace = face
valid_value = value
self.displayMsg += str(valid_value) + ' / '
(x, y, w, h) = self.nFace
# if the last face isnt locked, we need to check the counter
if not self.valid_lock:
self.valid_lock_fnc(valid_value)
# if the last face was locked and the detected valid_value has more than 200 difference to the last face,
# we need to clal face_lost()
if valid_value > 200 and self.valid_lock:
self.nFace = self.last_seen
self.face_lost()
print('-- Face lost', valid_value)
return
# if the face was lost last frame, the lock is open to prevent that something wrong gonna be tracked
if self.is_face_lost:
self.valid_lock_count = 0
self.valid_lock = False
self.is_face_lost = False
self.objectCenter = (x + w // 2, y + h // 2) # face center
# calculate the distance vector from center of target to center of window(camera view)
vCenter = numpy.array(
(self.windowCenter[0], self.windowCenter[1], TARGET_SIZE))
vTarget = numpy.array(
(self.objectCenter[0], self.objectCenter[1], self.nFace[3]))
self.vDistance = vCenter - vTarget
# set the right speed for the distance
sX = sY = sZ = SPEED
if abs(self.vDistance[0]) < tx:
sX = sX // 2
if abs(self.vDistance[1]) < ty:
sY = sY // 2
if self.vDistance[2] > 2 * tz:
sZ = sZ // 2
# set the movement of the drone by analysing the data. (tx, ty, tz are the tolerance distances. Within these
# distances the drone stays still)
# X - axis correction
if self.vDistance[0] + tx // 2 < 0:
self.velocity_yaw = sX
self.displayMsg += 'turn right /'
elif self.vDistance[0] - tx // 2 > 0:
self.velocity_yaw = -sX
self.displayMsg += 'turn left /'
# Y - axis correction
if self.vDistance[1] + ty // 2 < 0:
self.velocity_up_down = -sY
self.displayMsg += 'move down /'
elif self.vDistance[1] - ty // 2 > 0:
self.velocity_up_down = sY
self.displayMsg += 'move up /'
# Z - axis correction
if self.vDistance[2] < 0:
self.velocity_forward_backward = -sZ
self.displayMsg += 'move backward /'
elif self.vDistance[2] - tz > 0:
self.velocity_forward_backward = sZ
self.displayMsg += 'move forward /'
# last seen valuation
def get_dist_vector(self, face):
(x, y, w, h) = face
(lx, ly, lw, lh) = self.last_seen
v1 = numpy.array(
(x + w // 2, y + h // 2, w)) # vector off detected face
v2 = numpy.array(
(lx + lw // 2, ly + lh // 2, lw)) # vector off last valid face
return v1 - v2
# we need to handle the drone in case the face got lost
def face_lost(self):
self.is_face_lost = True
(x, y, w, h) = self.last_seen
(lx, ly, lw, lh) = self.last_last_seen
self.displayMsg = 'x: {}, y: {} / x: {}, y: {}'.format(x, y, lx, ly)
if x > lx:
self.velocity_yaw = SPEED
else:
self.velocity_yaw = -SPEED
# if a new face is detected and no other was got locked, we need to valid this face.
# For example we need to check that the detected face is just caused of a noise image quality.
# For this we confirm that the face was 10 frames in a row detected. if not so, an other face could get locked
def valid_lock_fnc(self, valid_value):
if valid_value < 200:
self.valid_lock_count += 1
else:
self.valid_lock_count = 0
print('-- valid lock count: ', self.valid_lock_count)
if self.valid_lock_count >= 10:
self.valid_lock = True
print('-- valid lock: TRUE')
# override by key input
def override_detection(self):
key = cv.waitKey(1)
if key is not -1:
self.isOverride = True
if key == ord('t'):
if self.tello.takeoff():
self.is_flying = True
elif key == ord('l'):
self.tello.land()
self.is_flying = False
elif key == ord('q'):
self.tello.land()
self.frame_read.stop()
self.is_flying = False
close()
else:
if key == ord('w'):
self.velocity_forward_backward = SPEED
if key == ord('s'):
self.velocity_forward_backward = -SPEED
if key == ord('a'):
self.velocity_left_right = -SPEED
if key == ord('d'):
self.velocity_left_right = SPEED
if key == ord('y'):
self.velocity_up_down = SPEED
if key == ord('h'):
self.velocity_up_down = -SPEED
if key == ord('g'):
self.velocity_yaw = -3 * SPEED
if key == ord('j'):
self.velocity_yaw = 3 * SPEED
def status_update(self):
print('Tello Battery: {}'.format(self.tello.get_battery()[:2]))
print('Tello Temperature: {}'.format(self.tello.get_temperature()[:2]))
log.append('{}: Laptop identified, fire the lasers!'.format(datetime.now()))
index = label.index('laptop')
output_image = addLaser(output_image,index)
if 'dog' in label:
log.append('{}: Dog identified...woof'.format(datetime.now()))
else:
output_image = img
mpID, x, y, z = missionPadXYZ()
if mpID == -1:
mp_str = "404: Mission Pad Not Found"
else:
mp_str = "Mission Pad:{} at X:{} Y:{} Z:{}".format(mpID, x, y, z)
bat_str = "Battery: {}%".format(tello.get_battery())
temp_str = "Temperature: {} F".format(round(tello.get_temperature()*(9/5)+32),1)
alt_str = "Altitude: {}cm".format(tello.get_distance_tof())
textOnFrame(output_image, [bat_str, temp_str, alt_str, mp_str])
movie.write(output_image)
cv2.imshow('Drone TV', output_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
tello.streamoff()
cv2.destroyAllWindows()
break
if flyMission == True and inFlight == False:
tello.takeoff()
inFlight = True
if flyMission == True and inFlight == True:
t2 = datetime.now()
class Mi_Dron():
def __init__(self):
self.tello = Tello()
self.estado = 'Desconectado'
self.conectado = False
def conectar(self):
# conectar con el dron
try:
self.tello.connect()
except:
self.estado = 'PROBLEMAS CONEXION DRON...'
self.conectado = False
else:
self.conectado = True
self.estado = 'DRON CONECTADO...'
def takeoff(self):
self.estado = 'DESPEGANDO...'
if self.conectado:
self.tello.takeoff()
sleep(5)
self.estabilizar()
def land(self):
self.estado = 'ATERRIZANDO...'
if self.conectado:
self.tello.land()
def subir(self):
self.estado = 'SUBIENDO...'
if self.conectado:
self.tello.send_rc_control(0, 0, 50, 0)
sleep(3)
self.estabilizar()
def adelante(self):
self.estado = 'ADELANTE...'
if self.conectado:
self.tello.send_rc_control(0, 50, 0, 0)
sleep(3)
self.estabilizar()
def atras(self):
self.estado = 'ATRAS...'
if self.conectado:
self.tello.send_rc_control(0, -50, 0, 0)
sleep(3)
self.estabilizar()
def bajar(self):
self.estado = 'BAJANDO...'
if self.conectado:
self.tello.send_rc_control(0, 0, -50, 0)
sleep(3)
self.estabilizar()
def derecha(self):
self.estado = 'DERECHA...'
if self.conectado:
self.tello.send_rc_control(50, 0, 0, 0)
sleep(3)
self.estabilizar()
def izquierda(self):
self.estado = 'IZQUIERDA...'
if self.conectado:
self.tello.send_rc_control(-50, 0, 0, 0)
sleep(3)
self.estabilizar()
def giro_derecha(self):
self.estado = 'GIRO DERECHA...'
if self.conectado:
self.tello.send_rc_control(0, 0, 0, 45)
sleep(3)
self.estabilizar()
def giro_izquierda(self):
self.estado = 'GIRO IZQUIERDA...'
if self.conectado:
self.tello.send_rc_control(0, 0, 0, -45)
sleep(3)
self.estabilizar()
def estabilizar(self):
self.tello.send_rc_control(0, 0, 0, 0)
def adelante_plan(self):
self.tello.send_rc_control(0, 50, 0, 0)
sleep(5)
def atras_plan(self):
self.tello.send_rc_control(0, -50, 0, 0)
sleep(5)
def giro_der_plan(self):
self.tello.send_rc_control(0, 0, 0, 45)
sleep(3)
def giro_izq_plan(self):
self.tello.send_rc_control(0, 0, 0, -45)
sleep(3)
def izquierda_plan(self):
self.tello.send_rc_control(0, -50, 0, 0)
sleep(5)
def derecha_plan(self):
self.tello.send_rc_control(0, -50, 0, 0)
sleep(5)
def takeoff_plan(self):
self.tello.takeoff()
sleep(3)
def land_plan(self):
self.tello.land()
sleep(3)
def subir_plan(self):
self.tello.send_rc_control(0, 0, 50, 0)
sleep(5)
def bajar_plan(self):
self.tello.send_rc_control(0, 0, -50, 0)
sleep(5)
def mostrar_estado(self):
datos = 'ESTADO : ' + self.estado.upper() + '\n'
if self.conectado:
datos += 'Batería: ' + str(self.tello.get_battery()) + '\n'
datos += 'Altitud: ' + str(self.tello.get_barometer()) + '\n'
datos += 'Temperatura: ' + str(self.tello.get_temperature()) + '\n'
datos += 'Velocidad X: ' + str(self.tello.get_speed_x()) + '\n'
datos += 'Velocidad Y: ' + str(self.tello.get_speed_y()) + '\n'
datos += 'Velocidad Z: ' + str(self.tello.get_speed_z()) + '\n'
return (datos)
import matplotlib.pyplot as plt
import cvlib as cv
from cvlib.object_detection import draw_bbox
from djitellopy import Tello
import numpy as np
tello = Tello()
tello.connect()
tello.streamon()
def get_resized_frame(w=640, h=480):
return cv2.resize(tello.get_frame_read().frame, (w, h))
while True:
img = get_resized_frame()
bbox, label, conf = cv.detect_common_objects(img)
output_image = draw_bbox(img, bbox, label, conf)
bat = "Battery: {}%".format(tello.get_battery())
cv2.putText(output_image, bat, (5, 480 - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 255), 2)
temp = "Temperature: {}%C".format(tello.get_temperature())
cv2.putText(output_image, temp, (5, 480 - 25), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (0, 0, 255), 2)
cv2.imshow("Image", output_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
tello.streamoff()
cv2.destroyAllWindows()
