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