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 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  57 CLASS IMPLEMENTATION
  58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  59
  60 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
  61 {
  62   string tmp;
  63
  64   *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
  65             >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
  66                   >> rollingFCoeff >> sSteerType >> sBrakeGroup
  67                      >> maxSteerAngle >> sRetractable;
  68
  69   if      (sBrakeGroup == "LEFT"  ) eBrakeGrp = bgLeft;
  70   else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
  71   else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
  72   else if (sBrakeGroup == "NOSE"  ) eBrakeGrp = bgNose;
  73   else if (sBrakeGroup == "TAIL"  ) eBrakeGrp = bgTail;
  74   else if (sBrakeGroup == "NONE"  ) eBrakeGrp = bgNone;
  75   else {
  76     cerr << "Improper braking group specification in config file: "
  77          << sBrakeGroup << " is undefined." << endl;
  78   }
  79
  80   if      (sSteerType == "STEERABLE") eSteerType = stSteer;
  81   else if (sSteerType == "FIXED"    ) eSteerType = stFixed;
  82   else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
  83   else {
  84     cerr << "Improper steering type specification in config file: "
  85          << sSteerType << " is undefined." << endl;
  86   }
  87
  88   if ( sRetractable == "RETRACT" ) {
  89     isRetractable = true;
  90   } else  {
  91     isRetractable = false;
  92   }
  93
  94   GearUp = false;
  95   GearDown = true;
  96
  97 // Add some AI here to determine if gear is located properly according to its
  98 // brake group type ??
  99
 100   State       = Exec->GetState();
 101   Aircraft    = Exec->GetAircraft();
 102   Position    = Exec->GetPosition();
 103   Rotation    = Exec->GetRotation();
 104   FCS         = Exec->GetFCS();
 105   MassBalance = Exec->GetMassBalance();
 106
 107   WOW = lastWOW = false;
 108   ReportEnable = true;
 109   FirstContact = false;
 110   Reported = false;
 111   DistanceTraveled = 0.0;
 112   MaximumStrutForce = MaximumStrutTravel = 0.0;
 113   SinkRate = GroundSpeed = 0.0;
 114
 115   vWhlBodyVec     = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
 116   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
 117   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
 118
 119   vLocalGear = State->GetTb2l() * vWhlBodyVec;
 120
 121   compressLength  = 0.0;
 122   compressSpeed   = 0.0;
 123   brakePct        = 0.0;
 124   maxCompLen      = 0.0;
 125
 126   Debug(0);
 127 }
 128
 129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 130
 131 FGLGear::FGLGear(const FGLGear& lgear)
 132 {
 133   State    = lgear.State;
 134   Aircraft = lgear.Aircraft;
 135   Position = lgear.Position;
 136   Rotation = lgear.Rotation;
 137   Exec     = lgear.Exec;
 138   FCS      = lgear.FCS;
 139   MassBalance = lgear.MassBalance;
 140
 141   vXYZ = lgear.vXYZ;
 142   vMoment = lgear.vMoment;
 143   vWhlBodyVec = lgear.vWhlBodyVec;
 144   vLocalGear = lgear.vLocalGear;
 145
 146   WOW                = lgear.WOW;
 147   lastWOW            = lgear.lastWOW;
 148   ReportEnable       = lgear.ReportEnable;
 149   FirstContact       = lgear.FirstContact;
 150   DistanceTraveled   = lgear.DistanceTraveled;
 151   MaximumStrutForce  = lgear.MaximumStrutForce;
 152   MaximumStrutTravel = lgear.MaximumStrutTravel;
 153
 154   kSpring         = lgear.kSpring;
 155   bDamp           = lgear.bDamp;
 156   compressLength  = lgear.compressLength;
 157   compressSpeed   = lgear.compressSpeed;
 158   staticFCoeff    = lgear.staticFCoeff;
 159   dynamicFCoeff   = lgear.dynamicFCoeff;
 160   rollingFCoeff   = lgear.rollingFCoeff;
 161   brakePct        = lgear.brakePct;
 162   maxCompLen      = lgear.maxCompLen;
 163   SinkRate        = lgear.SinkRate;
 164   GroundSpeed     = lgear.GroundSpeed;
 165   Reported        = lgear.Reported;
 166   name            = lgear.name;
 167   sSteerType      = lgear.sSteerType;
 168   sRetractable    = lgear.sRetractable;
 169   eSteerType      = lgear.eSteerType;
 170   sBrakeGroup     = lgear.sBrakeGroup;
 171   eBrakeGrp       = lgear.eBrakeGrp;
 172   maxSteerAngle   = lgear.maxSteerAngle;
 173   isRetractable   = lgear.isRetractable;
 174   GearUp          = lgear.GearUp;
 175   GearDown        = lgear.GearDown;
 176 }
 177
 178 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 179
 180 FGLGear::~FGLGear()
 181 {
 182   Debug(1);
 183 }
 184
 185 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 186
 187 FGColumnVector3& FGLGear::Force(void)
 188 {
 189   double SteerGain = 0;
 190   double SinWheel, CosWheel;
 191
 192   vForce.InitMatrix();
 193   vMoment.InitMatrix();
 194
 195   if (isRetractable) {
 196     if (FCS->GetGearPos() < 0.01) {
 197       GearUp   = true;
 198       GearDown = false;
 199      } else if (FCS->GetGearPos() > 0.99) {
 200       GearDown = true;
 201       GearUp   = false;
 202      } else {
 203       GearUp   = false;
 204       GearDown = false;
 205      }
 206   } else {
 207       GearUp   = false;
 208       GearDown = true;
 209   }
 210
 211   if (GearDown) {
 212
 213     vWhlBodyVec     = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
 214     vWhlBodyVec(eX) = -vWhlBodyVec(eX);
 215     vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
 216
 217 // vWhlBodyVec now stores the vector from the cg to this wheel
 218
 219     vLocalGear = State->GetTb2l() * vWhlBodyVec;
 220
 221 // vLocalGear now stores the vector from the cg to the wheel in local coords.
 222
 223     compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
 224
 225 // The compression length is currently measured in the Z-axis, only, at this time.
 226 // It should be measured along the strut axis. If the local-frame gear position
 227 // "hangs down" below the CG greater than the altitude, then the compressLength
 228 // will be positive - i.e. the gear will have made contact.
 229
 230     if (compressLength > 0.00) {
 231
 232       WOW = true;// Weight-On-Wheels is true
 233
 234 // The next equation should really use the vector to the contact patch of the tire
 235 // including the strut compression and not vWhlBodyVec.  Will fix this later.
 236 // As it stands, now, the following equation takes the aircraft body-frame
 237 // rotational rate and calculates the cross-product with the vector from the CG
 238 // to the wheel, thus producing the instantaneous velocity vector of the tire
 239 // in Body coords. The frame is also converted to local coordinates. When the
 240 // aircraft local-frame velocity is added to this quantity, the total velocity of
 241 // the wheel in local frame is then known. Subsequently, the compression speed
 242 // (used for calculating damping force) is found by taking the Z-component of the
 243 // wheel velocity.
 244
 245       vWhlVelVec      =  State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
 246
 247       vWhlVelVec     +=  Position->GetVel();
 248
 249       compressSpeed   =  vWhlVelVec(eZ);
 250
 251 // If this is the first time the wheel has made contact, remember some values
 252 // for later printout.
 253
 254       if (!FirstContact) {
 255         FirstContact  = true;
 256         SinkRate      =  compressSpeed;
 257         GroundSpeed   =  Position->GetVel().Magnitude();
 258       }
 259
 260 // The following needs work regarding friction coefficients and braking and
 261 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
 262 // It also assumes that we won't be turning and braking at the same time.
 263 // Will fix this later.
 264 // [JSB] The braking force coefficients include normal rolling coefficient +
 265 // a percentage of the static friction coefficient based on braking applied.
 266
 267       switch (eBrakeGrp) {
 268       case bgLeft:
 269         SteerGain = -0.10;
 270         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
 271                                               staticFCoeff*FCS->GetBrake(bgLeft);
 272         break;
 273       case bgRight:
 274         SteerGain = -0.10;
 275         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
 276                                              staticFCoeff*FCS->GetBrake(bgRight);
 277         break;
 278       case bgCenter:
 279         SteerGain = -0.10;
 280         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 281                                              staticFCoeff*FCS->GetBrake(bgCenter);
 282         break;
 283       case bgNose:
 284         SteerGain = -0.50;
 285         BrakeFCoeff = rollingFCoeff;
 286         break;
 287       case bgTail:
 288         SteerGain = -0.10;
 289         BrakeFCoeff = rollingFCoeff;
 290         break;
 291       case bgNone:
 292         SteerGain = -0.10;
 293         BrakeFCoeff = rollingFCoeff;
 294         break;
 295       default:
 296         cerr << "Improper brake group membership detected for this gear." << endl;
 297         break;
 298       }
 299
 300       switch (eSteerType) {
 301       case stSteer:
 302         SteerAngle = SteerGain*FCS->GetDrPos();
 303         break;
 304       case stFixed:
 305         SteerAngle = 0.0;
 306         break;
 307       case stCaster:
 308 // Note to Jon: This is not correct for castering gear.  I'll fix it later.
 309         SteerAngle = 0.0;
 310         break;
 311       default:
 312         cerr << "Improper steering type membership detected for this gear." << endl;
 313         break;
 314       }
 315
 316 // Transform the wheel velocities from the local axis system to the wheel axis system.
 317 // For now, steering angle is assumed to happen in the Local Z axis,
 318 // not the strut axis as it should be.  Will fix this later.
 319
 320       SinWheel      = sin(Rotation->Getpsi() + SteerAngle);
 321       CosWheel      = cos(Rotation->Getpsi() + SteerAngle);
 322       RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
 323       SideWhlVel    = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
 324
 325 // Calculate tire slip angle.
 326
 327       if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
 328         WheelSlip = 0.0;
 329       } else {
 330         WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
 331       }
 332
 333 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
 334 // Allow a maximum of 10 degrees tire slip angle before wheel slides.  At that point,
 335 // transition from static to dynamic friction.  There are more complicated formulations
 336 // of this that avoid the discrete jump.  Will fix this later.
 337
 338       if (fabs(WheelSlip) <= 10.0) {
 339         FCoeff = staticFCoeff*WheelSlip/10.0;
 340       } else {
 341         FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
 342       }
 343
 344 // Compute the vertical force on the wheel using square-law damping (per comment
 345 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
 346 // allowing for both square and linear damping force calculation. Also need to
 347 // possibly give a "rebound damping factor" that differs from the compression
 348 // case.
 349
 350       vLocalForce(eZ) =  min(-compressLength * kSpring
 351                              - compressSpeed * bDamp, (double)0.0);
 352
 353       MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
 354       MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
 355
 356 // Compute the forces in the wheel ground plane.
 357
 358       RollingForce = 0;
 359       if (fabs(RollingWhlVel) > 1E-3) {
 360         RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
 361       }
 362       SideForce    = vLocalForce(eZ) * FCoeff;
 363
 364 // Transform these forces back to the local reference frame.
 365
 366       vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
 367       vLocalForce(eY) = SideForce*CosWheel    + RollingForce*SinWheel;
 368
 369 // Note to Jon: At this point the forces will be too big when the airplane is
 370 // stopped or rolling to a stop.  We need to make sure that the gear forces just
 371 // balance out the non-gear forces when the airplane is stopped.  That way the
 372 // airplane won't start to accelerate until the non-gear/ forces are larger than
 373 // the gear forces.  I think that the proper fix should go into FGAircraft::FMGear.
 374 // This routine would only compute the local strut forces and return them to
 375 // FMGear. All of the gear forces would get adjusted in FMGear using the total
 376 // non-gear forces. Then the gear moments would be calculated. If strange things
 377 // start happening to the airplane during testing as it rolls to a stop, then we
 378 // need to implement this change.  I ran out of time to do it now but have the
 379 // equations.
 380
 381 // Transform the forces back to the body frame and compute the moment.
 382
 383       vForce  = State->GetTl2b() * vLocalForce;
 384       vMoment = vWhlBodyVec * vForce;
 385
 386     } else {
 387
 388       WOW = false;
 389
 390       if (Position->GetDistanceAGL() > 200.0) {
 391         FirstContact = false;
 392         Reported = false;
 393         DistanceTraveled = 0.0;
 394         MaximumStrutForce = MaximumStrutTravel = 0.0;
 395       }
 396
 397       compressLength = 0.0; // reset compressLength to zero for data output validity
 398     }
 399
 400     if (FirstContact) {
 401       DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
 402     }
 403
 404     if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
 405       if (debug_lvl > 0) Report();
 406     }
 407
 408     if (lastWOW != WOW) {
 409       PutMessage("GEAR_CONTACT", WOW);
 410     }
 411
 412     lastWOW = WOW;
 413
 414 // Crash detection logic (really out-of-bounds detection)
 415
 416     if (compressLength > 500.0 ||
 417         vForce.Magnitude() > 100000000.0 ||
 418         vMoment.Magnitude() > 5000000000.0 ||
 419         SinkRate > 1.4666*30)
 420     {
 421       PutMessage("Crash Detected: Simulation FREEZE.");
 422       Exec->Freeze();
 423     }
 424   }
 425   return vForce;
 426 }
 427
 428 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 429
 430 void FGLGear::Report(void)
 431 {
 432   cout << endl << "Touchdown report for " << name << endl;
 433   cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
 434                               << SinkRate*0.3408          << " mps"     << endl;
 435   cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
 436                               << GroundSpeed*0.3408       << " mps"     << endl;
 437   cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
 438                               << MaximumStrutForce*4.448  << " Newtons" << endl;
 439   cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
 440                               << MaximumStrutTravel*30.48 << " cm"      << endl;
 441   cout << "  Distance traveled:     " << DistanceTraveled        << " ft,     "
 442                               << DistanceTraveled*0.3408  << " meters"  << endl;
 443   Reported = true;
 444 }
 445
 446 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 447 //    The bitmasked value choices are as follows:
 448 //    unset: In this case (the default) JSBSim would only print
 449 //       out the normally expected messages, essentially echoing
 450 //       the config files as they are read. If the environment
 451 //       variable is not set, debug_lvl is set to 1 internally
 452 //    0: This requests JSBSim not to output any messages
 453 //       whatsoever.
 454 //    1: This value explicity requests the normal JSBSim
 455 //       startup messages
 456 //    2: This value asks for a message to be printed out when
 457 //       a class is instantiated
 458 //    4: When this value is set, a message is displayed when a
 459 //       FGModel object executes its Run() method
 460 //    8: When this value is set, various runtime state variables
 461 //       are printed out periodically
 462 //    16: When set various parameters are sanity checked and
 463 //       a message is printed out when they go out of bounds
 464
 465 void FGLGear::Debug(int from)
 466 {
 467   if (debug_lvl <= 0) return;
 468
 469   if (debug_lvl & 1) { // Standard console startup message output
 470     if (from == 0) { // Constructor
 471       cout << "    Name: "               << name          << endl;
 472       cout << "      Location: "         << vXYZ          << endl;
 473       cout << "      Spring Constant:  " << kSpring       << endl;
 474       cout << "      Damping Constant: " << bDamp         << endl;
 475       cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
 476       cout << "      Static Friction:  " << staticFCoeff  << endl;
 477       cout << "      Rolling Friction: " << rollingFCoeff << endl;
 478       cout << "      Steering Type:    " << sSteerType    << endl;
 479       cout << "      Grouping:         " << sBrakeGroup   << endl;
 480       cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
 481       cout << "      Retractable:      " << sRetractable  << endl;
 482     }
 483   }
 484   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 485     if (from == 0) cout << "Instantiated: FGLGear" << endl;
 486     if (from == 1) cout << "Destroyed:    FGLGear" << endl;
 487   }
 488   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 489   }
 490   if (debug_lvl & 8 ) { // Runtime state variables
 491   }
 492   if (debug_lvl & 16) { // Sanity checking
 493   }
 494   if (debug_lvl & 64) {
 495     if (from == 0) { // Constructor
 496       cout << IdSrc << endl;
 497       cout << IdHdr << endl;
 498     }
 499   }
 500 }
 501