src/FDM/LaRCsim/navion_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.2  2000/04/10 18:09:41  curt
  40 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
  41 it's now possible to choose the LaRCsim model at runtime, as in
  42
  43   fgfs --aircraft=c172
  44
  45 or
  46
  47   fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
  48
  49 I did this so that I could play with the UIUC stuff without losing
  50 Tony's C172 with its flaps, etc.  I did my best to respect the design
  51 of the LaRCsim code by staying in C, making only minimal changes, and
  52 not introducing any dependencies on the rest of FlightGear.  The
  53 modified files are attached.
  54
  55 Revision 1.1.1.1  1999/06/17 18:07:34  curt
  56 Start of 0.7.x branch
  57
  58 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  59 Start of 0.6.x branch.
  60
  61 Revision 1.6  1998/10/17 01:34:16  curt
  62 C++ ifying ...
  63
  64 Revision 1.5  1998/09/29 02:03:00  curt
  65 Added a brake + autopilot mods.
  66
  67 Revision 1.4  1998/08/06 12:46:40  curt
  68 Header change.
  69
  70 Revision 1.3  1998/02/03 23:20:18  curt
  71 Lots of little tweaks to fix various consistency problems discovered by
  72 Solaris' CC.  Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
  73 passed arguments along to the real printf().  Also incorporated HUD changes
  74 by Michele America.
  75
  76 Revision 1.2  1998/01/19 18:40:29  curt
  77 Tons of little changes to clean up the code and to remove fatal errors
  78 when building with the c++ compiler.
  79
  80 Revision 1.1  1997/05/29 00:10:02  curt
  81 Initial Flight Gear revision.
  82
  83
  84 ----------------------------------------------------------------------------
  85
  86         REFERENCES:
  87
  88 ----------------------------------------------------------------------------
  89
  90         CALLED BY:
  91
  92 ----------------------------------------------------------------------------
  93
  94         CALLS TO:
  95
  96 ----------------------------------------------------------------------------
  97
  98         INPUTS:
  99
 100 ----------------------------------------------------------------------------
 101
 102         OUTPUTS:
 103
 104 --------------------------------------------------------------------------*/
 105 #include <math.h>
 106 #include "ls_types.h"
 107 #include "ls_constants.h"
 108 #include "ls_generic.h"
 109 #include "ls_cockpit.h"
 110
 111
 112 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
 113 {
 114     result[0] = v1[0] - v2[0];
 115     result[1] = v1[1] - v2[1];
 116     result[2] = v1[2] - v2[2];
 117 }
 118
 119 static void add3( DATA v1[],  DATA v2[], DATA result[] )
 120 {
 121     result[0] = v1[0] + v2[0];
 122     result[1] = v1[1] + v2[1];
 123     result[2] = v1[2] + v2[2];
 124 }
 125
 126 static void cross3( DATA v1[],  DATA v2[], DATA result[] )
 127 {
 128     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
 129     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
 130     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
 131 }
 132
 133 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 134 {
 135     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 136     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 137     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 138 }
 139
 140 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 141 {
 142     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 143     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 144     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 145 }
 146
 147 static void clear3( DATA v[] )
 148 {
 149     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 150 }
 151
 152 void navion_gear( SCALAR dt, int Initialize ) {
 153 char rcsid[] = "$Id$";
 154
 155   /*
 156    * Aircraft specific initializations and data goes here
 157    */
 158
 159 #define NUM_WHEELS 3
 160
 161     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 162     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations */
 163     {
 164         { 10.,  0., 4. },                               /* in feet */
 165         { -1.,  3., 4. },
 166         { -1., -3., 4. }
 167     };
 168     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 169         { 1500., 5000., 5000. };
 170     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 171         { 100.,  150.,  150. };
 172     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 173         { 0.,  0.,  0. };                           /* 0 = none, 1 = full */
 174     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 175         { 0., 0., 0.};                              /* radians, +CW */
 176   /*
 177    * End of aircraft specific code
 178    */
 179
 180   /*
 181    * Constants & coefficients for tyres on tarmac - ref [1]
 182    */
 183
 184     /* skid function looks like:
 185      *
 186      *           mu  ^
 187      *               |
 188      *       max_mu  |       +
 189      *               |      /|
 190      *   sliding_mu  |     / +------
 191      *               |    /
 192      *               |   /
 193      *               +--+------------------------>
 194      *               |  |    |      sideward V
 195      *               0 bkout skid
 196      *                 V     V
 197      */
 198
 199
 200     static DATA sliding_mu   = 0.5;
 201     static DATA rolling_mu   = 0.01;
 202     static DATA max_brake_mu = 0.6;
 203     static DATA max_mu       = 0.8;
 204     static DATA bkout_v      = 0.1;
 205     static DATA skid_v       = 1.0;
 206   /*
 207    * Local data variables
 208    */
 209
 210     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 211     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 212     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 213     DATA v_wheel_body_v[3];             /* wheel velocity,        X-Y-Z */
 214     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 215     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 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     /* replace with cockpit brake handle connection code */
 239     percent_brake[1] = Brake_pct;
 240     percent_brake[2] = percent_brake[1];
 241
 242     caster_angle_rad[0] = 0.03*Rudder_pedal;
 243
 244     for (i=0;i<num_wheels;i++)      /* Loop for each wheel */
 245     {
 246         /*========================================*/
 247         /* Calculate wheel position w.r.t. runway */
 248         /*========================================*/
 249
 250             /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 251
 252         sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 253
 254             /* then converting to local (North-East-Down) axes... */
 255
 256         multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 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, temp3b );
 281
 282             /* plus contribution due to cg velocities */
 283
 284         add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
 285
 286
 287         /*===========================================*/
 288         /* Calculate forces & moments for this wheel */
 289         /*===========================================*/
 290
 291             /* Add any anticipation, or frame lead/prediction, here... */
 292
 293                     /* no lead used at present */
 294
 295             /* Calculate sideward and forward velocities of the wheel
 296                     in the runway plane                                 */
 297
 298         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 299         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 300
 301         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 302                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 303         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 304                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 305
 306             /* Calculate normal load force (simple spring constant) */
 307
 308         reaction_normal_force = 0.;
 309         if( d_wheel_rwy_local_v[2] < 0. )
 310         {
 311             reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
 312                                   - v_wheel_local_v[2]*spring_damping[i];
 313             if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 314                 /* to prevent damping component from swamping spring component */
 315         }
 316
 317             /* Calculate friction coefficients */
 318
 319         forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
 320         abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 321         sideward_mu = sliding_mu;
 322         if (abs_v_wheel_sideward < skid_v)
 323             sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 324         if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 325
 326             /* Calculate foreward and sideward reaction forces */
 327
 328         forward_wheel_force  =   forward_mu*reaction_normal_force;
 329         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 330         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 331         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 332
 333             /* Rotate into local (N-E-D) axes */
 334
 335         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 336                           - sideward_wheel_force*sin_wheel_hdg_angle;
 337         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 338                           + sideward_wheel_force*cos_wheel_hdg_angle;
 339         f_wheel_local_v[2] = reaction_normal_force;
 340
 341             /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 342
 343         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 344
 345             /* Calculate moments from force and offsets in body axes */
 346
 347         cross3( d_wheel_cg_body_v, tempF, tempM );
 348
 349         /* Sum forces and moments across all wheels */
 350
 351         add3( tempF, F_gear_v, F_gear_v );
 352         add3( tempM, M_gear_v, M_gear_v );
 353
 354     }
 355 }