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