% The FDC toolbox. Trim routine ACTRIM.
% 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(' ');

% Check if AM, EM, GM1, and GM2 have been defined in the Matlab workspace.
% If not, run DATLOAD to load them from file.
% -------------------------------------------------------------------------
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


% Set xinco (= initial value of the state vector). 
% -------------------------------------------------------------------------
xinco = [45 0 0 0 0 0 0 0 0 0 0 0]';

% Here, all state variables are allowed to vary, so xfix will be set to
% one. Replace this line by:
% -------------------------------------------------------------------------
xfix = 1;
%%%% fixstate;

% The name of the aircraft model to be evaluated will be stored in the
% stringvariable sysname. 
% ------------------------------------------------------------------------
disp('Give name of system with aircraft model');
disp('default = beaver');
sysname = input('> ','s');
if isempty(sysname)
   sysname = 'beaver';
end

% Display menu in which the user can choose between a number of trimmed-
% flight conditions and quitting.
% ----------------------------------------------------------------------
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).
matrnostra = eye(0);

speed = [41.15 42.15 43.2 45.39 46.56]; n = 5500; pot_attesa = 67.89e3;
coeffgamma = 1;
%  speed = [66.88 67.91 68.93 69.96 70.99]; n = 5000; pot_attesa = 51.29e3;
%  coeffgamma = 0;
% speed = [63.79 65.02 66.26 67.49 68.73]; n = 4800; pot_attesa = 45.258e3;
% coeffgamma = 0;
% speed = [59.16 60.19 61.21 62.24 63.27]; n = 4300; pot_attesa = 37.72e3;
% coeffgamma = 0;

for cond = 1:1:5
% Define flight condition, depending upon trim option chosen
% ----------------------------------------------------------
if opt == 1                                  % STEADY WINGS-LEVEL FLIGHT
    if cond ==  1
        V = speed(cond);
        H = 0;
        psi = 0;
        gammatype = 'f';
        gamma = (8.38*pi/180)*coeffgamma;
        pz = 22;     
    elseif cond == 2
        V = speed(cond);
        H = 609.76;
        psi = 0;
        gammatype = 'f';
        gamma = (7.62*pi/180)*coeffgamma;
        pz = 21;
        
    elseif cond == 3
        V = speed(cond);
        H = 1220;
        psi = 0;
        gammatype = 'f';
        gamma = (6.75*pi/180)*coeffgamma;
        pz = 24;
       
    elseif cond == 4
        V = speed(cond);
        H = 1829.27;
        psi = 0;
        gammatype = 'f';
        gamma = (5.78*pi/180)*coeffgamma;
        pz = 21;
        
    elseif cond == 5
        V = speed(cond);
        H = 2439;
        psi = 0;
        gammatype = 'f';
        gamma = (5.01*pi/180)*coeffgamma;
        pz = 24;
       
    end
      
   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.

elseif opt == 2                                       % STEADY TURNING FLIGHT
                                                      % ---------------------
   clc
   disp('Steady turning flight.');
   disp('======================');

   ok = 0;
   while ok~= 1
      turntype = input('Do you want a coordinated or uncoordinated turn ([c]/u)? ','s');
      if isempty(turntype)
         turntype = 'c';
      end
      if turntype == 'c' | turntype == 'u'
         ok = 1;
      else
         disp('   Invalid entry. Please choose either ''c'', or ''u''');
      end
   end

   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 initial heading [deg], default = 0: ')*pi/180;
   if isempty(psi)
      psi = 0;
   end

   ok = 0;
   while ok~= 1
      gammatype = input('Use specified manifold pressure or flight-path angle ([m]/f)? ','s');
      if isempty(gammatype)
         gammatype = 'm';
      end
      if gammatype == 'f'
         gamma = input('Give flightpath angle [deg], default = 0: ')*pi/180;
         if isempty(gamma)
            gamma = 0;
            ok = 1;
         end
      elseif gammatype == 'm'
         ok = 1;
      else
         disp('   Invalid entry. Please choose either ''m'', or ''f''');
      end
   end

   phidot = 0;
   thetadot = 0;

   ok = 0;
   while ok~= 1
      answ = input('Use specified turnrate or radius ([t]/r)? ','s'); 
      if isempty(answ)
         answ = 't';
      end
      if answ == 't'
         psidot = input('Give desired rate of turn [deg/s], default = 0: ')*pi/180;
         if isempty(psidot)
            psidot = 0;
         end
         ok = 1;
      elseif answ == 'r'
         R = input('Give desired turn radius (>0) [m], default = straight & level: ');
         if isempty(R) ~= 1
            psidot = V/R;
         else
            psidot = 0;
         end
         ok = 1;
      else
         disp('   Invalid entry. Please choose either ''t'', or ''r''');
      end
   end

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

elseif opt == 3                                              % STEADY PULL-UP
                                                             % --------------
   clc
   disp('Steady pull-up.');
   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 initial heading [deg], default = 0: ')*pi/180;
   if isempty(psi)
      psi = 0;
   end

   gammatype = 'f';  % Use specified flightpath angle and numerically
                     % adjust manifold pressure
   gamma = input('Give initial flightpath angle [deg], default = 0: ')*pi/180;
   if isempty(gamma)
      gamma = 0;
   end

   phidot = 0;
   psidot = 0;

   thetadot = input('Give pull-up rate [deg/s], default = 0: ')*pi/180;
   if isempty(thetadot)
      thetadot = 0;
   end

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

elseif opt == 4                                                 % STEADY ROLL
                                                                % -----------
   clc
   disp('Steady roll.');
   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 initial heading [deg], default = 0: ')*pi/180;
   if isempty(psi)
      psi = 0;
   end

   ok = 0;
   while ok~= 1
      gammatype = input('Use specified manifold pressure or flight-path angle ([m]/f)? ','s');
      if isempty(gammatype)
         gammatype = 'm';
      end
      if gammatype == 'f'
         gamma = input('Give flightpath angle [deg], default = 0: ')*pi/180;
         if isempty(gamma)
            gamma = 0;
            ok = 1;
         end
      elseif gammatype == 'm'
         ok = 1;
      else
         disp('   Invalid entry. Please choose either ''m'', or ''f''');
      end
   end

   thetadot = 0;
   psidot = 0;

   phidot = input('Give desired roll-rate [deg/s], default = 0: ')*pi/180;
   if isempty(phidot)
      phidot = 0;
   end

   if phidot ~= 0
      ok = 0;
      while ok~= 1
         rolltype = input('Roll in body or stability axes reference frame ([b]/s)? ','s');
         if isempty(rolltype)
            rolltype = 'b';
         end
         if rolltype == 'b' | rolltype == 's'
            ok = 1;
         else
            disp('   Invalid entry. Please choose either ''b'', or ''s''');
         end
      end
   else
      rolltype = 'b';   % No rolling, so for reasons of simplicity the default setting, 
                        % i.e. a body-axes roll, will be used.
   end

else
   % Set helpvariable skip = 1, to ensure that the aircraft configuration
   % does not have to be entered if the user chooses option 5 = QUIT.
   % --------------------------------------------------------------------
   skip = 1;
end

%%%%

if skip ~= 1         % DEFINE CONFIGURATION OF THE AIRPLANE, IF NOT QUITTING
                     % -----------------------------------------------------

   % For the 'Beaver' model, the flap angle and engine speed define the
   % configuration of the aircraft
   % ---------------------------------------------------------------------
   deltaf = 0;
   % G is the centripetal acceleration, used for the coordinated turn
   % constraint.
   % ----------------------------------------------------------------
   G = psidot*V/9.80665;
   % --------------------------------------------------------------------
   phi = [];
   if opt == 2 & turntype == 'u';  % Steady turn, uncoordinated.
      phi = input('Give desired roll angle phi [deg], default = 0: ')*pi/180;
   end

   if isempty(phi)
      phi = 0;
   end
   % Note: vtrim contains the first estimation of the non-constant states
   % and inputs. The final values will be determined iteratively.
   % -----------------------------------------------------------------------
   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
   % the function call xdot = feval('beaver',t,x,u,'outputs'), 
   % -----------------------------------------------------------------------
   modfun   = ['xdot = feval(''',sysname,''',0,x,u,''outputs''); '];
   modfun   = [modfun 'xdot = feval(''',sysname,''',0,x,u,''derivs'');'];
    %--------------------------------------------------------------------
   warning off
   feval(sysname,[],[],[],'compile');
   clear ans % (the above feval statement somehow generates an 'ans' variable)
   warning on

   % Call minimization routine FMINSEARCH. 
   % ----------------------------------------------------------------------
   clc;
   disp('Searching for stable solution. Wait a moment...');
   disp(' ');
   
   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
   % ----------------------------------------------------------
   if exitflag == 0
      warning('Maximum number of iterations was exceeded!');
      disp(['- number of function evaluations: ' num2str(output.funcCount)]);
      disp(['- number of iterations: ' num2str(output.iterations)]);
   else
      disp('Converged to a trimmed solution...');
   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,rolltype,turntype,gammatype);
   
   % Call a/c model once more, to obtain the time-derivatives of the states
   % in trimmed-flight condition.
   % -----------------------------------------------------------------------
   eval(modfun);
   
   % Release compiled aircraft model now that all results are known
   % --------------------------------------------------------------
   feval(sysname,[],[],[],'term');
   
   %-------------------------------------------------------------------
   xinco  = x;          % Initial value of state vector
   xdot0  = xdot;       % Initial value of time-derivative of state vector
   uaero0 = u(1:4);     % Initial value of input vector for aerodynamic model
   uprop0 = u(5:6);     % Initial value of input vector for engine
                        %                            forces and moments model
   % ---------------------------------------------------------------------
   
   end
Power = 1.7*(uprop0(2)-16.14)*(uprop0(1)-1922);
deltaP = Power-pot_attesa;
manetta = 0.00136*Power-2.033;
MAP = uprop0(2);
vettnostro = [Power manetta MAP xinco(1) xinco(2)*180/pi uaero0(1)*180/pi deltaP];
matrnostra = [matrnostra vettnostro'];
end                % END OF TRIM-LOOP (SKIPPED IF OPTION 5 = QUIT IS SELECTED)

disp('MAP')
matrnostra(3,:)
disp('potenza calcolata')
matrnostra(1,:)
disp('equilibratore')
matrnostra(6,:)


% Clear unneccessary variables
% ----------------------------
clear h ctrim vtrim vtrimmed modfun output options exitflag fval
clear opt V H psi gamma phidot psidot thetadot rolltype turntype gammatype
clear deltaf n pz Hdot G phi sysname x xdot u exitflag t t1 t2 t3 ii linestr
clear ok answ dirname filename savecmmnd defdir currentdir skip

disp(' ');
disp('Ready.');
disp(' ');



