def on_release(key):
try: k = key.char # single-char keys
except: k = key.name # other keys
if key == keyboard.Key.esc:
return False # stop listener
elif k in ['up', 'down', 'left', 'right', 'w', 's']:
send_global_velocity(vehicle, 0, 0, 0)
def on_press(key):
control_speed = 0.3
try: k = key.char # single-char keys
except: k = key.name # other keys
if key == keyboard.Key.esc:
return False # stop listener
elif k in ['up', 'down']:
if k == 'up':
velocity_z = -control_speed
elif k == 'down':
velocity_z = control_speed
else:
print('ERROR up down: ' + k)
send_global_velocity(vehicle, 0, 0, velocity_z)
elif k in ['left', 'right', 'w', 's']:
direction_delta = 0
if k == 'left':
direction_delta = -math.pi/2
elif k == 'right':
direction_delta = math.pi/2
elif k == 'w':
direction_delta = 0
elif k == 's':
direction_delta = math.pi
else:
print('ERROR w s left right: ' + k)
direction_delta = 0
direction = vehicle.attitude.yaw + direction_delta
velocity_x = control_speed * math.cos(direction)
velocity_y = control_speed * math.sin(direction)
send_global_velocity(vehicle, velocity_x, velocity_y, 0)
elif k in ['a', 'd']:
if k == 'a':
yaw_delta = -10
elif k == 'd':
yaw_delta = 10
else:
yaw_delta = 0
vehicle.gimbal.rotate(0, 0, rad2deg(vehicle.attitude.yaw) + yaw_delta)
elif k in ['p', 'l']:
if k == 'p':
cmd = 'detect %d' % idx
else:
cmd = 'detect %d 1' % idx
idx += 1
sys.argv = cmd.split()
execfile(sys.argv[0] + '.py')
print(" is pressed")
def key2act(k):
control_speed = 0.5
global idx
if k in ['i', 'k']:
if k == 'i':
velocity_z = -control_speed
elif k == 'k':
velocity_z = control_speed
else:
print('ERROR up down: ' + k)
send_global_velocity(vehicle, 0, 0, velocity_z)
elif k in ['j', 'l', 'w', 's']:
direction_delta = 0
if k == 'j':
direction_delta = -math.pi/2
elif k == 'l':
direction_delta = math.pi/2
elif k == 'w':
direction_delta = 0
elif k == 's':
direction_delta = math.pi
else:
print('ERROR w s left right: ' + k)
direction_delta = 0
direction = vehicle.attitude.yaw + direction_delta
velocity_x = control_speed * math.cos(direction)
velocity_y = control_speed * math.sin(direction)
send_global_velocity(vehicle, velocity_x, velocity_y, 0)
elif k in ['a', 'd']:
if k == 'a':
yaw_delta = -10
elif k == 'd':
yaw_delta = 10
else:
yaw_delta = 0
vehicle.gimbal.rotate(0, 0, rad2deg(vehicle.attitude.yaw) + yaw_delta)
elif k in ['p', 'o']:
if k == 'p':
cmd = 'detect %d' % idx
else:
cmd = 'detect %d 1' % idx
idx += 1
sys.argv = cmd.split()
execfile(sys.argv[0] + '.py')
print(" is pressed")
idx += 1
# take photo
camera.capture(img_path)
print(img_path)
# detect circles
img = imread(img_path)
img_ = rgb2blue(img)
blue_img_name = img_name[:-4]+'_blue.jpg'
imwrite(join(img_dir, blue_img_name), img_)
circles = detect_circles(img_, 0)
circle = best_circle(circles, img, resolution=camera.resolution)
if circle is None:
send_global_velocity(vehicle, 0, 0, -0.1)
print("can't detect any circle")
hit_twice = False
else:
displacement = circle[0]-camera.resolution[0]//2, circle[1]-camera.resolution[1]//2
radius = circle[2]
direction = math.atan2(displacement[1], displacement[0])
direction -= math.pi
relative_direction = direction
direction += vehicle.attitude.yaw
try:
distance_per_pixel = 2670 / radius
except Exception, e:
print(e)
# 在无人机上升到目标高度之前，阻塞程序
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# 当高度上升到目标高度的0.95倍时，即认为达到了目标高度，退出循环
# vehicle.location.global_relative_frame.alt为相对于home点的高度
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
# 等待1s
time.sleep(1)
print_important(vehicle)
arm_and_takeoff(2)
send_global_velocity(vehicle, 0, 0, 0)
print("Enter:", end='')
lis = keyboard.Listener(on_press=on_press, on_release=on_release)
lis.start() # start to listen on a separate thread
lis.join() # no this if main thread is polling self.keys
vehicle.mode = VehicleMode('ALT_HOLD')
while True:
time.sleep(1)
# while True:
# print_important(vehicle)
# try:
# print('#' * 30)
# sys.argv = raw_input('execute: ').split()
# execfile(sys.argv[0] + '.py')