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