imu_count = len(imu)
imu_fs = int(round((imu_count / (imu_max - imu_min))))
print("imu fs:", imu_fs)
# resample imu data
print("flight range = %.3f - %.3f (%.3f)" % (imu_min, imu_max,
imu_max-imu_min))
flight_interp = []
flight_len = imu_max - imu_min
p_interp = interpolate.interp1d(imu['time'], imu['p'], bounds_error=False, fill_value=0.0)
q_interp = interpolate.interp1d(imu['time'], imu['q'], bounds_error=False, fill_value=0.0)
r_interp = interpolate.interp1d(imu['time'], imu['r'], bounds_error=False, fill_value=0.0)
alt_interp = interp.group['filter'].interp['alt']
for x in np.linspace(imu_min, imu_max, int(round(flight_len*hz))):
flight_interp.append( [x, p_interp(x), q_interp(x), r_interp(x) ] )
print("flight len:", len(flight_interp))
# load camera rotation rate data (derived from feature matching video
# frames)
feat_data = FeatureData()
feat_data.load(video_rates)
feat_data.smooth(smooth_cutoff_hz)
feat_data.make_interp()
if args.plot:
feat_data.plot()
feat_interp = feat_data.resample(args.resample_hz)
# find the time correlation of video vs flight data
time_shift = \
correlate.sync_gyros(flight_interp, feat_interp, feat_data.span_sec,
p_interp = interp.group['imu'].interp['p']
q_interp = interp.group['imu'].interp['q']
elif args.cam_mount == 'left' or args.cam_mount == 'right':
# it might be interesting to support an out-the-wing view
print("Not currently supported camera orientation, sorry!")
quit()
flight_len = imu_max - imu_min
phi_interp = interpolate.interp1d(ekf['time'], ekf['phi'], bounds_error=False, fill_value=0.0)
the_interp = interpolate.interp1d(ekf['time'], ekf['the'], bounds_error=False, fill_value=0.0)
psix_interp = interpolate.interp1d(ekf['time'], ekf['psix'], bounds_error=False, fill_value=0.0)
psiy_interp = interpolate.interp1d(ekf['time'], ekf['psiy'], bounds_error=False, fill_value=0.0)
alt_interp = interp.group['filter'].interp['alt']
for x in np.linspace(imu_min, imu_max, int(round(flight_len*hz))):
flight_interp.append( [x, p_interp(x), q_interp(x),
phi_interp(x), the_interp(x),
psix_interp(x), psiy_interp(x)] )
print("flight len:", len(flight_interp))
# find the time correlation of video vs flight data
time_shift = \
correlate.sync_horizon(flight_data, flight_interp,
horiz_data.data, horiz_interp, horiz_data.span_sec,
method=method, hz=hz, cam_mount=args.cam_mount,
force_time_shift=args.time_shift, plot=args.plot)
# presample datas to save work in the error function
tmin = np.amax( [horiz_data.tmin + time_shift, imu_min ] )
tmax = np.amin( [horiz_data.tmax + time_shift, imu_max ] )
tlen = tmax - tmin
print("overlap range (flight sec):", tmin, " - ", tmax)