src/FDM/LaRCsim/ls_accel.c

   1 /***************************************************************************
   2
   3   TITLE:        ls_Accel
   4
   5   ----------------------------------------------------------------------------
   6
   7   FUNCTION:     Sums forces and moments and calculates accelerations
   8
   9   ----------------------------------------------------------------------------
  10
  11   MODULE STATUS:        developmental
  12
  13   ----------------------------------------------------------------------------
  14
  15   GENEALOGY:    Written 920731 by Bruce Jackson.  Based upon equations
  16   given in reference [1] and a Matrix-X/System Build block
  17   diagram model of equations of motion coded by David Raney
  18   at NASA-Langley in June of 1992.
  19
  20   ----------------------------------------------------------------------------
  21
  22   DESIGNED BY:  Bruce Jackson
  23
  24   CODED BY:             Bruce Jackson
  25
  26   MAINTAINED BY:
  27
  28   ----------------------------------------------------------------------------
  29
  30   MODIFICATION HISTORY:
  31
  32   DATE          PURPOSE
  33
  34   931007    Moved calculations of auxiliary accelerations here from ls_aux.c                                                                    BY
  35             and corrected minus sign in front of A_Y_Pilot
  36             contribution from Q_body*P_body*D_X_pilot term.
  37   940111    Changed DATA to SCALAR; updated header files
  38
  39 $Header$
  40 $Log$
  41 Revision 1.1  2002/09/10 01:14:01  curt
  42 Initial revision
  43
  44 Revision 1.1.1.1  1999/06/17 18:07:33  curt
  45 Start of 0.7.x branch
  46
  47 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  48 Start of 0.6.x branch.
  49
  50 Revision 1.4  1998/08/24 20:09:26  curt
  51 Code optimization tweaks from Norman Vine.
  52
  53 Revision 1.3  1998/08/06 12:46:38  curt
  54 Header change.
  55
  56 Revision 1.2  1998/01/19 18:40:24  curt
  57 Tons of little changes to clean up the code and to remove fatal errors
  58 when building with the c++ compiler.
  59
  60 Revision 1.1  1997/05/29 00:09:53  curt
  61 Initial Flight Gear revision.
  62
  63  * Revision 1.5  1994/01/11  18:42:16  bjax
  64  * Oops! Changed data types from DATA to SCALAR for precision control.
  65  *
  66  * Revision 1.4  1994/01/11  18:36:58  bjax
  67  * Removed ls_eom.h include directive; replaced with ls_types, ls_constants,
  68  * and ls_generic.h includes.
  69  *
  70  * Revision 1.3  1993/10/07  18:45:24  bjax
  71  * Added local defn of d[xyz]_pilot_from_cg to support previous mod. EBJ
  72  *
  73  * Revision 1.2  1993/10/07  18:41:31  bjax
  74  * Moved calculations of auxiliary accelerations here from ls_aux, and
  75  * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc.  EBJ
  76  *
  77  * Revision 1.1  1992/12/30  13:17:02  bjax
  78  * Initial revision
  79  *
  80
  81   ----------------------------------------------------------------------------
  82
  83   REFERENCES:
  84
  85   [  1] McFarland, Richard E.: "A Standard Kinematic Model
  86   for Flight Simulation at NASA-Ames", NASA CR-2497,
  87   January 1975
  88
  89   ----------------------------------------------------------------------------
  90
  91   CALLED BY:
  92
  93   ----------------------------------------------------------------------------
  94
  95   CALLS TO:
  96
  97   ----------------------------------------------------------------------------
  98
  99   INPUTS:  Aero, engine, gear forces & moments
 100
 101   ----------------------------------------------------------------------------
 102
 103   OUTPUTS:      State derivatives
 104
 105   -------------------------------------------------------------------------*/
 106 #include "ls_types.h"
 107 #include "ls_generic.h"
 108 #include "ls_constants.h"
 109 #include "ls_accel.h"
 110 #include <math.h>
 111
 112 void ls_accel( void ) {
 113
 114   SCALAR        inv_Mass, inv_Radius;
 115   SCALAR        ixz2, c0, c1, c2, c3, c4, c5, c6, c7, c8, c9, c10;
 116   SCALAR        dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
 117   SCALAR        tan_Lat_geocentric;
 118
 119
 120   /* Sum forces and moments at reference point */
 121
 122
 123   F_X = F_X_aero + F_X_engine + F_X_gear;
 124   F_Y = F_Y_aero + F_Y_engine + F_Y_gear;
 125   F_Z = F_Z_aero + F_Z_engine + F_Z_gear;
 126
 127   M_l_rp = M_l_aero + M_l_engine + M_l_gear;
 128   M_m_rp = M_m_aero + M_m_engine + M_m_gear;
 129   M_n_rp = M_n_aero + M_n_engine + M_n_gear;
 130
 131   /* Transfer moments to center of gravity */
 132
 133   M_l_cg = M_l_rp + F_Y*Dz_cg - F_Z*Dy_cg;
 134   M_m_cg = M_m_rp + F_Z*Dx_cg - F_X*Dz_cg;
 135   M_n_cg = M_n_rp + F_X*Dy_cg - F_Y*Dx_cg;
 136
 137   /* Transform from body to local frame */
 138
 139   F_north = T_local_to_body_11*F_X + T_local_to_body_21*F_Y
 140     + T_local_to_body_31*F_Z;
 141   F_east  = T_local_to_body_12*F_X + T_local_to_body_22*F_Y
 142     + T_local_to_body_32*F_Z;
 143   F_down  = T_local_to_body_13*F_X + T_local_to_body_23*F_Y
 144     + T_local_to_body_33*F_Z;
 145
 146   /* Calculate linear accelerations */
 147
 148   tan_Lat_geocentric = tan(Lat_geocentric);
 149   inv_Mass = 1/Mass;
 150   inv_Radius = 1/Radius_to_vehicle;
 151   V_dot_north = inv_Mass*F_north +
 152     inv_Radius*(V_north*V_down - V_east*V_east*tan_Lat_geocentric);
 153   V_dot_east  = inv_Mass*F_east  +
 154     inv_Radius*(V_east*V_down + V_north*V_east*tan_Lat_geocentric);
 155   V_dot_down  = inv_Mass*(F_down) + Gravity -
 156     inv_Radius*(V_north*V_north + V_east*V_east);
 157
 158   /* Invert the symmetric inertia matrix */
 159
 160   ixz2 = I_xz*I_xz;
 161   c0  = 1/(I_xx*I_zz - ixz2);
 162   c1  = c0*((I_yy-I_zz)*I_zz - ixz2);
 163   c4  = c0*I_xz;
 164   /* c2  = c0*I_xz*(I_xx - I_yy + I_zz); */
 165   c2  = c4*(I_xx - I_yy + I_zz);
 166   c3  = c0*I_zz;
 167   c7  = 1/I_yy;
 168   c5  = c7*(I_zz - I_xx);
 169   c6  = c7*I_xz;
 170   c8  = c0*((I_xx - I_yy)*I_xx + ixz2);
 171   /* c9  = c0*I_xz*(I_yy - I_zz - I_xx); */
 172   c9  = c4*(I_yy - I_zz - I_xx);
 173   c10 = c0*I_xx;
 174
 175   /* Calculate the rotational body axis accelerations */
 176
 177   P_dot_body = (c1*R_body + c2*P_body)*Q_body + c3*M_l_cg +  c4*M_n_cg;
 178   Q_dot_body = c5*R_body*P_body + c6*(R_body*R_body - P_body*P_body) + c7*M_m_cg;
 179   R_dot_body = (c8*P_body + c9*R_body)*Q_body + c4*M_l_cg + c10*M_n_cg;
 180
 181   /* Calculate body axis accelerations (move to ls_accel?)      */
 182
 183     inv_Mass = 1/Mass;
 184
 185     A_X_cg = F_X * inv_Mass;
 186     A_Y_cg = F_Y * inv_Mass;
 187     A_Z_cg = F_Z * inv_Mass;
 188
 189     dx_pilot_from_cg = Dx_pilot - Dx_cg;
 190     dy_pilot_from_cg = Dy_pilot - Dy_cg;
 191     dz_pilot_from_cg = Dz_pilot - Dz_cg;
 192
 193     A_X_pilot = A_X_cg  + (-R_body*R_body - Q_body*Q_body)*dx_pilot_from_cg
 194                                             + ( P_body*Q_body - R_dot_body   )*dy_pilot_from_cg
 195                                             + ( P_body*R_body + Q_dot_body   )*dz_pilot_from_cg;
 196     A_Y_pilot = A_Y_cg  + ( P_body*Q_body + R_dot_body   )*dx_pilot_from_cg
 197                                             + (-P_body*P_body - R_body*R_body)*dy_pilot_from_cg
 198                                             + ( Q_body*R_body - P_dot_body   )*dz_pilot_from_cg;
 199     A_Z_pilot = A_Z_cg  + ( P_body*R_body - Q_dot_body   )*dx_pilot_from_cg
 200                                             + ( Q_body*R_body + P_dot_body   )*dy_pilot_from_cg
 201                                             + (-Q_body*Q_body - P_body*P_body)*dz_pilot_from_cg;
 202
 203     N_X_cg = INVG*A_X_cg;
 204     N_Y_cg = INVG*A_Y_cg;
 205     N_Z_cg = INVG*A_Z_cg;
 206
 207     N_X_pilot = INVG*A_X_pilot;
 208     N_Y_pilot = INVG*A_Y_pilot;
 209     N_Z_pilot = INVG*A_Z_pilot;
 210
 211
 212     U_dot_body = T_local_to_body_11*V_dot_north + T_local_to_body_12*V_dot_east
 213                                     + T_local_to_body_13*V_dot_down - Q_total*W_body + R_total*V_body;
 214     V_dot_body = T_local_to_body_21*V_dot_north + T_local_to_body_22*V_dot_east
 215                                     + T_local_to_body_23*V_dot_down - R_total*U_body + P_total*W_body;
 216     W_dot_body = T_local_to_body_31*V_dot_north + T_local_to_body_32*V_dot_east
 217                                     + T_local_to_body_33*V_dot_down - P_total*V_body + Q_total*U_body;
 218     /* End of ls_accel */
 219
 220 }
 221 /**************************************************************************/
 222
 223
 224
 225