%--------------------------------------------------------------------------
%
% 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.
%
  clear all
%
% Flags:
%
  if exist('hardflag')==0, hardflag=0; end
%
% 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
  kph2mps  = 1852/3600;     % Knots per hour to meters per sec
  ftlbf2nm = 1.3558179;     % Foot-pounds force to Newton-meters.
  oi2nm    = .0070615516;   % Ounce-inches to Newton-meters.
  m2c= 200*12/in2m;         % Meters-to-counts
  c2m= 1/m2c;               % Counts-to-meters
  g  = 9.806;               % Free-fall acceleration due to gravity; m/sec^2.
%
  caseName = '-1Deg';       % Plot name suffix
  caseName = 'ClosedLoop';  % Plot name suffix
%
% Start screen log:
  eval(['!rm run', caseName,'.txt'])
  diary(['run',caseName,'.txt'])
%
% Read in all parameters:
%
  setupAltex
%
% Run the incremental setup file here, if there is one.

% setHeading180
% setOpenLoop
  LinFlg = 0;    %Set it to 1 in setLinear.m
  setLinear
%
% Computed constants:
%
  compute
%
%
% Print parameters to the screen and diary file.
  disp(' ')
  document
%
% Linear Analysis:
% LinFlg now removed from linear.m.  Just execute setupAltex.m and compute.m
% first to setup the stack.
%
  linear
%
% -------------Simulink--------------
%
  disp(' ')
  disp(' -------------Simulation of Nonlinear EOM:-------------')
  disp(' ')
%
% Build parameter vector:
  MSwayInv = inv(MSway);
%
% DEBUG: Zero out the nonlinear terms:
%  Cdc = 0;
%  Yvabsv = 0;
%  Nvabsv = 0;
%  Nrabsr = 0;
%
  CDynVec = [ GenMassInv(1,:)';
              GenMassInv(2,:)';
              GenMassInv(3,:)';
              GenMassInv(4,:)';
              GenMassInv(5,:)';
              GenMassInv(6,:)';
              mass;
	      Ixx;
	      Iyy;
	      Izz;
	      Ixy;
	      Iyz;
	      Izx;        
              weight;
              xG;
              yG;
              zG;
              buoy;
              xB;
              yB;
              zB;
              MSurge;
              vel;
              Xrv;
              Xrr;
              Xuu;
              Xvv;
              Xqq;
              Xwq;
              Xww;
              Yv;
              Yr;
              Ywp;
              Yrabsr;
              Yvabsr;
              Yvabsv;
              Zq;
              Zw;
              Zvp;
              Zqabsq;
              Zwabsw;
              Kpabsp;
              Mq;
              Mw;
              Mpr;
              Mqabsq;
              Mwabsw;
              Nv;
              Nr;
              Npq;
              Nrabsv;
              Nvabsv;
              Nrabsr;
              Decouple;
              xR;          %Dist from the CM to Ro, deltaR=0.
              xP;          %Dist from the CM to Eo, first pivot pt.
              xPR;         %Dist from Eo to Ro, about 1 inch
              r_Ro_S1_R;   %BEWARE These each are VECTORS of dim 3
              r_Ro_S2_R;   %BEWARE
              r_Ro_S3_R;   %BEWARE
              r_Ro_S4_R;   %BEWARE
              r_Ro_S5_R;   %BEWARE These each are VECTORS of dim 3
              r_Ro_S6_R;   %BEWARE
              r_Ro_S7_R;   %BEWARE
              r_Ro_S8_R;   %BEWARE
              aR;
              rR1;
              rR2;
              xRR;
              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*ControlArea;% Coeff for L and D eqns    
              dClR;            % Coeff of lift; linear coeff
              CdcR/ArR;        % Coeff of lift; second order coeff
              Cd0R;            % Coeff of drag; constant term
              1/pi/ArR/EfR;    % Coeff of drag; Cl^2 coeff
              .5*rho*RingArea; % Coeff for L and D eqns
              Stall;
              u0;
              v0;
              w0;
              p0;
              q0;
              r0;
              x0;
              y0;
              z0;
              phi0;
              theta0;
              psi0;
            ]';
%
% Run it!
%
  [tout] = sim('Altex',tsim,simset('maxstep',Ts));
%
% -------------End Simulink--------------
%
%
% Indirect addressing for the columns of yout.
  lbls = { 'vIx'
           'rIx'
           'betaIx'
           'betaRIx'
           'AofAIx'
           'TrqNRIx'
           'TrqPIx'
           'xPosIx'
           'yPosIx'
           'yawIx'
           'FYRIx'
           'FYTIx'
           'FRAIx'
           'deltaRIx'
           'uIx'
           'wIx'
           'pIx'
           'qIx'
           'zPosIx'
           'rollIx'
           'pitchIx'
           'deltaEIx'
           'alphaRudIx'
           'alphaElvIx'                %END VEHDYN.C
           'rAmpsIx'
           'rAmpSecIx'
           'rActCntsIx'
           'rActDotIx'
           'rCmdIx'
           'pitchCmdIx'
           'KppErrorIx'
           'KdppIx'
           'headingCmdIx'
         };
if 0
  lbls = { 'uIx'
           'vIx'
           'rIx'
           'xPosIx'
           'yPosIx'
           'yawIx'
           'betaIx'
           'betaRIx'
           'AofAIx'
           'TrqNRIx'
           'TrqPIx'
           'FYRIx'
           'FYTIx'
           'FRAIx'
           'deltaRIx'
           'rAmpsIx'
           'rAmpSecIx'
           'rActCntsIx'
           'rActDotIx'
           'rCmdIx'
         };
end
  sz = size(lbls);
  i = 1;
  while( i <= sz(1) ),
    eval([ char(lbls(i))  ' = '  num2str(i) ';']);
    i = i + 1;
  end
%
%
% ------------ Nonlinear Steady-State Analysis --------------
%
% Now compute some steady-state values from the nonlinear steering
% equations from Knut's derivation.  See my notes, p. 12, 99/3/2.
% I've assumed that rSS < 0, so this won't work for positive values 
% without a tweak.
%
% WARNING: This will match the sim SS values ONLY IF Tp = 0.
%
% Also, the deltaR from above does not apply.  The deltaR here is different.
%
  rSS = yout(Npts+1,rIx);
  Ap = (xR*Yvabsv - Nvabsv)/mass/vel^2;
  Bp = (xR*Yv - Nv)/mass/vel;
  Cp = ((xR*Yr - Nr)/mass - xR)*rSS/vel + Nrabsr/mass*(rSS/vel)^2;
  vSS = (-Bp + sqrt(Bp^2-4*Ap*Cp))/2/Ap;  %Positive root of a quadratic
  FYRSS = mass*vel*rSS - Yr*vel*rSS - Yv*vel*vSS - Yvabsv*vSS^2;
  turnDia = 0;
%  turnDia = 2*vel/abs(rSS);
%
% Compute uSS, assuming that the Surge Eqn is uncoupled from the 2DOF.  That
% is, all of the U's (=uSS = vel) in the 2DOF are constant.
%
  vSS1 = yout(Npts+1,vIx);
  uSS = sqrt(-(mass*vSS1*rSS + Xvv*vSS1^2 + Xrv*vSS1*rSS + Xrr*rSS^2 + Tp)/...
              (Xuu - .5*rho*ControlArea*Cd0/pi/Ar/Ef));
  disp(' ------- Nonlinear Steady-State Analysis: ----------')
  disp(' ')
  disp(sprintf(' %-15f: Computed vSS, Meters/Sec', vSS ));
  disp(sprintf(' %-15f: Simulated vSS, Meters/Sec', yout(Npts+1,vIx)));
  disp(sprintf(...
       ' %-15f: Tp. MUST BE ZERO, or else this analysis does not apply.', Tp));
  disp(sprintf(' %-15f: Yaw rate, Deg/Sec.', rSS/d2r));
  disp(sprintf(' %-15f: Turn Diameter, Meters.', turnDia));
  disp(sprintf(' %-15f: Beta, Deg/Sec.', asin(vSS/vel)/d2r));
  disp(sprintf(' %-15f: BetaR, Deg/Sec.', asin((vSS + xR*rSS)/vel)/d2r));
  disp(sprintf(' %-15f: Rudder force resolved to body y axis, Newtons.',FYRSS));
  disp(sprintf(' %-15f: Simulated Rudder force, Newtons.',yout(Npts+1,FYRIx)));
  disp(sprintf(' %-15f %-15f: uSS [Sim Comp], m/sec.',yout(Npts+1,uIx),uSS));
%
  diary off    
  clear functions mex

  plotAltex

%
% Set LinFlg back to zero so that if the user runs "linear" it won't take
% values off the stack.
  LinFlg = 0;

  return



















%
%----------START  ODYSSEY---------------
%
% Lets try it with Odyssey numbers:
  if VehFlg == 1,
    disp(' Using Odyssey Parameters:')
    tStop   = 20;                % Stop time, sec
%   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
    Yvabsr = 0;               
    Yvabsv = -4930;           % Deriv of Y wrto v|v|; kg/m; sim.c
    Nrabsv = 0;               
    Nvabsv = 0;               % Deriv of N wrto v|v|; kg; sim.c
    Nrabsr = -150;            % Deriv of N wrto r|r|; kg-m^2; sim.c
  end
%
%----------END ODYSSEY---------------
%----------START  DSRV---------------
%
% Lets try it with DSRV numbers.  These are all from pp. 36,37 & 46.
  if VehFlg == 2
    disp(' Using DSRV Parameters:')
    tStop  = 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
    Yvabsr = 0;               % 
    Yvabsv = -2.38;           % Deriv of Y wrto v|v|; kg/m;
    Nrabsv = 0;               % 
    Nvabsv = 0;               % Deriv of N wrto v|v|; kg;
    Nrabsr = 0;               % Deriv of N wrto r|r|; kg-m^2;
    Cd0 = 0.05;               % 
    dCl = 1.35/(15*pi/180);   % Lift coefficient slope 
    Cdc =    0;               % Crossflow Cd
    Ef  = 0.90;               % "Oswald" efficiency factor; PNA p. 503
    Ar  = (20*d2r*dCl)^2/.35/pi/Ef;   % Aspect ratio
  end
%
%----------END DSRV---------------


%
%  This was an attempt to compute AofASS.  But, I later realized that
%  it is necessary to find the steady-state Drag since AofASS depends on it.
%  So, its a little more complicated than it first seemed.
%
%  Solve for a deltaR, given rCmd = rSS.
%  First,Find the magnitude of the velocity wrto N at the rudder.
   magVelR2 = vel^2 + (vSS + xR*rCmd)^2;
   magVelR  = sqrt(magVelR2);
   betaRSS = asin( (vSS + xR*rCmd)/magVelR );  % Drift angle at the rudder */
   halfRhoS = .5*rho*ControlArea;
%
%  Assume that AofASS is < 0; so take the negative root.   
   rts = roots([Cdc/Ar -dCl FYRSS/halfRhoS/magVelR2]);
   if rts(1) < 0 
     AofASS = rts(1);
   else
     AofASS = rts(2);
   end
   deltaR = AofASS + betaRSS


