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.11  1999/11/15 22:54:07  curt
  40 Updates from Tony, mostly to landing gear.
  41
  42
  43 ----------------------------------------------------------------------------
  44
  45         REFERENCES:
  46
  47 ----------------------------------------------------------------------------
  48
  49         CALLED BY:
  50
  51 ----------------------------------------------------------------------------
  52
  53         CALLS TO:
  54
  55 ----------------------------------------------------------------------------
  56
  57         INPUTS:
  58
  59 ----------------------------------------------------------------------------
  60
  61         OUTPUTS:
  62
  63 --------------------------------------------------------------------------*/
  64 #include <math.h>
  65 #include "ls_types.h"
  66 #include "ls_constants.h"
  67 #include "ls_generic.h"
  68 #include "ls_cockpit.h"
  69
  70 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  71
  72
  73 sub3( DATA v1[],  DATA v2[], DATA result[] )
  74 {
  75     result[0] = v1[0] - v2[0];
  76     result[1] = v1[1] - v2[1];
  77     result[2] = v1[2] - v2[2];
  78 }
  79
  80 add3( DATA v1[],  DATA v2[], DATA result[] )
  81 {
  82     result[0] = v1[0] + v2[0];
  83     result[1] = v1[1] + v2[1];
  84     result[2] = v1[2] + v2[2];
  85 }
  86
  87 cross3( DATA v1[],  DATA v2[], DATA result[] )
  88 {
  89     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
  90     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
  91     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
  92 }
  93
  94 multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
  95 {
  96     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
  97     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
  98     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
  99 }
 100
 101 mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 102 {
 103     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 104     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 105     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 106 }
 107
 108 clear3( DATA v[] )
 109 {
 110     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 111 }
 112
 113 gear()
 114 {
 115 char rcsid[] = "$Id$";
 116 char gear_strings[3][12]={"nose","right main", "left main"};
 117   /*
 118    * Aircraft specific initializations and data goes here
 119    */
 120
 121 #define NUM_WHEELS 3
 122
 123     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 124     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations */
 125     {
 126         { 5,  0., 7.0 },                                /*nose*/ /* in feet */
 127         { -2.0,  3.6, 6.5 },        /*right main*/
 128         { -2.0, -3.6, 6.5 }         /*left main*/
 129     };
 130     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 131         { 1500., 5000., 5000. };
 132     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 133         { 1000.,  1500.,  1500. };
 134     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 135         { 0.,  0.,  0. };                           /* 0 = none, 1 = full */
 136     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 137         { 0., 0., 0.};                              /* radians, +CW */
 138   /*
 139    * End of aircraft specific code
 140    */
 141
 142   /*
 143    * Constants & coefficients for tyres on tarmac - ref [1]
 144    */
 145
 146     /* skid function looks like:
 147      *
 148      *           mu  ^
 149      *               |
 150      *       max_mu  |       +
 151      *               |      /|
 152      *   sliding_mu  |     / +------
 153      *               |    /
 154      *               |   /
 155      *               +--+------------------------>
 156      *               |  |    |      sideward V
 157      *               0 bkout skid
 158      *                 V     V
 159      */
 160
 161
 162     static DATA sliding_mu   = 0.5;
 163     static DATA rolling_mu   = 0.01;
 164     static DATA max_brake_mu = 0.6;
 165     static DATA max_mu       = 0.8;
 166     static DATA bkout_v      = 0.1;
 167     static DATA skid_v       = 1.0;
 168   /*
 169    * Local data variables
 170    */
 171
 172     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 173     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 174     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 175     DATA v_wheel_body_v[3];             /* wheel velocity,        X-Y-Z */
 176     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 177     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 178     DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
 179     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 180     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 181     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 182     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 183     DATA beta_mu;                       /* breakout friction slope      */
 184     DATA forward_wheel_force, sideward_wheel_force;
 185
 186     int i;                              /* per wheel loop counter */
 187
 188   /*
 189    * Execution starts here
 190    */
 191
 192     beta_mu = max_mu/(skid_v-bkout_v);
 193     clear3( F_gear_v );         /* Initialize sum of forces...  */
 194     clear3( M_gear_v );         /* ...and moments               */
 195
 196   /*
 197    * Put aircraft specific executable code here
 198    */
 199
 200     percent_brake[1] = Brake_pct; /* replace with cockpit brake handle connection code */
 201     percent_brake[2] = percent_brake[1];
 202
 203     caster_angle_rad[0] = 0.52*Rudder_pedal;
 204
 205
 206         for (i=0;i<num_wheels;i++)          /* Loop for each wheel */
 207     {
 208                 /* printf("%s:\n",gear_strings[i]); */
 209
 210                 /*========================================*/
 211                 /* Calculate wheel position w.r.t. runway */
 212                 /*========================================*/
 213
 214                 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 215
 216                 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 217
 218                 /* then converting to local (North-East-Down) axes... */
 219
 220                 multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 221
 222                 /* Runway axes correction - third element is Altitude, not (-)Z... */
 223
 224                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 225
 226                 /* Add wheel offset to cg location in local axes */
 227
 228                 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 229
 230                 /* remove Runway axes correction so right hand rule applies */
 231
 232                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 233
 234                 /*============================*/
 235                 /* Calculate wheel velocities */
 236                 /*============================*/
 237
 238                 /* contribution due to angular rates */
 239
 240                 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 241
 242                 /* transform into local axes */
 243
 244                 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
 245
 246                 /* plus contribution due to cg velocities */
 247
 248                 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
 249
 250                 clear3(f_wheel_local_v);
 251                 reaction_normal_force=0;
 252                 if( HEIGHT_AGL_WHEEL < 0. )
 253                         /*the wheel is underground -- which implies ground contact
 254                           so calculate reaction forces */
 255                         {
 256                         /*===========================================*/
 257                         /* Calculate forces & moments for this wheel */
 258                         /*===========================================*/
 259
 260                         /* Add any anticipation, or frame lead/prediction, here... */
 261
 262                                 /* no lead used at present */
 263
 264                         /* Calculate sideward and forward velocities of the wheel
 265                                 in the runway plane                                     */
 266
 267                         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 268                         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 269
 270                         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 271                                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 272                         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 273                                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 274
 275                 /* Calculate normal load force (simple spring constant) */
 276
 277                 reaction_normal_force = 0.;
 278
 279                 reaction_normal_force = spring_constant[i]*HEIGHT_AGL_WHEEL
 280                                           - v_wheel_local_v[2]*spring_damping[i];
 281                 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 282                         /* to prevent damping component from swamping spring component */
 283
 284
 285                 /* Calculate friction coefficients */
 286
 287                         forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
 288                         abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 289                         sideward_mu = sliding_mu;
 290                         if (abs_v_wheel_sideward < skid_v)
 291                         sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 292                         if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 293
 294                         /* Calculate foreward and sideward reaction forces */
 295
 296                         forward_wheel_force  =   forward_mu*reaction_normal_force;
 297                         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 298                         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 299                         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 300 /*                      printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 301  */
 302                         /* Rotate into local (N-E-D) axes */
 303
 304                         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 305                                           - sideward_wheel_force*sin_wheel_hdg_angle;
 306                         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 307                                           + sideward_wheel_force*cos_wheel_hdg_angle;
 308                         f_wheel_local_v[2] = reaction_normal_force;
 309
 310                          /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 311                         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 312
 313                         /* Calculate moments from force and offsets in body axes */
 314
 315                         cross3( d_wheel_cg_body_v, tempF, tempM );
 316
 317                         /* Sum forces and moments across all wheels */
 318
 319                         add3( tempF, F_gear_v, F_gear_v );
 320                         add3( tempM, M_gear_v, M_gear_v );
 321
 322
 323                 }
 324
 325
 326
 327                 /* printf("\tN: %g,dZrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL);
 328                 printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 329                 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 330
 331
 332     }
 333 }