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:  Renamed navion_gear.c originally created 931012 by E. B. Jackson
  16
  17
  18 ----------------------------------------------------------------------------
  19
  20         DESIGNED BY:    E. B. Jackson
  21
  22         CODED BY:       E. B. Jackson
  23
  24         MAINTAINED BY:  E. B. Jackson
  25
  26 ----------------------------------------------------------------------------
  27
  28         MODIFICATION HISTORY:
  29
  30         DATE    PURPOSE                                         BY
  31
  32         931218  Added navion.h header to allow connection with
  33                 aileron displacement for nosewheel steering.    EBJ
  34         940511  Connected nosewheel to rudder pedal; adjusted gain.
  35
  36         CURRENT RCS HEADER:
  37
  38 $Header$
  39 $Log$
  40 Revision 1.1  1999/06/17 18:07:34  curt
  41 Initial revision
  42
  43 Revision 1.1  1999/06/15 20:05:27  curt
  44 Added c172 model from Tony Peden.
  45
  46 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  47 Start of 0.6.x branch.
  48
  49 Revision 1.6  1998/10/17 01:34:16  curt
  50 C++ ifying ...
  51
  52 Revision 1.5  1998/09/29 02:03:00  curt
  53 Added a brake + autopilot mods.
  54
  55 Revision 1.4  1998/08/06 12:46:40  curt
  56 Header change.
  57
  58 Revision 1.3  1998/02/03 23:20:18  curt
  59 Lots of little tweaks to fix various consistency problems discovered by
  60 Solaris' CC.  Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
  61 passed arguments along to the real printf().  Also incorporated HUD changes
  62 by Michele America.
  63
  64 Revision 1.2  1998/01/19 18:40:29  curt
  65 Tons of little changes to clean up the code and to remove fatal errors
  66 when building with the c++ compiler.
  67
  68 Revision 1.1  1997/05/29 00:10:02  curt
  69 Initial Flight Gear revision.
  70
  71
  72 ----------------------------------------------------------------------------
  73
  74         REFERENCES:
  75
  76 ----------------------------------------------------------------------------
  77
  78         CALLED BY:
  79
  80 ----------------------------------------------------------------------------
  81
  82         CALLS TO:
  83
  84 ----------------------------------------------------------------------------
  85
  86         INPUTS:
  87
  88 ----------------------------------------------------------------------------
  89
  90         OUTPUTS:
  91
  92 --------------------------------------------------------------------------*/
  93 #include <math.h>
  94 #include "ls_types.h"
  95 #include "ls_constants.h"
  96 #include "ls_generic.h"
  97 #include "ls_cockpit.h"
  98
  99
 100 void sub3( DATA v1[],  DATA v2[], DATA result[] )
 101 {
 102     result[0] = v1[0] - v2[0];
 103     result[1] = v1[1] - v2[1];
 104     result[2] = v1[2] - v2[2];
 105 }
 106
 107 void add3( DATA v1[],  DATA v2[], DATA result[] )
 108 {
 109     result[0] = v1[0] + v2[0];
 110     result[1] = v1[1] + v2[1];
 111     result[2] = v1[2] + v2[2];
 112 }
 113
 114 void cross3( DATA v1[],  DATA v2[], DATA result[] )
 115 {
 116     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
 117     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
 118     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
 119 }
 120
 121 void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 122 {
 123     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 124     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 125     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 126 }
 127
 128 void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 129 {
 130     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 131     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 132     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 133 }
 134
 135 void clear3( DATA v[] )
 136 {
 137     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 138 }
 139
 140 void gear( SCALAR dt, int Initialize ) {
 141 char rcsid[] = "$Id$";
 142
 143   /*
 144    * Aircraft specific initializations and data goes here
 145    */
 146
 147 #define NUM_WHEELS 3
 148
 149     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 150     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations */
 151     {
 152         { 10.,  0., 4. },                               /* in feet */
 153         { -1.,  3., 4. },
 154         { -1., -3., 4. }
 155     };
 156     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 157         { 1500., 5000., 5000. };
 158     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 159         { 100.,  150.,  150. };
 160     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 161         { 0.,  0.,  0. };                           /* 0 = none, 1 = full */
 162     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 163         { 0., 0., 0.};                              /* radians, +CW */
 164   /*
 165    * End of aircraft specific code
 166    */
 167
 168   /*
 169    * Constants & coefficients for tyres on tarmac - ref [1]
 170    */
 171
 172     /* skid function looks like:
 173      *
 174      *           mu  ^
 175      *               |
 176      *       max_mu  |       +
 177      *               |      /|
 178      *   sliding_mu  |     / +------
 179      *               |    /
 180      *               |   /
 181      *               +--+------------------------>
 182      *               |  |    |      sideward V
 183      *               0 bkout skid
 184      *                 V     V
 185      */
 186
 187
 188     static DATA sliding_mu   = 0.5;
 189     static DATA rolling_mu   = 0.01;
 190     static DATA max_brake_mu = 0.6;
 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_body_v[3];             /* wheel velocity,        X-Y-Z */
 202     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 203     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 204     DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
 205     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 206     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 207     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 208     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 209     DATA beta_mu;                       /* breakout friction slope      */
 210     DATA forward_wheel_force, sideward_wheel_force;
 211
 212     int i;                              /* per wheel loop counter */
 213
 214   /*
 215    * Execution starts here
 216    */
 217
 218     beta_mu = max_mu/(skid_v-bkout_v);
 219     clear3( F_gear_v );         /* Initialize sum of forces...  */
 220     clear3( M_gear_v );         /* ...and moments               */
 221
 222   /*
 223    * Put aircraft specific executable code here
 224    */
 225
 226     /* replace with cockpit brake handle connection code */
 227     percent_brake[1] = Brake_pct;
 228     percent_brake[2] = percent_brake[1];
 229
 230     caster_angle_rad[0] = 0.03*Rudder_pedal;
 231
 232     for (i=0;i<num_wheels;i++)      /* Loop for each wheel */
 233     {
 234         /*========================================*/
 235         /* Calculate wheel position w.r.t. runway */
 236         /*========================================*/
 237
 238             /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 239
 240         sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 241
 242             /* then converting to local (North-East-Down) axes... */
 243
 244         multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 245
 246             /* Runway axes correction - third element is Altitude, not (-)Z... */
 247
 248         d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 249
 250             /* Add wheel offset to cg location in local axes */
 251
 252         add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 253
 254             /* remove Runway axes correction so right hand rule applies */
 255
 256         d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 257
 258         /*============================*/
 259         /* Calculate wheel velocities */
 260         /*============================*/
 261
 262             /* contribution due to angular rates */
 263
 264         cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 265
 266             /* transform into local axes */
 267
 268         multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
 269
 270             /* plus contribution due to cg velocities */
 271
 272         add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
 273
 274
 275         /*===========================================*/
 276         /* Calculate forces & moments for this wheel */
 277         /*===========================================*/
 278
 279             /* Add any anticipation, or frame lead/prediction, here... */
 280
 281                     /* no lead used at present */
 282
 283             /* Calculate sideward and forward velocities of the wheel
 284                     in the runway plane                                 */
 285
 286         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 287         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 288
 289         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 290                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 291         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 292                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 293
 294             /* Calculate normal load force (simple spring constant) */
 295
 296         reaction_normal_force = 0.;
 297         if( d_wheel_rwy_local_v[2] < 0. )
 298         {
 299             reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
 300                                   - v_wheel_local_v[2]*spring_damping[i];
 301             if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 302                 /* to prevent damping component from swamping spring component */
 303         }
 304
 305             /* Calculate friction coefficients */
 306
 307         forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
 308         abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 309         sideward_mu = sliding_mu;
 310         if (abs_v_wheel_sideward < skid_v)
 311             sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 312         if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 313
 314             /* Calculate foreward and sideward reaction forces */
 315
 316         forward_wheel_force  =   forward_mu*reaction_normal_force;
 317         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 318         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 319         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 320
 321             /* Rotate into local (N-E-D) axes */
 322
 323         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 324                           - sideward_wheel_force*sin_wheel_hdg_angle;
 325         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 326                           + sideward_wheel_force*cos_wheel_hdg_angle;
 327         f_wheel_local_v[2] = reaction_normal_force;
 328
 329             /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 330
 331         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 332
 333             /* Calculate moments from force and offsets in body axes */
 334
 335         cross3( d_wheel_cg_body_v, tempF, tempM );
 336
 337         /* Sum forces and moments across all wheels */
 338
 339         add3( tempF, F_gear_v, F_gear_v );
 340         add3( tempM, M_gear_v, M_gear_v );
 341
 342     }
 343 }