% Section3.m
%
% Generate figures for Section 3 of paper
% "The Effect of Rotor Tilt on the Stability and Gust Rejection Properties
%  of VTOL Multirotor Aircraft"
% by James Whidborne Arthur Mendez and Alastair Cooke

% (c) James Whidborne
% Cranfield University, 18 August 2020

disp('3 PVTOL Equations of Motion')
disp('---------------------------')

[~, pvtol, ~, ~] = Coefficients; % Get rotor coefficents for PVTOL

%%  Variation in trim  for $V_W\in[0,20]$ m/s with $\Upsilon={0,10}$ deg

Upsilon = [0   10]*pi/180; % Upsilon = {0, 10} deg
Vw = [0:0.5:20];           % horizontal airspeed (m/s)
n_Vw = length(Vw);

trimoptions = optimset('Display','on','MaxFunEvals',1000,'TolFun',1e-7,'TolX',1e-7);% search minimization options
thetatrim = zeros(length(Upsilon),length(Vw)); % pre-allocate trim pitch angle array
Omegaltrim = thetatrim;Omegartrim=thetatrim;   % pre-allocate left and right rotor speed arrays
for i=1:length(Upsilon)
    Omegal0 = 150; Omegar0 = 150; theta0 = 0; % initial trim values guess
    x0 = [theta0,(Omegal0+Omegar0) ,(Omegal0-Omegar0)];% [theta, omegal+omegar,omegal-omegar]
    for j=1:n_Vw
        [xtrim ,fmin] = fminsearch(@(x) PvtolTrim(x,pvtol,Vw(j),Upsilon(i)),x0,trimoptions);
        if(fmin>1e-10),warning(['Non-convergence: fmin = ',num2str(fmin)]),end % non convergence
        thetatrim(i,j) = xtrim(1);
        Omegaltrim(i,j) = (xtrim(2)+xtrim(3))/2;
        Omegartrim(i,j) = (xtrim(2)-xtrim(3))/2;
        if(j==1),x0 = [-2*pi/180,xtrim(2),0]; % otherwise convergence problem
        else
            x0 = [xtrim(1:3)]*1.02; 
        end
    end
end

%% Plot Figure 8 - Trim for the Draganflyer X-Pro quadrotor with Upsilon = 0.

figure(8),
yyaxis left
plot(Vw,thetatrim(1,:)*180/pi,'--','LineWidth',lw)
grid on
h99=gca; ylim(h99,[0 20]); % change axes limits
h99.YTick=[0:2:20];
h99.YMinorTick='on';
xlabel('$V_h$ (m/s)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$\hat{\theta}$ (deg)','Interpreter' ,'Latex','fontsize',fs)

yyaxis right
plot(Vw,Omegaltrim(1,:),'LineWidth',lw )% Omegaltrim=Omegartrim
h99=gca; ylim(h99,[120 170]); % change axes limits
h99.YTick=[120:5:170];
h99.XMinorTick='on'; h99.YMinorTick='on';
grid on

xlabel('$V_W$ (m/s)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$\hat{\Omega}$ (rad/s)','Interpreter' ,'Latex','fontsize',fs)
title('$\Upsilon = 0^\circ$','Interpreter' ,'Latex' )
legend('$\hat{\theta}$','$\hat{\Omega} $',...
    'Interpreter' ,'Latex','fontsize',12,'Location','north west')
% save
if(printflag) save_pdf(gcf,'trim0'); end

%% Plot Figure 9 - Trim for the Draganflyer X-Pro quadrotor with Upsilon = 10.

figure(9),
plot(Vw,(Omegaltrim(2,:)+Omegartrim(2,:))/2,'-k',Vw,Omegaltrim(2,:),':b',Vw,Omegartrim(2,:),'-.b','LineWidth',lw ) 
h99=gca; ylim(h99,[120 240]); % change axes limits
h99.YTick=[120:20:240];
h99.XMinorTick='on'; h99.YMinorTick='on';
grid on

xlabel('$V_W$ (m/s)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$(\hat{\Omega}_l, \hat{\Omega}_r)$ (rad/s)','Interpreter' ,'Latex','fontsize',fs)
title('$\Upsilon = 10^\circ$','Interpreter' ,'Latex' )
g99=legend('$(\hat{\Omega}_l+\hat{\Omega}_r)/2$','$\hat{\Omega}_l$','$\hat{\Omega}_r$');
set(g99,'Interpreter' ,'Latex','fontsize',12,'Location','north west')
% save
if(printflag) save_pdf(gcf,'trim10'); end


%% Figure 10 - Dependance of mean trim on $\Upsilon\in[0,30]$ at constant V_w.

Vw = 20;                     % horizontal airspeed (m/s) 
Upsilon = [0:0.5:30]*pi/180; % Upsilon
trimoptions = optimset('Display','on','MaxIter',5000,'MaxFunEvals',5000,'TolFun',1e-8,'TolX',1e-8);
thetatrim = 0*Upsilon;Omegaltrim=thetatrim;Omegartrim=thetatrim; % pre-allocate arrays 
% initial trim values guess
Omegal0 = 150; Omegar0 = 150; theta0 = 0;
x0=[theta0,(Omegal0+Omegar0) ,(Omegal0-Omegar0)]; % [theta, omegal+omegar,omegal-omegar]
for i=1:length(Upsilon)
    [xtrim ,fmin] = fminsearch(@(x) PvtolTrim(x,pvtol,Vw,Upsilon(i)),x0,trimoptions);
    if(fmin>1e-10),warning(['Non-convergence: fmin = ',num2str(fmin)]),end % non convergence
    thetatrim(i) = xtrim(1);
    Omegaltrim(i) = (xtrim(2)+xtrim(3))/2;
    Omegartrim(i) = (xtrim(2)-xtrim(3))/2;
    %x0=[-2*pi/180,xtrim(2),0];
    if(i==1)
        x0 = [xtrim(1:2),5]*.99;%   otherwise convergence problem
    else
        x0 = [xtrim(1:3)]*.99; 
    end
end

% plot
figure(10),
plot(Upsilon*180/pi,(Omegaltrim+Omegartrim)/2, 'LineWidth',lw ) 
h99=gca; ylim(h99,[160 260]); % change axes limits
h99.YTick=[160:20:260];
h99.XMinorTick='on'; h99.YMinorTick='on';
grid on

xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$(\hat{\Omega}_l+\hat{\Omega}_r)/2$ (rad/s)','Interpreter' ,'Latex','fontsize',fs)
title('$V_m = 20$ m/s','Interpreter' ,'Latex' )
% save
if(printflag) save_pdf(gcf,'TrimDependance'); end