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