close all
clear all

filenm = 'FFD_Log_July20.txt';

data = load(filenm);

Col = 3;
XVel = data(:,Col); Col = Col+1;
YVel = data(:,Col); Col = Col+1;
ZVel = data(:,Col); Col = Col+1;
Pressure = data(:,Col); Col = Col+1;
N = 10;
FilteredPressure = filtfilt(ones(1,N)/N, 1,Pressure); 

VertVel = [0 ; (1/1.5)*diff(FilteredPressure)/.1];

ID = 1;
BeamAmp1 = data(:,Col); Col = Col+1;
BeamAmp2 = data(:,Col); Col = Col+1;
BeamAmp3 = data(:,Col); Col = Col+1;
BeamCorr1 = data(:,Col); Col = Col+1;
BeamCorr2 = data(:,Col); Col = Col+1;
BeamCorr3 = data(:,Col); Col = Col+1;
TargetHeading = data(:,Col); Col = Col+1;

ID = 2;
MeasuredHeading = data(:,Col); Col = Col+1;
FilteredHeading = data(:,Col); Col = Col+1;
RudderCommand = data(:,Col); Col = Col+1;
RawWingAngle = data(:,Col); Col = Col+1;
m = -60/64.57;
z = (2+66.57)/2;
WingAngle = m*((RawWingAngle/100)-z);

time = 0.1*(0:length(MeasuredHeading)-1);


figure('Position',[ 1474          27        1024        1361]);
ah(1) = subplot(5,1,1)
plot(time,Pressure,time,FilteredPressure)
ylabel('Pressure (psi)');

ah(2) = subplot(5,1,2)
plot(time,WingAngle);
ylabel('Wing Angle (deg)');


ah(3) = subplot(5,1,3)
plot(time,TargetHeading+360,time,MeasuredHeading);
ylabel('Heading (deg)');
legend('Target','Measured');
linkaxes(ah,'x');

