%--------------------------------------------------------------------------
%
% PURPOSE: Compute constants for the Altex Dynamics Model
% AUTHOR:  Rob McEwen
% DATE:    99/11/15
% REFS:    MIT Odyssey Sim; Franz Hover's reports
%
%--------------------------------------------------------------------------
% When in Way Point Mode, set the variable "Heading" = bearing. 
  if (WpMode==0)
    Heading=atan2(r_No_WP1_N_y, r_No_WP1_N_x);  % Bearing
  end 
%
% Computed Constants: Tailcone Actuators.
  disp0 = sqrt( (rR1 - xRR)^2 + (rR2 - aR)^2 ); %Act length at deltaR=0
  bAct  = sqrt(rR1^2 + rR2^2);   % radius of circle generated by pivot pt.
  d1Act = sqrt( aR^2 + xRR^2);   % Dist as shown on Act. notes, p. 21
  delta0= acos( (d1Act^2 + bAct^2 - disp0^2)/(2*d1Act*bAct) ); %See p.21
  ampsAct = .5/in2m*c2m*maxActRate+.9; %.5 is amps/(in/sec) from GT's diagram;
                                       %.9 is the y intercept.
%
% Open loop rudder profile
  tvals = [0, tdeltaR, tdeltaR+tdur, 2*tdeltaR+tdur, tStop];
  ovals = [0, deltaR,  deltaR,       0,              0 ];
% ovals = [0, 0,       0,            0,              0 ];
%
% Computed Constants: Vehicle Dynamics.
%
  weight = mass*g;          % Weight, Newtons.
  buoy = weight + deltaBuoy;% Buoyancy, Newtons.
  xR  = -len*.45;           % Distance from CG to rudder; m
  xP  = xR - xPcp;          % Distance from CG to pivot point.
  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
  Iyy = Izz;
  Ixy = 0.;
  Iyz = 0.;
  Izx = 0.;
  Xrv   = .003358*rho/2*len^3;  % Deriv of X wrto rv; kg; DSRV
  Xrr   = -1.81e-4*rho/2*len^4; % Deriv of X wrto rr; N-sec^2; DSRV
  Xvv   = .0098*rho/2*len^2;    % Deriv of X wrto vv; kg/m; ODYSSEY
  Xqq   = Xrr;
  Xww   = Xvv;
  Xwq   = -Xrv;
  Xuu   = -5.03e-3*rho/2*len^2; % Deriv of X wrto uu; kg/m; DSRV
  Xuu   = -.5*rho*pi*(dia/2)^2*.15;  % Knut's computation from Brooks et. al.
  Xuu   = .5*rho*TotalArea*Cd0 - Tp/vel^2;  % See Notes, Eqn. 101.
  Yr  = .064*mass;          % Deriv of Y wrto r; kg (*m/sec^2=N), L&J
  Yv  = -.5*mass/len;       % Deriv of Y wrto v; kg/m   (*m^2/sec^2=N), L&J
  Ywp = 313.6 ;             %???????? Odyssey #; Fix this - Knut
  Yrabsr = 31.5;            %???????? Odyssey #; Fix this - Knut
  Yvabsr = 0;               % Knut
  Yvabsv = -2.94*mass/len;  % Deriv of Y wrto v|v|; kg/m; Knut
  Zq     = -Yr;
  Zw     =  Yv;
  Zvp    = -Ywp;
  Zqabsq = -Yrabsr;
  Zwabsw =  Yvabsv;         %???????? Why *17 in Ody?
  Kpabsp = -300.;           % Roll drag, kg-m^2
  Mq  = -.045*mass*len;     % Deriv of N wrto r; kg-m (*m/sec^2=Nm), L&J
  Mw  =  .795*mass;         % Deriv of N wrto v; kg (*m^2/sec^2=Nm), L&J
  Mpr =  50.7;              %???????? Odyssey #; Fix this - Knut
  Mwabsw = -.344*mass;      % Deriv of N wrto v|v|; kg; Knut
  Mqabsq = -.0705*mass*len^2;% Deriv of N wrto r|r|; kg-m^2; Knut
  Nr     =  Mq;
  Nv     = -Mw;
  Npq    = -Mpr;
  Nrabsv = 0;               % Knut
  Nvabsv = -Mwabsw;
  Nrabsr =  Mqabsq;
%
% Added mass terms:
%
  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
  Xudot = -.1*mass;         % Deriv of X wrto u-dot (Added Mass); kg; WAG
  Xudot = -1.583e-3*rho/2*len^3;% Deriv of X wrto u-dot; kg; DSRV
  Nvdot = 0;                % Deriv of N wrto v-dot; (Coupled) kg-m; WAG
  Mqdot = 0;
  Nrdot = 0;
  Kpdot = 0;
  Kvdot = 0;
%
  Zqdot = -Nvdot;
  Zwdot = Yvdot;
  Yrdot = Nvdot;            % Deriv of Y wrto r-dot; (Coupled) kg-m; WAG
  Mwdot = -Nvdot;
%
  Ypdot = 0.;               % Deriv of Y wrto p-dot; (Added Mass) kg; WAG
%
% Build the generalized mass matrix:
%
%      1           2          3          4                 5           6
  GenMass = ...
[mass-Xudot        0.         0.         0.             mass*zG     -mass*yG;
    0.        mass-Yvdot      0.   -mass*zG-Ypdot          0.     mass*xG-Yrdot;
    0.             0.     mass-Zwdot  mass*yG       -mass*xG-Zqdot     0.;
    0.      -mass*zG-Kvdot mass*yG     Ixx-Kpdot        -Ixy         -Izx;
 mass*zG           0.   -mass*xG-Mwdot   -Ixy          Iyy-Mqdot     -Iyz;
 -mass*yG   mass*xG-Nvdot     0.         -Izx           -Iyz       Izz-Nrdot   ];
%
 GenMassInv = inv(GenMass);
%
% Set up time vector:
  Npts  = tStop/Ts;         % Number of points
  tsim = [0:Npts]'*tStop/Npts;
