cx_gps = cx_rate.CXRate(noise=0)
tb1_gps = np.zeros(central_complex.N_TB1)
memory_gps = 0.5 * np.ones(central_complex.N_CPU4)
cpu4_gps = np.zeros(16)
# initialize camera and optical flow
frame_num = 0
picam = picameraThread(1, "picamera_video", resolution, 30)
picam.start()
fw, fh = resolution
time.sleep(0.1) # allow the camera to warmup
print("Frame size: {}*{}".format(fw, fh))
optflow = Optical_flow(resolution)
temp = picam.get_frame()
prvs = optflow.undistort(temp)
(fh, fw) = prvs.shape
print("Undistorted frame size: {0}*{1}".format(fw, fh))
left_filter, right_filter = optflow.get_filter(fh, fw)
# Define the codec and create VideoWriter object
if RECORDING == 'true':
fname = 'video/' + time_string + '.avi'
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter(fname, fourcc, 20.0, (fw, fh), False)
# take off adn set to mission mode.
state = arm_and_takeoff(drone, HEIGHT)
drone.mode = VehicleMode("AUTO")
while drone.mode.name != "AUTO":
print "Waiting for the mission mode."
fw = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
fh = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
print("Frame size: {}*{}".format(fw, fh))
'''
picam = picameraThread(1, "picamera_video", resolution, 30)
picam.start()
optflow = Optical_flow(resolution)
picture_num = 100
column_num = 0
time.sleep(2)
while True:
gray = picam.get_frame()
# undistorte image
gray = optflow.undistort(gray)
# draw lines on image for calibration angles
gray = cv2.line(gray, (1, column_num), (fw, column_num), (255, 255, 0), 1)
gray = cv2.line(gray, (1, int(fh / 2)), (fw, int(fh / 2)), (255, 255, 0),
1)
gray = cv2.line(gray, (column_num, 1), (column_num, fh), (255, 255, 0), 1)
gray = cv2.line(gray, (int(fw / 2), 1), (int(fw / 2), fh), (255, 255, 0),
1)
cv2.imshow('frame', cv2.resize(gray, (0, 0), fx=scale, fy=scale))
ch = 0xFF & cv2.waitKey(1)
if ch == ord('q'):
break
elif ch == ord('s'):
picture_name = "image" + str(picture_num) + '.jpg'