src/FDM/JSBSim/models/FGLGear.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGLGear.cpp
   4  Author:       Jon S. Berndt
   5                Norman H. Princen
   6                Bertrand Coconnier
   7  Date started: 11/18/99
   8  Purpose:      Encapsulates the landing gear elements
   9  Called by:    FGAircraft
  10
  11  ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
  12
  13  This program is free software; you can redistribute it and/or modify it under
  14  the terms of the GNU Lesser General Public License as published by the Free Software
  15  Foundation; either version 2 of the License, or (at your option) any later
  16  version.
  17
  18  This program is distributed in the hope that it will be useful, but WITHOUT
  19  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  20  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  21  details.
  22
  23  You should have received a copy of the GNU Lesser General Public License along with
  24  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  25  Place - Suite 330, Boston, MA  02111-1307, USA.
  26
  27  Further information about the GNU Lesser General Public License can also be found on
  28  the world wide web at http://www.gnu.org.
  29
  30 FUNCTIONAL DESCRIPTION
  31 --------------------------------------------------------------------------------
  32
  33 HISTORY
  34 --------------------------------------------------------------------------------
  35 11/18/99   JSB   Created
  36 01/30/01   NHP   Extended gear model to properly simulate steering and braking
  37 07/08/09   BC    Modified gear model to support large angles between aircraft and ground
  38
  39 /%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40 INCLUDES
  41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  42
  43 #include "FGLGear.h"
  44 #include "FGGroundReactions.h"
  45 #include "FGFCS.h"
  46 #include "FGAuxiliary.h"
  47 #include "FGAtmosphere.h"
  48 #include "FGMassBalance.h"
  49 #include "math/FGTable.h"
  50 #include <cstdlib>
  51
  52 using namespace std;
  53
  54 namespace JSBSim {
  55
  56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  57 DEFINITIONS
  58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  59
  60 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  61 GLOBAL DATA
  62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  63
  64 static const char *IdSrc = "$Id: FGLGear.cpp,v 1.74 2010/05/18 10:54:14 jberndt Exp $";
  65 static const char *IdHdr = ID_LGEAR;
  66
  67 // Body To Structural (body frame is rotated 180 deg about Y and lengths are given in
  68 // ft instead of inches)
  69 const FGMatrix33 FGLGear::Tb2s(-1./inchtoft, 0., 0., 0., 1./inchtoft, 0., 0., 0., -1./inchtoft);
  70
  71 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  72 CLASS IMPLEMENTATION
  73 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  74
  75 FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
  76   FGForce(fdmex),
  77   GearNumber(number),
  78   SteerAngle(0.0),
  79   Castered(false)
  80 {
  81   Element *force_table=0;
  82   Element *dampCoeff=0;
  83   Element *dampCoeffRebound=0;
  84   string force_type="";
  85
  86   kSpring = bDamp = bDampRebound = dynamicFCoeff = staticFCoeff = rollingFCoeff = maxSteerAngle = 0;
  87   sSteerType = sBrakeGroup = sSteerType = "";
  88   isRetractable = 0;
  89   eDampType = dtLinear;
  90   eDampTypeRebound = dtLinear;
  91
  92   name = el->GetAttributeValue("name");
  93   sContactType = el->GetAttributeValue("type");
  94   if (sContactType == "BOGEY") {
  95     eContactType = ctBOGEY;
  96   } else if (sContactType == "STRUCTURE") {
  97     eContactType = ctSTRUCTURE;
  98   } else {
  99     // Unknown contact point types will be treated as STRUCTURE.
 100     eContactType = ctSTRUCTURE;
 101   }
 102
 103   if (el->FindElement("spring_coeff"))
 104     kSpring = el->FindElementValueAsNumberConvertTo("spring_coeff", "LBS/FT");
 105   if (el->FindElement("damping_coeff")) {
 106     dampCoeff = el->FindElement("damping_coeff");
 107     if (dampCoeff->GetAttributeValue("type") == "SQUARE") {
 108       eDampType = dtSquare;
 109       bDamp   = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT2/SEC2");
 110     } else {
 111       bDamp   = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT/SEC");
 112     }
 113   }
 114
 115   if (el->FindElement("damping_coeff_rebound")) {
 116     dampCoeffRebound = el->FindElement("damping_coeff_rebound");
 117     if (dampCoeffRebound->GetAttributeValue("type") == "SQUARE") {
 118       eDampTypeRebound = dtSquare;
 119       bDampRebound   = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT2/SEC2");
 120     } else {
 121       bDampRebound   = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT/SEC");
 122     }
 123   } else {
 124     bDampRebound   = bDamp;
 125     eDampTypeRebound = eDampType;
 126   }
 127
 128   if (el->FindElement("dynamic_friction"))
 129     dynamicFCoeff = el->FindElementValueAsNumber("dynamic_friction");
 130   if (el->FindElement("static_friction"))
 131     staticFCoeff = el->FindElementValueAsNumber("static_friction");
 132   if (el->FindElement("rolling_friction"))
 133     rollingFCoeff = el->FindElementValueAsNumber("rolling_friction");
 134   if (el->FindElement("max_steer"))
 135     maxSteerAngle = el->FindElementValueAsNumberConvertTo("max_steer", "DEG");
 136   if (el->FindElement("retractable"))
 137     isRetractable = ((unsigned int)el->FindElementValueAsNumber("retractable"))>0.0?true:false;
 138
 139   ForceY_Table = 0;
 140   force_table = el->FindElement("table");
 141   while (force_table) {
 142     force_type = force_table->GetAttributeValue("type");
 143     if (force_type == "CORNERING_COEFF") {
 144       ForceY_Table = new FGTable(fdmex->GetPropertyManager(), force_table);
 145     } else {
 146       cerr << "Undefined force table for " << name << " contact point" << endl;
 147     }
 148     force_table = el->FindNextElement("table");
 149   }
 150
 151   sBrakeGroup = el->FindElementValue("brake_group");
 152
 153   if (maxSteerAngle == 360) sSteerType = "CASTERED";
 154   else if (maxSteerAngle == 0.0) sSteerType = "FIXED";
 155   else sSteerType = "STEERABLE";
 156
 157   Element* element = el->FindElement("location");
 158   if (element) vXYZn = element->FindElementTripletConvertTo("IN");
 159   else {cerr << "No location given for contact " << name << endl; exit(-1);}
 160   SetTransformType(FGForce::tCustom);
 161
 162   element = el->FindElement("orientation");
 163   if (element && (eContactType == ctBOGEY)) {
 164     vGearOrient = element->FindElementTripletConvertTo("RAD");
 165
 166     double cp,sp,cr,sr,cy,sy;
 167
 168     cp=cos(vGearOrient(ePitch)); sp=sin(vGearOrient(ePitch));
 169     cr=cos(vGearOrient(eRoll));  sr=sin(vGearOrient(eRoll));
 170     cy=cos(vGearOrient(eYaw));   sy=sin(vGearOrient(eYaw));
 171
 172     mTGear(1,1) =  cp*cy;
 173     mTGear(2,1) =  cp*sy;
 174     mTGear(3,1) = -sp;
 175
 176     mTGear(1,2) = sr*sp*cy - cr*sy;
 177     mTGear(2,2) = sr*sp*sy + cr*cy;
 178     mTGear(3,2) = sr*cp;
 179
 180     mTGear(1,3) = cr*sp*cy + sr*sy;
 181     mTGear(2,3) = cr*sp*sy - sr*cy;
 182     mTGear(3,3) = cr*cp;
 183   }
 184   else {
 185     mTGear(1,1) = 1.;
 186     mTGear(2,2) = 1.;
 187     mTGear(3,3) = 1.;
 188   }
 189
 190   if      (sBrakeGroup == "LEFT"  ) eBrakeGrp = bgLeft;
 191   else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
 192   else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
 193   else if (sBrakeGroup == "NOSE"  ) eBrakeGrp = bgNose;
 194   else if (sBrakeGroup == "TAIL"  ) eBrakeGrp = bgTail;
 195   else if (sBrakeGroup == "NONE"  ) eBrakeGrp = bgNone;
 196   else if (sBrakeGroup.empty()    ) {eBrakeGrp = bgNone;
 197                                      sBrakeGroup = "NONE (defaulted)";}
 198   else {
 199     cerr << "Improper braking group specification in config file: "
 200          << sBrakeGroup << " is undefined." << endl;
 201   }
 202
 203   if      (sSteerType == "STEERABLE") eSteerType = stSteer;
 204   else if (sSteerType == "FIXED"    ) eSteerType = stFixed;
 205   else if (sSteerType == "CASTERED" ) {eSteerType = stCaster; Castered = true;}
 206   else if (sSteerType.empty()       ) {eSteerType = stFixed;
 207                                        sSteerType = "FIXED (defaulted)";}
 208   else {
 209     cerr << "Improper steering type specification in config file: "
 210          << sSteerType << " is undefined." << endl;
 211   }
 212
 213   RFRV = 0.7;  // Rolling force relaxation velocity, default value
 214   SFRV = 0.7;  // Side force relaxation velocity, default value
 215
 216   Element* relax_vel = el->FindElement("relaxation_velocity");
 217   if (relax_vel) {
 218     if (relax_vel->FindElement("rolling")) {
 219       RFRV = relax_vel->FindElementValueAsNumberConvertTo("rolling", "FT/SEC");
 220     }
 221     if (relax_vel->FindElement("side")) {
 222       SFRV = relax_vel->FindElementValueAsNumberConvertTo("side", "FT/SEC");
 223     }
 224   }
 225
 226   Aircraft    = fdmex->GetAircraft();
 227   Propagate   = fdmex->GetPropagate();
 228   Auxiliary   = fdmex->GetAuxiliary();
 229   FCS         = fdmex->GetFCS();
 230   MassBalance = fdmex->GetMassBalance();
 231
 232   LongForceLagFilterCoeff = 1/fdmex->GetDeltaT(); // default longitudinal force filter coefficient
 233   LatForceLagFilterCoeff  = 1/fdmex->GetDeltaT(); // default lateral force filter coefficient
 234
 235   Element* force_lag_filter_elem = el->FindElement("force_lag_filter");
 236   if (force_lag_filter_elem) {
 237     if (force_lag_filter_elem->FindElement("rolling")) {
 238       LongForceLagFilterCoeff = force_lag_filter_elem->FindElementValueAsNumber("rolling");
 239     }
 240     if (force_lag_filter_elem->FindElement("side")) {
 241       LatForceLagFilterCoeff = force_lag_filter_elem->FindElementValueAsNumber("side");
 242     }
 243   }
 244
 245   LongForceFilter = Filter(LongForceLagFilterCoeff, fdmex->GetDeltaT());
 246   LatForceFilter = Filter(LatForceLagFilterCoeff, fdmex->GetDeltaT());
 247
 248   WheelSlipLagFilterCoeff = 1/fdmex->GetDeltaT();
 249
 250   Element *wheel_slip_angle_lag_elem = el->FindElement("wheel_slip_filter");
 251   if (wheel_slip_angle_lag_elem) {
 252     WheelSlipLagFilterCoeff = wheel_slip_angle_lag_elem->GetDataAsNumber();
 253   }
 254
 255   WheelSlipFilter = Filter(WheelSlipLagFilterCoeff, fdmex->GetDeltaT());
 256
 257   GearUp = false;
 258   GearDown = true;
 259   GearPos  = 1.0;
 260   useFCSGearPos = false;
 261   Servicable = true;
 262
 263 // Add some AI here to determine if gear is located properly according to its
 264 // brake group type ??
 265
 266   WOW = lastWOW = false;
 267   ReportEnable = true;
 268   FirstContact = false;
 269   StartedGroundRun = false;
 270   TakeoffReported = LandingReported = false;
 271   LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;
 272   MaximumStrutForce = MaximumStrutTravel = 0.0;
 273   SinkRate = GroundSpeed = 0.0;
 274
 275   vWhlBodyVec = MassBalance->StructuralToBody(vXYZn);
 276   vLocalGear = Propagate->GetTb2l() * vWhlBodyVec;
 277   vWhlVelVec.InitMatrix();
 278
 279   compressLength  = 0.0;
 280   compressSpeed   = 0.0;
 281   brakePct        = 0.0;
 282   maxCompLen      = 0.0;
 283
 284   WheelSlip = 0.0;
 285   TirePressureNorm = 1.0;
 286
 287   // Set Pacejka terms
 288
 289   Stiffness = 0.06;
 290   Shape = 2.8;
 291   Peak = staticFCoeff;
 292   Curvature = 1.03;
 293
 294   Debug(0);
 295 }
 296
 297 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 298
 299 FGLGear::~FGLGear()
 300 {
 301   delete ForceY_Table;
 302   Debug(1);
 303 }
 304
 305 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 306
 307 FGColumnVector3& FGLGear::GetBodyForces(void)
 308 {
 309   double t = fdmex->GetSimTime();
 310   dT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
 311
 312   vFn.InitMatrix();
 313
 314   if (isRetractable) ComputeRetractionState();
 315
 316   if (GearDown) {
 317     double verticalZProj = 0.;
 318
 319     vWhlBodyVec = MassBalance->StructuralToBody(vXYZn); // Get wheel in body frame
 320     vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; // Get local frame wheel location
 321
 322     gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear);
 323     // Compute the height of the theoretical location of the wheel (if strut is not compressed) with
 324     // respect to the ground level
 325     double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel);
 326     vGroundNormal = Propagate->GetTec2b() * normal;
 327
 328     // The height returned above is the AGL and is expressed in the Z direction of the local
 329     // coordinate frame. We now need to transform this height in actual compression of the strut (BOGEY)
 330     // of in the normal direction to the ground (STRUCTURE)
 331     switch (eContactType) {
 332     case ctBOGEY:
 333       verticalZProj = (Propagate->GetTb2l()*mTGear*FGColumnVector3(0.,0.,1.))(eZ);
 334       compressLength = verticalZProj > 0.0 ? -height / verticalZProj : 0.0;
 335       break;
 336     case ctSTRUCTURE:
 337       verticalZProj = -(Propagate->GetTec2l()*normal)(eZ);
 338       compressLength = fabs(verticalZProj) > 0.0 ? -height / verticalZProj : 0.0;
 339       break;
 340     }
 341
 342     if (compressLength > 0.00) {
 343
 344       WOW = true;
 345
 346       // [The next equation should really use the vector to the contact patch of
 347       // the tire including the strut compression and not the original vWhlBodyVec.]
 348
 349       FGColumnVector3 vWhlDisplVec = mTGear * FGColumnVector3(0., 0., compressLength);
 350       FGColumnVector3 vWhlContactVec = vWhlBodyVec - vWhlDisplVec;
 351       vActingXYZn = vXYZn - Tb2s * vWhlDisplVec;
 352       FGColumnVector3 vBodyWhlVel  = Propagate->GetPQR() * vWhlContactVec;
 353       vBodyWhlVel += Propagate->GetUVW() - Propagate->GetTec2b() * cvel;
 354
 355       vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
 356
 357       InitializeReporting();
 358       ComputeSteeringAngle();
 359       ComputeGroundCoordSys();
 360
 361       vLocalWhlVel = Transform().Transposed() * vBodyWhlVel;
 362
 363       switch (eContactType) {
 364       case ctBOGEY:
 365         // Compression speed along the strut
 366         compressSpeed = -vWhlVelVec(eZ);
 367       case ctSTRUCTURE:
 368         // Compression speed along the ground normal
 369         compressSpeed = -vLocalWhlVel(eX);
 370       }
 371
 372       ComputeVerticalStrutForce();
 373
 374       // Compute the forces in the wheel ground plane.
 375       if (eContactType == ctBOGEY) {
 376         ComputeSlipAngle();
 377         ComputeBrakeForceCoefficient();
 378         ComputeSideForceCoefficient();
 379         double sign = vLocalWhlVel(eY)>0?1.0:(vLocalWhlVel(eY)<0?-1.0:0.0);
 380         vFn(eY) = - ((1.0 - TirePressureNorm) * 30 + vFn(eX) * BrakeFCoeff) * sign;
 381         vFn(eZ) = vFn(eX) * FCoeff;
 382       }
 383       else if (eContactType == ctSTRUCTURE) {
 384         FGColumnVector3 vSlipVec = vLocalWhlVel;
 385         vSlipVec(eX) = 0.;
 386         vSlipVec.Normalize();
 387         vFn -= staticFCoeff * vFn(eX) * vSlipVec;
 388       }
 389
 390       // Lag and attenuate the XY-plane forces dependent on velocity. This code
 391       // uses a lag filter, C/(s + C) where "C" is the filter coefficient. When
 392       // "C" is chosen at the frame rate (in Hz), the jittering is significantly
 393       // reduced. This is because the jitter is present *at* the execution rate.
 394       // If a coefficient is set to something equal to or less than zero, the
 395       // filter is bypassed.
 396
 397       if (LongForceLagFilterCoeff > 0) vFn(eY) = LongForceFilter.execute(vFn(eY));
 398       if (LatForceLagFilterCoeff > 0)  vFn(eZ) = LatForceFilter.execute(vFn(eZ));
 399
 400       if ((fabs(vLocalWhlVel(eY)) <= RFRV) && RFRV > 0) vFn(eY) *= fabs(vLocalWhlVel(eY))/RFRV;
 401       if ((fabs(vLocalWhlVel(eZ)) <= SFRV) && SFRV > 0) vFn(eZ) *= fabs(vLocalWhlVel(eZ))/SFRV;
 402
 403       // End section for attenuating gear jitter
 404
 405     } else { // Gear is NOT compressed
 406
 407       WOW = false;
 408       compressLength = 0.0;
 409       compressSpeed = 0.0;
 410       WheelSlip = 0.0;
 411       StrutForce = 0.0;
 412
 413       // Let wheel spin down slowly
 414       vWhlVelVec(eX) -= 13.0*dT;
 415       if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0;
 416
 417       // Return to neutral position between 1.0 and 0.8 gear pos.
 418       SteerAngle *= max(GetGearUnitPos()-0.8, 0.0)/0.2;
 419
 420       ResetReporting();
 421     }
 422   }
 423
 424   ReportTakeoffOrLanding();
 425
 426   // Require both WOW and LastWOW to be true before checking crash conditions
 427   // to allow the WOW flag to be used in terminating a scripted run.
 428   if (WOW && lastWOW) CrashDetect();
 429
 430   lastWOW = WOW;
 431
 432   return FGForce::GetBodyForces();
 433 }
 434
 435 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 436 // Build a local "ground" coordinate system defined by
 437 //  eX : normal to the ground
 438 //  eY : projection of the rolling direction on the ground
 439 //  eZ : projection of the sliping direction on the ground
 440
 441 void FGLGear::ComputeGroundCoordSys(void)
 442 {
 443   // Euler angles are built up to create a local frame to describe the forces
 444   // applied to the gear by the ground. Here pitch, yaw and roll do not have
 445   // any physical meaning. It is just a convenient notation.
 446   // First, "pitch" and "yaw" are determined in order to align eX with the
 447   // ground normal.
 448   if (vGroundNormal(eZ) < -1.0)
 449     vOrient(ePitch) = 0.5*M_PI;
 450   else if (1.0 < vGroundNormal(eZ))
 451     vOrient(ePitch) = -0.5*M_PI;
 452   else
 453     vOrient(ePitch) = asin(-vGroundNormal(eZ));
 454
 455   if (fabs(vOrient(ePitch)) == 0.5*M_PI)
 456     vOrient(eYaw) = 0.;
 457   else
 458     vOrient(eYaw) = atan2(vGroundNormal(eY), vGroundNormal(eX));
 459
 460   vOrient(eRoll) = 0.;
 461   UpdateCustomTransformMatrix();
 462
 463   if (eContactType == ctBOGEY) {
 464     // In the case of a bogey, the third angle "roll" is used to align the axis eY and eZ
 465     // to the rolling and sliping direction respectively.
 466     FGColumnVector3 updatedRollingAxis = Transform().Transposed() * mTGear
 467                                        * FGColumnVector3(-sin(SteerAngle), cos(SteerAngle), 0.);
 468
 469     vOrient(eRoll) = atan2(updatedRollingAxis(eY), -updatedRollingAxis(eZ));
 470     UpdateCustomTransformMatrix();
 471   }
 472 }
 473
 474 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 475
 476 void FGLGear::ComputeRetractionState(void)
 477 {
 478   double gearPos = GetGearUnitPos();
 479   if (gearPos < 0.01) {
 480     GearUp   = true;
 481     WOW      = false;
 482     GearDown = false;
 483     vWhlVelVec.InitMatrix();
 484   } else if (gearPos > 0.99) {
 485     GearDown = true;
 486     GearUp   = false;
 487   } else {
 488     GearUp   = false;
 489     GearDown = false;
 490   }
 491 }
 492
 493 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 494
 495 void FGLGear::ComputeSlipAngle(void)
 496 {
 497   // Calculate tire slip angle.
 498   WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
 499
 500   // Filter the wheel slip angle
 501   if (WheelSlipLagFilterCoeff > 0) WheelSlip = WheelSlipFilter.execute(WheelSlip);
 502 }
 503
 504 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 505 // Compute the steering angle in any case.
 506 // This will also make sure that animations will look right.
 507
 508 void FGLGear::ComputeSteeringAngle(void)
 509 {
 510   switch (eSteerType) {
 511   case stSteer:
 512     SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
 513     break;
 514   case stFixed:
 515     SteerAngle = 0.0;
 516     break;
 517   case stCaster:
 518     if (!Castered)
 519       SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
 520     else {
 521       // Check that the speed is non-null otherwise use the current angle
 522       if (vWhlVelVec.Magnitude(eX,eY) > 1E-3)
 523         SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
 524     }
 525     break;
 526   default:
 527     cerr << "Improper steering type membership detected for this gear." << endl;
 528     break;
 529   }
 530 }
 531
 532 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 533 // Reset reporting functionality after takeoff
 534
 535 void FGLGear::ResetReporting(void)
 536 {
 537   if (Propagate->GetDistanceAGL() > 200.0) {
 538     FirstContact = false;
 539     StartedGroundRun = false;
 540     LandingReported = false;
 541     TakeoffReported = true;
 542     LandingDistanceTraveled = 0.0;
 543     MaximumStrutForce = MaximumStrutTravel = 0.0;
 544   }
 545 }
 546
 547 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 548
 549 void FGLGear::InitializeReporting(void)
 550 {
 551   // If this is the first time the wheel has made contact, remember some values
 552   // for later printout.
 553
 554   if (!FirstContact) {
 555     FirstContact  = true;
 556     SinkRate      =  compressSpeed;
 557     GroundSpeed   =  Propagate->GetVel().Magnitude();
 558     TakeoffReported = false;
 559   }
 560
 561   // If the takeoff run is starting, initialize.
 562
 563   if ((Propagate->GetVel().Magnitude() > 0.1) &&
 564       (FCS->GetBrake(bgLeft) == 0) &&
 565       (FCS->GetBrake(bgRight) == 0) &&
 566       (FCS->GetThrottlePos(0) > 0.90) && !StartedGroundRun)
 567   {
 568     TakeoffDistanceTraveled = 0;
 569     TakeoffDistanceTraveled50ft = 0;
 570     StartedGroundRun = true;
 571   }
 572 }
 573
 574 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 575 // Takeoff and landing reporting functionality
 576
 577 void FGLGear::ReportTakeoffOrLanding(void)
 578 {
 579   double deltaT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
 580
 581   if (FirstContact)
 582     LandingDistanceTraveled += Auxiliary->GetVground()*deltaT;
 583
 584   if (StartedGroundRun) {
 585     TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*deltaT;
 586     if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*deltaT;
 587   }
 588
 589   if ( ReportEnable
 590        && Auxiliary->GetVground() <= 0.05
 591        && !LandingReported
 592        && fdmex->GetGroundReactions()->GetWOW())
 593   {
 594     if (debug_lvl > 0) Report(erLand);
 595   }
 596
 597   if ( ReportEnable
 598        && !TakeoffReported
 599        && (Propagate->GetDistanceAGL() - vLocalGear(eZ)) > 50.0
 600        && !fdmex->GetGroundReactions()->GetWOW())
 601   {
 602     if (debug_lvl > 0) Report(erTakeoff);
 603   }
 604
 605   if (lastWOW != WOW) PutMessage("GEAR_CONTACT: " + name, WOW);
 606 }
 607
 608 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 609 // Crash detection logic (really out-of-bounds detection)
 610
 611 void FGLGear::CrashDetect(void)
 612 {
 613   if ( (compressLength > 500.0 ||
 614       vFn.Magnitude() > 100000000.0 ||
 615       GetMoments().Magnitude() > 5000000000.0 ||
 616       SinkRate > 1.4666*30 ) && !fdmex->IntegrationSuspended())
 617   {
 618     PutMessage("Crash Detected: Simulation FREEZE.");
 619     fdmex->SuspendIntegration();
 620   }
 621 }
 622
 623 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 624 // The following needs work regarding friction coefficients and braking and
 625 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
 626 // It also assumes that we won't be turning and braking at the same time.
 627 // Will fix this later.
 628 // [JSB] The braking force coefficients include normal rolling coefficient +
 629 // a percentage of the static friction coefficient based on braking applied.
 630
 631 void FGLGear::ComputeBrakeForceCoefficient(void)
 632 {
 633   switch (eBrakeGrp) {
 634   case bgLeft:
 635     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
 636                      staticFCoeff*FCS->GetBrake(bgLeft) );
 637     break;
 638   case bgRight:
 639     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
 640                      staticFCoeff*FCS->GetBrake(bgRight) );
 641     break;
 642   case bgCenter:
 643     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 644                      staticFCoeff*FCS->GetBrake(bgCenter) );
 645     break;
 646   case bgNose:
 647     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 648                      staticFCoeff*FCS->GetBrake(bgCenter) );
 649     break;
 650   case bgTail:
 651     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 652                      staticFCoeff*FCS->GetBrake(bgCenter) );
 653     break;
 654   case bgNone:
 655     BrakeFCoeff =  rollingFCoeff;
 656     break;
 657   default:
 658     cerr << "Improper brake group membership detected for this gear." << endl;
 659     break;
 660   }
 661 }
 662
 663 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 664 // Compute the sideforce coefficients using Pacejka's Magic Formula.
 665 //
 666 //   y(x) = D sin {C arctan [Bx - E(Bx - arctan Bx)]}
 667 //
 668 // Where: B = Stiffness Factor (0.06, here)
 669 //        C = Shape Factor (2.8, here)
 670 //        D = Peak Factor (0.8, here)
 671 //        E = Curvature Factor (1.03, here)
 672
 673 void FGLGear::ComputeSideForceCoefficient(void)
 674 {
 675   if (ForceY_Table) {
 676     FCoeff = ForceY_Table->GetValue(WheelSlip);
 677   } else {
 678     double StiffSlip = Stiffness*WheelSlip;
 679     FCoeff = Peak * sin(Shape*atan(StiffSlip - Curvature*(StiffSlip - atan(StiffSlip))));
 680   }
 681 }
 682
 683 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 684 // Compute the vertical force on the wheel using square-law damping (per comment
 685 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
 686 // allowing for both square and linear damping force calculation. Also need to
 687 // possibly give a "rebound damping factor" that differs from the compression
 688 // case.
 689
 690 void FGLGear::ComputeVerticalStrutForce(void)
 691 {
 692   double springForce = 0;
 693   double dampForce = 0;
 694
 695   springForce = -compressLength * kSpring;
 696
 697   if (compressSpeed >= 0.0) {
 698
 699     if (eDampType == dtLinear)   dampForce = -compressSpeed * bDamp;
 700     else         dampForce = -compressSpeed * compressSpeed * bDamp;
 701
 702   } else {
 703
 704     if (eDampTypeRebound == dtLinear)
 705       dampForce   = -compressSpeed * bDampRebound;
 706     else
 707       dampForce   =  compressSpeed * compressSpeed * bDampRebound;
 708
 709   }
 710
 711   StrutForce = min(springForce + dampForce, (double)0.0);
 712
 713   // The reaction force of the wheel is always normal to the ground
 714   switch (eContactType) {
 715   case ctBOGEY:
 716     // Project back the strut force in the local coordinate frame of the ground
 717     vFn(eX) = StrutForce / (mTGear.Transposed()*vGroundNormal)(eZ);
 718     break;
 719   case ctSTRUCTURE:
 720     vFn(eX) = -StrutForce;
 721     break;
 722   }
 723
 724   // Remember these values for reporting
 725   MaximumStrutForce = max(MaximumStrutForce, fabs(StrutForce));
 726   MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
 727 }
 728
 729 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 730
 731 double FGLGear::GetGearUnitPos(void)
 732 {
 733   // hack to provide backward compatibility to gear/gear-pos-norm property
 734   if( useFCSGearPos || FCS->GetGearPos() != 1.0 ) {
 735     useFCSGearPos = true;
 736     return FCS->GetGearPos();
 737   }
 738   return GearPos;
 739 }
 740
 741 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 742
 743 void FGLGear::bind(void)
 744 {
 745   string property_name;
 746   string base_property_name;
 747   base_property_name = CreateIndexedPropertyName("gear/unit", GearNumber);
 748   if (eContactType == ctBOGEY) {
 749     property_name = base_property_name + "/slip-angle-deg";
 750     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WheelSlip );
 751     property_name = base_property_name + "/WOW";
 752     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WOW );
 753     property_name = base_property_name + "/wheel-speed-fps";
 754     fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGLGear*)this,
 755                           &FGLGear::GetWheelRollVel);
 756     property_name = base_property_name + "/z-position";
 757     fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGForce*)this,
 758                           &FGForce::GetLocationZ, &FGForce::SetLocationZ);
 759     property_name = base_property_name + "/compression-ft";
 760     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &compressLength );
 761     property_name = base_property_name + "/side_friction_coeff";
 762     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &FCoeff );
 763
 764     property_name = base_property_name + "/static_friction_coeff";
 765     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff );
 766
 767     if (eSteerType == stCaster) {
 768       property_name = base_property_name + "/steering-angle-deg";
 769       fdmex->GetPropertyManager()->Tie( property_name.c_str(), this, &FGLGear::GetSteerAngleDeg );
 770       property_name = base_property_name + "/castered";
 771       fdmex->GetPropertyManager()->Tie( property_name.c_str(), &Castered);
 772     }
 773   }
 774
 775   if( isRetractable ) {
 776     property_name = base_property_name + "/pos-norm";
 777     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &GearPos );
 778   }
 779 }
 780
 781 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 782
 783 void FGLGear::Report(ReportType repType)
 784 {
 785   if (fabs(TakeoffDistanceTraveled) < 0.001) return; // Don't print superfluous reports
 786
 787   switch(repType) {
 788   case erLand:
 789     cout << endl << "Touchdown report for " << name << " (WOW at time: "
 790          << fdmex->GetSimTime() << " seconds)" << endl;
 791     cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
 792                                 << SinkRate*0.3048          << " mps"     << endl;
 793     cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
 794                                 << GroundSpeed*0.3048       << " mps"     << endl;
 795     cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
 796                                 << MaximumStrutForce*4.448  << " Newtons" << endl;
 797     cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
 798                                 << MaximumStrutTravel*30.48 << " cm"      << endl;
 799     cout << "  Distance traveled:     " << LandingDistanceTraveled        << " ft,     "
 800                                 << LandingDistanceTraveled*0.3048  << " meters"  << endl;
 801     LandingReported = true;
 802     break;
 803   case erTakeoff:
 804     cout << endl << "Takeoff report for " << name << " (Liftoff at time: "
 805         << fdmex->GetSimTime() << " seconds)" << endl;
 806     cout << "  Distance traveled:                " << TakeoffDistanceTraveled
 807          << " ft,     " << TakeoffDistanceTraveled*0.3048  << " meters"  << endl;
 808     cout << "  Distance traveled (over 50'):     " << TakeoffDistanceTraveled50ft
 809          << " ft,     " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
 810     cout << "  [Altitude (ASL): " << fdmex->GetPropagate()->GetAltitudeASL() << " ft. / "
 811          << fdmex->GetPropagate()->GetAltitudeASLmeters() << " m  | Temperature: "
 812          << fdmex->GetAtmosphere()->GetTemperature() - 459.67 << " F / "
 813          << RankineToCelsius(fdmex->GetAtmosphere()->GetTemperature()) << " C]" << endl;
 814     cout << "  [Velocity (KCAS): " << fdmex->GetAuxiliary()->GetVcalibratedKTS() << "]" << endl;
 815     TakeoffReported = true;
 816     break;
 817   case erNone:
 818     break;
 819   }
 820 }
 821
 822 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 823 //    The bitmasked value choices are as follows:
 824 //    unset: In this case (the default) JSBSim would only print
 825 //       out the normally expected messages, essentially echoing
 826 //       the config files as they are read. If the environment
 827 //       variable is not set, debug_lvl is set to 1 internally
 828 //    0: This requests JSBSim not to output any messages
 829 //       whatsoever.
 830 //    1: This value explicity requests the normal JSBSim
 831 //       startup messages
 832 //    2: This value asks for a message to be printed out when
 833 //       a class is instantiated
 834 //    4: When this value is set, a message is displayed when a
 835 //       FGModel object executes its Run() method
 836 //    8: When this value is set, various runtime state variables
 837 //       are printed out periodically
 838 //    16: When set various parameters are sanity checked and
 839 //       a message is printed out when they go out of bounds
 840
 841 void FGLGear::Debug(int from)
 842 {
 843   if (debug_lvl <= 0) return;
 844
 845   if (debug_lvl & 1) { // Standard console startup message output
 846     if (from == 0) { // Constructor - loading and initialization
 847       cout << "    " << sContactType << " " << name          << endl;
 848       cout << "      Location: "         << vXYZn          << endl;
 849       cout << "      Spring Constant:  " << kSpring       << endl;
 850
 851       if (eDampType == dtLinear)
 852         cout << "      Damping Constant: " << bDamp << " (linear)" << endl;
 853       else
 854         cout << "      Damping Constant: " << bDamp << " (square law)" << endl;
 855
 856       if (eDampTypeRebound == dtLinear)
 857         cout << "      Rebound Damping Constant: " << bDampRebound << " (linear)" << endl;
 858       else
 859         cout << "      Rebound Damping Constant: " << bDampRebound << " (square law)" << endl;
 860
 861       cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
 862       cout << "      Static Friction:  " << staticFCoeff  << endl;
 863       if (eContactType == ctBOGEY) {
 864         cout << "      Rolling Friction: " << rollingFCoeff << endl;
 865         cout << "      Steering Type:    " << sSteerType    << endl;
 866         cout << "      Grouping:         " << sBrakeGroup   << endl;
 867         cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
 868         cout << "      Retractable:      " << isRetractable  << endl;
 869         cout << "      Relaxation Velocities:" << endl;
 870         cout << "        Rolling:          " << RFRV << endl;
 871         cout << "        Side:             " << SFRV << endl;
 872       }
 873     }
 874   }
 875   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 876     if (from == 0) cout << "Instantiated: FGLGear" << endl;
 877     if (from == 1) cout << "Destroyed:    FGLGear" << endl;
 878   }
 879   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 880   }
 881   if (debug_lvl & 8 ) { // Runtime state variables
 882   }
 883   if (debug_lvl & 16) { // Sanity checking
 884   }
 885   if (debug_lvl & 64) {
 886     if (from == 0) { // Constructor
 887       cout << IdSrc << endl;
 888       cout << IdHdr << endl;
 889     }
 890   }
 891 }
 892
 893 } // namespace JSBSim
 894