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