src/FDM/LaRCsim/cherokee_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  2001/07/30 20:53:54  curt
  40 Various MSVC tweaks and warning fixes.
  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
  65 ----------------------------------------------------------------------------
  66
  67         REFERENCES:
  68
  69 ----------------------------------------------------------------------------
  70
  71         CALLED BY:
  72
  73 ----------------------------------------------------------------------------
  74
  75         CALLS TO:
  76
  77 ----------------------------------------------------------------------------
  78
  79         INPUTS:
  80
  81 ----------------------------------------------------------------------------
  82
  83         OUTPUTS:
  84
  85 --------------------------------------------------------------------------*/
  86 #include <math.h>
  87 #include "ls_types.h"
  88 #include "ls_constants.h"
  89 #include "ls_generic.h"
  90 #include "ls_cockpit.h"
  91
  92
  93 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
  94 {
  95     result[0] = v1[0] - v2[0];
  96     result[1] = v1[1] - v2[1];
  97     result[2] = v1[2] - v2[2];
  98 }
  99
 100 static void add3( 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 static void cross3( DATA v1[],  DATA v2[], DATA result[] )
 108 {
 109     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
 110     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
 111     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
 112 }
 113
 114 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 115 {
 116     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 117     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 118     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 119 }
 120
 121 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 122 {
 123     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 124     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 125     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 126 }
 127
 128 static void clear3( DATA v[] )
 129 {
 130     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 131 }
 132
 133 void cherokee_gear()
 134 {
 135 char rcsid[] = "$Id$";
 136
 137   /*
 138    * Aircraft specific initializations and data goes here
 139    */
 140
 141 #define NUM_WHEELS 3
 142
 143     static int num_wheels = NUM_WHEELS;             /* number of wheels  */
 144     static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations */
 145     {
 146         { 10.,  0., 4. },                               /* in feet */
 147         { -1.,  3., 4. },
 148         { -1., -3., 4. }
 149     };
 150     static DATA spring_constant[NUM_WHEELS] =       /* springiness, lbs/ft */
 151         { 1500., 5000., 5000. };
 152     static DATA spring_damping[NUM_WHEELS] =        /* damping, lbs/ft/sec */
 153         { 100.,  150.,  150. };
 154     static DATA percent_brake[NUM_WHEELS] =         /* percent applied braking */
 155         { 0.,  0.,  0. };                           /* 0 = none, 1 = full */
 156     static DATA caster_angle_rad[NUM_WHEELS] =      /* steerable tires - in */
 157         { 0., 0., 0.};                              /* radians, +CW */
 158   /*
 159    * End of aircraft specific code
 160    */
 161
 162   /*
 163    * Constants & coefficients for tyres on tarmac - ref [1]
 164    */
 165
 166     /* skid function looks like:
 167      *
 168      *           mu  ^
 169      *               |
 170      *       max_mu  |       +
 171      *               |      /|
 172      *   sliding_mu  |     / +------
 173      *               |    /
 174      *               |   /
 175      *               +--+------------------------>
 176      *               |  |    |      sideward V
 177      *               0 bkout skid
 178      *                 V     V
 179      */
 180
 181
 182     static DATA sliding_mu   = 0.5;
 183     static DATA rolling_mu   = 0.01;
 184     static DATA max_brake_mu = 0.6;
 185     static DATA max_mu       = 0.8;
 186     static DATA bkout_v      = 0.1;
 187     static DATA skid_v       = 1.0;
 188   /*
 189    * Local data variables
 190    */
 191
 192     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 193     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 194     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 195     // DATA v_wheel_body_v[3];          /* wheel velocity,        X-Y-Z */
 196     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 197     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 198     DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
 199     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 200     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 201     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 202     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 203     DATA beta_mu;                       /* breakout friction slope      */
 204     DATA forward_wheel_force, sideward_wheel_force;
 205
 206     int i;                              /* per wheel loop counter */
 207
 208   /*
 209    * Execution starts here
 210    */
 211
 212     beta_mu = max_mu/(skid_v-bkout_v);
 213     clear3( F_gear_v );         /* Initialize sum of forces...  */
 214     clear3( M_gear_v );         /* ...and moments               */
 215
 216   /*
 217    * Put aircraft specific executable code here
 218    */
 219
 220     percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
 221     percent_brake[2] = percent_brake[1];
 222
 223     caster_angle_rad[0] = 0.03*Rudder_pedal;
 224
 225     for (i=0;i<num_wheels;i++)      /* Loop for each wheel */
 226     {
 227         /*========================================*/
 228         /* Calculate wheel position w.r.t. runway */
 229         /*========================================*/
 230
 231             /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 232
 233         sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 234
 235             /* then converting to local (North-East-Down) axes... */
 236
 237         multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 238
 239             /* Runway axes correction - third element is Altitude, not (-)Z... */
 240
 241         d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 242
 243             /* Add wheel offset to cg location in local axes */
 244
 245         add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 246
 247             /* remove Runway axes correction so right hand rule applies */
 248
 249         d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 250
 251         /*============================*/
 252         /* Calculate wheel velocities */
 253         /*============================*/
 254
 255             /* contribution due to angular rates */
 256
 257         cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 258
 259             /* transform into local axes */
 260
 261         multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
 262
 263             /* plus contribution due to cg velocities */
 264
 265         add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
 266
 267
 268         /*===========================================*/
 269         /* Calculate forces & moments for this wheel */
 270         /*===========================================*/
 271
 272             /* Add any anticipation, or frame lead/prediction, here... */
 273
 274                     /* no lead used at present */
 275
 276             /* Calculate sideward and forward velocities of the wheel
 277                     in the runway plane                                 */
 278
 279         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 280         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 281
 282         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 283                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 284         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 285                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 286
 287             /* Calculate normal load force (simple spring constant) */
 288
 289         reaction_normal_force = 0.;
 290         if( d_wheel_rwy_local_v[2] < 0. )
 291         {
 292             reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
 293                                   - v_wheel_local_v[2]*spring_damping[i];
 294             if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 295                 /* to prevent damping component from swamping spring component */
 296         }
 297
 298             /* Calculate friction coefficients */
 299
 300         forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
 301         abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 302         sideward_mu = sliding_mu;
 303         if (abs_v_wheel_sideward < skid_v)
 304             sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 305         if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 306
 307             /* Calculate foreward and sideward reaction forces */
 308
 309         forward_wheel_force  =   forward_mu*reaction_normal_force;
 310         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 311         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 312         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 313
 314             /* Rotate into local (N-E-D) axes */
 315
 316         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 317                           - sideward_wheel_force*sin_wheel_hdg_angle;
 318         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 319                           + sideward_wheel_force*cos_wheel_hdg_angle;
 320         f_wheel_local_v[2] = reaction_normal_force;
 321
 322             /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 323
 324         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 325
 326             /* Calculate moments from force and offsets in body axes */
 327
 328         cross3( d_wheel_cg_body_v, tempF, tempM );
 329
 330         /* Sum forces and moments across all wheels */
 331
 332         add3( tempF, F_gear_v, F_gear_v );
 333         add3( tempM, M_gear_v, M_gear_v );
 334
 335     }
 336 }