src/FDM/JSBSim/FGLGear.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGLGear.cpp
   4  Author:       Jon S. Berndt
   5                Norman H. Princen
   6  Date started: 11/18/99
   7  Purpose:      Encapsulates the landing gear elements
   8  Called by:    FGAircraft
   9
  10  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
  11
  12  This program is free software; you can redistribute it and/or modify it under
  13  the terms of the GNU General Public License as published by the Free Software
  14  Foundation; either version 2 of the License, or (at your option) any later
  15  version.
  16
  17  This program is distributed in the hope that it will be useful, but WITHOUT
  18  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  19  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  20  details.
  21
  22  You should have received a copy of the GNU General Public License along with
  23  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  24  Place - Suite 330, Boston, MA  02111-1307, USA.
  25
  26  Further information about the GNU General Public License can also be found on
  27  the world wide web at http://www.gnu.org.
  28
  29 FUNCTIONAL DESCRIPTION
  30 --------------------------------------------------------------------------------
  31
  32 HISTORY
  33 --------------------------------------------------------------------------------
  34 11/18/99   JSB   Created
  35 01/30/01   NHP   Extended gear model to properly simulate steering and braking
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 INCLUDES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #include "FGLGear.h"
  42 #include <algorithm>
  43
  44 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  45 DEFINITIONS
  46 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  47
  48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49 GLOBAL DATA
  50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  51
  52
  53 static const char *IdSrc = "$Id$";
  54 static const char *IdHdr = ID_LGEAR;
  55
  56 extern short debug_lvl;
  57
  58 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  59 CLASS IMPLEMENTATION
  60 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  61
  62 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
  63                                                            vMoment(3),
  64                                                            vWhlBodyVec(3),
  65                                                            Exec(fdmex)
  66 {
  67   string tmp;
  68   *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
  69             >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
  70                   >> rollingFCoeff >> sSteerType >> sBrakeGroup >> maxSteerAngle;
  71
  72   cout << "    Name: " << name << endl;
  73   cout << "      Location: " << vXYZ << endl;
  74   cout << "      Spring Constant:  " << kSpring << endl;
  75   cout << "      Damping Constant: " << bDamp << endl;
  76   cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
  77   cout << "      Static Friction:  " << staticFCoeff << endl;
  78   cout << "      Rolling Friction: " << rollingFCoeff << endl;
  79   cout << "      Steering Type:    " << sSteerType << endl;
  80   cout << "      Grouping:         " << sBrakeGroup << endl;
  81   cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
  82
  83   if      (sBrakeGroup == "LEFT"  ) eBrakeGrp = bgLeft;
  84   else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
  85   else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
  86   else if (sBrakeGroup == "NOSE"  ) eBrakeGrp = bgNose;
  87   else if (sBrakeGroup == "TAIL"  ) eBrakeGrp = bgTail;
  88   else if (sBrakeGroup == "NONE"  ) eBrakeGrp = bgNone;
  89   else {
  90     cerr << "Improper braking group specification in config file: "
  91          << sBrakeGroup << " is undefined." << endl;
  92   }
  93
  94   if      (sSteerType == "STEERABLE") eSteerType = stSteer;
  95   else if (sSteerType == "FIXED"    ) eSteerType = stFixed;
  96   else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
  97   else {
  98     cerr << "Improper steering type specification in config file: "
  99          << sSteerType << " is undefined." << endl;
 100   }
 101
 102 // Add some AI here to determine if gear is located properly according to its
 103 // brake group type ??
 104
 105   State       = Exec->GetState();
 106   Aircraft    = Exec->GetAircraft();
 107   Position    = Exec->GetPosition();
 108   Rotation    = Exec->GetRotation();
 109   FCS         = Exec->GetFCS();
 110
 111   WOW = false;
 112   ReportEnable = true;
 113   FirstContact = false;
 114   Reported = false;
 115   DistanceTraveled = 0.0;
 116   MaximumStrutForce = MaximumStrutTravel = 0.0;
 117
 118   vWhlBodyVec     = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
 119   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
 120   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
 121
 122   vLocalGear = State->GetTb2l() * vWhlBodyVec;
 123
 124   if (debug_lvl & 2) cout << "Instantiated: FGLGear" << endl;
 125 }
 126
 127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 128
 129 FGLGear::FGLGear(const FGLGear& lgear)
 130 {
 131   State    = lgear.State;
 132   Aircraft = lgear.Aircraft;
 133   Position = lgear.Position;
 134   Rotation = lgear.Rotation;
 135   Exec     = lgear.Exec;
 136   FCS      = lgear.FCS;
 137
 138   vXYZ = lgear.vXYZ;
 139   vMoment = lgear.vMoment;
 140   vWhlBodyVec = lgear.vWhlBodyVec;
 141   vLocalGear = lgear.vLocalGear;
 142
 143   WOW                = lgear.WOW;
 144   ReportEnable       = lgear.ReportEnable;
 145   FirstContact       = lgear.FirstContact;
 146   DistanceTraveled   = lgear.DistanceTraveled;
 147   MaximumStrutForce  = lgear.MaximumStrutForce;
 148   MaximumStrutTravel = lgear.MaximumStrutTravel;
 149
 150   kSpring         = lgear.kSpring;
 151   bDamp           = lgear.bDamp;
 152   compressLength  = lgear.compressLength;
 153   compressSpeed   = lgear.compressSpeed;
 154   staticFCoeff    = lgear.staticFCoeff;
 155   dynamicFCoeff   = lgear.dynamicFCoeff;
 156   rollingFCoeff   = lgear.rollingFCoeff;
 157   brakePct        = lgear.brakePct;
 158   maxCompLen      = lgear.maxCompLen;
 159   SinkRate        = lgear.SinkRate;
 160   GroundSpeed     = lgear.GroundSpeed;
 161   Reported        = lgear.Reported;
 162   name            = lgear.name;
 163   sSteerType      = lgear.sSteerType;
 164   eSteerType      = lgear.eSteerType;
 165   sBrakeGroup     = lgear.sBrakeGroup;
 166   eBrakeGrp       = lgear.eBrakeGrp;
 167   maxSteerAngle   = lgear.maxSteerAngle;
 168 }
 169
 170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 171
 172 FGLGear::~FGLGear()
 173 {
 174   if (debug_lvl & 2) cout << "Destroyed:    FGLGear" << endl;
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178
 179 FGColumnVector FGLGear::Force(void)
 180 {
 181   float SteerGain, SteerAngle, BrakeFCoeff;
 182   float SinWheel, CosWheel, SideWhlVel, RollingWhlVel;
 183   float RudderPedal, RollingForce, SideForce, FCoeff;
 184   float WheelSlip;
 185
 186   FGColumnVector vForce(3);
 187   FGColumnVector vLocalForce(3);
 188   FGColumnVector vWhlVelVec(3);     // Velocity of this wheel (Local)
 189
 190   vWhlBodyVec     = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
 191   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
 192   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
 193
 194 // vWhlBodyVec now stores the vector from the cg to this wheel
 195
 196   vLocalGear = State->GetTb2l() * vWhlBodyVec;
 197
 198 // vLocalGear now stores the vector from the cg to the wheel in local coords.
 199
 200   compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
 201
 202 // The compression length is currently measured in the Z-axis, only, at this time.
 203 // It should be measured along the strut axis. If the local-frame gear position
 204 // "hangs down" below the CG greater than the altitude, then the compressLength
 205 // will be positive - i.e. the gear will have made contact.
 206
 207   if (compressLength > 0.00) {
 208
 209     WOW = true; // Weight-On-Wheels is true
 210
 211 // The next equation should really use the vector to the contact patch of the tire
 212 // including the strut compression and not vWhlBodyVec.  Will fix this later.
 213 // As it stands, now, the following equation takes the aircraft body-frame
 214 // rotational rate and calculates the cross-product with the vector from the CG
 215 // to the wheel, thus producing the instantaneous velocity vector of the tire
 216 // in Body coords. The frame is also converted to local coordinates. When the
 217 // aircraft local-frame velocity is added to this quantity, the total velocity of
 218 // the wheel in local frame is then known. Subsequently, the compression speed
 219 // (used for calculating damping force) is found by taking the Z-component of the
 220 // wheel velocity.
 221
 222     vWhlVelVec      =  State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
 223     vWhlVelVec     +=  Position->GetVel();
 224
 225     compressSpeed   =  vWhlVelVec(eZ);
 226
 227 // If this is the first time the wheel has made contact, remember some values
 228 // for later printout.
 229
 230     if (!FirstContact) {
 231       FirstContact  = true;
 232       SinkRate      =  compressSpeed;
 233       GroundSpeed   =  Position->GetVel().Magnitude();
 234     }
 235
 236 // The following needs work regarding friction coefficients and braking and
 237 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
 238 // It also assumes that we won't be turning and braking at the same time.
 239 // Will fix this later.
 240 // [JSB] The braking force coefficients include normal rolling coefficient +
 241 // a percentage of the static friction coefficient based on braking applied.
 242
 243     switch (eBrakeGrp) {
 244     case bgLeft:
 245       SteerGain = -maxSteerAngle;
 246       BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
 247                                             staticFCoeff*FCS->GetBrake(bgLeft);
 248       break;
 249     case bgRight:
 250       SteerGain = -maxSteerAngle;
 251       BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
 252                                            staticFCoeff*FCS->GetBrake(bgRight);
 253       break;
 254     case bgCenter:
 255       SteerGain = -maxSteerAngle;
 256       BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 257                                            staticFCoeff*FCS->GetBrake(bgCenter);
 258       break;
 259     case bgNose:
 260       SteerGain = maxSteerAngle;
 261       BrakeFCoeff = rollingFCoeff;
 262       break;
 263     case bgTail:
 264       SteerGain = -maxSteerAngle;
 265       BrakeFCoeff = rollingFCoeff;
 266       break;
 267     case bgNone:
 268       SteerGain = -maxSteerAngle;
 269       BrakeFCoeff = rollingFCoeff;
 270       break;
 271     default:
 272       cerr << "Improper brake group membership detected for this gear." << endl;
 273       break;
 274     }
 275
 276     switch (eSteerType) {
 277     case stSteer:
 278       SteerAngle = SteerGain*FCS->GetDrCmd();
 279       break;
 280     case stFixed:
 281       SteerAngle = 0.0;
 282       break;
 283     case stCaster:
 284     // Note to Jon: This is not correct for castering gear.  I'll fix it later.
 285       SteerAngle = 0.0;
 286       break;
 287     default:
 288       cerr << "Improper steering type membership detected for this gear." << endl;
 289       break;
 290     }
 291
 292 // Transform the wheel velocities from the local axis system to the wheel axis system.
 293 // For now, steering angle is assumed to happen in the Local Z axis,
 294 // not the strut axis as it should be.  Will fix this later.
 295
 296     SinWheel      = sin(Rotation->Getpsi() + SteerAngle*DEGTORAD);
 297     CosWheel      = cos(Rotation->Getpsi() + SteerAngle*DEGTORAD);
 298     RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
 299     SideWhlVel    = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
 300
 301 // Calculate tire slip angle.
 302
 303     if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
 304       WheelSlip = 0.0;
 305     } else {
 306       WheelSlip = RADTODEG*atan2(SideWhlVel, RollingWhlVel);
 307     }
 308
 309 // The following code normalizes the wheel velocity vector, reverses it, and zeroes out
 310 // the z component of the velocity. The question is, should the Z axis velocity be zeroed
 311 // out first before the normalization takes place or not? Subsequent to that, the Wheel
 312 // Velocity vector now points as a unit vector backwards and parallel to the wheel
 313 // velocity vector. It acts AT the wheel.
 314
 315 // Note to Jon: I commented out this line because I wasn't sure we want to do this.
 316 //    vWhlVelVec      = -1.0 * vWhlVelVec.Normalize();
 317 //    vWhlVelVec(eZ)  =  0.00;
 318
 319 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
 320 // Allow a maximum of 10 degrees tire slip angle before wheel slides.  At that point,
 321 // transition from static to dynamic friction.  There are more complicated formulations
 322 // of this that avoid the discrete jump.  Will fix this later.
 323
 324     if (fabs(WheelSlip) <= 10.0) {
 325       FCoeff = staticFCoeff*WheelSlip/10.0;
 326     } else {
 327       FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
 328     }
 329
 330 // Compute the vertical force on the wheel using square-law damping (per comment
 331 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
 332 // allowing for both square and linear damping force calculation. Also need to
 333 // possibly give a "rebound damping factor" that differs from the compression
 334 // case. NOTE: SQUARE LAW DAMPING NO GOOD!
 335
 336     vLocalForce(eZ) =  min(-compressLength * kSpring
 337                            - compressSpeed * bDamp, (float)0.0);
 338
 339     MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
 340     MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
 341
 342 // Compute the forces in the wheel ground plane.
 343
 344     RollingForce = 0;
 345     if (fabs(RollingWhlVel) > 1E-3) {
 346       RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
 347     }
 348     SideForce    = vLocalForce(eZ) * FCoeff;
 349
 350 // Transform these forces back to the local reference frame.
 351
 352     vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
 353     vLocalForce(eY) = SideForce*CosWheel    + RollingForce*SinWheel;
 354
 355 // Note to Jon: At this point the forces will be too big when the airplane is
 356 // stopped or rolling to a stop.  We need to make sure that the gear forces just
 357 // balance out the non-gear forces when the airplane is stopped.  That way the
 358 // airplane won't start to accelerate until the non-gear/ forces are larger than
 359 // the gear forces.  I think that the proper fix should go into FGAircraft::FMGear.
 360 // This routine would only compute the local strut forces and return them to
 361 // FMGear. All of the gear forces would get adjusted in FMGear using the total
 362 // non-gear forces. Then the gear moments would be calculated. If strange things
 363 // start happening to the airplane during testing as it rolls to a stop, then we
 364 // need to implement this change.  I ran out of time to do it now but have the
 365 // equations.
 366
 367 // Transform the forces back to the body frame and compute the moment.
 368
 369     vForce  = State->GetTl2b() * vLocalForce;
 370     vMoment = vWhlBodyVec * vForce;
 371
 372   } else {
 373
 374     WOW = false;
 375
 376     if (Position->GetDistanceAGL() > 200.0) {
 377       FirstContact = false;
 378       Reported = false;
 379       DistanceTraveled = 0.0;
 380       MaximumStrutForce = MaximumStrutTravel = 0.0;
 381     }
 382
 383     vForce.InitMatrix();
 384     vMoment.InitMatrix();
 385   }
 386
 387   if (FirstContact) {
 388     DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
 389   }
 390
 391   if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
 392     Report();
 393   }
 394
 395   return vForce;
 396 }
 397
 398 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 399
 400 void FGLGear::Report(void)
 401 {
 402   cout << endl << "Touchdown report for " << name << endl;
 403   cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
 404                               << SinkRate*0.3408          << " mps"     << endl;
 405   cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
 406                               << GroundSpeed*0.3408       << " mps"     << endl;
 407   cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
 408                               << MaximumStrutForce*4.448  << " Newtons" << endl;
 409   cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
 410                               << MaximumStrutTravel*30.48 << " cm"      << endl;
 411   cout << "  Distance traveled:     " << DistanceTraveled        << " ft,     "
 412                               << DistanceTraveled*0.3408  << " meters"  << endl;
 413   Reported = true;
 414 }
 415
 416 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 417
 418 void FGLGear::Debug(void)
 419 {
 420   // TODO: Add user code here
 421 }
 422