src/FDM/UIUCModel/uiuc_gear.cpp

   1 /**********************************************************************
   2
   3  FILENAME:     uiuc_gear.cpp
   4
   5 ----------------------------------------------------------------------
   6
   7  DESCRIPTION:  determine the gear forces and moments
   8
   9 ----------------------------------------------------------------------
  10
  11  STATUS:       alpha version
  12
  13 ----------------------------------------------------------------------
  14
  15  REFERENCES:   based on c172_gear by Tony Peden and others
  16
  17 ----------------------------------------------------------------------
  18
  19  HISTORY:      03/09/2001   initial release
  20
  21 ----------------------------------------------------------------------
  22
  23  AUTHOR(S):    David Megginson <david@megginson.com
  24
  25 ----------------------------------------------------------------------
  26
  27  VARIABLES:    [TODO]
  28
  29 ----------------------------------------------------------------------
  30
  31  INPUTS:       [TODO]
  32
  33 ----------------------------------------------------------------------
  34
  35  OUTPUTS:      [TODO]
  36
  37 ----------------------------------------------------------------------
  38
  39  CALLED BY:   uiuc_wrapper.cpp
  40
  41 ----------------------------------------------------------------------
  42
  43  CALLS TO:     none
  44
  45 ----------------------------------------------------------------------
  46
  47  COPYRIGHT:    (c) 2001 by David Megginson
  48
  49  This program is free software; you can redistribute it and/or
  50  modify it under the terms of the GNU General Public License
  51  as published by the Free Software Foundation.
  52
  53  This program is distributed in the hope that it will be useful,
  54  but WITHOUT ANY WARRANTY; without even the implied warranty of
  55  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  56  GNU General Public License for more details.
  57
  58  You should have received a copy of the GNU General Public License
  59  along with this program; if not, write to the Free Software
  60  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  61
  62 **********************************************************************/
  63
  64 #ifdef HAVE_CONFIG_H
  65 #  include <config.h>
  66 #endif
  67
  68 #include <simgear/compiler.h>
  69 #include <simgear/misc/sg_path.hxx>
  70 #include <Main/fg_props.hxx>
  71
  72 #include "uiuc_gear.h"
  73
  74 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  75
  76
  77 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
  78 {
  79   result[0] = v1[0] - v2[0];
  80   result[1] = v1[1] - v2[1];
  81   result[2] = v1[2] - v2[2];
  82 }
  83
  84 static void add3( DATA v1[],  DATA v2[], DATA result[] )
  85 {
  86   result[0] = v1[0] + v2[0];
  87   result[1] = v1[1] + v2[1];
  88   result[2] = v1[2] + v2[2];
  89 }
  90
  91 static void cross3( DATA v1[],  DATA v2[], DATA result[] )
  92 {
  93   result[0] = v1[1]*v2[2] - v1[2]*v2[1];
  94   result[1] = v1[2]*v2[0] - v1[0]*v2[2];
  95   result[2] = v1[0]*v2[1] - v1[1]*v2[0];
  96 }
  97
  98 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
  99 {
 100   result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 101   result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 102   result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 103 }
 104
 105 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 106 {
 107   result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 108   result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 109   result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 110 }
 111
 112 static void clear3( DATA v[] )
 113 {
 114   v[0] = 0.; v[1] = 0.; v[2] = 0.;
 115 }
 116
 117 void uiuc_gear()
 118 {
 119
 120   /*
 121    * Aircraft specific initializations and data goes here
 122    */
 123
 124   static DATA percent_brake[MAX_GEAR] =     /* percent applied braking */
 125   { 0.,  0.,  0., 0.,
 126     0.,  0.,  0., 0.,
 127     0.,  0.,  0., 0.,
 128     0.,  0.,  0., 0. };                     /* 0 = none, 1 = full */
 129   static DATA caster_angle_rad[MAX_GEAR] =          /* steerable tires - in */
 130   { 0., 0., 0., 0.,
 131     0., 0., 0., 0.,
 132     0., 0., 0., 0.,
 133     0., 0., 0., 0. };                         /* radians, +CW */
 134   /*
 135    * End of aircraft specific code
 136    */
 137
 138   /*
 139    * Constants & coefficients for tyres on tarmac - ref [1]
 140    */
 141
 142   /* skid function looks like:
 143    *
 144    *           mu  ^
 145    *               |
 146    *       max_mu  |       +
 147    *               |      /|
 148    *   sliding_mu  |     / +------
 149    *               |    /
 150    *               |   /
 151    *               +--+------------------------>
 152    *               |  |    |      sideward V
 153    *               0 bkout skid
 154    *                   V     V
 155    */
 156
 157
 158   static int it_rolls[MAX_GEAR] =
 159   { 1, 1, 1, 0,
 160     0, 0, 0, 0,
 161     0, 0, 0, 0,
 162     0, 0, 0, 0 };
 163   static DATA sliding_mu[MAX_GEAR] =
 164   { 0.5, 0.5, 0.5, 0.3,
 165     0.3, 0.3, 0.3, 0.3,
 166     0.3, 0.3, 0.3, 0.3,
 167     0.3, 0.3, 0.3, 0.3 };
 168   static DATA max_brake_mu[MAX_GEAR] =
 169   { 0.0, 0.6, 0.6, 0.0,
 170     0.0, 0.0, 0.0, 0.0,
 171     0.0, 0.0, 0.0, 0.0,
 172     0.0, 0.0, 0.0, 0.0 };
 173   static DATA max_mu         = 0.8;
 174   static DATA bkout_v        = 0.1;
 175   static DATA skid_v       = 1.0;
 176   /*
 177    * Local data variables
 178    */
 179
 180   DATA d_wheel_cg_body_v[3];            /* wheel offset from cg,  X-Y-Z */
 181   DATA d_wheel_cg_local_v[3];           /* wheel offset from cg,  N-E-D */
 182   DATA d_wheel_rwy_local_v[3];  /* wheel offset from rwy, N-E-U */
 183   DATA v_wheel_cg_local_v[3];    /*wheel velocity rel to cg N-E-D*/
 184   // DATA v_wheel_body_v[3];            /* wheel velocity,        X-Y-Z */
 185   DATA v_wheel_local_v[3];              /* wheel velocity,        N-E-D */
 186   DATA f_wheel_local_v[3];              /* wheel reaction force,  N-E-D */
 187   // DATA altitude_local_v[3];       /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
 188   // DATA altitude_body_v[3];        /*altitude vector in body (X,Y,Z)*/
 189   DATA temp3a[3];
 190   // DATA temp3b[3];
 191   DATA tempF[3];
 192   DATA tempM[3];
 193   DATA reaction_normal_force;           /* wheel normal (to rwy) force  */
 194   DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;        /* temp storage */
 195   DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 196   DATA forward_mu, sideward_mu; /* friction coefficients        */
 197   DATA beta_mu;                 /* breakout friction slope      */
 198   DATA forward_wheel_force, sideward_wheel_force;
 199
 200   int i;                                /* per wheel loop counter */
 201
 202   /*
 203    * Execution starts here
 204    */
 205
 206   beta_mu = max_mu/(skid_v-bkout_v);
 207   clear3( F_gear_v );           /* Initialize sum of forces...  */
 208   clear3( M_gear_v );           /* ...and moments               */
 209
 210   /*
 211    * Put aircraft specific executable code here
 212    */
 213
 214   percent_brake[1] = Brake_pct[0];
 215   percent_brake[2] = Brake_pct[1];
 216
 217   caster_angle_rad[0] =
 218     (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
 219
 220
 221   for (i=0;i<MAX_GEAR;i++)          /* Loop for each wheel */
 222     {
 223       // Execute only if the gear has been defined
 224       if (!gear_model[i])
 225         {
 226           // do nothing
 227           continue;
 228         }
 229       else
 230         {
 231
 232           /*========================================*/
 233           /* Calculate wheel position w.r.t. runway */
 234           /*========================================*/
 235
 236           /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 237
 238           /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 239
 240           sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 241
 242           /* then converting to local (North-East-Down) axes... */
 243
 244           multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 245
 246
 247           /* Runway axes correction - third element is Altitude, not (-)Z... */
 248
 249           d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 250
 251           /* Add wheel offset to cg location in local axes */
 252
 253           add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 254
 255           /* remove Runway axes correction so right hand rule applies */
 256
 257           d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 258
 259           /*============================*/
 260           /* Calculate wheel velocities */
 261           /*============================*/
 262
 263           /* contribution due to angular rates */
 264
 265           cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 266
 267           /* transform into local axes */
 268
 269           multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
 270
 271           /* plus contribution due to cg velocities */
 272
 273           add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
 274
 275           clear3(f_wheel_local_v);
 276           reaction_normal_force=0;
 277 #if 0
 278           static const SGPropertyNode * gear_wow
 279             = fgGetNode("/gear/gear[0]/wow", false);
 280           static const SGPropertyNode * gear_wow1
 281             = fgGetNode("/gear/gear[1]/wow", false);
 282           static const SGPropertyNode * gear_wow2
 283             = fgGetNode("/gear/gear[2]/wow", false);
 284 #endif
 285           fgSetBool("/gear/gear[0]/wow", false);
 286           fgSetBool("/gear/gear[1]/wow", false);
 287           fgSetBool("/gear/gear[2]/wow", false);
 288           if( HEIGHT_AGL_WHEEL < 0. )
 289             /*the wheel is underground -- which implies ground contact
 290               so calculate reaction forces */
 291             {
 292               //set the property - weight on wheels
 293               //          if (i==0)
 294               //            {
 295               //              fgSetBool("/gear/gear[0]/wow", true);
 296               //            }
 297               //          if (i==1)
 298               //            {
 299               //              fgSetBool("/gear/gear[1]/wow", true);
 300               //            }
 301               //          if (i==2)
 302               //            {
 303               //              fgSetBool("/gear/gear[2]/wow", true);
 304               //            }
 305
 306               /*===========================================*/
 307               /* Calculate forces & moments for this wheel */
 308               /*===========================================*/
 309
 310               /* Add any anticipation, or frame lead/prediction, here... */
 311
 312                                 /* no lead used at present */
 313
 314               /* Calculate sideward and forward velocities of the wheel
 315                  in the runway plane                                    */
 316
 317               cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 318               sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 319
 320               v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 321                 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 322               v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 323                 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 324
 325
 326               /* Calculate normal load force (simple spring constant) */
 327
 328               reaction_normal_force = 0.;
 329
 330               reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
 331                 - v_wheel_local_v[2]*cgear[i];
 332               /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 333
 334               if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 335               /* to prevent damping component from swamping spring component */
 336
 337
 338               /* Calculate friction coefficients */
 339
 340               if(it_rolls[i])
 341                 {
 342                   forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
 343                   abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 344                   sideward_mu = sliding_mu[i];
 345                   if (abs_v_wheel_sideward < skid_v)
 346                     sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 347                   if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 348                 }
 349               else
 350                 {
 351                   forward_mu=sliding_mu[i];
 352                   sideward_mu=sliding_mu[i];
 353                 }
 354
 355               /* Calculate foreward and sideward reaction forces */
 356
 357               forward_wheel_force  =   forward_mu*reaction_normal_force;
 358               sideward_wheel_force =  sideward_mu*reaction_normal_force;
 359               if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 360               if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 361               /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 362                */
 363               /* Rotate into local (N-E-D) axes */
 364
 365               f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 366                 - sideward_wheel_force*sin_wheel_hdg_angle;
 367               f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 368                 + sideward_wheel_force*cos_wheel_hdg_angle;
 369               f_wheel_local_v[2] = reaction_normal_force;
 370
 371               /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 372               mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 373
 374               /* Calculate moments from force and offsets in body axes */
 375
 376               cross3( d_wheel_cg_body_v, tempF, tempM );
 377
 378               /* Sum forces and moments across all wheels */
 379               if (tempF[0] != 0.0 || tempF[1] != 0.0 || tempF[2] != 0.0) {
 380                 fgSetBool("/gear/gear[1]/wow", true);
 381               }
 382
 383               add3( tempF, F_gear_v, F_gear_v );
 384               add3( tempM, M_gear_v, M_gear_v );
 385
 386             }
 387         }
 388
 389
 390
 391       /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
 392
 393       /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 394         printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 395
 396
 397     }
 398 }
 399
 400 // end uiuc_engine.cpp