def fly(drone):
print "Going to go for a small trip ..."
# drone.videoCbk = videoCallback
drone.videoEnable()
try:
drone.trim()
drone.takeoff()
drone.takePicture()
speed = 5
drone.update(cmd=movePCMDCmd(True, 0, speed, 0, 0))
drone.flyToAltitude(1.5)
drone.wait(2.0)
drone.update(cmd=movePCMDCmd(True, 0, speed, 0, 0))
drone.flyToAltitude(0.5)
drone.takePicture()
drone.wait(2.0)
drone.update(cmd=movePCMDCmd(True, 0, speed, 0, 0))
drone.flyToAltitude(1)
drone.wait(2.0)
drone.takePicture()
# Stop
drone.update(cmd=movePCMDCmd(True, 0, 0, 0, 0))
drone.land()
except ManualControlException, e:
print
print "ManualControlException"
if drone.flyingState is None or drone.flyingState == 1: # taking off
drone.emergency()
drone.land()
def fly( drone ):
print "Going to go for a small trip ..."
# drone.videoCbk = videoCallback
drone.videoEnable()
try:
drone.trim()
drone.takeoff()
drone.takePicture()
speed = 5
drone.update( cmd=movePCMDCmd( True, 0, speed, 0, 0 ) )
drone.flyToAltitude( 1.5 )
drone.wait( 2.0 )
drone.update( cmd=movePCMDCmd( True, 0, speed, 0, 0 ) )
drone.flyToAltitude( 0.5 )
drone.takePicture()
drone.wait( 2.0 )
drone.update( cmd=movePCMDCmd( True, 0, speed, 0, 0 ) )
drone.flyToAltitude( 1 )
drone.wait( 2.0 )
drone.takePicture()
# Stop
drone.update( cmd=movePCMDCmd( True, 0, 0, 0, 0 ) )
drone.land()
except ManualControlException, e:
print
print "ManualControlException"
if drone.flyingState is None or drone.flyingState == 1: # taking off
drone.emergency()
drone.land()
def videoCallback( frame, robot=None, debug=False ):
if frame:
print "Video", len(frame)
# workaround for a single frame
f = open( TMP_VIDEO_FILE, "wb" )
f.write( frame[-1] )
f.close()
cap = cv2.VideoCapture( TMP_VIDEO_FILE )
ret, img = cap.read()
cap.release()
if ret:
detImg, detected = detectTwoColors( img, loadColors("cap-colors.txt") )
print "Detected", detected
if len(detected) > 0:
target = detected[0]
for alt in detected[1:]:
if alt[1] > target[1]: # area
target = alt
print "Target", target, img.shape
cv2.circle( img, target[0], 20, (255,0,0), 3)
if robot:
diff = target[0][0] - 320
if diff < 0:
# robot.moveCamera( robot.cameraTilt, robot.cameraPan - 1 )
robot.update( movePCMDCmd(active=True, roll=0, pitch=0, yaw=-10, gaz=0) )
elif diff > 0:
# robot.moveCamera( robot.cameraTilt, robot.cameraPan + 1 )
robot.update( movePCMDCmd(active=True, roll=0, pitch=0, yaw=+10, gaz=0) )
if debug:
cv2.imshow('image', img)
key = cv2.waitKey(200)
def testAutomaticLanding(drone):
"Takeoff and land"
drone.setVideoCallback(videoCallback, videoCallbackResults)
drone.moveCamera(tilt=-100, pan=0)
drone.videoEnable()
try:
drone.trim()
drone.takeoff()
print "COMPLETED", drone.altitude
prev = drone.lastImageResult
up = 0
left = 0
fwd = 0
for i in xrange(1000):
if drone.altitude > 1.5:
up = -20
elif drone.altitude < 1.3:
up = 20
else:
up = 0
print "ALT", drone.altitude, up
if prev != drone.lastImageResult:
left, fwd = 0, 0
print drone.lastImageResult
prev = drone.lastImageResult
if drone.lastImageResult is not None:
index, result = drone.lastImageResult
if result is not None:
(centerX, centerY), point = result
if centerY < 368 / 2 - 30:
fwd = 20
elif centerY > 368 / 2 + 30:
fwd = -20
else:
fwd = 0
if centerX < 640 / 2 - 30:
left = 20
elif centerY > 640 / 2 + 30:
left = -20
else:
left = 0
print "HETREERERE!", left, fwd
print i,
drone.update(cmd=movePCMDCmd(
active=True, roll=left, pitch=fwd, yaw=0, gaz=up))
drone.land()
except ManualControlException, e:
print
print "ManualControlException"
drone.hover()
if drone.flyingState is None or drone.flyingState == 1: # taking off
drone.emergency()
drone.land()
def videoCallback(frame, robot=None, debug=False):
if frame:
print "Video", len(frame)
# workaround for a single frame
f = open(TMP_VIDEO_FILE, "wb")
f.write(frame[-1])
f.close()
cap = cv2.VideoCapture(TMP_VIDEO_FILE)
ret, img = cap.read()
cap.release()
if ret:
detImg, detected = detectTwoColors(img,
loadColors("cap-colors.txt"))
print "Detected", detected
if len(detected) > 0:
target = detected[0]
for alt in detected[1:]:
if alt[1] > target[1]: # area
target = alt
print "Target", target, img.shape
cv2.circle(img, target[0], 20, (255, 0, 0), 3)
if robot:
diff = target[0][0] - 320
if diff < 0:
# robot.moveCamera( robot.cameraTilt, robot.cameraPan - 1 )
robot.update(
movePCMDCmd(active=True,
roll=0,
pitch=0,
yaw=-10,
gaz=0))
elif diff > 0:
# robot.moveCamera( robot.cameraTilt, robot.cameraPan + 1 )
robot.update(
movePCMDCmd(active=True,
roll=0,
pitch=0,
yaw=+10,
gaz=0))
if debug:
cv2.imshow('image', img)
key = cv2.waitKey(200)
def testAutomaticLanding( drone ):
"Takeoff and land"
drone.setVideoCallback( videoCallback, videoCallbackResults )
drone.moveCamera( tilt=-100, pan=0 )
drone.videoEnable()
try:
drone.trim()
drone.takeoff()
print "COMPLETED", drone.altitude
prev = drone.lastImageResult
up = 0
left = 0
fwd = 0
for i in xrange(1000):
if drone.altitude > 1.5:
up = -20
elif drone.altitude < 1.3:
up = 20
else:
up = 0
print "ALT", drone.altitude, up
if prev != drone.lastImageResult:
left, fwd = 0, 0
print drone.lastImageResult
prev = drone.lastImageResult
if drone.lastImageResult is not None:
index,result = drone.lastImageResult
if result is not None:
(centerX,centerY), point = result
if centerY < 368/2 - 30:
fwd = 20
elif centerY > 368/2 + 30:
fwd = -20
else:
fwd = 0
if centerX < 640/2 - 30:
left = 20
elif centerY > 640/2 + 30:
left = -20
else:
left = 0
print "HETREERERE!", left, fwd
print i,
drone.update( cmd=movePCMDCmd( active=True, roll=left, pitch=fwd, yaw=0, gaz=up ) )
drone.land()
except ManualControlException, e:
print
print "ManualControlException"
drone.hover()
if drone.flyingState is None or drone.flyingState == 1: # taking off
drone.emergency()
drone.land()
