|
|
# @Time : 2022/4/20 12:27
|
|
|
# @Author : 2890199310@qq.com
|
|
|
# @File : KeyboardControl.py.py
|
|
|
# @Software: PyCharm
|
|
|
# @Function:
|
|
|
import os
|
|
|
import sys
|
|
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../PaddleClas-release-2.3")
|
|
|
import logging
|
|
|
import time
|
|
|
import cv2
|
|
|
from djitellopy import tello
|
|
|
# import Tello.KeyPressModule as kp # 用于获取键盘按键
|
|
|
from time import sleep
|
|
|
|
|
|
# Tello初始化设置
|
|
|
Drone = tello.Tello() # 创建飞行器对象
|
|
|
Drone.connect() # 连接到飞行器
|
|
|
Drone.streamon() # 开启视频传输
|
|
|
Drone.LOGGER.setLevel(logging.ERROR) # 只显示错误信息
|
|
|
sleep(5) # 等待视频初始化
|
|
|
# kp.init() # 初始化按键处理模块
|
|
|
|
|
|
def getKeyboardInput(key):
|
|
|
image = cv2.resize(OriginalImage, (Camera_Width, Camera_Height))
|
|
|
speed = 70
|
|
|
drone = Drone
|
|
|
lr, fb, ud, yv = 0, 0, 0, 0
|
|
|
key_pressed = 0
|
|
|
# if kp.getKey("e"): #
|
|
|
if key == "e":
|
|
|
cv2.imwrite('D:/snap-{}.jpg'.format(time.strftime("%H%M%S", time.localtime())), image)
|
|
|
# if kp.getKey("UP"):# 上升
|
|
|
if key == "UP":
|
|
|
Drone.takeoff()
|
|
|
# elif kp.getKey("DOWN"):#下降
|
|
|
if key == "DOWN":
|
|
|
Drone.land()
|
|
|
|
|
|
# if kp.getKey("j"):# 向左飞行
|
|
|
if key == "j":
|
|
|
key_pressed = 1
|
|
|
lr = -speed
|
|
|
# elif kp.getKey("l"): #向右飞行
|
|
|
if key == "l":
|
|
|
key_pressed = 1
|
|
|
lr = speed
|
|
|
|
|
|
# if kp.getKey("i"): #向前飞行
|
|
|
if key == "i":
|
|
|
key_pressed = 1
|
|
|
fb = speed
|
|
|
# elif kp.getKey("k"):# 向后飞行
|
|
|
if key == "k":
|
|
|
key_pressed = 1
|
|
|
fb = -speed
|
|
|
|
|
|
# if kp.getKey("w"):# 向上飞行
|
|
|
if key == "w":
|
|
|
key_pressed = 1
|
|
|
ud = speed
|
|
|
# elif kp.getKey("s"): #向下飞行
|
|
|
if key == "s":
|
|
|
key_pressed = 1
|
|
|
ud = -speed
|
|
|
|
|
|
# if kp.getKey("a"): # 向左旋转
|
|
|
if key == "a":
|
|
|
key_pressed = 1
|
|
|
yv = -speed
|
|
|
# elif kp.getKey("d"): #向右旋转
|
|
|
if key == "d":
|
|
|
key_pressed = 1
|
|
|
yv = speed
|
|
|
InfoText = "battery : {0}% height: {1}cm time: {2}".format(drone.get_battery(), drone.get_height(), time.strftime("%H:%M:%S",time.localtime()))
|
|
|
cv2.putText(image, InfoText, (10, 20), font, fontScale, (0, 0, 255), lineThickness)
|
|
|
if key_pressed == 1:
|
|
|
InfoText = "Command : lr:{0}% fb:{1} ud:{2} yv:{3}".format(lr, fb, ud, yv)
|
|
|
cv2.putText(image, InfoText, (10, 40), font, fontScale, (0, 0, 255), lineThickness)
|
|
|
|
|
|
drone.send_rc_control(lr, fb, ud, yv)
|
|
|
|
|
|
# 主程序
|
|
|
# 摄像头设置
|
|
|
Camera_Width = 720
|
|
|
Camera_Height = 480
|
|
|
DetectRange = [6000, 11000] # DetectRange[0] 是保持静止的检测人脸面积阈值下限，DetectRange[0] 是保持静止的检测人脸面积阈值上限
|
|
|
PID_Parameter = [0.5, 0.0004, 0.4]
|
|
|
pErrorRotate, pErrorUp = 0, 0
|
|
|
|
|
|
# 字体设置
|
|
|
font = cv2.FONT_HERSHEY_SIMPLEX
|
|
|
fontScale = 0.5
|
|
|
fontColor = (255, 0, 0)
|
|
|
lineThickness = 1
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
while True:
|
|
|
OriginalImage = Drone.get_frame_read().frame
|
|
|
Image = cv2.resize(OriginalImage, (Camera_Width, Camera_Height))
|
|
|
# getKeyboardInput(drone=Drone, speed=70, image=Image) # 按键控制
|
|
|
cv2.imshow("Drone Control Centre", Image)
|
|
|
cv2.waitKey(1) |
