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