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], temp3b[3], tempF[3], tempM[3];
 190     DATA reaction_normal_force;         /* wheel normal (to rwy) force  */
 191     DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;      /* temp storage */
 192     DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
 193     DATA forward_mu, sideward_mu;       /* friction coefficients        */
 194     DATA beta_mu;                       /* breakout friction slope      */
 195     DATA forward_wheel_force, sideward_wheel_force;
 196
 197     int i;                              /* per wheel loop counter */
 198
 199   /*
 200    * Execution starts here
 201    */
 202
 203     beta_mu = max_mu/(skid_v-bkout_v);
 204     clear3( F_gear_v );         /* Initialize sum of forces...  */
 205     clear3( M_gear_v );         /* ...and moments               */
 206
 207   /*
 208    * Put aircraft specific executable code here
 209    */
 210
 211     percent_brake[1] = Brake_pct[0];
 212     percent_brake[2] = Brake_pct[1];
 213
 214     caster_angle_rad[0] =
 215         (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
 216
 217
 218         for (i=0;i<MAX_GEAR;i++)            /* Loop for each wheel */
 219     {
 220                                 // Execute only if the gear has been defined
 221       if (!gear_model[i])
 222         continue;
 223
 224                 /* printf("%s:\n",gear_strings[i]); */
 225
 226
 227
 228                 /*========================================*/
 229                 /* Calculate wheel position w.r.t. runway */
 230                 /*========================================*/
 231
 232
 233                 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
 234
 235
 236                         /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
 237
 238                 sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
 239
 240                 /* then converting to local (North-East-Down) axes... */
 241
 242                 multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
 243
 244
 245                 /* Runway axes correction - third element is Altitude, not (-)Z... */
 246
 247                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
 248
 249                 /* Add wheel offset to cg location in local axes */
 250
 251                 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
 252
 253                 /* remove Runway axes correction so right hand rule applies */
 254
 255                 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
 256
 257                 /*============================*/
 258                 /* Calculate wheel velocities */
 259                 /*============================*/
 260
 261                 /* contribution due to angular rates */
 262
 263                 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
 264
 265                 /* transform into local axes */
 266
 267                 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
 268
 269                 /* plus contribution due to cg velocities */
 270
 271                 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
 272
 273                 clear3(f_wheel_local_v);
 274                 reaction_normal_force=0;
 275                 if( HEIGHT_AGL_WHEEL < 0. )
 276                         /*the wheel is underground -- which implies ground contact
 277                           so calculate reaction forces */
 278                         {
 279                         /*===========================================*/
 280                         /* Calculate forces & moments for this wheel */
 281                         /*===========================================*/
 282
 283                         /* Add any anticipation, or frame lead/prediction, here... */
 284
 285                                 /* no lead used at present */
 286
 287                         /* Calculate sideward and forward velocities of the wheel
 288                                 in the runway plane                                     */
 289
 290                         cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
 291                         sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
 292
 293                         v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
 294                                          + v_wheel_local_v[1]*sin_wheel_hdg_angle;
 295                         v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
 296                                          - v_wheel_local_v[0]*sin_wheel_hdg_angle;
 297
 298
 299                 /* Calculate normal load force (simple spring constant) */
 300
 301                 reaction_normal_force = 0.;
 302
 303                 reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
 304                                           - v_wheel_local_v[2]*cgear[i];
 305                         /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
 306
 307                 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
 308                         /* to prevent damping component from swamping spring component */
 309
 310
 311                 /* Calculate friction coefficients */
 312
 313                         if(it_rolls[i])
 314                         {
 315                            forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
 316                            abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
 317                            sideward_mu = sliding_mu[i];
 318                            if (abs_v_wheel_sideward < skid_v)
 319                            sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
 320                            if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
 321                         }
 322                         else
 323                         {
 324                                 forward_mu=sliding_mu[i];
 325                                 sideward_mu=sliding_mu[i];
 326                         }
 327
 328                         /* Calculate foreward and sideward reaction forces */
 329
 330                         forward_wheel_force  =   forward_mu*reaction_normal_force;
 331                         sideward_wheel_force =  sideward_mu*reaction_normal_force;
 332                         if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
 333                         if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
 334 /*                      printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
 335  */
 336                         /* Rotate into local (N-E-D) axes */
 337
 338                         f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
 339                                           - sideward_wheel_force*sin_wheel_hdg_angle;
 340                         f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
 341                                           + sideward_wheel_force*cos_wheel_hdg_angle;
 342                         f_wheel_local_v[2] = reaction_normal_force;
 343
 344                          /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
 345                         mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
 346
 347                         /* Calculate moments from force and offsets in body axes */
 348
 349                         cross3( d_wheel_cg_body_v, tempF, tempM );
 350
 351                         /* Sum forces and moments across all wheels */
 352
 353                         add3( tempF, F_gear_v, F_gear_v );
 354                         add3( tempM, M_gear_v, M_gear_v );
 355
 356
 357                         }
 358
 359
 360
 361                 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
 362
 363                 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
 364                 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
 365
 366
 367     }
 368 }
 369
 370 // end uiuc_engine.cpp