%--------------------------------------------------------------------------
%
% PURPOSE: Simulate the steering dynamics of Altex for small angles,
%          in the horizontal plane.
% AUTHOR:  Rob McEwen
% DATE:    99/2/12
%
%--------------------------------------------------------------------------
%
% Assume that: 1) The vehicle is in straight and level flight, so this
%                 analysis is in the horizontal plane.
%              2) All angles remain less than 6 degrees.
%              3) The ring and struts have the lift and drag of 4 
%                 Odyssey fins.
%
% NOTES: 1) The effect of the tailcone body deflection on the hydrodynamic 
%           derivatives is not modeled here.
%        2) The signs of Nv and Yr are flipped from Knut's eqns due to
%           a suspected reference frame flip in Landweber et. al.
%
% Units and Fundamental Constants:
  in2m = .0254;             % Inches to meters.
  d2r  = pi/180;            % Degrees to radians
  r2d  = 180/pi;            % Radians to degrees
  rho  = 1028;              % Density of sea water at 0 Deg C; kg/m^3
  s2kg = 14.59390;          % Slugs to kg
  ft2m = .3048;             % Feet to meters
  sft2kgm2 = 1.355818;      % Slug-ft^2 to kg-m^2
  lbf2n= 4.448;             % Pounds force to Newtons
%
% Altex/Dorado Specific Constants:
  vel = 1852*2.5/3600;      % speed in m/s
  Tp  = 52;                 % Thrust in Newtons
  mass= 795;                % Mass; kg
  len = 4.19;               % Length; meters
  dia = 21*in2m;            % Diameter; meters
  xG  = 0;                  % Longitudinal CG offset, meters
  xP  = -15*in2m;           % Distance from pivot to ring CP in tailcone coords
%
% Altex/Dorado constants taken from the Odyssey sim:
% NOTE: Assume that for Yaw/Pitch control purposes, the ring has 
% the properties of an Odyssey fin, with 4 times the surface area.  For 
% the surge equation, use the actual (approximate, of course) area.
% 
  StrutArea  = 0.0299;      % Odyssey fin area; m^2
  ControlArea = StrutArea*4; % Equivalent rudder/elevator area (m^2). 
%
  RingArea = 24*in2m*pi*10*in2m + StrutArea*4;
                         % Ring + 4 struts;m^2. For drag calc in the Surge Eqn.
  Ar  = 4.26;            % Aspect ratio
  Cdc = 0.723;           % Crossflow Cd
  Cd0 = 0.0065;          % Cd from NACA 0015 section; PNA p. 503
  Ef  = 0.90;            % "Oswald" efficiency factor; PNA p. 503
  dCl = 3.392;           % Lift coefficient slope 
  Xu  = 10.0;            % Body drag; N/(m/s) at "vel" m/s
%
% Computed Constants:
  xR  = -len*.45;           % Distance from CG to rudder; m
  Izz = mass/5*((len/2)^2+(dia/2)^2); %Ellipsoid; Greenwood p501. kg-m^2
  Ixx = 2*mass/5*(dia/2)^2; % Greenwood p501. Assume homogenous dist. kg-m^2
  Nr  = -.045*mass*len*vel; % Deriv of N wrto r; kg-m^2/sec (*1/sec=Nm), L&J
  Nv  = -.795*mass*vel;     % Deriv of N wrto v; kg-m/sec (*m/sec=Nm), L&J
  Yr  = -(.036-.1)*mass*vel;% Deriv of Y wrto r; kg-m/sec (*1/sec=N), L&J
  Yv  = -.415*mass/len*vel; % Deriv of Y wrto v; kg/sec   (*m/sec=N), L&J
  Xudot = -.1*mass;         % Deriv of X wrto u-dot (Added Mass); kg; WAG
  Yvdot = -.8*mass;         % Deriv of Y wrto v-dot; (Added Mass) kg; WAG
  Nrdot = -.8*Izz;          % Deriv of N wrto r-dot; (Added Iner) kg-m^2; WAG
  Yrdot = 0;                % Deriv of Y wrto r-dot; (Coupled) kg-m; WAG
  Nvdot = 0;                % Deriv of N wrto v-dot; (Coupled) kg-m; WAG
%
% Nonlinear terms:
  Yrabsv = 0;               % Knut
  Yvabsv = -2.94*mass/len;  % Deriv of Y wrto v|v|; kg/m; Knut
  Nrabsv = 0;               % Knut
  Nvabsv = .344*mass;       % Deriv of N wrto v|v|; kg; Knut
  Nrabsr = -.0705*mass*len^2;% Deriv of N wrto r|r|; kg-m^2; Knut
%
% Sim Parameters:
  tf   = 40;                % Stop time, sec
  Npts = 500;               % Number of points;
  deltaR = 5*d2r;           % Rudder angle, rad
%
%----------START  ODYSSEY---------------
%
% Lets try it with Odyssey numbers:
  if 0
    disp(' Using Odyssey Parameters:')
%   vel = 1852*3.0/3600;      % speed in m/s ????
    mass= 339;                % Mass; kg
    len = 4.19/2;             % Length; meters  ????
    ControlArea = StrutArea*2; % Equivalent rudder/elevator area (m^2). 
    xR  = -1.069;             % Distance from CG to rudder; m
    Izz = 71.95;              % kg-m^2
    Ixx = 10.59;              % kg-m^2
    Nr  = -74.9;              % Deriv of N wrto r; kg-m^2/sec (*1/sec=Nm), L&J
    Nv  = -167.3;             % kg-m/sec
    Yr  = 112.6;              % kg-m/sec
    Yv  = -117.5;             % kg/sec
    Yvdot = -313.6;           % kg
    Nrdot = -50.7;            % kg-m^2
  end
%
%----------END ODYSSEY---------------
%----------START  DSRV---------------
%
% Lets try it with DSRV numbers.  These are all from pp. 36,37 & 46.
  if 0
    disp(' Using DSRV Parameters:')
    tf  = 100;                % Sim stop time, sec
%   vel = 1852*3.0/3600;      % speed in m/s
    mass= 4363*s2kg;          % Mass; kg
    len = 49.33*ft2m;         % Length; meters 
    ControlArea = 8*1.83*(ft2m)^2; % Equivalent rudder/elevator area (m^2). 
    xR  = -(46.89-23.4)*ft2m;% Distance from CG to rudder; m
    Izz = 475868*sft2kgm2;    % kg-m^2
    Ixx = 38244*sft2kgm2;     % kg-m^2
    Nr  = -49200*sft2kgm2;    % Deriv of N wrto r; kg-m^2/sec (*1/sec=Nm)
    Nv  = -3050*s2kg*ft2m;    % kg-m/sec
    Yr  = 792*s2kg*ft2m;      % kg-m/sec
    Yv  = -136.2*s2kg;        % kg/sec
    Yvdot = -4030*s2kg;       % kg
    Nrdot = -351000*sft2kgm2; % kg-m^2
    Cd0 = 0.0065;             % Cd from NACA 0015 section; PNA p. 503
    dCl = 3.392;              % Lift coefficient slope 
    Xu  = -12.34*lbf2n;       % Body drag; N/(m/s) at "vel" m/s
  end
%
%----------END DSRV---------------
%
% Set up time vector:
  tsim = [0:Npts]'*tf/Npts;
%
% Form the mass matrices:
  MSway  = [  mass-Yvdot,     mass*xG-Yrdot;
              mass*xG-Nvdot,        Izz-Nrdot];
  MSurge =    mass-Xudot;
%
% Form the AA matrices:
  YrudFixed = .5*rho*ControlArea*vel*(Cd0+dCl);
  AASway  = [  Yv-YrudFixed,          Yr-mass*vel-xR*YrudFixed;
               Nv-xR*YrudFixed,   Nr-mass*xG*vel-xR^2*YrudFixed];
%
% AASwayIdeal has no rudder, just a pure turning moment
  AASwayIdeal = [  Yv,      Yr-mass*vel;
                   Nv,   Nr-mass*xG*vel];
  AASurge = Xu-rho*RingArea*Cd0*vel;
%
% Form the control matrices.  The first term in Ydelta is from the rudder,
% and the second is the contribution from the vectored thrust.
  Ydelta = .5*rho*ControlArea*vel^2*dCl + Tp;
  Ydelta = .5*rho*ControlArea*vel^2*dCl;
  BBSway  = [    Ydelta;
              xR*Ydelta];
  BBSurge = Tp - .5*rho*RingArea*vel^2*Cd0 - Xu*vel;
%
% The ideal system has no rudder, just a pure turning moment of xR*Yideal Nm.
  Yideal  = -75;
  BBSwayIdeal  = [  Yideal;
                  xR*Yideal];
%
% If Ideal System;
%  AASway = AASwayIdeal;
%  BBSway = BBSwayIdeal;
%
% First, compute the steady-state analysis.  Accelerations are zero.
  ySwaySS  = -AASway\BBSway*deltaR;
  betaCGSS = ySwaySS(1,1)/vel;
  Rturn    = abs(vel/ySwaySS(2,1));
  Ncontrol = xR*Ydelta*deltaR;
  NcontrolP= xP*Ydelta*deltaR;
  NrudderSS= xR*(Ydelta*deltaR - YrudFixed*(ySwaySS(1,1)+ySwaySS(2,1)*xR));
  NpivotSS = xP*(Ydelta*deltaR - YrudFixed*(ySwaySS(1,1)+ySwaySS(2,1)*xR));
  !rm run.txt
  diary('run.txt')
  disp(' ')
  disp(' Steady-State Values:')
  disp([' The rudder angle is ', num2str(deltaR*r2d), ' Deg.'])
  disp([' The CG drift angle is ', num2str(betaCGSS*r2d),' Deg.'])
  disp([' The turn rate is ', num2str(ySwaySS(2,1)*r2d),' Deg/Sec'])
  disp([' The turn radius is ', num2str(Rturn),' Meters',...
        ' or ', num2str(Rturn/len), ' vehicle lengths.'])
  disp([' The control-torque component about the CG is ',...
        num2str(Ncontrol),' N-m'])
  disp([' The control-torque component about the pivot point is ',...
        num2str(NcontrolP),' N-m'])
  disp([' The total steady-state torque about the CG is ',...
        num2str(NrudderSS),' N-m'])
  disp([' The total steady-state torque about the pivot point is ',...
        num2str(NpivotSS),' N-m'])
%
% Form state-space representation:
  ASway = MSway\AASway;
  BSway = MSway\BBSway;
  CSway = eye(2);
  DSway = [0;0];
  SwaySys  = ss(ASway, BSway, CSway, DSway);
  SurgeSys = ss(MSurge\AASurge,  MSurge\BBSurge, 1, 0);
%
% -------------Simulink--------------
%
% Build parameter vector:
  MSwayInv = inv(MSway);
  CDynVec = [ MSwayInv(1,:)';
              MSwayInv(2,:)';
              xR;
              dCl;             % Coeff of lift; linear coeff
              Cdc/Ar;          % Coeff of lift; second order coeff
              Cd0;             % Coeff of drag; constant term
              1/pi/Ar/Ef;      % Coeff of drag; Cl^2 coeff
              .5*rho*vel^2*ControlArea; % Coeff for L and D eqns
              Yv;
              Yr;
              Yrabsv;
              Yvabsv;
              xG;
              Nv;
              Nr;
              Nrabsv;
              Nvabsv;
              Nrabsr;
            ]';
return
%
% Run it!
  [tout] = sim('AltexSim',tsim);
%
% -------------End Simulink--------------
%
%
% Check the eigenvalues:
  disp(' The eigenvalues are: ')
  evals = eig(ssdata(SwaySys))
  disp(' The natural frequency, damping ratio, and time constant are:')
  disp ' f_n , damping ratio, time constant '
  [abs(evals)/2/pi -real(evals)./abs(evals) -1./real(evals)]
  diary off    
%
% Simulate a turn;
  [ySway, t] = step(SwaySys,tsim);
%
% Scale for rudder deflection:
  ySway = ySway*deltaR;
%
  figure(1)
  clf
  subplot(2,1,1)
  plot(t, [ySway(:,1) yout(:,1)]);
  ylabel('Sway Rate')
  title('Sway Rate, Meters per Second');
  grid on
  subplot(2,1,2)
  plot(t, [ySway(:,2) yout(:,2)]*r2d);
  ylabel('Yaw Rate, Degrees/Second')
  title('Yaw Rate, Degrees/Second');
  xlabel('Seconds');
  grid on
%
% Compute drift angles at CG and stern.  See notes, Eqn.'s 8 & 9.
  figure(2)
  clf
  beta  = ySway(:,1)/vel;
  betaR = (ySway(:,1)+xR*ySway(:,2))/vel;
  AofA  = (deltaR-betaR);
  beta_NL  = yout(:,1)/vel;
  betaR_NL = (yout(:,1)+xR*yout(:,2))/vel;
  AofA_NL  = (deltaR-betaR);
  plot(t, [beta beta_NL betaR betaR_NL AofA AofA_NL]*r2d)
  title('Drift Angle at CG, Drift Angle at Rudder, Angle of Attack')
  ylabel('Degrees')
  xlabel('Seconds');
  legend('CG Drift', 'Stern Drift', 'Rudder Attack',0)
  grid on
%
% Compute turning moment due to rudder.  See notes, Eqn. 32.  Also
% compute the torque about the tailcone pivot point.
  figure(3)
  clf
  Nrudder  = xR*(Ydelta*deltaR - YrudFixed*(ySway(:,1)+ySway(:,2)*xR));
  Npivot   = xP*(Ydelta*deltaR - YrudFixed*(ySway(:,1)+ySway(:,2)*xR));
  plot(t, [Nrudder Npivot]);
  title('Turning Moments about Vehicle CG and Tailcone Pivot Point')
  ylabel('Newton-Meters')
  xlabel('Seconds');
  legend('Vehicle CG','Pivot Point',0)
  grid on
%
% if hardflag==1,
    for i = 1:3
      eval(['print -f',num2str(i),'  -depsc simfig',num2str(i)])
      clear hardflag
    end
     
