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.76 2010/07/30 11:50:01 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   Auxiliary       = fdmex->GetAuxiliary();
 214   Propagate       = fdmex->GetPropagate();
 215   FCS             = fdmex->GetFCS();
 216   MassBalance     = fdmex->GetMassBalance();
 217   GroundReactions = fdmex->GetGroundReactions();
 218
 219   GearUp = false;
 220   GearDown = true;
 221   GearPos  = 1.0;
 222   useFCSGearPos = false;
 223   Servicable = true;
 224
 225 // Add some AI here to determine if gear is located properly according to its
 226 // brake group type ??
 227
 228   WOW = lastWOW = false;
 229   ReportEnable = true;
 230   FirstContact = false;
 231   StartedGroundRun = false;
 232   TakeoffReported = LandingReported = false;
 233   LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;
 234   MaximumStrutForce = MaximumStrutTravel = 0.0;
 235   SinkRate = GroundSpeed = 0.0;
 236
 237   vWhlBodyVec = MassBalance->StructuralToBody(vXYZn);
 238   vLocalGear = Propagate->GetTb2l() * vWhlBodyVec;
 239   vWhlVelVec.InitMatrix();
 240
 241   compressLength  = 0.0;
 242   compressSpeed   = 0.0;
 243   brakePct        = 0.0;
 244   maxCompLen      = 0.0;
 245
 246   WheelSlip = 0.0;
 247   TirePressureNorm = 1.0;
 248
 249   // Set Pacejka terms
 250
 251   Stiffness = 0.06;
 252   Shape = 2.8;
 253   Peak = staticFCoeff;
 254   Curvature = 1.03;
 255
 256   // Initialize Lagrange multipliers
 257   LMultiplier[ftRoll].value = 0.;
 258   LMultiplier[ftSide].value = 0.;
 259   LMultiplier[ftRoll].value = 0.;
 260
 261   Debug(0);
 262 }
 263
 264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 265
 266 FGLGear::~FGLGear()
 267 {
 268   delete ForceY_Table;
 269   Debug(1);
 270 }
 271
 272 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 273
 274 FGColumnVector3& FGLGear::GetBodyForces(void)
 275 {
 276   double t = fdmex->GetSimTime();
 277   dT = fdmex->GetDeltaT()*GroundReactions->GetRate();
 278
 279   vFn.InitMatrix();
 280
 281   if (isRetractable) ComputeRetractionState();
 282
 283   if (GearDown) {
 284     vWhlBodyVec = MassBalance->StructuralToBody(vXYZn); // Get wheel in body frame
 285     vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; // Get local frame wheel location
 286
 287     gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear);
 288     // Compute the height of the theoretical location of the wheel (if strut is
 289     // not compressed) with respect to the ground level
 290     double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel);
 291     vGroundNormal = Propagate->GetTec2b() * normal;
 292
 293     // The height returned above is the AGL and is expressed in the Z direction
 294     // of the ECEF coordinate frame. We now need to transform this height in
 295     // actual compression of the strut (BOGEY) of in the normal direction to the
 296     // ground (STRUCTURE)
 297     double normalZ = (Propagate->GetTec2l()*normal)(eZ);
 298     double LGearProj = -(mTGear.Transposed() * vGroundNormal)(eZ);
 299
 300     switch (eContactType) {
 301     case ctBOGEY:
 302       compressLength = LGearProj > 0.0 ? height * normalZ / LGearProj : 0.0;
 303       break;
 304     case ctSTRUCTURE:
 305       compressLength = height * normalZ / DotProduct(normal, normal);
 306       break;
 307     }
 308
 309     if (compressLength > 0.00) {
 310
 311       WOW = true;
 312
 313       // The following equations use the vector to the tire contact patch
 314       // including the strut compression.
 315       FGColumnVector3 vWhlDisplVec;
 316
 317       switch(eContactType) {
 318       case ctBOGEY:
 319         vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength);
 320         break;
 321       case ctSTRUCTURE:
 322         vWhlDisplVec = compressLength * vGroundNormal;
 323         break;
 324       }
 325
 326       FGColumnVector3 vWhlContactVec = vWhlBodyVec + vWhlDisplVec;
 327       vActingXYZn = vXYZn + Tb2s * vWhlDisplVec;
 328       FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec;
 329       vBodyWhlVel += Propagate->GetUVW() - Propagate->GetTec2b() * cvel;
 330
 331       vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
 332
 333       InitializeReporting();
 334       ComputeSteeringAngle();
 335       ComputeGroundCoordSys();
 336
 337       vLocalWhlVel = Transform().Transposed() * vBodyWhlVel;
 338
 339       compressSpeed = -vLocalWhlVel(eX);
 340       if (eContactType == ctBOGEY)
 341         compressSpeed /= LGearProj;
 342
 343       ComputeVerticalStrutForce();
 344
 345       // Compute the friction coefficients in the wheel ground plane.
 346       if (eContactType == ctBOGEY) {
 347         ComputeSlipAngle();
 348         ComputeBrakeForceCoefficient();
 349         ComputeSideForceCoefficient();
 350       }
 351
 352       // Prepare the Jacobians and the Lagrange multipliers for later friction
 353       // forces calculations.
 354       ComputeJacobian(vWhlContactVec);
 355
 356     } else { // Gear is NOT compressed
 357
 358       WOW = false;
 359       compressLength = 0.0;
 360       compressSpeed = 0.0;
 361       WheelSlip = 0.0;
 362       StrutForce = 0.0;
 363
 364       // Let wheel spin down slowly
 365       vWhlVelVec(eX) -= 13.0*dT;
 366       if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0;
 367
 368       // Return to neutral position between 1.0 and 0.8 gear pos.
 369       SteerAngle *= max(GetGearUnitPos()-0.8, 0.0)/0.2;
 370
 371       ResetReporting();
 372     }
 373   }
 374
 375   ReportTakeoffOrLanding();
 376
 377   // Require both WOW and LastWOW to be true before checking crash conditions
 378   // to allow the WOW flag to be used in terminating a scripted run.
 379   if (WOW && lastWOW) CrashDetect();
 380
 381   lastWOW = WOW;
 382
 383   return FGForce::GetBodyForces();
 384 }
 385
 386 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 387 // Build a local "ground" coordinate system defined by
 388 //  eX : normal to the ground
 389 //  eY : projection of the rolling direction on the ground
 390 //  eZ : projection of the sliping direction on the ground
 391
 392 void FGLGear::ComputeGroundCoordSys(void)
 393 {
 394   // Euler angles are built up to create a local frame to describe the forces
 395   // applied to the gear by the ground. Here pitch, yaw and roll do not have
 396   // any physical meaning. It is just a convenient notation.
 397   // First, "pitch" and "yaw" are determined in order to align eX with the
 398   // ground normal.
 399   if (vGroundNormal(eZ) < -1.0)
 400     vOrient(ePitch) = 0.5*M_PI;
 401   else if (1.0 < vGroundNormal(eZ))
 402     vOrient(ePitch) = -0.5*M_PI;
 403   else
 404     vOrient(ePitch) = asin(-vGroundNormal(eZ));
 405
 406   if (fabs(vOrient(ePitch)) == 0.5*M_PI)
 407     vOrient(eYaw) = 0.;
 408   else
 409     vOrient(eYaw) = atan2(vGroundNormal(eY), vGroundNormal(eX));
 410
 411   vOrient(eRoll) = 0.;
 412   UpdateCustomTransformMatrix();
 413
 414   if (eContactType == ctBOGEY) {
 415     // In the case of a bogey, the third angle "roll" is used to align the axis eY and eZ
 416     // to the rolling and sliping direction respectively.
 417     FGColumnVector3 updatedRollingAxis = Transform().Transposed() * mTGear
 418                                        * FGColumnVector3(-sin(SteerAngle), cos(SteerAngle), 0.);
 419
 420     vOrient(eRoll) = atan2(updatedRollingAxis(eY), -updatedRollingAxis(eZ));
 421     UpdateCustomTransformMatrix();
 422   }
 423 }
 424
 425 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 426
 427 void FGLGear::ComputeRetractionState(void)
 428 {
 429   double gearPos = GetGearUnitPos();
 430   if (gearPos < 0.01) {
 431     GearUp   = true;
 432     WOW      = false;
 433     GearDown = false;
 434     vWhlVelVec.InitMatrix();
 435   } else if (gearPos > 0.99) {
 436     GearDown = true;
 437     GearUp   = false;
 438   } else {
 439     GearUp   = false;
 440     GearDown = false;
 441   }
 442 }
 443
 444 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 445 // Calculate tire slip angle.
 446
 447 void FGLGear::ComputeSlipAngle(void)
 448 {
 449 // Check that the speed is non-null otherwise use the current angle
 450   if (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)
 451     WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
 452 }
 453
 454 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 455 // Compute the steering angle in any case.
 456 // This will also make sure that animations will look right.
 457
 458 void FGLGear::ComputeSteeringAngle(void)
 459 {
 460   switch (eSteerType) {
 461   case stSteer:
 462     SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
 463     break;
 464   case stFixed:
 465     SteerAngle = 0.0;
 466     break;
 467   case stCaster:
 468     if (!Castered)
 469       SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
 470     else {
 471       // Check that the speed is non-null otherwise use the current angle
 472       if (vWhlVelVec.Magnitude(eX,eY) > 0.1)
 473         SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
 474     }
 475     break;
 476   default:
 477     cerr << "Improper steering type membership detected for this gear." << endl;
 478     break;
 479   }
 480 }
 481
 482 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 483 // Reset reporting functionality after takeoff
 484
 485 void FGLGear::ResetReporting(void)
 486 {
 487   if (Propagate->GetDistanceAGL() > 200.0) {
 488     FirstContact = false;
 489     StartedGroundRun = false;
 490     LandingReported = false;
 491     TakeoffReported = true;
 492     LandingDistanceTraveled = 0.0;
 493     MaximumStrutForce = MaximumStrutTravel = 0.0;
 494   }
 495 }
 496
 497 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 498
 499 void FGLGear::InitializeReporting(void)
 500 {
 501   // If this is the first time the wheel has made contact, remember some values
 502   // for later printout.
 503
 504   if (!FirstContact) {
 505     FirstContact  = true;
 506     SinkRate      =  compressSpeed;
 507     GroundSpeed   =  Propagate->GetVel().Magnitude();
 508     TakeoffReported = false;
 509   }
 510
 511   // If the takeoff run is starting, initialize.
 512
 513   if ((Propagate->GetVel().Magnitude() > 0.1) &&
 514       (FCS->GetBrake(bgLeft) == 0) &&
 515       (FCS->GetBrake(bgRight) == 0) &&
 516       (FCS->GetThrottlePos(0) > 0.90) && !StartedGroundRun)
 517   {
 518     TakeoffDistanceTraveled = 0;
 519     TakeoffDistanceTraveled50ft = 0;
 520     StartedGroundRun = true;
 521   }
 522 }
 523
 524 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 525 // Takeoff and landing reporting functionality
 526
 527 void FGLGear::ReportTakeoffOrLanding(void)
 528 {
 529   if (FirstContact)
 530     LandingDistanceTraveled += Auxiliary->GetVground()*dT;
 531
 532   if (StartedGroundRun) {
 533     TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*dT;
 534     if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*dT;
 535   }
 536
 537   if ( ReportEnable
 538        && Auxiliary->GetVground() <= 0.05
 539        && !LandingReported
 540        && GroundReactions->GetWOW())
 541   {
 542     if (debug_lvl > 0) Report(erLand);
 543   }
 544
 545   if ( ReportEnable
 546        && !TakeoffReported
 547        && (Propagate->GetDistanceAGL() - vLocalGear(eZ)) > 50.0
 548        && !GroundReactions->GetWOW())
 549   {
 550     if (debug_lvl > 0) Report(erTakeoff);
 551   }
 552
 553   if (lastWOW != WOW) PutMessage("GEAR_CONTACT: " + name, WOW);
 554 }
 555
 556 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 557 // Crash detection logic (really out-of-bounds detection)
 558
 559 void FGLGear::CrashDetect(void)
 560 {
 561   if ( (compressLength > 500.0 ||
 562       vFn.Magnitude() > 100000000.0 ||
 563       GetMoments().Magnitude() > 5000000000.0 ||
 564       SinkRate > 1.4666*30 ) && !fdmex->IntegrationSuspended())
 565   {
 566     PutMessage("Crash Detected: Simulation FREEZE.");
 567     fdmex->SuspendIntegration();
 568   }
 569 }
 570
 571 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 572 // The following needs work regarding friction coefficients and braking and
 573 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
 574 // It also assumes that we won't be turning and braking at the same time.
 575 // Will fix this later.
 576 // [JSB] The braking force coefficients include normal rolling coefficient +
 577 // a percentage of the static friction coefficient based on braking applied.
 578
 579 void FGLGear::ComputeBrakeForceCoefficient(void)
 580 {
 581   switch (eBrakeGrp) {
 582   case bgLeft:
 583     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
 584                      staticFCoeff*FCS->GetBrake(bgLeft) );
 585     break;
 586   case bgRight:
 587     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
 588                      staticFCoeff*FCS->GetBrake(bgRight) );
 589     break;
 590   case bgCenter:
 591     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 592                      staticFCoeff*FCS->GetBrake(bgCenter) );
 593     break;
 594   case bgNose:
 595     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 596                      staticFCoeff*FCS->GetBrake(bgCenter) );
 597     break;
 598   case bgTail:
 599     BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 600                      staticFCoeff*FCS->GetBrake(bgCenter) );
 601     break;
 602   case bgNone:
 603     BrakeFCoeff =  rollingFCoeff;
 604     break;
 605   default:
 606     cerr << "Improper brake group membership detected for this gear." << endl;
 607     break;
 608   }
 609 }
 610
 611 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 612 // Compute the sideforce coefficients using Pacejka's Magic Formula.
 613 //
 614 //   y(x) = D sin {C arctan [Bx - E(Bx - arctan Bx)]}
 615 //
 616 // Where: B = Stiffness Factor (0.06, here)
 617 //        C = Shape Factor (2.8, here)
 618 //        D = Peak Factor (0.8, here)
 619 //        E = Curvature Factor (1.03, here)
 620
 621 void FGLGear::ComputeSideForceCoefficient(void)
 622 {
 623   if (ForceY_Table) {
 624     FCoeff = ForceY_Table->GetValue(WheelSlip);
 625   } else {
 626     double StiffSlip = Stiffness*WheelSlip;
 627     FCoeff = Peak * sin(Shape*atan(StiffSlip - Curvature*(StiffSlip - atan(StiffSlip))));
 628   }
 629 }
 630
 631 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 632 // Compute the vertical force on the wheel using square-law damping (per comment
 633 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
 634 // allowing for both square and linear damping force calculation. Also need to
 635 // possibly give a "rebound damping factor" that differs from the compression
 636 // case.
 637
 638 void FGLGear::ComputeVerticalStrutForce(void)
 639 {
 640   double springForce = 0;
 641   double dampForce = 0;
 642
 643   springForce = -compressLength * kSpring;
 644
 645   if (compressSpeed >= 0.0) {
 646
 647     if (eDampType == dtLinear)   dampForce = -compressSpeed * bDamp;
 648     else         dampForce = -compressSpeed * compressSpeed * bDamp;
 649
 650   } else {
 651
 652     if (eDampTypeRebound == dtLinear)
 653       dampForce   = -compressSpeed * bDampRebound;
 654     else
 655       dampForce   =  compressSpeed * compressSpeed * bDampRebound;
 656
 657   }
 658
 659   StrutForce = min(springForce + dampForce, (double)0.0);
 660
 661   // The reaction force of the wheel is always normal to the ground
 662   switch (eContactType) {
 663   case ctBOGEY:
 664     // Project back the strut force in the local coordinate frame of the ground
 665     vFn(eX) = StrutForce / (mTGear.Transposed()*vGroundNormal)(eZ);
 666     break;
 667   case ctSTRUCTURE:
 668     vFn(eX) = -StrutForce;
 669     break;
 670   }
 671
 672   // Remember these values for reporting
 673   MaximumStrutForce = max(MaximumStrutForce, fabs(StrutForce));
 674   MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
 675 }
 676
 677 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 678
 679 double FGLGear::GetGearUnitPos(void)
 680 {
 681   // hack to provide backward compatibility to gear/gear-pos-norm property
 682   if( useFCSGearPos || FCS->GetGearPos() != 1.0 ) {
 683     useFCSGearPos = true;
 684     return FCS->GetGearPos();
 685   }
 686   return GearPos;
 687 }
 688
 689 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 690 // Compute the jacobian entries for the friction forces resolution later
 691 // in FGPropagate
 692
 693 void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec)
 694 {
 695   // When the point of contact is moving, dynamic friction is used
 696   // This type of friction is limited to ctSTRUCTURE elements because their
 697   // friction coefficient is the same in every directions
 698   if ((eContactType == ctSTRUCTURE) && (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)) {
 699     FGColumnVector3 velocityDirection = vLocalWhlVel;
 700
 701     StaticFriction = false;
 702
 703     velocityDirection(eX) = 0.;
 704     velocityDirection.Normalize();
 705
 706     LMultiplier[ftDynamic].ForceJacobian = Transform()*velocityDirection;
 707     LMultiplier[ftDynamic].MomentJacobian = vWhlContactVec * LMultiplier[ftDynamic].ForceJacobian;
 708     LMultiplier[ftDynamic].Max = 0.;
 709     LMultiplier[ftDynamic].Min = -fabs(dynamicFCoeff * vFn(eX));
 710     LMultiplier[ftDynamic].value = Constrain(LMultiplier[ftDynamic].Min, LMultiplier[ftDynamic].value, LMultiplier[ftDynamic].Max);
 711   }
 712   else {
 713     // Static friction is used for ctSTRUCTURE when the contact point is not moving.
 714     // It is always used for ctBOGEY elements because the friction coefficients
 715     // of a tyre depend on the direction of the movement (roll & side directions).
 716     // This cannot be handled properly by the so-called "dynamic friction".
 717     StaticFriction = true;
 718
 719     LMultiplier[ftRoll].ForceJacobian = Transform()*FGColumnVector3(0.,1.,0.);
 720     LMultiplier[ftSide].ForceJacobian = Transform()*FGColumnVector3(0.,0.,1.);
 721     LMultiplier[ftRoll].MomentJacobian = vWhlContactVec * LMultiplier[ftRoll].ForceJacobian;
 722     LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian;
 723
 724     switch(eContactType) {
 725     case ctBOGEY:
 726       LMultiplier[ftRoll].Max = fabs(BrakeFCoeff * vFn(eX));
 727       LMultiplier[ftSide].Max = fabs(FCoeff * vFn(eX));
 728       break;
 729     case ctSTRUCTURE:
 730       LMultiplier[ftRoll].Max = fabs(staticFCoeff * vFn(eX));
 731       LMultiplier[ftSide].Max = fabs(staticFCoeff * vFn(eX));
 732       break;
 733     }
 734
 735     LMultiplier[ftRoll].Min = -LMultiplier[ftRoll].Max;
 736     LMultiplier[ftSide].Min = -LMultiplier[ftSide].Max;
 737     LMultiplier[ftRoll].value = Constrain(LMultiplier[ftRoll].Min, LMultiplier[ftRoll].value, LMultiplier[ftRoll].Max);
 738     LMultiplier[ftSide].value = Constrain(LMultiplier[ftSide].Min, LMultiplier[ftSide].value, LMultiplier[ftSide].Max);
 739   }
 740 }
 741
 742 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 743 // This function is used by the MultiplierIterator class to enumerate the
 744 // Lagrange multipliers of a landing gear. This allows to encapsulate the storage
 745 // of the multipliers in FGLGear without exposing it. From an outside point of
 746 // view, each FGLGear instance has a number of Lagrange multipliers which can be
 747 // accessed through this routine without knowing the exact constraint which they
 748 // model.
 749
 750 FGPropagate::LagrangeMultiplier* FGLGear::GetMultiplierEntry(int entry)
 751 {
 752   switch(entry) {
 753   case 0:
 754     if (StaticFriction)
 755       return &LMultiplier[ftRoll];
 756     else
 757       return &LMultiplier[ftDynamic];
 758   case 1:
 759     if (StaticFriction)
 760       return &LMultiplier[ftSide];
 761   default:
 762     return NULL;
 763   }
 764 }
 765
 766 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 767 // This routine is called after the Lagrange multiplier has been computed. The
 768 // friction forces of the landing gear are then updated accordingly.
 769 FGColumnVector3& FGLGear::UpdateForces(void)
 770 {
 771   if (StaticFriction) {
 772     vFn(eY) = LMultiplier[ftRoll].value;
 773     vFn(eZ) = LMultiplier[ftSide].value;
 774   }
 775   else
 776     vFn += LMultiplier[ftDynamic].value * (Transform ().Transposed() * LMultiplier[ftDynamic].ForceJacobian);
 777
 778   // Return the updated force in the body frame
 779   return FGForce::GetBodyForces();
 780 }
 781
 782 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 783
 784 void FGLGear::bind(void)
 785 {
 786   string property_name;
 787   string base_property_name;
 788   base_property_name = CreateIndexedPropertyName("gear/unit", GearNumber);
 789   if (eContactType == ctBOGEY) {
 790     property_name = base_property_name + "/slip-angle-deg";
 791     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WheelSlip );
 792     property_name = base_property_name + "/WOW";
 793     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WOW );
 794     property_name = base_property_name + "/wheel-speed-fps";
 795     fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGLGear*)this,
 796                           &FGLGear::GetWheelRollVel);
 797     property_name = base_property_name + "/z-position";
 798     fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGForce*)this,
 799                           &FGForce::GetLocationZ, &FGForce::SetLocationZ);
 800     property_name = base_property_name + "/compression-ft";
 801     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &compressLength );
 802     property_name = base_property_name + "/side_friction_coeff";
 803     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &FCoeff );
 804
 805     property_name = base_property_name + "/static_friction_coeff";
 806     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff );
 807
 808     if (eSteerType == stCaster) {
 809       property_name = base_property_name + "/steering-angle-deg";
 810       fdmex->GetPropertyManager()->Tie( property_name.c_str(), this, &FGLGear::GetSteerAngleDeg );
 811       property_name = base_property_name + "/castered";
 812       fdmex->GetPropertyManager()->Tie( property_name.c_str(), &Castered);
 813     }
 814   }
 815
 816   if( isRetractable ) {
 817     property_name = base_property_name + "/pos-norm";
 818     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &GearPos );
 819   }
 820 }
 821
 822 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 823
 824 void FGLGear::Report(ReportType repType)
 825 {
 826   if (fabs(TakeoffDistanceTraveled) < 0.001) return; // Don't print superfluous reports
 827
 828   switch(repType) {
 829   case erLand:
 830     cout << endl << "Touchdown report for " << name << " (WOW at time: "
 831          << fdmex->GetSimTime() << " seconds)" << endl;
 832     cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
 833                                 << SinkRate*0.3048          << " mps"     << endl;
 834     cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
 835                                 << GroundSpeed*0.3048       << " mps"     << endl;
 836     cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
 837                                 << MaximumStrutForce*4.448  << " Newtons" << endl;
 838     cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
 839                                 << MaximumStrutTravel*30.48 << " cm"      << endl;
 840     cout << "  Distance traveled:     " << LandingDistanceTraveled        << " ft,     "
 841                                 << LandingDistanceTraveled*0.3048  << " meters"  << endl;
 842     LandingReported = true;
 843     break;
 844   case erTakeoff:
 845     cout << endl << "Takeoff report for " << name << " (Liftoff at time: "
 846         << fdmex->GetSimTime() << " seconds)" << endl;
 847     cout << "  Distance traveled:                " << TakeoffDistanceTraveled
 848          << " ft,     " << TakeoffDistanceTraveled*0.3048  << " meters"  << endl;
 849     cout << "  Distance traveled (over 50'):     " << TakeoffDistanceTraveled50ft
 850          << " ft,     " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
 851     cout << "  [Altitude (ASL): " << Propagate->GetAltitudeASL() << " ft. / "
 852          << Propagate->GetAltitudeASLmeters() << " m  | Temperature: "
 853          << fdmex->GetAtmosphere()->GetTemperature() - 459.67 << " F / "
 854          << RankineToCelsius(fdmex->GetAtmosphere()->GetTemperature()) << " C]" << endl;
 855     cout << "  [Velocity (KCAS): " << Auxiliary->GetVcalibratedKTS() << "]" << endl;
 856     TakeoffReported = true;
 857     break;
 858   case erNone:
 859     break;
 860   }
 861 }
 862
 863 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 864 //    The bitmasked value choices are as follows:
 865 //    unset: In this case (the default) JSBSim would only print
 866 //       out the normally expected messages, essentially echoing
 867 //       the config files as they are read. If the environment
 868 //       variable is not set, debug_lvl is set to 1 internally
 869 //    0: This requests JSBSim not to output any messages
 870 //       whatsoever.
 871 //    1: This value explicity requests the normal JSBSim
 872 //       startup messages
 873 //    2: This value asks for a message to be printed out when
 874 //       a class is instantiated
 875 //    4: When this value is set, a message is displayed when a
 876 //       FGModel object executes its Run() method
 877 //    8: When this value is set, various runtime state variables
 878 //       are printed out periodically
 879 //    16: When set various parameters are sanity checked and
 880 //       a message is printed out when they go out of bounds
 881
 882 void FGLGear::Debug(int from)
 883 {
 884   if (debug_lvl <= 0) return;
 885
 886   if (debug_lvl & 1) { // Standard console startup message output
 887     if (from == 0) { // Constructor - loading and initialization
 888       cout << "    " << sContactType << " " << name          << endl;
 889       cout << "      Location: "         << vXYZn          << endl;
 890       cout << "      Spring Constant:  " << kSpring       << endl;
 891
 892       if (eDampType == dtLinear)
 893         cout << "      Damping Constant: " << bDamp << " (linear)" << endl;
 894       else
 895         cout << "      Damping Constant: " << bDamp << " (square law)" << endl;
 896
 897       if (eDampTypeRebound == dtLinear)
 898         cout << "      Rebound Damping Constant: " << bDampRebound << " (linear)" << endl;
 899       else
 900         cout << "      Rebound Damping Constant: " << bDampRebound << " (square law)" << endl;
 901
 902       cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
 903       cout << "      Static Friction:  " << staticFCoeff  << endl;
 904       if (eContactType == ctBOGEY) {
 905         cout << "      Rolling Friction: " << rollingFCoeff << endl;
 906         cout << "      Steering Type:    " << sSteerType    << endl;
 907         cout << "      Grouping:         " << sBrakeGroup   << endl;
 908         cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
 909         cout << "      Retractable:      " << isRetractable  << endl;
 910       }
 911     }
 912   }
 913   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 914     if (from == 0) cout << "Instantiated: FGLGear" << endl;
 915     if (from == 1) cout << "Destroyed:    FGLGear" << endl;
 916   }
 917   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 918   }
 919   if (debug_lvl & 8 ) { // Runtime state variables
 920   }
 921   if (debug_lvl & 16) { // Sanity checking
 922   }
 923   if (debug_lvl & 64) {
 924     if (from == 0) { // Constructor
 925       cout << IdSrc << endl;
 926       cout << IdHdr << endl;
 927     }
 928   }
 929 }
 930
 931 } // namespace JSBSim
 932