src/FDM/LaRCsim/uiuc_aero.c

   1 /***************************************************************************
   2
   3   TITLE:        uiuc_aero
   4
   5 ----------------------------------------------------------------------------
   6
   7   FUNCTION:     aerodynamics, engine and gear model
   8
   9 ----------------------------------------------------------------------------
  10
  11   MODULE STATUS:        developmental
  12
  13 ----------------------------------------------------------------------------
  14
  15   GENEALOGY:            Equations based on Part 1 of Roskam's S&C text
  16
  17 ----------------------------------------------------------------------------
  18
  19   DESIGNED BY:          Bipin Sehgal
  20
  21   CODED BY:             Bipin Sehgal
  22
  23   MAINTAINED BY:        Bipin Sehgal
  24
  25 ----------------------------------------------------------------------------
  26
  27   MODIFICATION HISTORY:
  28
  29   DATE          PURPOSE                                                                                         BY
  30   3/17/00   Initial test release
  31
  32
  33 ----------------------------------------------------------------------------
  34
  35   CALLED BY:
  36
  37 ----------------------------------------------------------------------------
  38
  39   CALLS TO:
  40
  41 ----------------------------------------------------------------------------
  42
  43   INPUTS:
  44
  45 ----------------------------------------------------------------------------
  46
  47   OUTPUTS:
  48
  49 --------------------------------------------------------------------------*/
  50
  51
  52 #include <math.h>
  53 #include "ls_types.h"
  54 #include "ls_generic.h"
  55 #include "ls_constants.h"
  56 #include "ls_cockpit.h"
  57 #include <FDM/UIUCModel/uiuc_wrapper.h>
  58
  59
  60 void uiuc_aero( SCALAR dt, int Initialize )
  61 {
  62     static int init = 0;
  63
  64     if (init==0)
  65     {
  66       init = -1;
  67       uiuc_init_aeromodel();
  68     }
  69
  70     uiuc_force_moment(dt);
  71 }
  72
  73
  74 void uiuc_engine( SCALAR dt, int Initialize )
  75 {
  76     uiuc_engine_routine();
  77 }
  78
  79 /* ***********************************************************************
  80  * Gear model. Exact copy of C172_gear.c. Additional gear models will be
  81  * added later and the choice of the gear model could be specified at
  82  * runtime.
  83  * ***********************************************************************/
  84 static sub3( DATA v1[],  DATA v2[], DATA result[] )
  85 {
  86     result[0] = v1[0] - v2[0];
  87     result[1] = v1[1] - v2[1];
  88     result[2] = v1[2] - v2[2];
  89 }
  90
  91 static add3( DATA v1[],  DATA v2[], DATA result[] )
  92 {
  93     result[0] = v1[0] + v2[0];
  94     result[1] = v1[1] + v2[1];
  95     result[2] = v1[2] + v2[2];
  96 }
  97
  98 static cross3( DATA v1[],  DATA v2[], DATA result[] )
  99 {
 100     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
 101     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
 102     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
 103 }
 104
 105 static multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 106 {
 107     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 108     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 109     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 110 }
 111
 112 static mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 113 {
 114     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 115     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 116     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 117 }
 118
 119 static clear3( DATA v[] )
 120 {
 121     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 122 }
 123
 124 uiuc_gear()
 125 {
 126 char rcsid[] = "$Id$";
 127
 128   /*
 129    * Aircraft specific initializations and data goes here
 130    */
 131
 132 #define NUM_WHEELS 3
 133
 134     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 135     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations */
 136     {
 137         { 10.,  0., 4. },                               /* in feet */
 138         { -1.,  3., 4. },
 139         { -1., -3., 4. }
 140     };
 141     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 142         { 1500., 5000., 5000. };
 143     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 144         { 100.,  150.,  150. };
 145     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 146         { 0.,  0.,  0. };                           /* 0 = none, 1 = full */
 147     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 148         { 0., 0., 0.};                              /* radians, +CW */
 149   /*
 150    * End of aircraft specific code
 151    */
 152
 153   /*
 154    * Constants & coefficients for tyres on tarmac - ref [1]
 155    */
 156
 157     /* skid function looks like:
 158      *
 159      *           mu  ^
 160      *               |
 161      *       max_mu  |       +
 162      *               |      /|
 163      *   sliding_mu  |     / +------
 164      *               |    /
 165      *               |   /
 166      *               +--+------------------------>
 167      *               |  |    |      sideward V
 168      *               0 bkout skid
 169      *                 V     V
 170      */
 171
 172
 173     static DATA sliding_mu   = 0.5;
 174     static DATA rolling_mu   = 0.01;
 175     static DATA max_brake_mu = 0.6;
 176     static DATA max_mu       = 0.8;
 177     static DATA bkout_v      = 0.1;
 178     static DATA skid_v       = 1.0;
 179   /*
 180    * Local data variables
 181    */
 182
 183     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 184     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 185     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 186     DATA v_wheel_body_v[3];             /* wheel velocity,        X-Y-Z */
 187     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 188     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 189     DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
 190     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 191     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 192     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 193     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 194     DATA beta_mu;                       /* breakout friction slope      */
 195     DATA forward_wheel_force, sideward_wheel_force;
 196
 197     int i;                              /* per wheel loop counter */
 198
 199   /*
 200    * Execution starts here
 201    */
 202
 203     beta_mu = max_mu/(skid_v-bkout_v);
 204     clear3( F_gear_v );         /* Initialize sum of forces...  */
 205     clear3( M_gear_v );         /* ...and moments               */
 206
 207   /*
 208    * Put aircraft specific executable code here
 209    */
 210
 211     percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
 212     percent_brake[2] = percent_brake[1];
 213
 214     caster_angle_rad[0] = 0.03*Rudder_pedal;
 215
 216     for (i=0;i<num_wheels;i++)      /* Loop for each wheel */
 217     {
 218         /*========================================*/
 219         /* Calculate wheel position w.r.t. runway */
 220         /*========================================*/
 221
 222             /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 223
 224         sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 225
 226             /* then converting to local (North-East-Down) axes... */
 227
 228         multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 229
 230             /* Runway axes correction - third element is Altitude, not (-)Z... */
 231
 232         d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 233
 234             /* Add wheel offset to cg location in local axes */
 235
 236         add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 237
 238             /* remove Runway axes correction so right hand rule applies */
 239
 240         d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 241
 242         /*============================*/
 243         /* Calculate wheel velocities */
 244         /*============================*/
 245
 246             /* contribution due to angular rates */
 247
 248         cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 249
 250             /* transform into local axes */
 251
 252         multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
 253
 254             /* plus contribution due to cg velocities */
 255
 256         add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
 257
 258
 259         /*===========================================*/
 260         /* Calculate forces & moments for this wheel */
 261         /*===========================================*/
 262
 263             /* Add any anticipation, or frame lead/prediction, here... */
 264
 265                     /* no lead used at present */
 266
 267             /* Calculate sideward and forward velocities of the wheel
 268                     in the runway plane                                 */
 269
 270         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 271         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 272
 273         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 274                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 275         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 276                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 277
 278             /* Calculate normal load force (simple spring constant) */
 279
 280         reaction_normal_force = 0.;
 281         if( d_wheel_rwy_local_v[2] < 0. )
 282         {
 283             reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
 284                                   - v_wheel_local_v[2]*spring_damping[i];
 285             if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 286                 /* to prevent damping component from swamping spring component */
 287         }
 288
 289             /* Calculate friction coefficients */
 290
 291         forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
 292         abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 293         sideward_mu = sliding_mu;
 294         if (abs_v_wheel_sideward < skid_v)
 295             sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 296         if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 297
 298             /* Calculate foreward and sideward reaction forces */
 299
 300         forward_wheel_force  =   forward_mu*reaction_normal_force;
 301         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 302         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 303         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 304
 305             /* Rotate into local (N-E-D) axes */
 306
 307         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 308                           - sideward_wheel_force*sin_wheel_hdg_angle;
 309         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 310                           + sideward_wheel_force*cos_wheel_hdg_angle;
 311         f_wheel_local_v[2] = reaction_normal_force;
 312
 313             /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 314
 315         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 316
 317             /* Calculate moments from force and offsets in body axes */
 318
 319         cross3( d_wheel_cg_body_v, tempF, tempM );
 320
 321         /* Sum forces and moments across all wheels */
 322
 323         add3( tempF, F_gear_v, F_gear_v );
 324         add3( tempM, M_gear_v, M_gear_v );
 325
 326     }
 327 }