event = pygame.event.poll()
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
center = event.pos
joy = event.pos
down = True
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
down = False
joy = width / 2, height / 2
center = width / 2, height / 2
elif event.type == pygame.MOUSEMOTION and down:
joy = event.pos
if down:
#Normilize joystick commands
x = (joy[0] - width / 2.0) / radius
y = -(joy[1] - height / 2.0) / radius
else: #decay the values after stick release
if time.time() - last_decay > .1:
x = x * DECAY
y = y * DECAY
last_decay = time.time()
draw_joystick(center, joy, radius)
dd.move(x=x, y=y, z=0)
finally:
dd.release()
print "connecting to drone..."
if SIM:
vehicle = connect('127.0.0.1:14551', wait_ready=True)
else:
vehicle = connect('0.0.0.0:14550', wait_ready=True) # connecting from GCS
#vehicle = connect('udpout:127.0.0.1:14560', wait_ready=True) #connecting from onboard solo
dd = DroneDirect(vehicle)
dd.take_control()
if SIM:
#arm and takeoff drone - DO NOT USE THIS ON A REAL DRONE ONLY IN SIMULATION
if vehicle.armed == False:
# Don't let the user try to arm until autopilot is ready
print " Waiting for vehicle to initialise..."
while not vehicle.is_armable:
time.sleep(1)
vehicle.armed = True
print 'Vehicle Armed'
dd.takeoff()
try:
'''
YOUR CODE HERE
'''
finally:
dd.release()
event = pygame.event.poll()
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
center = event.pos
joy = event.pos
down = True
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
down = False
joy = width / 2, height / 2
center = width / 2, height / 2
elif event.type == pygame.MOUSEMOTION and down:
joy = event.pos
if down:
#Normilize joystick commands
x = (joy[0] - width / 2.0) / radius
y = -(joy[1] - height / 2.0) / radius
else: #decay the values after stick release
if time.time() - last_decay > .1:
x = x * DECAY
y = y * DECAY
last_decay = time.time()
draw_joystick(center, joy, radius)
sd.move(x=x, y=y, z=0)
finally:
sd.release()
