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