In [ ]:
import psas_packet
from psas_packet.io import BinFile
import csv
import matplotlib.pyplot as plt
from matplotlib import gridspec
%matplotlib inline
FPS2M = 0.3048
LBF2N = 4.44822
LBS2KG = 0.453592
# Extend PSAS Packet to include our state message
psas_packet.messages.MESSAGES["STAT"] = psas_packet.messages.Message({
'name': "State Vector",
'fourcc': b'STAT',
'size': "Fixed",
'endianness': '!',
'members': [
{'key': "time", 'stype': "Q"},
{'key': "accel", 'stype': "d"},
{'key': "vel", 'stype': "d"},
{'key': "alt", 'stype': "d"},
{'key': "roll_rate", 'stype': "d"},
{'key': "roll_angle", 'stype': "d"},
]
})
# Read data from rust-fc
logfile = BinFile('../logfile-000')
max_acc = 0
rust_time = []
rust_accel_x = []
rust_accel_y = []
rust_accel_z = []
rust_state_time = []
rust_vel = []
rust_alt = []
for fourcc, data in logfile.read():
if fourcc == 'ADIS':
if data['Acc_X'] > max_acc:
max_acc = data['Acc_X']
rust_t = data['timestamp']/1.0e9
rust_time.append(data['timestamp']/1.0e9)
rust_accel_x.append(data['Acc_X'])
rust_accel_y.append(data['Acc_Y'])
rust_accel_z.append(data['Acc_Z'])
if fourcc == 'STAT':
rust_state_time.append(data['timestamp']/1.0e9)
rust_vel.append(data['vel'])
rust_alt.append(data['alt'])
# Read data from JSBSim
max_accel = 0
sim_time = []
measured_accel_x = []
sim_vel_up = []
sim_alt = []
with open('../simulation/data.csv') as datafile:
reader = csv.reader(datafile, delimiter=',')
for row in reader:
# ignore first line
if row[0][0] == 'T':
continue
sim_time.append(float(row[0]))
force_x = float(row[18]) * LBF2N
weight = float(row[6]) * LBS2KG
measured_accel_x.append(force_x/weight)
if (force_x/weight) > max_accel:
max_accel = force_x/weight
sim_t = sim_time[-1]
sim_vel_up.append(-float(row[10]) * FPS2M)
sim_alt.append(float(row[2]))
# line up time
sim_offset = rust_t - sim_t
sim_time = [t + sim_offset for t in sim_time]
In JSBSim the IMU messages are requested to be sent at the real IMU rate of 819.2 Hz:
<output name="localhost" type="SOCKET" protocol="UDP" port="5123" rate="819.2">
But there they are then processed in python for noise and binary packing. Then it's sent as UDP packets which may get lost. Let's see how they appear in the flight comptuer.
In [ ]:
# Get the time difference between each ADIS message
diff = [(rust_time[i+1] - t)*1000 for i, t in enumerate(rust_time[:-1])]
fig, ax1 = plt.subplots(figsize=(18,7))
plt.title(r"rust-fc ADIS Message Interval")
plt.ylabel(r"Time Since Last Sample [ms]")
plt.xlabel(r"Sample Number [#]")
plt.plot(range(len(diff)), diff, 'r.', alpha=1.0, ms=0.3, label="rust-fc Sample Interval")
plt.plot((0, len(diff)), (1.2207, 1.2207), 'k-', lw=0.6, alpha=0.7, label="Expected Sample Interval")
ax1.set_yscale("log", nonposy='clip')
plt.ylim([0.1,100])
#plt.xlim()
ax1.legend(loc=1)
plt.show()
In [ ]:
fig, ax1 = plt.subplots(figsize=(18,7))
plt.title(r"rust-fc ADIS Message Interval")
plt.ylabel(r"Number of Samples [#]")
plt.xlabel(r"Time Since Last Sample [ms]")
n, bins, patches = plt.hist(diff, 1000, histtype='step', normed=1, alpha=0.8, linewidth=1, fill=True)
plt.plot((1.2207, 1.2207), (0, 1000), 'k-', lw=0.6, alpha=0.7, label="Expected Sample Interval")
plt.ylim([0, 35])
#plt.xlim()
ax1.legend(loc=1)
plt.show()
In [ ]:
fig, ax1 = plt.subplots(figsize=(18,7))
plt.title(r"rust-fc Recorded IMU Acceleration")
plt.ylabel(r"Acceleration [m/s${}^2$]")
plt.xlabel(r"Run Time [s]")
plt.plot(rust_time, rust_accel_x, alpha=0.8, lw=0.5, label="rust-fc IMU 'Up'")
plt.plot(rust_time, rust_accel_y, alpha=0.8, lw=0.5, label="rust-fc IMU 'Y'")
plt.plot(rust_time, rust_accel_z, alpha=0.6, lw=0.5, label="rust-fc IMU 'Z'")
plt.plot(sim_time, measured_accel_x, 'k-', lw=1.3, alpha=0.6, label="JSBSim True Acceleration")
#plt.ylim()
#plt.xlim()
ax1.legend(loc=1)
plt.show()
In [ ]:
# Computer difference from FC State and simulation "real" numbers
sim_idx = 0
vel = 0
alt = 0
i_count = 0
sim_matched_vel = []
vel_diff = []
alt_diff = []
for i, t in enumerate(rust_state_time):
vel += rust_vel[i]
alt += rust_alt[i]
i_count += 1
if sim_time[sim_idx] < t:
sim_matched_vel.append(vel/float(i_count))
vel_diff.append(sim_vel_up[sim_idx] - (vel/float(i_count)))
alt_diff.append(sim_alt[sim_idx] - (alt/float(i_count)))
vel = 0
alt = 0
i_count = 0
sim_idx += 1
if sim_idx > len(sim_time)-1:
break
In [ ]:
fig = plt.figure(figsize=(18,9))
plt.subplots_adjust(hspace=0.001) # no space between vertical charts
gs = gridspec.GridSpec(2, 1, height_ratios=[2, 1]) # stretch main chart to be most of the width
ax1 = plt.subplot(gs[0])
plt.title(r"rust-fc State Tracking: Velocity And Velocity Integration Error")
plt.ylabel(r"Velocity [m/s]")
plt.plot(rust_state_time, rust_vel, alpha=0.8, lw=1.5, label="rust-fc State Vector Velocity")
plt.plot(sim_time, sim_vel_up, 'k-', lw=1.3, alpha=0.6, label="JSBSim True Velocity")
plt.ylim([-60,400])
ticklabels = ax1.get_xticklabels()
plt.setp(ticklabels, visible=False)
ax2 = plt.subplot(gs[1])
plt.xlabel(r"Run Time [s]")
plt.ylabel(r"Integration Drift Error [m/s]")
plt.plot(sim_time, vel_diff)
ax1.legend(loc=1)
plt.show()
In [ ]:
fig = plt.figure(figsize=(18,9))
plt.subplots_adjust(hspace=0.001) # no space between vertical charts
gs = gridspec.GridSpec(2, 1, height_ratios=[2, 1]) # stretch main chart to be most of the width
ax1 = plt.subplot(gs[0])
plt.title(r"rust-fc State Tracking: Altitude And ALtitude Integration Error")
plt.ylabel(r"Altitude MSL [m]")
plt.plot(rust_state_time, rust_alt, alpha=0.8, lw=1.5, label="rust-fc State Vector Altitude")
plt.plot(sim_time, sim_alt, 'k-', lw=1.3, alpha=0.6, label="JSBSim True Velocity")
plt.ylim([1390, 7500])
ticklabels = ax1.get_xticklabels()
plt.setp(ticklabels, visible=False)
ax2 = plt.subplot(gs[1])
plt.xlabel(r"Run Time [s]")
plt.ylabel(r"Integration Drift Error [m]")
plt.plot(sim_time, alt_diff)
#plt.xlim()
ax1.legend(loc=1)
plt.show()