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 #if 0
 279           static const SGPropertyNode * gear_wow
 280             = fgGetNode("/gear/gear[0]/wow", false);
 281           static const SGPropertyNode * gear_wow1
 282             = fgGetNode("/gear/gear[1]/wow", false);
 283           static const SGPropertyNode * gear_wow2
 284             = fgGetNode("/gear/gear[2]/wow", false);
 285 #endif
 286           fgSetBool("/gear/gear[0]/wow", false);
 287           fgSetBool("/gear/gear[1]/wow", false);
 288           fgSetBool("/gear/gear[2]/wow", false);
 289           if( HEIGHT_AGL_WHEEL < 0. )
 290             /*the wheel is underground -- which implies ground contact
 291               so calculate reaction forces */
 292             {
 293               //set the property - weight on wheels
 294               //          if (i==0)
 295               //            {
 296               //              fgSetBool("/gear/gear[0]/wow", true);
 297               //            }
 298               //          if (i==1)
 299               //            {
 300               //              fgSetBool("/gear/gear[1]/wow", true);
 301               //            }
 302               //          if (i==2)
 303               //            {
 304               //              fgSetBool("/gear/gear[2]/wow", true);
 305               //            }
 306
 307               /*===========================================*/
 308               /* Calculate forces & moments for this wheel */
 309               /*===========================================*/
 310
 311               /* Add any anticipation, or frame lead/prediction, here... */
 312
 313                                 /* no lead used at present */
 314
 315               /* Calculate sideward and forward velocities of the wheel
 316                  in the runway plane                                    */
 317
 318               cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 319               sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 320
 321               v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 322                 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 323               v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 324                 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 325
 326
 327               /* Calculate normal load force (simple spring constant) */
 328
 329               reaction_normal_force = 0.;
 330
 331               reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
 332                 - v_wheel_local_v[2]*cgear[i];
 333               /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 334
 335               if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 336               /* to prevent damping component from swamping spring component */
 337
 338
 339               /* Calculate friction coefficients */
 340
 341               if(it_rolls[i])
 342                 {
 343                   forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
 344                   abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 345                   sideward_mu = sliding_mu[i];
 346                   if (abs_v_wheel_sideward < skid_v)
 347                     sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 348                   if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 349                 }
 350               else
 351                 {
 352                   forward_mu=sliding_mu[i];
 353                   sideward_mu=sliding_mu[i];
 354                 }
 355
 356               /* Calculate foreward and sideward reaction forces */
 357
 358               forward_wheel_force  =   forward_mu*reaction_normal_force;
 359               sideward_wheel_force =  sideward_mu*reaction_normal_force;
 360               if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 361               if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 362               /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 363                */
 364               /* Rotate into local (N-E-D) axes */
 365
 366               f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 367                 - sideward_wheel_force*sin_wheel_hdg_angle;
 368               f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 369                 + sideward_wheel_force*cos_wheel_hdg_angle;
 370               f_wheel_local_v[2] = reaction_normal_force;
 371
 372               /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 373               mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 374
 375               /* Calculate moments from force and offsets in body axes */
 376
 377               cross3( d_wheel_cg_body_v, tempF, tempM );
 378
 379               /* Sum forces and moments across all wheels */
 380               if (tempF[0] != 0.0 || tempF[1] != 0.0 || tempF[2] != 0.0) {
 381                 fgSetBool("/gear/gear[1]/wow", true);
 382               }
 383
 384               add3( tempF, F_gear_v, F_gear_v );
 385               add3( tempM, M_gear_v, M_gear_v );
 386
 387             }
 388         }
 389
 390
 391
 392       /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
 393
 394       /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 395         printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 396
 397
 398     }
 399 }
 400
 401 // end uiuc_engine.cpp