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