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