src/FDM/LaRCsim/basic_gear.c

   1 /***************************************************************************
   2
   3         TITLE:  gear
   4
   5 ----------------------------------------------------------------------------
   6
   7         FUNCTION:       Landing gear model for example simulation
   8
   9 ----------------------------------------------------------------------------
  10
  11         MODULE STATUS:  developmental
  12
  13 ----------------------------------------------------------------------------
  14
  15         GENEALOGY:      Created 931012 by E. B. Jackson
  16
  17 ----------------------------------------------------------------------------
  18
  19         DESIGNED BY:    E. B. Jackson
  20
  21         CODED BY:       E. B. Jackson
  22
  23         MAINTAINED BY:  E. B. Jackson
  24
  25 ----------------------------------------------------------------------------
  26
  27         MODIFICATION HISTORY:
  28
  29 ----------------------------------------------------------------------------
  30
  31         REFERENCES:
  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 #include <math.h>
  51 #include "ls_types.h"
  52 #include "ls_constants.h"
  53 #include "ls_generic.h"
  54 #include "ls_cockpit.h"
  55
  56 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  57
  58
  59 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
  60 {
  61     result[0] = v1[0] - v2[0];
  62     result[1] = v1[1] - v2[1];
  63     result[2] = v1[2] - v2[2];
  64 }
  65
  66 static void add3( DATA v1[],  DATA v2[], DATA result[] )
  67 {
  68     result[0] = v1[0] + v2[0];
  69     result[1] = v1[1] + v2[1];
  70     result[2] = v1[2] + v2[2];
  71 }
  72
  73 static void cross3( DATA v1[],  DATA v2[], DATA result[] )
  74 {
  75     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
  76     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
  77     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
  78 }
  79
  80 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
  81 {
  82     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
  83     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
  84     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
  85 }
  86
  87 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
  88 {
  89     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
  90     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
  91     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
  92 }
  93
  94 static void clear3( DATA v[] )
  95 {
  96     v[0] = 0.; v[1] = 0.; v[2] = 0.;
  97 }
  98
  99 void basic_gear()
 100 {
 101 char rcsid[] = "junk";
 102 #define NUM_WHEELS 4
 103
 104 // char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
 105   /*
 106    * Aircraft specific initializations and data goes here
 107    */
 108
 109
 110     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 111     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations,full extension */
 112     {
 113         { .422,  0.,    .29 },             /*nose*/ /* in feet */
 114         { 0.026, 0.006, .409 },        /*right main*/
 115         { 0.026, -.006, .409 },        /*left main*/
 116         { -1.32, 0, .17 }            /*tail skid */
 117     };
 118     // static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
 119            // { -0.5, 2.5, 2.5, 0};
 120     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 121         { 2., .65, .65, 1. };
 122     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 123         { 1.,  .3, .3, .5 };
 124     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 125         { 0.,  0.,  0., 0. };                       /* 0 = none, 1 = full */
 126     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 127         { 0., 0., 0., 0};                                   /* radians, +CW */
 128   /*
 129    * End of aircraft specific code
 130    */
 131
 132   /*
 133    * Constants & coefficients for tyres on tarmac - ref [1]
 134    */
 135
 136     /* skid function looks like:
 137      *
 138      *           mu  ^
 139      *               |
 140      *       max_mu  |       +
 141      *               |      /|
 142      *   sliding_mu  |     / +------
 143      *               |    /
 144      *               |   /
 145      *               +--+------------------------>
 146      *               |  |    |      sideward V
 147      *               0 bkout skid
 148      *                 V     V
 149      */
 150
 151
 152     static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
 153         static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
 154     static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
 155     static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
 156     static DATA max_mu       = 0.8;
 157     static DATA bkout_v      = 0.1;
 158     static DATA skid_v       = 1.0;
 159   /*
 160    * Local data variables
 161    */
 162
 163     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 164     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 165     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 166         DATA v_wheel_cg_local_v[3];    /*wheel velocity rel to cg N-E-D*/
 167     // DATA v_wheel_body_v[3];          /* wheel velocity,        X-Y-Z */
 168     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 169     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 170     // DATA altitude_local_v[3];       /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
 171     // DATA altitude_body_v[3];        /*altitude vector in body (X,Y,Z)*/
 172     DATA temp3a[3];
 173     // DATA temp3b[3];
 174     DATA tempF[3];
 175     DATA tempM[3];
 176     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 177     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 178     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 179     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 180     DATA beta_mu;                       /* breakout friction slope      */
 181     DATA forward_wheel_force, sideward_wheel_force;
 182
 183     int i;                              /* per wheel loop counter */
 184
 185   /*
 186    * Execution starts here
 187    */
 188
 189     beta_mu = max_mu/(skid_v-bkout_v);
 190     clear3( F_gear_v );         /* Initialize sum of forces...  */
 191     clear3( M_gear_v );         /* ...and moments               */
 192
 193   /*
 194    * Put aircraft specific executable code here
 195    */
 196
 197     percent_brake[1] = Brake_pct[0];
 198     percent_brake[2] = Brake_pct[1];
 199
 200     caster_angle_rad[0] =
 201         (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
 202
 203
 204         for (i=0;i<num_wheels;i++)          /* Loop for each wheel */
 205     {
 206                 /* printf("%s:\n",gear_strings[i]); */
 207
 208
 209
 210                 /*========================================*/
 211                 /* Calculate wheel position w.r.t. runway */
 212                 /*========================================*/
 213
 214
 215                 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 216
 217
 218                         /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 219
 220                 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 221
 222                 /* then converting to local (North-East-Down) axes... */
 223
 224                 multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 225
 226
 227                 /* Runway axes correction - third element is Altitude, not (-)Z... */
 228
 229                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 230
 231                 /* Add wheel offset to cg location in local axes */
 232
 233                 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 234
 235                 /* remove Runway axes correction so right hand rule applies */
 236
 237                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 238
 239                 /*============================*/
 240                 /* Calculate wheel velocities */
 241                 /*============================*/
 242
 243                 /* contribution due to angular rates */
 244
 245                 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 246
 247                 /* transform into local axes */
 248
 249                 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
 250
 251                 /* plus contribution due to cg velocities */
 252
 253                 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
 254
 255                 clear3(f_wheel_local_v);
 256                 reaction_normal_force=0;
 257                 if( HEIGHT_AGL_WHEEL < 0. )
 258                         /*the wheel is underground -- which implies ground contact
 259                           so calculate reaction forces */
 260                         {
 261                         /*===========================================*/
 262                         /* Calculate forces & moments for this wheel */
 263                         /*===========================================*/
 264
 265                         /* Add any anticipation, or frame lead/prediction, here... */
 266
 267                                 /* no lead used at present */
 268
 269                         /* Calculate sideward and forward velocities of the wheel
 270                                 in the runway plane                                     */
 271
 272                         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 273                         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 274
 275                         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 276                                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 277                         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 278                                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 279
 280
 281                 /* Calculate normal load force (simple spring constant) */
 282
 283                 reaction_normal_force = 0.;
 284
 285                 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
 286                                           - v_wheel_local_v[2]*spring_damping[i];
 287                         /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 288
 289                 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 290                         /* to prevent damping component from swamping spring component */
 291
 292
 293                 /* Calculate friction coefficients */
 294
 295                         if(it_rolls[i])
 296                         {
 297                            forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
 298                            abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 299                            sideward_mu = sliding_mu[i];
 300                            if (abs_v_wheel_sideward < skid_v)
 301                            sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 302                            if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 303                         }
 304                         else
 305                         {
 306                                 forward_mu=sliding_mu[i];
 307                                 sideward_mu=sliding_mu[i];
 308                         }
 309
 310                         /* Calculate foreward and sideward reaction forces */
 311
 312                         forward_wheel_force  =   forward_mu*reaction_normal_force;
 313                         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 314                         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 315                         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 316 /*                      printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 317  */
 318                         /* Rotate into local (N-E-D) axes */
 319
 320                         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 321                                           - sideward_wheel_force*sin_wheel_hdg_angle;
 322                         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 323                                           + sideward_wheel_force*cos_wheel_hdg_angle;
 324                         f_wheel_local_v[2] = reaction_normal_force;
 325
 326                          /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 327                         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 328
 329                         /* Calculate moments from force and offsets in body axes */
 330
 331                         cross3( d_wheel_cg_body_v, tempF, tempM );
 332
 333                         /* Sum forces and moments across all wheels */
 334
 335                         add3( tempF, F_gear_v, F_gear_v );
 336                         add3( tempM, M_gear_v, M_gear_v );
 337
 338
 339                         }
 340
 341
 342
 343 /*                  printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]);  */
 344 /*                  printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); */
 345 /*                  printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 346
 347
 348     }
 349 }