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
  67 #include "uiuc_gear.h"
  68
  69 #if !defined (SG_HAVE_NATIVE_SGI_COMPILERS)
  70 SG_USING_STD(cerr);
  71 #endif
  72
  73
  74 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
  75
  76
  77 static void sub3( DATA v1[],  DATA v2[], DATA result[] )
  78 {
  79     result[0] = v1[0] - v2[0];
  80     result[1] = v1[1] - v2[1];
  81     result[2] = v1[2] - v2[2];
  82 }
  83
  84 static void add3( 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 cross3( DATA v1[],  DATA v2[], DATA result[] )
  92 {
  93     result[0] = v1[1]*v2[2] - v1[2]*v2[1];
  94     result[1] = v1[2]*v2[0] - v1[0]*v2[2];
  95     result[2] = v1[0]*v2[1] - v1[1]*v2[0];
  96 }
  97
  98 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
  99 {
 100     result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
 101     result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
 102     result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
 103 }
 104
 105 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
 106 {
 107     result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
 108     result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
 109     result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
 110 }
 111
 112 static void clear3( DATA v[] )
 113 {
 114     v[0] = 0.; v[1] = 0.; v[2] = 0.;
 115 }
 116
 117 void uiuc_gear()
 118 {
 119
 120   /*
 121    * Aircraft specific initializations and data goes here
 122    */
 123
 124     static DATA percent_brake[MAX_GEAR] =           /* percent applied braking */
 125         { 0.,  0.,  0., 0.,
 126           0.,  0.,  0., 0.,
 127           0.,  0.,  0., 0.,
 128           0.,  0.,  0., 0. };                       /* 0 = none, 1 = full */
 129     static DATA caster_angle_rad[MAX_GEAR] =        /* steerable tires - in */
 130         { 0., 0., 0., 0.,
 131           0., 0., 0., 0.,
 132           0., 0., 0., 0.,
 133           0., 0., 0., 0. };                         /* radians, +CW */
 134   /*
 135    * End of aircraft specific code
 136    */
 137
 138   /*
 139    * Constants & coefficients for tyres on tarmac - ref [1]
 140    */
 141
 142     /* skid function looks like:
 143      *
 144      *           mu  ^
 145      *               |
 146      *       max_mu  |       +
 147      *               |      /|
 148      *   sliding_mu  |     / +------
 149      *               |    /
 150      *               |   /
 151      *               +--+------------------------>
 152      *               |  |    |      sideward V
 153      *               0 bkout skid
 154      *                 V     V
 155      */
 156
 157
 158     static int it_rolls[MAX_GEAR] =
 159     { 1, 1, 1, 0,
 160       0, 0, 0, 0,
 161       0, 0, 0, 0,
 162       0, 0, 0, 0 };
 163     static DATA sliding_mu[MAX_GEAR] =
 164     { 0.5, 0.5, 0.5, 0.3,
 165       0.3, 0.3, 0.3, 0.3,
 166       0.3, 0.3, 0.3, 0.3,
 167       0.3, 0.3, 0.3, 0.3 };
 168     static DATA max_brake_mu[MAX_GEAR] =
 169     { 0.0, 0.6, 0.6, 0.0,
 170       0.0, 0.0, 0.0, 0.0,
 171       0.0, 0.0, 0.0, 0.0,
 172       0.0, 0.0, 0.0, 0.0 };
 173     static DATA max_mu       = 0.8;
 174     static DATA bkout_v      = 0.1;
 175     static DATA skid_v       = 1.0;
 176   /*
 177    * Local data variables
 178    */
 179
 180     DATA d_wheel_cg_body_v[3];          /* wheel offset from cg,  X-Y-Z */
 181     DATA d_wheel_cg_local_v[3];         /* wheel offset from cg,  N-E-D */
 182     DATA d_wheel_rwy_local_v[3];        /* wheel offset from rwy, N-E-U */
 183         DATA v_wheel_cg_local_v[3];    /*wheel velocity rel to cg N-E-D*/
 184     // DATA v_wheel_body_v[3];          /* wheel velocity,        X-Y-Z */
 185     DATA v_wheel_local_v[3];            /* wheel velocity,        N-E-D */
 186     DATA f_wheel_local_v[3];            /* wheel reaction force,  N-E-D */
 187     // DATA altitude_local_v[3];       /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
 188     // DATA altitude_body_v[3];        /*altitude vector in body (X,Y,Z)*/
 189     DATA temp3a[3];
 190     // DATA temp3b[3];
 191     DATA tempF[3];
 192     DATA tempM[3];
 193     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 194     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 195     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 196     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 197     DATA beta_mu;                       /* breakout friction slope      */
 198     DATA forward_wheel_force, sideward_wheel_force;
 199
 200     int i;                              /* per wheel loop counter */
 201
 202   /*
 203    * Execution starts here
 204    */
 205
 206     beta_mu = max_mu/(skid_v-bkout_v);
 207     clear3( F_gear_v );         /* Initialize sum of forces...  */
 208     clear3( M_gear_v );         /* ...and moments               */
 209
 210   /*
 211    * Put aircraft specific executable code here
 212    */
 213
 214     percent_brake[1] = Brake_pct[0];
 215     percent_brake[2] = Brake_pct[1];
 216
 217     caster_angle_rad[0] =
 218         (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
 219
 220
 221         for (i=0;i<MAX_GEAR;i++)            /* Loop for each wheel */
 222     {
 223                                 // Execute only if the gear has been defined
 224       if (!gear_model[i])
 225         continue;
 226
 227                 /* printf("%s:\n",gear_strings[i]); */
 228
 229
 230
 231                 /*========================================*/
 232                 /* Calculate wheel position w.r.t. runway */
 233                 /*========================================*/
 234
 235
 236                 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 237
 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( HEIGHT_AGL_WHEEL < 0. )
 279                         /*the wheel is underground -- which implies ground contact
 280                           so calculate reaction forces */
 281                         {
 282                         /*===========================================*/
 283                         /* Calculate forces & moments for this wheel */
 284                         /*===========================================*/
 285
 286                         /* Add any anticipation, or frame lead/prediction, here... */
 287
 288                                 /* no lead used at present */
 289
 290                         /* Calculate sideward and forward velocities of the wheel
 291                                 in the runway plane                                     */
 292
 293                         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 294                         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 295
 296                         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 297                                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 298                         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 299                                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 300
 301
 302                 /* Calculate normal load force (simple spring constant) */
 303
 304                 reaction_normal_force = 0.;
 305
 306                 reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
 307                                           - v_wheel_local_v[2]*cgear[i];
 308                         /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 309
 310                 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 311                         /* to prevent damping component from swamping spring component */
 312
 313
 314                 /* Calculate friction coefficients */
 315
 316                         if(it_rolls[i])
 317                         {
 318                            forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
 319                            abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 320                            sideward_mu = sliding_mu[i];
 321                            if (abs_v_wheel_sideward < skid_v)
 322                            sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 323                            if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 324                         }
 325                         else
 326                         {
 327                                 forward_mu=sliding_mu[i];
 328                                 sideward_mu=sliding_mu[i];
 329                         }
 330
 331                         /* Calculate foreward and sideward reaction forces */
 332
 333                         forward_wheel_force  =   forward_mu*reaction_normal_force;
 334                         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 335                         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 336                         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 337 /*                      printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 338  */
 339                         /* Rotate into local (N-E-D) axes */
 340
 341                         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 342                                           - sideward_wheel_force*sin_wheel_hdg_angle;
 343                         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 344                                           + sideward_wheel_force*cos_wheel_hdg_angle;
 345                         f_wheel_local_v[2] = reaction_normal_force;
 346
 347                          /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 348                         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 349
 350                         /* Calculate moments from force and offsets in body axes */
 351
 352                         cross3( d_wheel_cg_body_v, tempF, tempM );
 353
 354                         /* Sum forces and moments across all wheels */
 355
 356                         add3( tempF, F_gear_v, F_gear_v );
 357                         add3( tempM, M_gear_v, M_gear_v );
 358
 359
 360                         }
 361
 362
 363
 364                 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
 365
 366                 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 367                 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 368
 369
 370     }
 371 }
 372
 373 // end uiuc_engine.cpp