%-----------------------------------------------------------------
% TRIMDEMO
% ========
% Demonstrates the trimming facility by determining trimmed-flight
% elevator deflections as a function of the true airspeed V for
% the DHC-2 'Beaver' aircraft (horizontal, wings-level flight).
%
% The trim code from this program has largely been copied from
% ACTRIM.M. See the source code of that program for more info
% about the optimization process. Note: in future versions of the
% FDC toolbox, it is planned to put the actual trim-computations
% completely into separate Matlab functions in order to enhance
% the flexibility of ACTRIM. That will make it easier to create
% applications such as this trim-demo program.
%-----------------------------------------------------------------

clc
disp('  The FDC toolbox - TRIMDEMO');
disp('  ==========================');
disp('  Demonstration of the trimming facility. Determining trimmed-');
disp('  flight elevator deflections for ten different airspeeds V by');
disp('  calling the Beaver model. This will take some time to compute!');
disp(' ');

% Load aircraft parameters for the 'Beaver' model.
% ------------------------------------------------
load aircraft.dat -mat

xinco=[45,zeros(1,11)]';
xfix=1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% USER INPUTS: (PARTLY) DEFINE FLIGHT CONDITION %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Steady horizontal wings-level flight-condition (user-input
% for altitude, flaps and engine speed only).
% ----------------------------------------------------------
H = input('  Altitude [m], default = 1000: ');
if isempty(H)
   H = 1000;
end


deltaf = input('  Flap angle [deg], default = 0: ')*pi/180;
if isempty(deltaf)
   deltaf = 0;
end

n = input('  Engine speed [RPM], default = 1800: ');
if isempty(n)
   n = 1800;
end

psi      = 0;                        % heading [rad]
phi      = 0;                        % roll angle [rad]
gamma    = 0;                        % flightpath angle [rad]
phidot   = 0;                        % roll rate [rad/s]
psidot   = 0;                        % yaw rate [rad/s]
thetadot = 0;                        % pitch rate [rad/s]
G        = 0;                        % centripetal acceleration [m/s^2]

pz       = 20; % Initial guess of manifold pressure ["Hg]

deltae   = []; % Will be filled with deltae-value for 10 velocities.
deltaa   = [];
deltar   = [];
mfp   = [];

% Display results header
% ----------------------
disp(' ');
disp('  ---');
disp(' ');
disp('  TRIMDEMO will now compute steady-state flight conditions');
disp('  for ten different values of the airspeed.');
disp(' ');
disp('  <<< Press any key to continue >>>');
pause;
disp(' ');
disp('  ---');
disp(' ');
disp('  Working ... ');
disp(' ');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ACTUAL TRIM PROCESS                                       %
%                                                           %
% MOST OF THE FOLLOWING CODE HAS BEEN COPIED FROM ACTRIM.M! %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% The Simulink system BEAVER or an equivalent aircraft model is called by
% the function call xdot = feval('beaver',t,x,u,'outputs'), followed by
% xdot = feval('beaver',t,x,u,'derivs') to obtain the time-derivatives of
% the state vector x. Here t = 0 (system is time-independent). The
% function call is created here, and stored in the variable modfun.
% Note: the aircraft model itself will be compiled before applying these
% function calls!
% -----------------------------------------------------------------------
modfun   = ['xdot = feval(''beaver'',0,x,u,''outputs''); '];
modfun   = [modfun 'xdot = feval(''beaver'',0,x,u,''derivs'');'];

% Pre-compile the aircraft model.
% -------------------------------
warning off
feval('beaver',[],[],[],'compile');
warning on

% Set default warning flag (will be set to 1 if optimization solution
% doesn't converge)
% -------------------------------------------------------------------
warningflag = 0;

% Take 10 different velocities within the 'Beaver' flight-envelope
% ================================================================
%for V = 59:1:60,                                 % TAS-range [m/s]
V =60;
  % Initial guess of state vector
  % -----------------------------
  xinco = [V 0 0 0 0 0 0 0 0 0 0 0]';

  % constant variables
  % ------------------
  ctrim = [V H psi gamma G psidot thetadot phidot deltaf n phi]';

  % vtrim = [alpha beta deltae deltaa deltar pz]', will be adjusted
  % by trim algorithm
  % ---------------------------------------------------------------
  vtrim = [0 0 0 0 0 pz]';

  options = optimset('Display','off','TolX',1e-30,'MaxFunEvals',5e5,'MaxIter',5e5);
  [vtrimmed,fval,exitflag,output] = fminsearch('accost',vtrim,options,...
                                   ctrim,'s','c','f',modfun);
  
  % Display error message when maximum number of iterations is 
  % reached before finding converged solution
  % ----------------------------------------------------------
  if exitflag == 0
      warning('Maximum number of iterations was exceeded!');
      disp(['- velocity in loop: ' num2str(V) 'm/s']);
      disp(['- number of function evaluations: ' num2str(output.funcCount)]);
      disp(['- number of iterations: ' num2str(output.iterations)]);
      disp(' ');
      warningflag = 1;
  else 
      disp(['Converged to a trimmed solution at V = ' num2str(V) 'm/s']);
  end
  
   % Call subroutine ACCONSTR, which contains the flight-path constraints
   % once more to obtain the final values of the inputvector and states
   % --------------------------------------------------------------------
   [x,u] = acconstr(vtrimmed,ctrim,'s','c','f');

   
%end


% Release compiled aircraft model now that all results are known
% --------------------------------------------------------------
feval('beaver',[],[],[],'term');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CREATE TRIMMED-FLIGHT ELEVATOR DEFLECTION CURVE %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Plot results
% ------------
V = [30:3:60];
plot(V,-deltae*180/pi);
title('Elevator deflection vs. true airspeed');
xlabel('V [m/s]');
ylabel('-\delta_{e} [deg]');
text(35, 0  , ['Flap angle: ' num2str(deltaf*180/pi) ' deg']);
text(35, 0.8, ['Engine speed: ' num2str(n) ' RPM']);
text(35, 1.6, ['Altitude: ' num2str(H) ' m']);

% If one or more points did not converge, display warning message
% ---------------------------------------------------------------
if warningflag == 1
   h=text(35,5,'Warning: one or more solutions did not converge!');
   set(h,'Color',[1 0 0]);
end

% Plot ready message
% ------------------
disp(' ');
disp('  ---');
disp(' ');
disp('  Trimming process is finished. The resulting elevator');
disp('  deflection curve has been plotted in the figure window.');
if warningflag == 1
   disp(' ');
   disp('  Warning: one or more solutions did not converge!');
   disp('  Try to increase max. number of iterations in FMINSEARCH');
   disp('  call to obtain a more accurate solution.');
end
disp(' ');
figure(gcf);
disp(' ');

% Get rid of variables from optimization process which
% are not needed anymore.
% ----------------------------------------------------
clear ctrim vtrim vtrimmed fval options output modfun exitflag warningflag

%-----------------------------------------------------------------------
% The Flight Dynamics and Control Toolbox version 1.4.0. 
% (c) Copyright Marc Rauw, 1994-2003. Licensed under the Open Software 
% License version 2.1; see COPYING.TXT and LICENSE.TXT for more details.
% Last revision of this file: December 31, 2004.  
%-----------------------------------------------------------------------