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