def horiz_errorFunc():
# compute error function using global data structures
horiz_ned = [0, 0, 0] # any value works here (as long as it's consistent
result = []
for r in data:
camera.update_PROJ(horiz_ned, r[3], r[4], r[5]) # aircraft body attit
#print("video:", r[1]*r2d, r[2]*r2d)
roll, pitch = camera.find_horizon()
#result.append( r[1] - (r[5] + xk[2]) )
#result.append( r[2] - (r[4] + xk[1]) )
if not roll is None:
result.append( r[1] - roll )
result.append( r[2] - pitch )
return np.array(result)
flight_mode = 'auto'
if 'mission' in data:
excite_mode = mission['excite']
test_index = mission['test_index']
record = iter.next()
hud1.update_task(record)
while 'imu' in record and record['imu']['time'] < time:
record = iter.next()
hud1.update_task(record)
ned = navpy.lla2ned( lat_deg, lon_deg, filt_alt,
ref[0], ref[1], ref[2] )
#print 'ned:', ned
camera.update_PROJ(ned, yaw_rad, pitch_rad, roll_rad)
method = cv2.INTER_AREA
#method = cv2.INTER_LANCZOS4
frame_scale = cv2.resize(frame, (0,0), fx=args.scale, fy=args.scale,
interpolation=method)
frame_undist = cv2.undistort(frame_scale, K, np.array(dist))
# Create hud draw space
if not experimental_overlay:
hud1_frame = frame_undist.copy()
else:
hud1_frame = np.zeros((frame_undist.shape), np.uint8)
hud1.update_time(time, gps['unix_sec'])
if 'event' in data: