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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  61  USA or view http://www.gnu.org/copyleft/gpl.html.
  62
  63 **********************************************************************/
  64
  65 #include <simgear/compiler.h>
  66 #include <simgear/misc/sg_path.hxx>
  67 #include <Aircraft/aircraft.hxx>
  68 #include <Main/fg_props.hxx>
  69
  70 #include "uiuc_gear.h"
  71
  72
  73
  74 SG_USING_STD(cerr);
  75
  76
  77 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  78
  79
  80 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
  81 {
  82   result[0] = v1[0] - v2[0];
  83   result[1] = v1[1] - v2[1];
  84   result[2] = v1[2] - v2[2];
  85 }
  86
  87 static void add3( DATA v1[],  DATA v2[], DATA result[] )
  88 {
  89   result[0] = v1[0] + v2[0];
  90   result[1] = v1[1] + v2[1];
  91   result[2] = v1[2] + v2[2];
  92 }
  93
  94 static void cross3( DATA v1[],  DATA v2[], DATA result[] )
  95 {
  96   result[0] = v1[1]*v2[2] - v1[2]*v2[1];
  97   result[1] = v1[2]*v2[0] - v1[0]*v2[2];
  98   result[2] = v1[0]*v2[1] - v1[1]*v2[0];
  99 }
 100
 101 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
 102 {
 103   result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 104   result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 105   result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 106 }
 107
 108 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 109 {
 110   result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 111   result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 112   result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 113 }
 114
 115 static void clear3( DATA v[] )
 116 {
 117   v[0] = 0.; v[1] = 0.; v[2] = 0.;
 118 }
 119
 120 void uiuc_gear()
 121 {
 122
 123   /*
 124    * Aircraft specific initializations and data goes here
 125    */
 126
 127   static DATA percent_brake[MAX_GEAR] =     /* percent applied braking */
 128   { 0.,  0.,  0., 0.,
 129     0.,  0.,  0., 0.,
 130     0.,  0.,  0., 0.,
 131     0.,  0.,  0., 0. };                     /* 0 = none, 1 = full */
 132   static DATA caster_angle_rad[MAX_GEAR] =          /* steerable tires - in */
 133   { 0., 0., 0., 0.,
 134     0., 0., 0., 0.,
 135     0., 0., 0., 0.,
 136     0., 0., 0., 0. };                         /* radians, +CW */
 137   /*
 138    * End of aircraft specific code
 139    */
 140
 141   /*
 142    * Constants & coefficients for tyres on tarmac - ref [1]
 143    */
 144
 145   /* skid function looks like:
 146    *
 147    *           mu  ^
 148    *               |
 149    *       max_mu  |       +
 150    *               |      /|
 151    *   sliding_mu  |     / +------
 152    *               |    /
 153    *               |   /
 154    *               +--+------------------------>
 155    *               |  |    |      sideward V
 156    *               0 bkout skid
 157    *                   V     V
 158    */
 159
 160
 161   static int it_rolls[MAX_GEAR] =
 162   { 1, 1, 1, 0,
 163     0, 0, 0, 0,
 164     0, 0, 0, 0,
 165     0, 0, 0, 0 };
 166   static DATA sliding_mu[MAX_GEAR] =
 167   { 0.5, 0.5, 0.5, 0.3,
 168     0.3, 0.3, 0.3, 0.3,
 169     0.3, 0.3, 0.3, 0.3,
 170     0.3, 0.3, 0.3, 0.3 };
 171   static DATA max_brake_mu[MAX_GEAR] =
 172   { 0.0, 0.6, 0.6, 0.0,
 173     0.0, 0.0, 0.0, 0.0,
 174     0.0, 0.0, 0.0, 0.0,
 175     0.0, 0.0, 0.0, 0.0 };
 176   static DATA max_mu         = 0.8;
 177   static DATA bkout_v        = 0.1;
 178   static DATA skid_v       = 1.0;
 179   /*
 180    * Local data variables
 181    */
 182
 183   DATA d_wheel_cg_body_v[3];            /* wheel offset from cg,  X-Y-Z */
 184   DATA d_wheel_cg_local_v[3];           /* wheel offset from cg,  N-E-D */
 185   DATA d_wheel_rwy_local_v[3];  /* wheel offset from rwy, N-E-U */
 186   DATA v_wheel_cg_local_v[3];    /*wheel velocity rel to cg N-E-D*/
 187   // DATA v_wheel_body_v[3];            /* wheel velocity,        X-Y-Z */
 188   DATA v_wheel_local_v[3];              /* wheel velocity,        N-E-D */
 189   DATA f_wheel_local_v[3];              /* wheel reaction force,  N-E-D */
 190   // DATA altitude_local_v[3];       /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
 191   // DATA altitude_body_v[3];        /*altitude vector in body (X,Y,Z)*/
 192   DATA temp3a[3];
 193   // DATA temp3b[3];
 194   DATA tempF[3];
 195   DATA 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] = Brake_pct[0];
 218   percent_brake[2] = Brake_pct[1];
 219
 220   caster_angle_rad[0] =
 221     (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
 222
 223
 224   for (i=0;i<MAX_GEAR;i++)          /* Loop for each wheel */
 225     {
 226       // Execute only if the gear has been defined
 227       if (!gear_model[i])
 228         {
 229           // do nothing
 230           continue;
 231         }
 232       else
 233         {
 234
 235           /*========================================*/
 236           /* Calculate wheel position w.r.t. runway */
 237           /*========================================*/
 238
 239           /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 240
 241           /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 242
 243           sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 244
 245           /* then converting to local (North-East-Down) axes... */
 246
 247           multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 248
 249
 250           /* Runway axes correction - third element is Altitude, not (-)Z... */
 251
 252           d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 253
 254           /* Add wheel offset to cg location in local axes */
 255
 256           add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 257
 258           /* remove Runway axes correction so right hand rule applies */
 259
 260           d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 261
 262           /*============================*/
 263           /* Calculate wheel velocities */
 264           /*============================*/
 265
 266           /* contribution due to angular rates */
 267
 268           cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 269
 270           /* transform into local axes */
 271
 272           multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
 273
 274           /* plus contribution due to cg velocities */
 275
 276           add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
 277
 278           clear3(f_wheel_local_v);
 279           reaction_normal_force=0;
 280           static const SGPropertyNode * gear_wow
 281             = fgGetNode("/gear/gear[0]/wow", false);
 282           static const SGPropertyNode * gear_wow1
 283             = fgGetNode("/gear/gear[1]/wow", false);
 284           static const SGPropertyNode * gear_wow2
 285             = fgGetNode("/gear/gear[2]/wow", false);
 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) {
 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