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