% Taylor, Brian R.
% Processing Fenrir Flight 17 Data
% Flight on 2015-02-09
%     Straight and level after launch, pitch damper, and 3211 excitations on
%     de and da channels. Pilot input blended onto outboard flap channels.
%
% brtaylor@umn.edu
%

% setting up the environment
close all
clear all
clc

% load the flight data
load flightdata_830.7216sec.mat

% looks like the flight starts at about 668 seconds and ends around 704 seconds
ifly = find(time>660,1,'first');
iland = find(time<704,1,'last');

figure;plot(time(ifly:iland),ias(ifly:iland));
grid on
xlabel('Time, (s)');
ylabel('Indicated Airspeed (m/s)');

% plot throttle command
figure;plot(time(ifly:iland),dthr_in(ifly:iland));
grid on
xlabel('Time (s)');
ylabel('Throttle Command (ND)')

% map pilot stick input to deflection
de_pilot =  ((de_in-.03785)*39.498370692209)*pi/180;
da_pilot =  -1*((da_l_in-.0158)*39.498370692209)*pi/180;

L4_pilot = de_pilot + da_pilot;
R4_pilot = de_pilot - da_pilot;

% map q to de due to pitch damper
de_damper = .2*q;

% figure out when the excitation started
istart = 33665;
istop = 34946;

% elevator deflection due to excitation
de_excite = de(istart:istop) - de_damper(istart:istop) - de_pilot(istart:istop);

% aileron deflection due to excitation
da_excite = da_l(istart:istop) - da_pilot(istart:istop);

% plot pitch channel
figure;plot(time(istart:istop),de_pilot(istart:istop)*180/pi,time(istart:istop),de_damper(istart:istop)*180/pi,time(istart:istop),de_excite*180/pi,time(istart:istop),de(istart:istop)*180/pi,time(istart:istop),q(istart:istop)*180/pi,time(istart:istop),theta(istart:istop)*180/pi)
legend('Pilot','Damper','Excitation','Total Elevator','Pitch Rate','Pitch Angle')
grid on
xlabel('Time (s)');
ylabel('Deflection (deg) / Rate (deg/s) / Angle (deg)');

% plot L4/R4 surfaces
figure;plot(time(istart:istop),L4_pilot(istart:istop)*180/pi,time(istart:istop),R4_pilot(istart:istop)*180/pi);
legend('L4','R4')
grid on
xlabel('Time (s)');
ylabel('Surface Deflection (deg)');

% plot roll channel
figure;plot(time(istart:istop),da_pilot(istart:istop)*180/pi,time(istart:istop),da_excite*180/pi,time(istart:istop),da_l(istart:istop)*180/pi,time(istart:istop),p(istart:istop)*180/pi,time(istart:istop),r(istart:istop)*180/pi,time(istart:istop),phi(istart:istop)*180/pi)
legend('Pilot','Excitation','Total Aileron','Roll Rate','Yaw Rate','Roll Angle')
grid on
xlabel('Time (s)');
ylabel('Deflection (deg) / Rate (deg/s) / Angle (deg)');

% inertial velocity
navv = sqrt(navve.^2 + navvn.^2 + navvd.^2);

% plot speeds
figure;plot(time(istart:istop),ias(istart:istop),time(istart:istop),ias_filt(istart:istop),time(istart:istop),navv(istart:istop),time(istart:istop),q(istart:istop))
legend('IAS','Filt IAS','Inertial','Pitch Rate')
grid on
xlabel('Time (s)');
ylabel('Speed (m/s) / Rate (rad/s)');