apmode.m

matlab的FDC工具箱

% APMODE - specify autopilot modes and reference signals
%
% The block "Mode Controller" within the auto-pilot simulation model 
% uses data defined by APMODE to define the initial autopilot mode,
% and to find out which switch criteria need to be monitored. The block 
% "Reference Signals" uses data from APMODE to specify the reference 
% values of theta, phi, H, Hdot, or psi. Only step-inputs at t=0 are 
% possible at this time; other types of signals require adaptation of
% the "Reference Signals" block. 
%
% Running APMODE will create the vectors ymod1S, ymod2S, ymod1A, ymod2A,
% yrefS, and yrefA in your workspace. S = symmetrical autopilot mode, 
% A = asymmetrical autopilot mode. The vectors 'ymod' contain mode-
% switches, the vectors 'yref' contain reference values. The number 1 
% denotes the initial mode, 2 is used for a possible second mode for 
% control laws that have distinct Armed and Coupled phases. 
%
% Mode-switch signals
% ===================
% ymod#S = [Symm. mode on/off; Symm. outer-loop on/off; ALH on/off;
%           ALS on/off; GS on/off]
%
%           Pitch Attitude Hold = [1 0 0 0 0] 
%           Altitude Hold       = [1 1 1 0 0] (also GS armed)
%           Altitude Select     = [1 1 0 1 0]
%           Glideslope Coupled  = [1 1 0 0 1]
%
% ymod#A = [Asymm. mode on/off; Asymm. outer-loop on/off; HH on/off;
%           NAV on/off; LOC on/off]
%
%           Roll Attitude Hold  = [1 0 0 0 0]
%           Heading Hold/Select = [1 1 1 0 0] (also LOC/NAV armed)
%           Navigation Coupled  = [1 1 0 1 0]
%           Localizer Coupled   = [1 1 0 0 1]
%
% In order to allow mode-switching, the navigation and approach modes
% have an initial phase ('Armed'), and a second phase ('Coupled'). The 
% other modes remain in their 'initial' phase (the 'second' phase is not 
% defined for those modes).
%
% ymod1S contains the setting for the initial phase of the symmetrical 
% modes (PAH, ALH, ALS, and GS Armed); ymod2S contains the setting for 
% the second phase of the symmetrical modes (GS Coupled, not defined for
% other symmetrical modes).
%
% ymod1A contains the setting for the initial phase of the asymmetrical 
% modes (RAH, HH, NAV Armed, and LOC Armed); ymod2A contains the setting 
% for the second phase of the asymmetrical modes (NAV Coupled and 
% LOC Coupled, not defined for other asymmetrical modes).
%
% Reference signals
% =================
% yrefS = [Dtheta_ref; DH_ref; DHdot_ref] contains reference values for
%          PAH, ALH, and ALS modes, respectively. Only constant reference
%          values are used here (corresponding to a step at t=0 if the
%          values are not equal to zero), so change the block "Reference
%          Signals" in the system APILOT1, APILOT2, or APILOT3 if you
%          want other inputs.
%
% yrefA = [Dphi_ref; Dpsi_ref] contains reference values for the RAH and 
%          HH modes. Here too, only constant values are allowed.


% Verify if the required model parameters have been defined. If not, 
% run APINIT first.
% ------------------------------------------------------------------
if not(exist('AM') & exist('EM') & exist('GM1') & exist('GM2') & exist('xinco'))
   apinit;
end

% Initialize asymmetrical and symmetrical mode selectors
% ------------------------------------------------------
amode = 0;
smode = 0;

% SYMMETRICAL AUTOPILOT MODE AND REFERENCE SIGNALS.
% -------------------------------------------------

% First initialize the vectors ymod1S, ymod2S, and yrefS:
ymod1S = [0 0 0 0 0]';  % Symmetrical mode-switch vector for initial mode
ymod2S = [0 0 0 0 0]';  % Symmetrical mode-switch vector for second mode after
                        %   mode-switching (used for Glideslope Coupled only)
yrefS  = [0 0 0]';      % Symmetrical reference values


clc
disp(' ');
disp('FDC toolbox: set autopilot modes and reference values');
disp('=====================================================');
disp(' ');


smode = 0;
smode  = txtmenu('Specify symmetrical autopilot mode',...
                 'Pitch Attitude Hold',...
                 'Altitude Hold',...
                 'Altitude Select',...
                 'Glideslope',...
                 'No symmetrical autopilot mode');

if smode == 1                          % Pitch Attitude Hold
                                       % -------------------
   mode_description1 = 'Pitch Attitude Hold';
   ymod1S = [1 0 0 0 0]'; % PAH
   PAHopt = txtmenu('Requested reference signal',...
                    'Step input Dtheta_ref',...
                    'Dtheta_ref = 0');
   if PAHopt == 1
      disp(' ');
      Dthetar = input('Give desired theta-step [deg] (default = 0): ');
      if isempty(Dthetar)
         Dthetar = 0;
      end
      Dthetar = Dthetar*pi/180;
   else
      Dthetar = 0;
   end
   yrefS= [Dthetar 0 0]'; % reference pitch angle: theta_ref = theta0+Dthetar
   clear Dthetar PAHopt

elseif smode == 2                      % Altitude Hold
                                       % -------------
   mode_description1 = 'Altitude Hold';
   ymod1S = [1 1 1 0 0]'; % ALH
   ALHopt = txtmenu('Requested reference signal',...
                    'Step input DH_ref',...
                    'DH_ref = 0');
   if ALHopt == 1
      disp(' ');
      DHr = input('Give desired altitude-step [m] (default = 0): ');
      if isempty(DHr)
         DHr = 0;
      end
   else
      DHr = 0;
   end
   yrefS = [0 DHr 0]'; % reference altitude: H_ref = H0 + DHr
   clear DHr ALHopt

elseif smode == 3                      % Altitude Select
                                       % ---------------
   mode_description1 = 'Altitude Select';
   ymod1S = [1 1 0 1 0]'; % ALS
   ALSopt = txtmenu('Requested reference signal',...
                    'Step input DHdot_ref',...
                    'DHdot_ref = 0');
   if ALSopt == 1
      disp(' ');
      DHdotr = input('Give desired Hdot-step [m/s] (default = 0): ');
      if isempty(DHdotr)
         DHdotr = 0;
      end
   else
      DHdotr = 0;
   end
   yrefS = [0 0 DHdotr]'; % reference rate-of-climb: Hdot_ref = Hdot0 + DHdotr
   clear DHdotr ALSopt

elseif smode == 4                      % Glideslope Capture & Hold
                                       % -------------------------
   mode_description1 = 'Glideslope';
   ymod1S = [1 1 1 0 0]'; % GS-armed (control logic equal to ALH mode)
   ymod2S = [1 1 0 0 1]'; % GS-coupled
   % Note: DHr = 0 (Armed phase), glideslope-coupled reference signal
   % comes from ILS block.
   
else
   % Keep ymod1S = [0 0 0 0 0]', and ymod2S = [0 0 0 0 0]'.
   mode_description1 = 'No symmetrical mode';
end


% ASYMMETRICAL AUTOPILOT MODE AND REFERENCE SIGNALS.
% --------------------------------------------------

% First initialize the vectors ymod1A, ymod2A, and yrefA:
ymod1A = [0 0 0 0 0]';  % Asymmetrical mode-switch vector for initial mode
ymod2A = [0 0 0 0 0]';  % Asymmetrical mode-switch vector for second mode
                        %   after mode-switching (used for LOC-coupled and
                        %   NAV-coupled only)
yrefA  = [0 0]';        % Asymmetrical reference values

clc
amode = 0;
amode = txtmenu('Specify asymmetrical autopilot mode',...
                'Roll Attitude Hold',...
                'Heading Hold/Heading Select',...
                'VOR navigation mode',...
                'Localizer',...
                'No asymmetrical autopilot mode');

if amode == 1                          % Roll Attitude Hold
                                       % ------------------
   mode_description2 = 'Roll Attitude Hold';
   ymod1A = [1 0 0 0 0]'; % RAH
   RAHopt = txtmenu('Requested reference signal',...
                    'Step input Dphi_ref',...
                    'Dphi_ref = 0');
   if RAHopt == 1
      disp(' ');
      Dphir = input('Give desired phi-step [deg] (default = 0): ');
      if isempty(Dphir)
         Dphir = 0;
      end
      Dphir = Dphir*pi/180;
   else
      Dphir = 0;
   end
   yrefA = [Dphir 0]'; % Reference roll-angle: phi_ref = phi0 + Dphir
   clear Dphir RAHopt

elseif amode == 2                      % Heading Hold / Heading Select
                                       % -----------------------------
   mode_description2 = 'Heading Hold / Heading Select';
   ymod1A = [1 1 1 0 0]'; % HH
   HHopt  = txtmenu('Requested reference signal',...
                    'Step input Dpsi_ref',...
                    'Dpsi_ref = 0');
   if HHopt == 1
      disp(' ');
      Dpsir = input('Give desired psi-step [deg] (default = 0): ');
      if isempty(Dpsir)
         Dpsir = 0;
      end
      Dpsir = Dpsir*pi/180;
   else
      Dpsir = 0;
   end
   yrefA = [0 Dpsir]'; % Reference yaw-angle = reference heading:
                       %                                psi_ref = psi0 + Dpsir
   clear Dpsir HHopt

elseif amode == 3                      % VOR Navigation mode
                                       % -------------------
   mode_description2 = 'VOR Navigation';
   ymod1A = [1 1 1 0 0]'; % NAV-armed (control-logic equal to HH mode)
   ymod2A = [1 1 0 1 0]'; % NAV-coupled
                          %
                          % Note: Dpsir = 0 (Armed phase), reference signal for 
                          % NAV-coupled comes from VOR block .

elseif amode == 4                      % Localizer mode
   mode_description2 = 'Localizer';
   ymod1A = [1 1 1 0 0]'; % LOC-armed (control-logic equal to HH mode)
   ymod2A = [1 1 0 0 1]'; % LOC-coupled
   % Note: Dpsir = 0 (Armed phase), reference signal for LOC-coupled comes
   % from ILS block.

else
   % Keep ymod1A = [0 0 0 0 0]' and ymod2A = [0 0 0 0 0]'
   mode_description2 = 'No asymmetrical mode';
end

% If ILS or VOR mode has been selected, also run the ILS and/or VOR 
% initialization routines. 
% -----------------------------------------------------------------
if smode == 4 | amode == 4
   ilsinit;
end

if amode == 3
   vorinit;
end

% Display information box with mode information
% ---------------------------------------------
clc
disp(' ');
disp('The following autopilot modes were selected:');
disp(' ');
disp(['   Symmetrical:  ' mode_description1]);
disp(' ');
disp(['   Asymmetrical: ' mode_description2]);
disp(' ');

% Clear unnecessary variables
% ---------------------------
clear smode amode mode_description1 mode_description2

%-----------------------------------------------------------------------
% The Flight Dynamics and Control Toolbox version 1.4.0. 
% (c) Copyright Marc Rauw, 1994-2005. 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. 
%-----------------------------------------------------------------------