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