function AVDS_FCS_Matlab()
%  AVDS_FCS.m - This is a simple example of the AVDS to MATLAB shared memory connection.
%  The aircraft model is executed inside of AVDS. The flight control system (FCS) model
%  is implemented in MATLAB. Inputs to the FCS are sent through shared memory from AVDS to MATLAB,
%  Outputs from the FCS are sent from MATLAB through shared memory to AVDS.
%
%  To start the simulation:
%  1) Start AVDS
%  2) Load the simulation initialization file "MATLAB_FCS.sim.ini". From the AVDS 
%     menu select "File->Simulation Init->Open" and choose the file "MATLAB_FCS.sim.ini".
%  3) In MATLAB run the AVDS_FCS.m script
%  4) In AVDS, choose Simulation Mode and press the "Start" button.
%
%  To stop the simulation:
%  1) Press the AVDS "Stop" button.
%
%	see also: SimulationConnection()
%


%   Steve Rasmussen, October 2000
%   Copyright (c) 2000 by RasSimTech Ltd
%   $Revision: 1.0 $  $Date: 2000/10/31 00:00:00 $
%   $Revision: 1.1 $  $Date: 2000/11/26 00:00:00 $
%   $Revision: 1.2 $  $Date: 2001/04/09 00:00:00 $


% Define the indices for the input vector:
InbRunning = 1; 
InStickPitch = 2;
InStickRoll = 3;
InStickYaw = 4;
InStickThrottle = 5;
InP = 6;
InQ = 7;
InR = 8;
InElapsedTime = 9;
% The following indicies in the input vector have not been used by this Aircraft, 
%   and can be used to receive other signals from AVDS:
ToMatlab01 = 10; ToMatlab02 = 11; ToMatlab03 = 12; ToMatlab04 = 13; ToMatlab05 = 14;
ToMatlab06 = 15; ToMatlab07 = 16; ToMatlab08 = 17; ToMatlab09 = 18; ToMatlab10 = 19;
% Total size of input (from AVDS) vector, must be the same size as the defintion found in the 
%   SimulationConnection function for the FCS type:
InputVector = zeros(19,1);	%intitialize input vector - the contents of this vector will be received from AVDS


% Define the indices for the output vector:
OutElevatorCmd = 1;
OutAileronCmd = 2;
RudderCmd = 3;
ThrottleCmd = 4;
% The following indicies in the output vector have not been used by this FCS, 
%   and can be used to send other signals to AVDS:
ToAVDS01 = 5; ToAVDS02 = 6; ToAVDS03 = 7; ToAVDS04 = 8; ToAVDS05 = 9;
ToAVDS06 = 10; ToAVDS07 = 11; ToAVDS08 = 12; ToAVDS09 = 13; ToAVDS10 = 14;
% Total size of output (to AVDS) vector, must be the same size as the defintion found 
%   in the SimulationConnection function.
OutputVector = zeros(14,1);	%intitialize output vector - the contents of this vector will be sent to AVDS


PitchGain = 1.0;
RollGain = 1.0;
YawGain = 1.0;

% Delta time for model updates. 
% -  Using a simple rectangular integration
TimeIncrement = 0.05;

MatlabSimulationTime = 0.0;	% local function's simulation time
WasRunning = 0;	% flag to prevent exit before simulation has ran once.
                      

%Open the AVDS shared memory link
SimulationConnection('OpenConnection',1,'FCS');

while 1	% loop until the break is encountered
   InputVector = SimulationConnection('ReceiveData',1);
   if ((InputVector(1) == 0) & (WasRunning == 1)),
      break;
   end;	%if ((InputVector(1) == 0( & (WasRunning == 1))
   if (InputVector(1) == 1),
      WasRunning = 1;
      %Retrive the current simulation time from AVDS.
      AVDSSimulationTime = InputVector(InElapsedTime);
      if (MatlabSimulationTime < AVDSSimulationTime), %NOTE: this can be <= 
         % increment Matlab simulation time
         MatlabSimulationTime = MatlabSimulationTime + TimeIncrement;
         
         %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
         % !!!! Prevent the Matlab simulation from getting behind:
         % NOTE: this may not be desirable, but if Matlab is running too slow it is necessary
         DeltaTime = TimeIncrement;
         while(MatlabSimulationTime < AVDSSimulationTime), %NOTE: this can be <=
            MatlabSimulationTime = MatlabSimulationTime + TimeIncrement;
            DeltaTime = DeltaTime + TimeIncrement;
         end;
          %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
          
			%update FCS
			OutputVector(OutElevatorCmd) = PitchGain * (InputVector(InStickPitch) + InputVector(InQ));
			OutputVector(OutAileronCmd) = RollGain * (InputVector(InStickRoll) + InputVector(InP));
   		OutputVector(RudderCmd) = YawGain * (InputVector(InStickYaw) - InputVector(InR));
   		OutputVector(ThrottleCmd) = 0.45;
			SimulationConnection('SendData',1,OutputVector);
		end;	%if (MatlabSimulationTime > AVDSSimulationTime),
   end;	%if (InputVector(1) == 0),
end;
SimulationConnection('CloseConnection');

return;	%AVDS_FCS_Matlab
