close all
clear all

data = load('Nav440_1_machine.log');
Prgn_unix_time = data(:,1);
Prgn_Pitch = data(:,15);
Prgn_PitchRate = data(:,17);

t = Prgn_unix_time-Prgn_unix_time(1);  %Convert to seconds since start of program
dt = t(end)/length(t);

Prgn_PitchVel = [0 ; diff(Prgn_Pitch)./diff(t)];


data = load('StabPole_1_machine.log');

unix_time = data(:,1);
PropGain = data(:,2);
IntGain = data(:,3);
DerGain = data(:,4);
PitchTarg = data(:,5);
Pitch = data(:,7);
Cmd = data(:,9);
Vout = data(:,10);
mode = data(:,11);
Amp = data(:,12);
Freq = data(:,13);

t = unix_time-unix_time(1);  %Convert to seconds since start of program
dt = t(end)/length(t);

PitchVel = [0 ; diff(Pitch)./diff(t)];


time = unix_time;
Prgn_time = Prgn_unix_time;



figure
ah1 = subplot(2,1,1)
plot(time,PitchTarg);
hold all
plot(time,Pitch);
plot(Prgn_time,Prgn_Pitch);
legend('Pitch Target','Pole Pitch','Prgn Pitch');
ylabel('Degrees');
xlabel('time');
grid on;


ah2 = subplot(2,1,2);
plot(time,Vout);
ylabel('Valve Command Voltage (V)');
xlabel('time');
grid on;

% 
% ah3 = subplot(3,1,3);
% plot(time,PitchVel);
% hold all
% plot(Prgn_time,Prgn_PitchVel);
% ylabel('Pitch Rate (deg/s)');
% xlabel('time');
% legend('Pole Pitch Rate','Prgn Pitch Rate');
% grid on;


linkaxes([ah1 ah2],'x');

