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 <Aircraft/aircraft.hxx>
  71 #include <Main/fg_props.hxx>
  72
  73 #include "uiuc_gear.h"
  74
  75 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  76
  77
  78 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
  79 {
  80   result[0] = v1[0] - v2[0];
  81   result[1] = v1[1] - v2[1];
  82   result[2] = v1[2] - v2[2];
  83 }
  84
  85 static void add3( DATA v1[],  DATA v2[], DATA result[] )
  86 {
  87   result[0] = v1[0] + v2[0];
  88   result[1] = v1[1] + v2[1];
  89   result[2] = v1[2] + v2[2];
  90 }
  91
  92 static void cross3( DATA v1[],  DATA v2[], DATA result[] )
  93 {
  94   result[0] = v1[1]*v2[2] - v1[2]*v2[1];
  95   result[1] = v1[2]*v2[0] - v1[0]*v2[2];
  96   result[2] = v1[0]*v2[1] - v1[1]*v2[0];
  97 }
  98
  99 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 100 {
 101   result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 102   result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 103   result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 104 }
 105
 106 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 107 {
 108   result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 109   result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 110   result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 111 }
 112
 113 static void clear3( DATA v[] )
 114 {
 115   v[0] = 0.; v[1] = 0.; v[2] = 0.;
 116 }
 117
 118 void uiuc_gear()
 119 {
 120
 121   /*
 122    * Aircraft specific initializations and data goes here
 123    */
 124
 125   static DATA percent_brake[MAX_GEAR] =     /* percent applied braking */
 126   { 0.,  0.,  0., 0.,
 127     0.,  0.,  0., 0.,
 128     0.,  0.,  0., 0.,
 129     0.,  0.,  0., 0. };                     /* 0 = none, 1 = full */
 130   static DATA caster_angle_rad[MAX_GEAR] =          /* steerable tires - in */
 131   { 0., 0., 0., 0.,
 132     0., 0., 0., 0.,
 133     0., 0., 0., 0.,
 134     0., 0., 0., 0. };                         /* radians, +CW */
 135   /*
 136    * End of aircraft specific code
 137    */
 138
 139   /*
 140    * Constants & coefficients for tyres on tarmac - ref [1]
 141    */
 142
 143   /* skid function looks like:
 144    *
 145    *           mu  ^
 146    *               |
 147    *       max_mu  |       +
 148    *               |      /|
 149    *   sliding_mu  |     / +------
 150    *               |    /
 151    *               |   /
 152    *               +--+------------------------>
 153    *               |  |    |      sideward V
 154    *               0 bkout skid
 155    *                   V     V
 156    */
 157
 158
 159   static int it_rolls[MAX_GEAR] =
 160   { 1, 1, 1, 0,
 161     0, 0, 0, 0,
 162     0, 0, 0, 0,
 163     0, 0, 0, 0 };
 164   static DATA sliding_mu[MAX_GEAR] =
 165   { 0.5, 0.5, 0.5, 0.3,
 166     0.3, 0.3, 0.3, 0.3,
 167     0.3, 0.3, 0.3, 0.3,
 168     0.3, 0.3, 0.3, 0.3 };
 169   static DATA max_brake_mu[MAX_GEAR] =
 170   { 0.0, 0.6, 0.6, 0.0,
 171     0.0, 0.0, 0.0, 0.0,
 172     0.0, 0.0, 0.0, 0.0,
 173     0.0, 0.0, 0.0, 0.0 };
 174   static DATA max_mu         = 0.8;
 175   static DATA bkout_v        = 0.1;
 176   static DATA skid_v       = 1.0;
 177   /*
 178    * Local data variables
 179    */
 180
 181   DATA d_wheel_cg_body_v[3];            /* wheel offset from cg,  X-Y-Z */
 182   DATA d_wheel_cg_local_v[3];           /* wheel offset from cg,  N-E-D */
 183   DATA d_wheel_rwy_local_v[3];  /* wheel offset from rwy, N-E-U */
 184   DATA v_wheel_cg_local_v[3];    /*wheel velocity rel to cg N-E-D*/
 185   // DATA v_wheel_body_v[3];            /* wheel velocity,        X-Y-Z */
 186   DATA v_wheel_local_v[3];              /* wheel velocity,        N-E-D */
 187   DATA f_wheel_local_v[3];              /* wheel reaction force,  N-E-D */
 188   // DATA altitude_local_v[3];       /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
 189   // DATA altitude_body_v[3];        /*altitude vector in body (X,Y,Z)*/
 190   DATA temp3a[3];
 191   // DATA temp3b[3];
 192   DATA tempF[3];
 193   DATA tempM[3];
 194   DATA reaction_normal_force;           /* wheel normal (to rwy) force  */
 195   DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;        /* temp storage */
 196   DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 197   DATA forward_mu, sideward_mu; /* friction coefficients        */
 198   DATA beta_mu;                 /* breakout friction slope      */
 199   DATA forward_wheel_force, sideward_wheel_force;
 200
 201   int i;                                /* per wheel loop counter */
 202
 203   /*
 204    * Execution starts here
 205    */
 206
 207   beta_mu = max_mu/(skid_v-bkout_v);
 208   clear3( F_gear_v );           /* Initialize sum of forces...  */
 209   clear3( M_gear_v );           /* ...and moments               */
 210
 211   /*
 212    * Put aircraft specific executable code here
 213    */
 214
 215   percent_brake[1] = Brake_pct[0];
 216   percent_brake[2] = Brake_pct[1];
 217
 218   caster_angle_rad[0] =
 219     (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
 220
 221
 222   for (i=0;i<MAX_GEAR;i++)          /* Loop for each wheel */
 223     {
 224       // Execute only if the gear has been defined
 225       if (!gear_model[i])
 226         {
 227           // do nothing
 228           continue;
 229         }
 230       else
 231         {
 232
 233           /*========================================*/
 234           /* Calculate wheel position w.r.t. runway */
 235           /*========================================*/
 236
 237           /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 238
 239           /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 240
 241           sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 242
 243           /* then converting to local (North-East-Down) axes... */
 244
 245           multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 246
 247
 248           /* Runway axes correction - third element is Altitude, not (-)Z... */
 249
 250           d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 251
 252           /* Add wheel offset to cg location in local axes */
 253
 254           add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 255
 256           /* remove Runway axes correction so right hand rule applies */
 257
 258           d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 259
 260           /*============================*/
 261           /* Calculate wheel velocities */
 262           /*============================*/
 263
 264           /* contribution due to angular rates */
 265
 266           cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 267
 268           /* transform into local axes */
 269
 270           multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
 271
 272           /* plus contribution due to cg velocities */
 273
 274           add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
 275
 276           clear3(f_wheel_local_v);
 277           reaction_normal_force=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           fgSetBool("/gear/gear[0]/wow", false);
 285           fgSetBool("/gear/gear[1]/wow", false);
 286           fgSetBool("/gear/gear[2]/wow", false);
 287           if( HEIGHT_AGL_WHEEL < 0. )
 288             /*the wheel is underground -- which implies ground contact
 289               so calculate reaction forces */
 290             {
 291               //set the property - weight on wheels
 292               //          if (i==0)
 293               //            {
 294               //              fgSetBool("/gear/gear[0]/wow", true);
 295               //            }
 296               //          if (i==1)
 297               //            {
 298               //              fgSetBool("/gear/gear[1]/wow", true);
 299               //            }
 300               //          if (i==2)
 301               //            {
 302               //              fgSetBool("/gear/gear[2]/wow", true);
 303               //            }
 304
 305               /*===========================================*/
 306               /* Calculate forces & moments for this wheel */
 307               /*===========================================*/
 308
 309               /* Add any anticipation, or frame lead/prediction, here... */
 310
 311                                 /* no lead used at present */
 312
 313               /* Calculate sideward and forward velocities of the wheel
 314                  in the runway plane                                    */
 315
 316               cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 317               sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 318
 319               v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 320                 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 321               v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 322                 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 323
 324
 325               /* Calculate normal load force (simple spring constant) */
 326
 327               reaction_normal_force = 0.;
 328
 329               reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
 330                 - v_wheel_local_v[2]*cgear[i];
 331               /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 332
 333               if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 334               /* to prevent damping component from swamping spring component */
 335
 336
 337               /* Calculate friction coefficients */
 338
 339               if(it_rolls[i])
 340                 {
 341                   forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
 342                   abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 343                   sideward_mu = sliding_mu[i];
 344                   if (abs_v_wheel_sideward < skid_v)
 345                     sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 346                   if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 347                 }
 348               else
 349                 {
 350                   forward_mu=sliding_mu[i];
 351                   sideward_mu=sliding_mu[i];
 352                 }
 353
 354               /* Calculate foreward and sideward reaction forces */
 355
 356               forward_wheel_force  =   forward_mu*reaction_normal_force;
 357               sideward_wheel_force =  sideward_mu*reaction_normal_force;
 358               if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 359               if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 360               /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 361                */
 362               /* Rotate into local (N-E-D) axes */
 363
 364               f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 365                 - sideward_wheel_force*sin_wheel_hdg_angle;
 366               f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 367                 + sideward_wheel_force*cos_wheel_hdg_angle;
 368               f_wheel_local_v[2] = reaction_normal_force;
 369
 370               /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 371               mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 372
 373               /* Calculate moments from force and offsets in body axes */
 374
 375               cross3( d_wheel_cg_body_v, tempF, tempM );
 376
 377               /* Sum forces and moments across all wheels */
 378               if (tempF) {
 379                 fgSetBool("/gear/gear[1]/wow", true);
 380               }
 381
 382               add3( tempF, F_gear_v, F_gear_v );
 383               add3( tempM, M_gear_v, M_gear_v );
 384
 385             }
 386         }
 387
 388
 389
 390       /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
 391
 392       /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 393         printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 394
 395
 396     }
 397 }
 398
 399 // end uiuc_engine.cpp