def run_filter(filter, data, call_init=True):
results = []
# Using while loop starting at k (set to kstart) and going to end
# of .mat file
run_start = time.time()
if call_init:
filter_init = False
else:
filter_init = True
gps_init_sec = None
gpspt = None
iter = flight_interp.IterateGroup(data)
for i in tqdm(range(iter.size())):
record = iter.next()
imupt = record['imu']
if 'gps' in record:
gpspt = record['gps']
if gps_init_sec is None:
gps_init_sec = gpspt['time']
# if not inited or gps not yet reached it's settle time
if gps_init_sec is None or gpspt['time'] < gps_init_sec + gps_settle_secs:
continue
navpt = filter.update(imupt, gpspt)
# Store the desired results obtained from the compiled test
# navigation filter and the baseline filter
results.append(navpt)
run_end = time.time()
elapsed_sec = run_end - run_start
return results, elapsed_sec
def estimate(self, data, wind_time_factor):
print("Estimating winds aloft:")
if wind_time_factor:
self.wind_time_factor = wind_time_factor
self.filt_wn = lowpass.LowPassFilter(self.wind_time_factor, 0.0)
self.filt_we = lowpass.LowPassFilter(self.wind_time_factor, 0.0)
self.filt_ps = lowpass.LowPassFilter(self.pitot_time_factor, 1.0)
self.last_time = 0.0
winds = []
airspeed = 0
psi = 0
vn = 0
ve = 0
wind_deg = 0
wind_kt = 0
ps = 1.0
iter = flight_interp.IterateGroup(data)
for i in tqdm(range(iter.size())):
record = iter.next()
if len(record):
t = record['imu']['time']
if 'air' in record:
airspeed = record['air']['airspeed']
if 'filter' in record:
psi = record['filter']['psi']
vn = record['filter']['vn']
ve = record['filter']['ve']
if airspeed > 10.0:
(wn, we, ps) = self.update(t, airspeed, psi, vn, ve)
#print wn, we, math.atan2(wn, we), math.atan2(wn, we)*r2d
wind_deg = 90 - math.atan2(wn, we) * r2d
if wind_deg < 0: wind_deg += 360.0
wind_kt = math.sqrt(we * we + wn * wn) * mps2kt
#print wn, we, ps, wind_deg, wind_kt
# make sure we log one record per each imu record
winds.append({
'time': t,
'wind_deg': wind_deg,
'wind_kt': wind_kt,
'pitot_scale': ps
})
return winds
def estimate(data):
print("Estimating magnetometer/throttle correlation:")
result = []
throttle = 0.0
iter = flight_interp.IterateGroup(data)
for i in tqdm(range(iter.size())):
record = iter.next()
if len(record):
imu = record['imu']
t = imu['time']
hx = imu['hx']
hy = imu['hy']
hz = imu['hz']
mag_norm = np.linalg.norm(np.array([hx, hy, hz]))
if 'act' in record:
throttle = record['act']['throttle']
result.append( { 'time': t,
'throttle': throttle,
'mag_norm': mag_norm } )
return result
data, flight_format = flight_loader.load(args.flight)
print("imu records:", len(data['imu']))
print("gps records:", len(data['gps']))
if 'air' in data:
print("airdata records:", len(data['air']))
print("filter records:", len(data['filter']))
if 'pilot' in data:
print("pilot records:", len(data['pilot']))
if 'act' in data:
print("act records:", len(data['act']))
if len(data['imu']) == 0 and len(data['gps']) == 0:
print("not enough data loaded to continue.")
quit()
interp = flight_interp.InterpolationGroup(data)
iter = flight_interp.IterateGroup(data)
# imu gyro data
imu = pd.DataFrame(data['imu'])
imu.set_index('time', inplace=True, drop=False)
imu_min = imu['time'].iat[0]
imu_max = imu['time'].iat[-1]
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
