%Quadrotor Data Reduction
clear all
close all


%This is the code I used to reduce the Quad Rotor Data
%The data we took to verify the force and moment coupling was from data
%Runs 77 - 106. Before these were the shakedown runs and the attempt to
%observe ground effect and vortex interference











%%
%For simplicity, I save the wind tunnel data files in excel spreadsheets. It takes a
%while longer to run, but it was easier to work with
for i = 77:106 %run numbers
    runnum = int2str(i);
    if i < 100
    filename = strcat('run_00',runnum,'.xls');
    else
    filename = strcat('run_0',runnum,'.xls');
    end
    var = xlsread(filename);
    %Remove header, first four rows
    index = length(var(:,1));
    
    
    QuadData.(['R',num2str(i)])=var(5:index,:);
    
    
    
end
%%
%QuadData.R Run Number

%Yellow = front motor, Green = back, Blue = right, Red = left
WZ = 0;
%The very first data point of Run 77 was our electronic zero data. This
%loop tares the data from the other runs. 
for i = 77:106
    for j = 1:length(QuadData.(['R',num2str(i)])(:,1));
    checkcode = QuadData.(['R',num2str(i)])(j,1);
    if checkcode == 1
    %Woz found, store the values
    WZ = WZ + 1;
    WozRun(WZ) = i;
    WozVals(WZ,:) = QuadData.(['R',num2str(i)])(1,:);
    else
   QuadData.(['R',num2str(i)])(j,7:12) = QuadData.(['R',num2str(i)])(j,7:12) + WozVals(WZ,7:12);
    end
    end
end

%This loads the Balance Interactions from the wind tunnel, see the test report:
 %Bal Cal Matrix:
        Bal = [0.960243      0.014501       0.000112       0.000008      -0.000000032866       0.000038       0.016829       0.00000580;
              -0.000560     0.993306      -0.000021      -0.000042       0.000000005029      -0.000002       0.001818      -0.00001003;
               0.012984     -0.170853       0.994981       0.001376      -0.000000684977       0.004198      -0.046574       0.00015670;
               0.004947     -0.071589       0.000952       0.997615      -0.000000011928       0.000553      -0.047117       0.00000056;
               0.012250     -0.020584       0.011246      -0.001458       0.000000832141       0.992053      -0.042142       0.00020551;
               -0.000539     0.000114      -0.000006       0.000047      -0.000000027025      -0.000050       1.000882      -0.00000897];
for i = 77:106
FMO= QuadData.(['R',num2str(i)])(:,7:12);
for j = 1:length(FMO(:,1))
    FMoVec(1) = FMO(j,1);
    FMoVec(2) = FMO(j,2);
    FMoVec(3) = FMO(j,3);
    FMoVec(4) = FMO(j,4);
    FMoVec(5) = FMO(j,4).^2;
    FMoVec(6) = FMO(j,5);
    FMoVec(7) = FMO(j,6);
    FMoVec(8) = FMO(j,6).^2;
    
  BalanceReduced = Bal*FMoVec';
   BalanceReduced = BalanceReduced';
   QuadData.(['R',num2str(i)])(j,7:12) = BalanceReduced;
    
end

end






%%



%Moment corrections: All balance data is taken around the Model Moment
%Center, which is at the center of the wind tunnel test section. For Runs
%77-106, Quadrotor CG was -2.75 inches beneath MMC. See the test report
for i = 77:106
    
Tx = -2.75;
QuadData.(['R',num2str(i)])(:,9) = QuadData.(['R',num2str(i)])(:,9) + (Tx* QuadData.(['R',num2str(i)])(:,8));
QuadData.(['R',num2str(i)])(:,10) = QuadData.(['R',num2str(i)])(:,10);
QuadData.(['R',num2str(i)])(:,11) = QuadData.(['R',num2str(i)])(:,11) - (Tx* QuadData.(['R',num2str(i)])(:,12));
end


%Average the data. Data sample time was reduced to 10 Hz in an effort to
%see the vortex interference. Unfortunately, this proved to noisy to be
%useful

for i = 77:106    
    for j = 7:17;
        tps = length(QuadData.(['R',num2str(i)])(:,j));
        total = (sum(QuadData.(['R',num2str(i)])(:,j)));
        QuadDataAvg.(['R',num2str(i)])(:,j-5)= total./tps;
      
    end
    %The voltmeter was hooked up backwards for the "Yellow" motor. 
   j = 18;
   tps = length(QuadData.(['R',num2str(i)])(:,j));
   total = abs(sum(QuadData.(['R',num2str(i)])(:,j)));
   QuadDataAvg.(['R',num2str(i)])(:,j-5)= total./tps;
    QuadDataAvg.(['R',num2str(i)])(:,1) = i;
    
end


for i = 77:106
    for j = 1:13;
        k = i - 76;
    TotalData(k,j) = QuadDataAvg.(['R',num2str(i)])(1,j);
    end
end


%Convert all this to Newtons and N-m
TotalData(:,2) = TotalData(:,2).*4.44822; %Lift
TotalData(:,3) = TotalData(:,3).*4.44822; %Drag
TotalData(:,7) = TotalData(:,7).*4.44822;  %Side Force

TotalData(:,4) = TotalData(:,4).*0.112984829; %Pitch
TotalData(:,5) = TotalData(:,5).*0.112984829; %Yaw
TotalData(:,6) = TotalData(:,6).*0.112984829; %Roll



%All voltages we tested are identified as separate Cases to compare the
%model to
index = 0;
for i = 1:length(TotalData(:,1))
    index = index + 1;
    Case(index,:) = TotalData(i,10:13);
end

Ftest = [-TotalData(:,7),-TotalData(:,3),TotalData(:,2)]; %Now the vector is proper, with a positive displacement x being opposite direction as drag
Mtest = [TotalData(:,4),TotalData(:,6),TotalData(:,5)]; %Finally, its good now.
Vtest = [TotalData(:,10),TotalData(:,11),TotalData(:,12),TotalData(:,13)];


%The following lines are from the code


% Model constants
R = 6.1875; %Rotor blade radius


Ir = 1.1998e-4;
I1 = 6.532e-3;
I2 = 6.6994e-3;
I3 = 1.2742e-2;
I = diag([I1,I2,I3]);

q=0;p=0;

m = 0.48;
g = -9.81;
b = 4.3248e-5; %thrust parameter
k = 5.96927e-8; %yaw moment parameter


%negative r values?
r22 = 2.319e-1;
r24 = -2.319e-1;
r11 = 2.3259e-1;
r13 = -2.3259e-1;

Ir = 1.1998e-4;
I1 = 6.532e-3;
I2 = 6.6994e-3;
I3 = 1.2742e-2;
I = diag([I1,I2,I3]);

%Unpack state vector
vx = 0;
vy = 0;
vz = 0;
pp = 0;
qq = 0;
rr = 0;
phi = 0;
theta = 0;
psi= 0;
x = 0;
y = 0;
z = 0;


%Voltage is directly related to rotation rate. Case 1 refers to the
%voltages applied during Run 77, and so on.
for i = 1:length(Case(:,1))

V1 = Case(i,1);
V2 = Case(i,4);
V3 = Case(i,3);
V4 = Case(i,2);



%Convert the voltage cases to the rotation of the blade
w1 = ((21.093*V1) + 38.9422);
w2 = ((21.093*V2) + 38.9422);
w3 = ((21.093*V3) + 38.9422);
w4 = (21.093*V4) + 38.9422;

%---------Rotation Matrices----------------------
c1 = [1 0 0;0 cos(phi) sin(phi);0 -sin(phi) cos(phi)];
c2 = [cos(theta) 0 -sin(theta);0 1 0;sin(theta) 0 cos(theta)];
c3 = [cos(psi) sin(psi) 0;-sin(psi) cos(psi) 0; 0 0 1];

inert2body = c1*c2*c3;

body2inert = inert2body';

%---------Rotate Velocity/Angular Rate Vector-----------------
uvw = inert2body*[vx;vy;vz];

u = uvw(1);
v = uvw(2);
w = uvw(3);

pqr = inert2body*[pp;qq;rr];
p = pqr(1);
q = pqr(2);
r = pqr(3);



%----------Calculate Translational Acceleration in Body Frame------------
%Fg_b = inert2body*[0;0;m*g]; Gravity was wozzed out of data
Ft_b = [0;0;b*(w1^2+w2^2+w3^2+w4^2)];
Fmid = Ft_b;
Fmodel(i,:) = Fmid';

vdot_b = (1/m)*(Ft_b)-[q*w-r*v;r*u-p*w;p*v-q*u];

%-------------Angular Acceleration------------------------

MT = [b*(w2^2*r22+w4^2*r24);-b*(w1^2*r11+w3^2*r13);0];
MG = [-Ir*q*(w1+w2+w3+w4);Ir*p*(w1+w2+w3+w4);0]; 
MR = [0;0;k*(-w1^2+w2^2-w3^2+w4^2)];

M = MT+MG+MR;
Mmodel(i,:) = M';
omegadot_b = inv(I)*M-inv(I)*cross([p;q;r],I*[p;q;r]);





%------------Euler Angles-----------------------------------------
A = [1 tan(theta)*sin(phi) tan(theta)*cos(phi);
    0 cos(phi) -sin(phi);
    0 sin(phi)/cos(theta) cos(phi)/cos(theta)];
    

eulerdot = A*[p;q;r];
     
      
 %------------------Navigation Equations------------------------------

 posdot = [vx;vy;vz];
 
 %---------Rotate Translational Acceleration to Inertial Frame--------
 vdot = body2inert*vdot_b;
 
 omegadot = body2inert*omegadot_b;
 
end
Fcomparison = [Fmodel(:,3),Ftest(:,3)];
%Data Plots:
%First: Model vs Test
figure
plot(Fmodel(:,3),Ftest(:,3),'o') 

xlabel('Model Results (N)','fontsize',14,'fontweight','b')
ylabel('Test Results (N)','fontsize',14,'fontweight','b')
title('UW1961 Results: Lift','fontsize',18,'fontweight','b')

figure
PitchComparison = [Mmodel(:,1),Mtest(:,1)];
plot(Mmodel(:,1),Mtest(:,1),'o')

xlabel('Model Results (N-m)','fontsize',10,'fontweight','b')
ylabel('Test Results (N-m)','fontsize',10,'fontweight','b')
title('UW1961 Results: Pitch','fontsize',12,'fontweight','b')

figure
plot(Mmodel(:,2),Mtest(:,2),'o')

xlabel('Model Results (N-m)','fontsize',10,'fontweight','b')
ylabel('Test Results (N-m)','fontsize',10,'fontweight','b')
title('UW1961 Results: Roll','fontsize',12,'fontweight','b')



figure
YawComparison = [Mmodel(:,3),Mtest(:,3)];
plot(Mmodel(:,3),Mtest(:,3),'o')

xlabel('Model Results (N-m)','fontsize',10,'fontweight','b')
ylabel('Test Results (N-m)','fontsize',10,'fontweight','b')
title('UW1961 Results: Yaw','fontsize',12,'fontweight','b')




%Case vector:
cases = 77:106;


%Thrust (Lift)
figure
set(gca,'fontsize',14);
plot(cases,Fmodel(:,3),'o','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r') 
hold on
plot(cases,Ftest(:,3),'^','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','b')
xlabel('Run Number','fontsize',18,'fontweight','b')
ylabel('Thrust (N)','fontsize',18,'fontweight','b')
title('UW1961 Results: Thrust','fontsize',22,'fontweight','b')
legend('Model Results','Test Results','fontsize',18,'fontweight','b')


%Negative Drag (Displacement in Y)
%figure
%plot(cases,Fmodel(:,2),'o','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r') 
%hold on
%plot(cases,Ftest(:,2),'^','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','b')
%xlabel('Run Number','fontsize',10,'fontweight','b')
%ylabel('Fy (N)','fontsize',10,'fontweight','b')
%title('UW1961 Results: Thrust','fontsize',12,'fontweight','b')
%legend('Model Results','Test Results','fontsize',10,'fontweight','b')


%Pitch 
figure
set(gca,'fontsize',14);
plot(cases,Mmodel(:,1),'o','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r') 
hold on
plot(cases,Mtest(:,1),'^','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','b')
xlabel('Run Number','fontsize',18,'fontweight','b')
ylabel('Pitch (N-m)','fontsize',18,'fontweight','b')
title('UW1961 Results: Pitch','fontsize',22,'fontweight','b')
legend('Model Results','Test Results','fontsize',18,'fontweight','b')

%Roll
figure
set(gca,'fontsize',14);
plot(cases,Mmodel(:,2),'o','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r') 
hold on
plot(cases,Mtest(:,2),'^','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','b')
xlabel('Run Number','fontsize',18,'fontweight','b')
ylabel('Roll (N-m)','fontsize',18,'fontweight','b')
title('UW1961 Results: Roll','fontsize',22,'fontweight','b')
legend('Model Results','Test Results','fontsize',18,'fontweight','b')

%Yaw
figure
set(gca,'fontsize',14);
plot(cases,Mmodel(:,3),'o','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r') 
hold on
plot(cases,Mtest(:,3),'^','Linewidth',2,'MarkerEdgeColor','k','MarkerFaceColor','b')
xlabel('Run Number','fontsize',18,'fontweight','b')
ylabel('Yaw (N-m)','fontsize',18,'fontweight','b')
title('UW1961 Results: Yaw','fontsize',22,'fontweight','b')
legend('Model Results','Test Results','fontsize',18,'fontweight','b')

%Showing the data this way makes Yaw look worse, but again the magnitude is
%much less

%Moments
% figure
% plot(cases,Mmodel(:,1),'o','Linewidth',2,'MarkerEdgeColor','r','MarkerFaceColor','r') 
% hold on
% plot(cases,Mmodel(:,2),'+','Linewidth',2,'MarkerEdgeColor','r','MarkerFaceColor','r')
% plot(cases,Mmodel(:,3),'^','Linewidth',2,'MarkerEdgeColor','r','MarkerFaceColor','r') 
% plot(cases,Mtest(:,1),'o','Linewidth',2,'MarkerEdgeColor','b','MarkerFaceColor','b')
% plot(cases,Mtest(:,2),'+','Linewidth',2,'MarkerEdgeColor','b','MarkerFaceColor','b')
% plot(cases,Mtest(:,3),'^','Linewidth',2,'MarkerEdgeColor','b','MarkerFaceColor','b')
% xlabel('Run Number','fontsize',10,'fontweight','b')
% ylabel('Moment (N-m)','fontsize',10,'fontweight','b')
% title('UW1961 Results: Moment','fontsize',12,'fontweight','b')
% legend('Model Results: Pitch','Model Results: Roll','Model Results: Yaw','Test Results: Pitch','Test Results: Roll','Test Results: Yaw')


 %----------------------Package Output------------------------------
 
 xdot = [vdot;omegadot;eulerdot;posdot];