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