% ILSINIT - initializes the ILS block in the autopilot model
%
% This routine transfers the fixed geometrical data and signal 
% sensitivity values for the ILS model to the ILS subsystem in the 
% current autopilot model (normally APILOT2 or APILOT3). This data
% is obtained from file. 
%
% If the glideslope mode is active, an appropriate glideslope intercept 
% distance will be computed (ensuring a glideslope capture within the 
% first 20 seconds of simulated flight). If the localizer mode is also 
% active, the user will be asked to specify a localizer intercept angle 
% (the initial heading of the airplane will subsequently be modified), 
% and the lateral offset from the extended runway centerline will also be 
% determined, such that the localizer is captured before the glideslope.
% If only the localizer mode is active, the user may freely specify the 
% distance to the threshold.
%
% NOTE: ILSINIT is called by APMODE, which defines the variables smode
% and amode. If smode = 4, the glideslope mode is active, if amode = 4
% the localizer mode is active. ILSMODE should not be used as stand-
% alone program!

% If smode and/or amode don't exist, display an error message
% -----------------------------------------------------------
if not(exist('amode') | exist('smode'))
   error('ILSINIT is not a stand-alone program! Please run APMODE instead.');
end

% Load the fixed ILS data from file
% ---------------------------------
clc;
disp(' ');
disp('<<< Press any key to load ILS data from file (e.g. ils.dat) >>>');
pause
datload('ils');

% Determine 'optimal' intercept distance from aircraft position to the glideslope 
% antenna. This intercept distance is equal to the distance to the glideslope 
% antenna that corresponds with the airplane flying on the glideslope at the initial 
% flight altitude, plus a margin corresponding with 30 seconds flight-time that 
% allows the simulation to settle while ensuring a glideslope capture within a 
% reasonable timeframe.
% ----------------------------------------------------------------------------------
Rgs         = (xinco(12)-HRW)/tan(abs(gamgs)*pi/180);
R_intercept = Rgs + 30*xinco(1);

% If localizer mode is active, ask the user to specify a localizer intercept angle and
% lateral offset from runway centerline. Positive values of the intercept angle will 
% result in a localizer intercept from the right; negative values will result in a 
% localizer intercept from the left. Only angles between zero and 90 degrees will be 
% accepted, to prevent extreme overshoots. The initial aircraft heading will be modified 
% such that it will correspond with the defined intercept angle, taking into account the 
% defined runway heading. If the glideslope mode is also active, determine the additional 
% margin that is needed to ensure localizer capture before glideslope capture. If the 
% glideslope mode is not active, let the user specify the initial distance to the 
% runway threshold. 
%----------------------------------------------------------------------------------------
if amode == 4
   disp(' ');
   
   intercept_angle = input('Localizer intercept angle [deg] (default 30 deg): ');
   if isempty(intercept_angle)
      intercept_angle = 30;
   end
   xinco(7) = mod((psiRW - intercept_angle)*pi/180,2*pi);      % initial aircraft heading
   
   dloc = input('Lateral deviation from extended RWY centerline [m] (default 0 m): ');
   if isempty(dloc)
      dloc = 0;
   end
   dloc = sign(intercept_angle)*abs(dloc);          % ensures compatible sign conventions
      
   if smode == 4
      R_intercept = R_intercept + dloc/sin(intercept_angle*pi/180);
   else
      R_intercept = input('Initial distance to runway threshold [m] (default 10000 m): ');
      if isempty(R_intercept)
         R_intercept = 10000;
      end
   end
end

% Determine the aircraft coordinates at t=0 in the runway-fixed reference frame (with 
% its origin located at the runway threshold, on the runway centerline; X_F pointing 
% in take-off and landing direction, Y_F pointing to the right, Z_F pointing downwards).
% The small lateral offset of the glideslope antenna (ygs) will be neglected, but its
% horizontal coordinate (xgs) will be accounted for.
% --------------------------------------------------------------------------------------
x_f = R_intercept - xgs;
y_f = dloc;

% Transform these values to xRW and yRW coordinates, measured in the North-oriented
% Earth-fixed reference frame.
% ---------------------------------------------------------------------------------
xRW = x_f*cos(psiRW*pi/180) - y_f*sin(psiRW*pi/180);
yRW = x_f*sin(psiRW*pi/180) + y_f*cos(psiRW*pi/180);

if smode == 4
   xRW = xRW - xgs;
   yRW = yRW - ygs;
end

if amode == 4
   yRW = yRW + dloc;
end

% Find name of current autopilot model. If the current system is not apilot2 
% or apilot3, open apilot2 as the default autopilot system. 
% --------------------------------------------------------------------------
sysname = gcs;

if not(strcmp(sysname,'apilot2')|strcmp(sysname,'apilot3'))
   open_system('apilot2');
   sysname = 'apilot2';
   disp(' ');
   disp('Using apilot2 as default system.');
   disp(' ');
   disp('<<< Press any key to continue >>>');
   pause;
end


% Determine glideslope and localizer sensitivities (these values are
% already determined by the block ILS which computes the nominal ILS 
% values, but we also need these values for recovering the values of
% the angles epsilon_gs en Gamma_loc from the glideslope and localizer
% currents that leave the ILS error blocks). 
% --------------------------------------------------------------------
if gamgs == 0
   Sgs = 1;
else
   Sgs = 625/abs(gamgs*pi/180);
end
if xloc == 0
   Sloc = 1;
else
   Sloc = 1.4*xloc;
end

% Set the ILS parameters in the autopilot system
% ----------------------------------------------
ilsparams = {num2str(psiRW); ...
             ['[' num2str(xRW) ' ' num2str(yRW) ' ' num2str(HRW) ']']; ...
             num2str(xloc); ...
             num2str(xgs); ...
             num2str(ygs); ...
             num2str(gamgs)};

set_param([sysname '/ILS/ILS'],'MaskValues', ilsparams);

gserr_params    = get_param([sysname '/ILS/GSerr'],'MaskValues');
gserr_params{4} = ilsparams{6};
set_param([sysname '/ILS/GSerr'],'MaskValues',gserr_params);

locerr_params    = get_param([sysname '/ILS/LOCerr'],'MaskValues');
locerr_params{4} = ilsparams{3};
set_param([sysname '/ILS/LOCerr'],'MaskValues',locerr_params);

get_param([sysname '/ILS/1//Sgs'],'Gain');   % Somewhat redundant, but this seems to 
                                             % be necessary to prevent 'Attempt to modify 
                                             % library block or subsystem' errors.
set_param([sysname '/ILS/1//Sgs'],'Gain',num2str(1/Sgs));

get_param([sysname '/ILS/1//Sloc'],'Gain');  % See remark for 1/Sgs block
set_param([sysname '/ILS/1//Sloc'],'Gain',num2str(1/Sloc));

% Display message about ILS initialization
% ----------------------------------------
disp(' ');
disp('ILS system initialized.');
disp(' ');

%-----------------------------------------------------------------------
% 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.
%-----------------------------------------------------------------------