def load(csv_file):
result = {}
result['imu'] = []
result['gps'] = []
result['air'] = []
result['filter'] = []
#result['pilot'] = []
result['act'] = []
result['ap'] = []
last_gps_time = -1.0
with open(csv_file, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
if row['TIME_StartTime'] == '':
# empty time
continue
imu = IMUdata()
imu.time = float(row['TIME_StartTime']) / 1000000.0
imu.p = float(row['IMU_GyroX'])
imu.q = float(row['IMU_GyroY'])
imu.r = float(row['IMU_GyroZ'])
imu.ax = float(row['IMU_AccX'])
imu.ay = float(row['IMU_AccY'])
imu.az = float(row['IMU_AccZ'])
hx = float(row['IMU_MagX'])
hy = float(row['IMU_MagY'])
hz = float(row['IMU_MagZ'])
hf = np.array([hx, hy, hz])
norm = np.linalg.norm(hf)
hf /= norm
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#print imu.hx, imu.hy, imu.hz
imu.temp = 15.0
result['imu'].append(imu)
if row['GPS_GPSTime'] != '':
gps = GPSdata()
gps.time = imu.time
gps.unix_sec = float(row['GPS_GPSTime']) / 1000000.0
gps.lat = float(row['GPS_Lat'])
gps.lon = float(row['GPS_Lon'])
gps.alt = float(row['GPS_Alt'])
gps.vn = float(row['GPS_VelN'])
gps.ve = float(row['GPS_VelE'])
gps.vd = float(row['GPS_VelD'])
gps.sats = int(row['GPS_nSat'])
if gps.sats >= 5 and gps.unix_sec > last_gps_time:
result['gps'].append(gps)
last_gps_time = gps.unix_sec
if row['SENS_BaroPres'] != '':
air = Airdata()
air.time = imu.time
air.static_press = float(row['SENS_BaroPres'])
if 'AIR1_DiffPres' in row and row['AIR1_DiffPres'] != '':
air.diff_press = float(row['AIR1_DiffPres'])
else:
air.diff_press = 0.0
if 'AIRS_Temp' in row and row['AIRS_Temp'] != '':
air.temp = float(row['AIRS_Temp'])
else:
air.temp = 0.0
if 'AIRS_IAS' in row and row['AIRS_IAS'] != '':
air.airspeed = float(row['AIRS_IAS']) * mps2kt
else:
air.airspeed = 0.0
if 'AIR1_BaroAlt' in row and row['AIR1_BaroAlt'] != '':
air.alt_press = float(row['AIR1_BaroAlt'])
else:
air.alt_press = 0.0
if 'GPS_Alt' in row and row['GPS_Alt'] != '':
air.alt_true = float(row['GPS_Alt'])
else:
air.alt_true = 0.0
result['air'].append(air)
if row['GPOS_Lat'] != '':
nav = NAVdata()
nav.time = imu.time
nav.lat = float(row['GPOS_Lat']) * d2r
nav.lon = float(row['GPOS_Lon']) * d2r
nav.alt = float(row['GPOS_Alt'])
nav.vn = float(row['GPOS_VelN'])
nav.ve = float(row['GPOS_VelE'])
nav.vd = float(row['GPOS_VelD'])
nav.phi = float(row['ATT_Roll'])
nav.the = float(row['ATT_Pitch'])
psi = float(row['ATT_Yaw'])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
nav.psi = psi
result['filter'].append(nav)
if row['GPSP_Alt'] != '':
ap = APdata()
ap.time = imu.time
ap.hdg = my_float(row['ATSP_YawSP']) * r2d
ap.roll = my_float(row['ATSP_RollSP']) * r2d
ap.alt = my_float(row['GPSP_Alt']) * m2ft
ap.pitch = my_float(row['ATSP_PitchSP']) * r2d
ap.speed = my_float(row['TECS_AsSP']) * mps2kt
result['ap'].append(ap)
if row['OUT0_Out0'] != '':
act = Controldata()
ch0 = (float(row['OUT0_Out0']) - 1500) / 500
ch1 = (float(row['OUT0_Out1']) - 1500) / 500
ch2 = (float(row['OUT0_Out2']) - 1000) / 1000
ch3 = (float(row['OUT0_Out3']) - 1500) / 500
#print ch0, ch1, ch2, ch3
act.time = imu.time
act.aileron = (ch0 - ch1)
act.elevator = -(ch0 + ch1)
act.throttle = ch2
act.rudder = ch3
act.gear = 0.0
act.flaps = 0.0
act.aux1 = 0.0
act.auto_manual = 0.0
result['act'].append(act)
return result
def load(flight_dir, recalibrate=None):
result = {}
# load imu/gps data files
imu_file = flight_dir + "/imu-0.txt"
imucal_json = flight_dir + "/imucal.json"
imucal_xml = flight_dir + "/imucal.xml"
gps_file = flight_dir + "/gps-0.txt"
air_file = flight_dir + "/air-0.txt"
filter_file = flight_dir + "/filter-0.txt"
filter_post = flight_dir + "/filter-post.txt"
pilot_file = flight_dir + "/pilot-0.txt"
act_file = flight_dir + "/act-0.txt"
ap_file = flight_dir + "/ap-0.txt"
imu_bias_file = flight_dir + "/imubias.txt"
# HEY: in the latest aura code, calibrated magnetometer is logged,
# not raw magnetometer, so we don't need to correct here. We
# could 'back correct' if we wanted original values for some
# reason (using the calibration matrix saved with each flight
# log.)
# # vireo_01
# mag_affine = np.array(
# [[ 1.6207467043, 0.0228992488, 0.0398638786, 0.1274248748],
# [-0.0169905025, 1.7164397411, -0.0001290047, -0.1140304977],
# [ 0.0424979948, -0.0038515935, 1.7193766423, -0.1449816095],
# [ 0. , 0. , 0. , 1. ]]
# )
# Tyr
# mag_affine = np.array(
# [[ 1.810, 0.109, 0.285, -0.237],
# [-0.078, 1.931, -0.171, -0.060],
# [ 0.008, 0.109, 1.929, 0.085],
# [ 0. , 0. , 0. , 1. ]]
# )
# telemaster apm2_101
mag_affine = np.array(
[[0.0026424919, 0.0001334248, 0.0000984977, -0.2235908546],
[-0.000081925, 0.0026419229, 0.0000751835, -0.0010757621],
[0.0000219407, 0.0000560341, 0.002541171, 0.040221458],
[0., 0., 0., 1.]])
# skywalker apm2_105
# mag_affine = np.array(
# [[ 0.0025834778, 0.0001434776, 0.0001434961, -0.7900775707 ],
# [-0.0001903118, 0.0024796553, 0.0001365238, 0.1881142449 ],
# [ 0.0000556437, 0.0001329724, 0.0023791184, 0.1824851582, ],
# [ 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000 ]]
# )
#np.set_printoptions(precision=10,suppress=True)
#print mag_affine
result['imu'] = []
fimu = fileinput.input(imu_file)
for line in fimu:
tokens = re.split('[,\s]+', line.rstrip())
#s = [float(tokens[7]), float(tokens[8]), float(tokens[9]), 1.0]
#hf = np.dot(mag_affine, s)
#print hf
imu = IMUdata()
imu.time = float(tokens[0])
imu.p = float(tokens[1])
imu.q = float(tokens[2])
imu.r = float(tokens[3])
imu.ax = float(tokens[4])
imu.ay = float(tokens[5])
imu.az = float(tokens[6])
imu.hx = float(tokens[7])
imu.hy = float(tokens[8])
imu.hz = float(tokens[9])
imu.temp = float(tokens[10])
result['imu'].append(imu)
result['gps'] = []
fgps = fileinput.input(gps_file)
last_time = -1.0
for line in fgps:
# Note: the aura logs unix time of the gps record, not tow,
# but for the purposes of the insgns algorithm, it's only
# important to have a properly incrementing clock, it doesn't
# really matter what the zero reference point of time is.
tokens = re.split('[,\s]+', line.rstrip())
time = float(tokens[0])
sats = int(tokens[8])
if sats >= 5 and time > last_time:
gps = GPSdata()
gps.time = time
gps.unix_sec = float(tokens[7])
gps.lat = float(tokens[1])
gps.lon = float(tokens[2])
gps.alt = float(tokens[3])
gps.vn = float(tokens[4])
gps.ve = float(tokens[5])
gps.vd = float(tokens[6])
gps.sats = sats
result['gps'].append(gps)
last_time = time
if os.path.exists(air_file):
result['air'] = []
fair = fileinput.input(air_file)
for line in fair:
tokens = re.split('[,\s]+', line.rstrip())
air = Airdata()
air.time = float(tokens[0])
air.static_press = float(tokens[1])
air.diff_press = 0.0 # not directly available in flight log
air.temp = float(tokens[2])
air.airspeed = float(tokens[3])
air.alt_press = float(tokens[4])
air.alt_true = float(tokens[5])
result['air'].append(air)
# load filter records if they exist (for comparison purposes)
result['filter'] = []
ffilter = fileinput.input(filter_file)
for line in ffilter:
tokens = re.split('[,\s]+', line.rstrip())
lat = float(tokens[1])
lon = float(tokens[2])
if abs(lat) > 0.0001 and abs(lon) > 0.0001:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat * d2r
nav.lon = lon * d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7]) * d2r
nav.the = float(tokens[8]) * d2r
psi = float(tokens[9])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
nav.psi = psi * d2r
result['filter'].append(nav)
# load filter (post process) records if they exist (for comparison
# purposes)
if os.path.exists(filter_post):
result['filter_post'] = []
ffilter = fileinput.input(filter_post)
for line in ffilter:
tokens = re.split('[,\s]+', line.rstrip())
lat = float(tokens[1])
lon = float(tokens[2])
if abs(lat) > 0.0001 and abs(lon) > 0.0001:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat * d2r
nav.lon = lon * d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7]) * d2r
nav.the = float(tokens[8]) * d2r
psi = float(tokens[9])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
nav.psi = psi * d2r
result['filter_post'].append(nav)
if os.path.exists(pilot_file):
result['pilot'] = []
fpilot = fileinput.input(pilot_file)
for line in fpilot:
tokens = re.split('[,\s]+', line.rstrip())
pilot = Controldata()
pilot.time = float(tokens[0])
pilot.aileron = float(tokens[1])
pilot.elevator = float(tokens[2])
pilot.throttle = float(tokens[3])
pilot.rudder = float(tokens[4])
pilot.gear = float(tokens[5])
pilot.flaps = float(tokens[6])
pilot.aux1 = float(tokens[7])
pilot.auto_manual = float(tokens[8])
result['pilot'].append(pilot)
if os.path.exists(act_file):
result['act'] = []
fact = fileinput.input(act_file)
for line in fact:
tokens = re.split('[,\s]+', line.rstrip())
act = Controldata()
act.time = float(tokens[0])
act.aileron = float(tokens[1])
act.elevator = float(tokens[2])
act.throttle = float(tokens[3])
act.rudder = float(tokens[4])
act.gear = float(tokens[5])
act.flaps = float(tokens[6])
act.aux1 = float(tokens[7])
act.auto_manual = float(tokens[8])
result['act'].append(act)
if os.path.exists(ap_file):
result['ap'] = []
fap = fileinput.input(ap_file)
for line in fap:
tokens = re.split('[,\s]+', line.rstrip())
ap = APdata()
ap.time = float(tokens[0])
ap.hdg = float(tokens[1])
ap.roll = float(tokens[2])
ap.alt = float(tokens[3])
ap.pitch = float(tokens[5])
ap.speed = float(tokens[7])
#ap.flight_time = float(tokens[8])
#ap.target_wp = int(tokens[9])
#ap.wp_lon = float(tokens[10])
#ap.wp_lat = float(tokens[11])
#ap.wp_index = int(tokens[12])
#ap.route_size = int(tokens[13])
result['ap'].append(ap)
cal = imucal.Calibration()
if os.path.exists(imucal_json):
imucal_file = imucal_json
elif os.path.exists(imucal_xml):
imucal_file = imucal_xml
else:
imucal_file = None
if imucal_file:
cal.load(imucal_file)
print 'back correcting imu data (to get original raw values)'
result['imu'] = cal.back_correct(result['imu'])
if recalibrate:
print 'recalibrating imu data using alternate calibration file:', recalibrate
rcal = imucal.Calibration()
rcal.load(recalibrate)
result['imu'] = rcal.correct(result['imu'])
return result
def load(flight_dir, recalibrate=None):
result = {}
# load imu/gps data files
imu_file = flight_dir + "/imu-0.txt"
imucal_json = flight_dir + "/imucal.json"
imucal_xml = flight_dir + "/imucal.xml"
gps_file = flight_dir + "/gps-0.txt"
air_file = flight_dir + "/air-0.txt"
filter_file = flight_dir + "/filter-0.txt"
pilot_file = flight_dir + "/pilot-0.txt"
act_file = flight_dir + "/act-0.txt"
ap_file = flight_dir + "/ap-0.txt"
imu_bias_file = flight_dir + "/imubias.txt"
# HEY: in the latest aura code, calibrated magnetometer is logged,
# not raw magnetometer, so we don't need to correct here. We
# could 'back correct' if we wanted original values for some
# reason (using the calibration matrix saved with each flight
# log.)
# # vireo_01
# mag_affine = np.array(
# [[ 1.6207467043, 0.0228992488, 0.0398638786, 0.1274248748],
# [-0.0169905025, 1.7164397411, -0.0001290047, -0.1140304977],
# [ 0.0424979948, -0.0038515935, 1.7193766423, -0.1449816095],
# [ 0. , 0. , 0. , 1. ]]
# )
# Tyr
# mag_affine = np.array(
# [[ 1.810, 0.109, 0.285, -0.237],
# [-0.078, 1.931, -0.171, -0.060],
# [ 0.008, 0.109, 1.929, 0.085],
# [ 0. , 0. , 0. , 1. ]]
# )
# telemaster apm2_101
mag_affine = np.array(
[[ 0.0026424919, 0.0001334248, 0.0000984977, -0.2235908546],
[-0.000081925 , 0.0026419229, 0.0000751835, -0.0010757621],
[ 0.0000219407, 0.0000560341, 0.002541171 , 0.040221458 ],
[ 0. , 0. , 0. , 1. ]]
)
# skywalker apm2_105
# mag_affine = np.array(
# [[ 0.0025834778, 0.0001434776, 0.0001434961, -0.7900775707 ],
# [-0.0001903118, 0.0024796553, 0.0001365238, 0.1881142449 ],
# [ 0.0000556437, 0.0001329724, 0.0023791184, 0.1824851582, ],
# [ 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000 ]]
# )
#np.set_printoptions(precision=10,suppress=True)
#print mag_affine
result['imu'] = []
fimu = fileinput.input(imu_file)
for line in fimu:
tokens = re.split('[,\s]+', line.rstrip())
#s = [float(tokens[7]), float(tokens[8]), float(tokens[9]), 1.0]
#hf = np.dot(mag_affine, s)
#print hf
imu = IMUdata()
imu.time = float(tokens[0])
imu.p = float(tokens[1])
imu.q = float(tokens[2])
imu.r = float(tokens[3])
imu.ax = float(tokens[4])
imu.ay = float(tokens[5])
imu.az = float(tokens[6])
imu.hx = float(tokens[7])
imu.hy = float(tokens[8])
imu.hz = float(tokens[9])
imu.temp = float(tokens[10])
result['imu'].append( imu )
result['gps'] = []
fgps = fileinput.input(gps_file)
last_time = -1.0
for line in fgps:
# Note: the aura logs unix time of the gps record, not tow,
# but for the purposes of the insgns algorithm, it's only
# important to have a properly incrementing clock, it doesn't
# really matter what the zero reference point of time is.
tokens = re.split('[,\s]+', line.rstrip())
time = float(tokens[0])
sats = int(tokens[8])
if sats >= 5 and time > last_time:
gps = GPSdata()
gps.time = time
gps.unix_sec = float(tokens[7])
gps.lat = float(tokens[1])
gps.lon = float(tokens[2])
gps.alt = float(tokens[3])
gps.vn = float(tokens[4])
gps.ve = float(tokens[5])
gps.vd = float(tokens[6])
gps.sats = sats
result['gps'].append(gps)
last_time = time
if os.path.exists(air_file):
result['air'] = []
fair = fileinput.input(air_file)
for line in fair:
tokens = re.split('[,\s]+', line.rstrip())
air = Airdata()
air.time = float(tokens[0])
air.static_press = float(tokens[1])
air.diff_press = 0.0 # not directly available in flight log
air.temp = float(tokens[2])
air.airspeed = float(tokens[3])
air.alt_press = float(tokens[4])
air.alt_true = float(tokens[5])
result['air'].append( air )
# load filter records if they exist (for comparison purposes)
result['filter'] = []
ffilter = fileinput.input(filter_file)
for line in ffilter:
tokens = re.split('[,\s]+', line.rstrip())
lat = float(tokens[1])
lon = float(tokens[2])
if abs(lat) > 0.0001 and abs(lon) > 0.0001:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat*d2r
nav.lon = lon*d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7])*d2r
nav.the = float(tokens[8])*d2r
psi = float(tokens[9])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
nav.psi = psi*d2r
result['filter'].append(nav)
if os.path.exists(pilot_file):
result['pilot'] = []
fpilot = fileinput.input(pilot_file)
for line in fpilot:
tokens = re.split('[,\s]+', line.rstrip())
pilot = Controldata()
pilot.time = float(tokens[0])
pilot.aileron = float(tokens[1])
pilot.elevator = float(tokens[2])
pilot.throttle = float(tokens[3])
pilot.rudder = float(tokens[4])
pilot.gear = float(tokens[5])
pilot.flaps = float(tokens[6])
pilot.aux1 = float(tokens[7])
pilot.auto_manual = float(tokens[8])
result['pilot'].append(pilot)
if os.path.exists(act_file):
result['act'] = []
fact = fileinput.input(act_file)
for line in fact:
tokens = re.split('[,\s]+', line.rstrip())
act = Controldata()
act.time = float(tokens[0])
act.aileron = float(tokens[1])
act.elevator = float(tokens[2])
act.throttle = float(tokens[3])
act.rudder = float(tokens[4])
act.gear = float(tokens[5])
act.flaps = float(tokens[6])
act.aux1 = float(tokens[7])
act.auto_manual = float(tokens[8])
result['act'].append(act)
if os.path.exists(ap_file):
result['ap'] = []
fap = fileinput.input(ap_file)
for line in fap:
tokens = re.split('[,\s]+', line.rstrip())
ap = APdata()
ap.time = float(tokens[0])
ap.hdg = float(tokens[1])
ap.roll = float(tokens[2])
ap.alt = float(tokens[3])
ap.pitch = float(tokens[5])
ap.speed = float(tokens[7])
#ap.flight_time = float(tokens[8])
#ap.target_wp = int(tokens[9])
#ap.wp_lon = float(tokens[10])
#ap.wp_lat = float(tokens[11])
#ap.wp_index = int(tokens[12])
#ap.route_size = int(tokens[13])
result['ap'].append(ap)
cal = imucal.Calibration()
if os.path.exists(imucal_json):
imucal_file = imucal_json
elif os.path.exists(imucal_xml):
imucal_file = imucal_xml
else:
imucal_file = None
if imucal_file:
cal.load(imucal_file)
print 'back correcting imu data (to get original raw values)'
result['imu'] = cal.back_correct(result['imu'])
if recalibrate:
print 'recalibrating imu data using alternate calibration file:', recalibrate
rcal = imucal.Calibration()
rcal.load(recalibrate)
result['imu'] = rcal.correct(result['imu'])
return result
def load(csv_base):
result = {}
comb_path = csv_base + '_sensor_combined_0.csv'
gps_path = csv_base + '_vehicle_gps_position_0.csv'
att_path = csv_base + '_vehicle_attitude_0.csv'
pos_path = csv_base + '_vehicle_global_position_0.csv'
ap_path = csv_base + '_vehicle_attitude_setpoint_0.csv'
air_path = csv_base + '_airspeed_0.csv'
act_path = csv_base + '_actuator_outputs_0.csv'
filter_post = csv_base + '_filter_post.txt'
result['imu'] = []
with open(comb_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
imu = IMUdata()
imu.time = float(row['timestamp']) / 1000000.0
imu.p = float(row['gyro_rad[0]'])
imu.q = float(row['gyro_rad[1]'])
imu.r = float(row['gyro_rad[2]'])
imu.ax = float(row['accelerometer_m_s2[0]'])
imu.ay = float(row['accelerometer_m_s2[1]'])
imu.az = float(row['accelerometer_m_s2[2]'])
hx = float(row['magnetometer_ga[0]'])
hy = float(row['magnetometer_ga[0]'])
hz = float(row['magnetometer_ga[0]'])
hf = np.array([hx, hy, hz])
norm = np.linalg.norm(hf)
hf /= norm
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#print imu.hx, imu.hy, imu.hz
imu.temp = 15.0
result['imu'].append(imu)
result['gps'] = []
with open(gps_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
gps = GPSdata()
gps.time = float(row['timestamp']) / 1000000.0
gps.unix_sec = float(row['time_utc_usec']) / 1000000.0
gps.lat = float(row['lat']) / 1e7
gps.lon = float(row['lon']) / 1e7
gps.alt = float(row['alt']) / 1e3
# print gps.lat, gps.lon, gps.alt
gps.vn = float(row['vel_n_m_s'])
gps.ve = float(row['vel_e_m_s'])
gps.vd = float(row['vel_d_m_s'])
gps.sats = int(row['satellites_used'])
if gps.sats >= 5:
result['gps'].append(gps)
result['air'] = []
with open(air_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
air = Airdata()
air.time = float(row['timestamp']) / 1000000.0
#air.static_press = float(row['SENS_BaroPres'])
air.diff_press = float(row['differential_pressure_filtered_pa'])
air.temp = float(row['air_temperature_celsius'])
air.airspeed = float(row['indicated_airspeed_m_s']) * mps2kt
#air.alt_press = float(row['SENS_BaroAlt'])
air.alt_true = gps.alt
result['air'].append(air)
att = []
with open(att_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
time = float(row['timestamp']) / 1000000.0
q = [
float(row['q[0]']),
float(row['q[1]']),
float(row['q[2]']),
float(row['q[3]'])
]
# either of these will work ...
(roll, pitch, yaw) = px4_quat2euler(q)
att.append([time, yaw, pitch, roll])
pos = []
with open(pos_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
time = float(row['timestamp']) / 1000000.0
lat = float(row['lat']) * d2r
lon = float(row['lon']) * d2r
alt = float(row['alt'])
vn = float(row['vel_n'])
ve = float(row['vel_e'])
vd = float(row['vel_d'])
pos.append([time, lat, lon, alt, vn, ve, vd])
pos_array = np.array(pos)
lat_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 1],
bounds_error=False,
fill_value='extrapolate')
lon_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 2],
bounds_error=False,
fill_value='extrapolate')
alt_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 3],
bounds_error=False,
fill_value='extrapolate')
vn_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 4],
bounds_error=False,
fill_value=0.0)
ve_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 5],
bounds_error=False,
fill_value=0.0)
vd_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 6],
bounds_error=False,
fill_value=0.0)
if os.path.exists(ap_path):
result['ap'] = []
with open(ap_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
ap = APdata()
ap.time = float(row['timestamp']) / 1000000.0
ap.hdg = float(row['yaw_body']) * r2d
ap.roll = float(row['roll_body']) * r2d
ap.pitch = float(row['pitch_body']) * r2d
ap.alt = 0
ap.speed = 0
#ap.alt = float(row['GPSP_Alt']) * m2ft
#ap.speed = float(row['TECS_AsSP']) * mps2kt
result['ap'].append(ap)
result['filter'] = []
for a in att:
nav = NAVdata()
nav.time = a[0]
nav.lat = float(lat_interp(nav.time))
nav.lon = float(lon_interp(nav.time))
nav.alt = float(alt_interp(nav.time))
nav.vn = float(vn_interp(nav.time))
nav.ve = float(ve_interp(nav.time))
nav.vd = float(vd_interp(nav.time))
nav.phi = a[3]
nav.the = a[2]
nav.psi = a[1]
result['filter'].append(nav)
#result['pilot'] = []
#result['act'] = []
#result['ap'] = []
# load filter (post process) records if they exist (for comparison
# purposes)
if os.path.exists(filter_post):
print "found filter_post.txt file"
result['filter_post'] = []
ffilter = fileinput.input(filter_post)
for line in ffilter:
tokens = re.split('[,\s]+', line.rstrip())
lat = float(tokens[1])
lon = float(tokens[2])
if abs(lat) > 0.0001 and abs(lon) > 0.0001:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat * d2r
nav.lon = lon * d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7]) * d2r
nav.the = float(tokens[8]) * d2r
psi = float(tokens[9])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
nav.psi = psi * d2r
result['filter_post'].append(nav)
if os.path.exists(act_path):
result['act'] = []
with open(act_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
act = Controldata()
act.time = float(row['timestamp']) / 1000000.0
ch = [0] * 8
for i in range(len(ch)):
pwm = float(row['output[%d]' % i])
ch[i] = (pwm - 1500) / 500
act.aileron = ch[0]
act.elevator = ch[1]
act.throttle = ch[2]
act.rudder = ch[3]
act.gear = 0
act.flaps = 0
act.aux1 = 0
act.auto_manual = 0
result['act'].append(act)
return result