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