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