
% Initialization commands
% -----------------------
format short e;
options = [];
turntype = 'c';

% Display header; welcome to ACTRIM!
% ----------------------------------
clc
disp('The FDC toolbox - ACTRIM');
disp('========================');
disp(' ');
disp('This program searches determines a steady-state trimmed-flight condition');
disp('for a non-linear aircraft model in Simulink.');
disp(' ');
disp(' ');


if exist('AM')==0 | exist('EM')==0 | exist('GM1')==0 | exist('GM2')==0
    h=newMsgBox(['First, the model parameters need to be retrieved from file ', ...
                 '(e.g. AIRCRAFT.DAT). Click ''OK'' or press Enter to continue.']);
    uiwait(h);
    datload('aircraft');
end

% If model parameters are still not present in the workspace,
% e.g. due to an incorrect datafile, force an abort of ACTRIM.
% ------------------------------------------------------------
if exist('AM')==0 | exist('EM')==0 | exist('GM1')==0 | exist('GM2')==0
    error(['ERROR: the model parameters are still not present in the workspace! ', ...
                 'Aborting ACTRIM.']);
end



xinco = [45 0 0 0 0 0 0 0 0 0 0 0]';


xfix = 1;
%%%% fixstate;

disp('Give name of system with aircraft model');
disp('default = beaver');
sysname = input('> ','s');
if isempty(sysname)
   sysname = 'beaver';
end


clc
opt = txtmenu('Select type of steady-state flight',...
              'Steady wings-level flight','Steady turning flight',...
              'Steady pull-up','Steady roll','Quit');

skip  = 0; % Do not skip iteration block, unless option 'quit' is used
           % (then skip will be set to 1).

% Define flight condition, depending upon trim option chosen
% ----------------------------------------------------------
if opt == 1                                       % STEADY WINGS-LEVEL FLIGHT
                                                  % -------------------------
   clc
   disp('Steady wings-level flight.');
   disp('==========================');

   V = input('Give desired airspeed [m/s], default = 45: ');
   if isempty(V)
      V = 45;
   end

   H = input('Give (initial) altitude [m], default = 0: ');
   if isempty(H)
      H = 0;
   end

   psi  = input('Give heading [deg], default = 0: ')*pi/180;
   if isempty(psi)
      psi = 0;
   end

   ok = 0;
  
      
         gammatype = 'm';
      
      
  
      

   phidot   = 0;
   psidot   = 0;
   thetadot = 0;

   rolltype = 'b';   % No rolling, so for reasons of simplicity the default setting, 
                     % i.e. a body-axes roll, will be used.
end

%%%%

         % DEFINE CONFIGURATION OF THE AIRPLANE, IF NOT QUITTING
                     % -----------------------------------------------------

   % For the 'Beaver' model, the flap angle and engine speed define the
   % configuration of the aircraft (engine speed is selected by the pilot
   % and maintained by the regulator of the propeller). Other aircraft
   % may have other definitions, like gear in/out, slats, settings of trim
   % surfaces, multiple engines, etc., so change the following lines if
   % required!
   % ---------------------------------------------------------------------
   deltaf = input('Give flap angle [deg], default = 0: ')*pi/180;
   if isempty(deltaf)
      deltaf = 0;
   end

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

   % For the 'Beaver' aircraft, the engine power is determined by the engine
   % speed, which has been set above, and the manifold pressure, which will
   % be involved in the trim process if the flightpath angle gamma is user-
   % specified, or kept constant if pz is user-specified (defined in string-
   % variable gammatype). If the manifold pressure is to be adjusted by the
   % numerical trim algorithm, it is important to specify a meaningful esti-
   % mation of the manifold pressure, because otherwise the trim process may
   % not converge, or may give unrealistic results if the optimization pro-
   % cess converges to a LOCAL minimum.
   % -----------------------------------------------------------------------
   if gammatype == 'f'
      pz = input('Give estimate of manifold pressure pz ["Hg], default = 20: ');
   else  % gammatype == 'm'
      pz = input('Give manifold pressure pz ["Hg], default = 20: ');
   end
   if isempty(pz)
      pz = 20;
   end

%   % Hdot is the rate-of-climb, which is a function of the flightpath angle
%   % (change the program if you want to specify Hdot itself in stead of
%   % the flightpath angle gamma).
%   % ----------------------------------------------------------------------
%   Hdot = V*sin(gamma);

   % G is the centripetal acceleration, used for the coordinated turn
   % constraint.
   % ----------------------------------------------------------------
   G = psidot*V/9.80665;

   % If steady, uncoordinated, turning condition must be determined, the
   % roll angle can be specified freely; equilibrium will be obtained for
   % a trimmed-flight sideslip angle, which in this case usually will be
   % quite large.
   % --------------------------------------------------------------------
   phi = [];
   

   if isempty(phi)
      phi = 0;
   end

   %*************************************************************
   ROC = [];
   deltae1 = [];
   alpha1 = [];
   TAS = [];
   for V = 30:1:60
   
   
   if gammatype == 'f' % gamma in ctrim, pz in vtrim
      ctrim = [V H psi gamma G psidot thetadot phidot deltaf n phi]';
      vtrim = [0 0 0 0 0 pz]';
   else % gamma in vtrim, pz in ctrim
      ctrim = [V H psi pz G psidot thetadot phidot deltaf n phi]';
      vtrim = [0 0 0 0 0 0]';  % <-- initial guess for gamma = 0!
   end

   % The Simulink system BEAVER or an equivalent aircraft model is called by
   
   modfun   = ['xdot = feval(''',sysname,''',0,x,u,''outputs''); '];
   modfun   = [modfun 'xdot = feval(''',sysname,''',0,x,u,''derivs'');'];

   % First pre-compile the aircraft model (temporarily suppress warnings
   % to prevent harmless, but inconvenient messages when the model is 
   % already compiled).
   %--------------------------------------------------------------------
   warning off
   feval(sysname,[],[],[],'compile');
   clear ans % (the above feval statement somehow generates an 'ans' variable)
   warning on

   % Call minimization routine FMINSEARCH. A scalar cost function is contained
   % in the M-file ACCOST, which also calls the constraints for coordinated
   % turns and rate-of-climb of the aircraft. FMINSEARCH returns the trim values
   % of alpha, beta, deltae, deltaa, deltar, and pz in the vector vtrimmed.
   % The convergence tolerance has been set to 1e-10 (default 1e-4), the 
   % maximum number of function evaluations has been set to 5000 (default 1200),
   % and the maximum number of iterations has been set to 3000 (default 1200)
   % ----------------------------------------------------------------------
   clc;
   
   
   options = optimset('Display','off','TolX',1e-30,'MaxFunEvals',5e5,'MaxIter',5e5);
   [vtrimmed,fval,exitflag,output] = fminsearch('accost',vtrim,options,...
      ctrim,rolltype,turntype,gammatype,modfun)
   
   % Display error message when maximum number of iterations is 
   % reached before finding converged solution
   % ----------------------------------------------------------
   

   

   % 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,rolltype,turntype,gammatype);
   
   
   % Call a/c model once more, to obtain the time-derivatives of the states
   % in trimmed-flight condition. By examining xdot, the user can decide if
   % the trimmed-flight condition is accurate enough. If not, either the
   % error tolerance of the minimization routine needs to be tightened, or
   % the cost function in ACCOST.M needs to be changed.
   % -----------------------------------------------------------------------
   eval(modfun);
   
   
   % Release compiled aircraft model now that all results are known
   % --------------------------------------------------------------
   feval(sysname,[],[],[],'term');
   
   ROC = [ROC xdot(end)];
   deltae1 = [deltae1 u(1)];
   alpha1 = [alpha1 x(2)];
   TAS = [TAS x(1)];
   end
   
   
   plot(TAS, ROC);
   % Display the final results. x, u, and xdot contain the inputvector,
   % state vector, and time-derivative of the state vector, respectively.
   % --------------------------------------------------------------------
   disp('end');