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