src/FDM/LaRCsim/c172_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         DATE    PURPOSE                                         BY
  30
  31         931218  Added navion.h header to allow connection with
  32                 aileron displacement for nosewheel steering.    EBJ
  33         940511  Connected nosewheel to rudder pedal; adjusted gain.
  34
  35         CURRENT RCS HEADER:
  36
  37 $Header$
  38 $Log$
  39 Revision 1.14  2000/04/10 18:09:41  curt
  40 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
  41 it's now possible to choose the LaRCsim model at runtime, as in
  42
  43   fgfs --aircraft=c172
  44
  45 or
  46
  47   fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
  48
  49 I did this so that I could play with the UIUC stuff without losing
  50 Tony's C172 with its flaps, etc.  I did my best to respect the design
  51 of the LaRCsim code by staying in C, making only minimal changes, and
  52 not introducing any dependencies on the rest of FlightGear.  The
  53 modified files are attached.
  54
  55 Revision 1.13  1999/12/13 20:43:41  curt
  56 Updates from Tony.
  57
  58
  59 ----------------------------------------------------------------------------
  60
  61         REFERENCES:
  62
  63 ----------------------------------------------------------------------------
  64
  65         CALLED BY:
  66
  67 ----------------------------------------------------------------------------
  68
  69         CALLS TO:
  70
  71 ----------------------------------------------------------------------------
  72
  73         INPUTS:
  74
  75 ----------------------------------------------------------------------------
  76
  77         OUTPUTS:
  78
  79 --------------------------------------------------------------------------*/
  80 #include <math.h>
  81 #include "ls_types.h"
  82 #include "ls_constants.h"
  83 #include "ls_generic.h"
  84 #include "ls_cockpit.h"
  85
  86 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  87
  88
  89 static sub3( DATA v1[],  DATA v2[], DATA result[] )
  90 {
  91     result[0] = v1[0] - v2[0];
  92     result[1] = v1[1] - v2[1];
  93     result[2] = v1[2] - v2[2];
  94 }
  95
  96 static add3( DATA v1[],  DATA v2[], DATA result[] )
  97 {
  98     result[0] = v1[0] + v2[0];
  99     result[1] = v1[1] + v2[1];
 100     result[2] = v1[2] + v2[2];
 101 }
 102
 103 static cross3( DATA v1[],  DATA v2[], DATA result[] )
 104 {
 105     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
 106     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
 107     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
 108 }
 109
 110 static multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 111 {
 112     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 113     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 114     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 115 }
 116
 117 static mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 118 {
 119     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 120     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 121     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 122 }
 123
 124 static clear3( DATA v[] )
 125 {
 126     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 127 }
 128
 129 c172_gear()
 130 {
 131 char rcsid[] = "$Id$";
 132 #define NUM_WHEELS 4
 133 char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
 134   /*
 135    * Aircraft specific initializations and data goes here
 136    */
 137
 138
 139     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 140     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations,full extension */
 141     {
 142         {  3.91,  0.,   6.67 },            /*nose*/ /* in feet */
 143         { -1.47,  3.58, 6.71 },        /*right main*/
 144         { -1.47, -3.58, 6.71 },        /*left main*/
 145         { -15.67, 0, 2.42 }            /*tail skid */
 146     };
 147         static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
 148         { -0.5, 2.5, 2.5, 0};
 149     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 150         { 1200., 900., 900., 10000. };
 151     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 152         { 200.,  300., 300., 400. };
 153     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 154         { 0.,  0.,  0., 0. };                       /* 0 = none, 1 = full */
 155     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 156         { 0., 0., 0., 0};                                   /* radians, +CW */
 157   /*
 158    * End of aircraft specific code
 159    */
 160
 161   /*
 162    * Constants & coefficients for tyres on tarmac - ref [1]
 163    */
 164
 165     /* skid function looks like:
 166      *
 167      *           mu  ^
 168      *               |
 169      *       max_mu  |       +
 170      *               |      /|
 171      *   sliding_mu  |     / +------
 172      *               |    /
 173      *               |   /
 174      *               +--+------------------------>
 175      *               |  |    |      sideward V
 176      *               0 bkout skid
 177      *                 V     V
 178      */
 179
 180
 181     static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
 182         static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
 183     static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
 184     static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
 185     static DATA max_mu       = 0.8;
 186     static DATA bkout_v      = 0.1;
 187     static DATA skid_v       = 1.0;
 188   /*
 189    * Local data variables
 190    */
 191
 192     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 193     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 194     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 195         DATA v_wheel_cg_local_v[3];    /*wheel velocity rel to cg N-E-D*/
 196     DATA v_wheel_body_v[3];             /* wheel velocity,        X-Y-Z */
 197     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 198     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 199         DATA altitude_local_v[3];       /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
 200         DATA altitude_body_v[3];        /*altitude vector in body (X,Y,Z)*/
 201     DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
 202     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 203     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 204     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 205     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 206     DATA beta_mu;                       /* breakout friction slope      */
 207     DATA forward_wheel_force, sideward_wheel_force;
 208
 209     int i;                              /* per wheel loop counter */
 210
 211   /*
 212    * Execution starts here
 213    */
 214
 215     beta_mu = max_mu/(skid_v-bkout_v);
 216     clear3( F_gear_v );         /* Initialize sum of forces...  */
 217     clear3( M_gear_v );         /* ...and moments               */
 218
 219   /*
 220    * Put aircraft specific executable code here
 221    */
 222
 223     percent_brake[1] = Brake_pct; /* replace with cockpit brake handle connection code */
 224     percent_brake[2] = percent_brake[1];
 225
 226     caster_angle_rad[0] = 0.52*Rudder_pedal;
 227
 228
 229         for (i=0;i<num_wheels;i++)          /* Loop for each wheel */
 230     {
 231                 /* printf("%s:\n",gear_strings[i]); */
 232
 233
 234
 235                 /*========================================*/
 236                 /* Calculate wheel position w.r.t. runway */
 237                 /*========================================*/
 238
 239
 240                 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 241
 242
 243                         /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 244
 245                 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 246
 247                 /* then converting to local (North-East-Down) axes... */
 248
 249                 multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 250
 251
 252                 /* Runway axes correction - third element is Altitude, not (-)Z... */
 253
 254                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 255
 256                 /* Add wheel offset to cg location in local axes */
 257
 258                 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 259
 260                 /* remove Runway axes correction so right hand rule applies */
 261
 262                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 263
 264                 /*============================*/
 265                 /* Calculate wheel velocities */
 266                 /*============================*/
 267
 268                 /* contribution due to angular rates */
 269
 270                 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 271
 272                 /* transform into local axes */
 273
 274                 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
 275
 276                 /* plus contribution due to cg velocities */
 277
 278                 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
 279
 280                 clear3(f_wheel_local_v);
 281                 reaction_normal_force=0;
 282                 if( HEIGHT_AGL_WHEEL < 0. )
 283                         /*the wheel is underground -- which implies ground contact
 284                           so calculate reaction forces */
 285                         {
 286                         /*===========================================*/
 287                         /* Calculate forces & moments for this wheel */
 288                         /*===========================================*/
 289
 290                         /* Add any anticipation, or frame lead/prediction, here... */
 291
 292                                 /* no lead used at present */
 293
 294                         /* Calculate sideward and forward velocities of the wheel
 295                                 in the runway plane                                     */
 296
 297                         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 298                         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 299
 300                         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 301                                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 302                         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 303                                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 304
 305
 306                 /* Calculate normal load force (simple spring constant) */
 307
 308                 reaction_normal_force = 0.;
 309
 310                 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
 311                                           - v_wheel_local_v[2]*spring_damping[i];
 312                         /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 313
 314                 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 315                         /* to prevent damping component from swamping spring component */
 316
 317
 318                 /* Calculate friction coefficients */
 319
 320                         if(it_rolls[i])
 321                         {
 322                            forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
 323                            abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 324                            sideward_mu = sliding_mu[i];
 325                            if (abs_v_wheel_sideward < skid_v)
 326                            sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 327                            if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 328                         }
 329                         else
 330                         {
 331                                 forward_mu=sliding_mu[i];
 332                                 sideward_mu=sliding_mu[i];
 333                         }
 334
 335                         /* Calculate foreward and sideward reaction forces */
 336
 337                         forward_wheel_force  =   forward_mu*reaction_normal_force;
 338                         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 339                         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 340                         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 341 /*                      printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 342  */
 343                         /* Rotate into local (N-E-D) axes */
 344
 345                         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 346                                           - sideward_wheel_force*sin_wheel_hdg_angle;
 347                         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 348                                           + sideward_wheel_force*cos_wheel_hdg_angle;
 349                         f_wheel_local_v[2] = reaction_normal_force;
 350
 351                          /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 352                         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 353
 354                         /* Calculate moments from force and offsets in body axes */
 355
 356                         cross3( d_wheel_cg_body_v, tempF, tempM );
 357
 358                         /* Sum forces and moments across all wheels */
 359
 360                         add3( tempF, F_gear_v, F_gear_v );
 361                         add3( tempM, M_gear_v, M_gear_v );
 362
 363
 364                 }
 365
 366
 367
 368                 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
 369
 370                 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 371                 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 372
 373
 374     }
 375 }