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