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 <cstdlib>
  44 #include <cstring>
  45
  46 #include "FGLGear.h"
  47 #include "input_output/FGPropertyManager.h"
  48 #include "models/FGGroundReactions.h"
  49 #include "math/FGTable.h"
  50
  51 using namespace std;
  52
  53 namespace JSBSim {
  54
  55 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  56 DEFINITIONS
  57 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  58
  59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  60 GLOBAL DATA
  61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  62
  63 static const char *IdSrc = "$Id: FGLGear.cpp,v 1.88 2011/08/30 21:05:56 bcoconni Exp $";
  64 static const char *IdHdr = ID_LGEAR;
  65
  66 // Body To Structural (body frame is rotated 180 deg about Y and lengths are given in
  67 // ft instead of inches)
  68 const FGMatrix33 FGLGear::Tb2s(-1./inchtoft, 0., 0., 0., 1./inchtoft, 0., 0., 0., -1./inchtoft);
  69
  70 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  71 CLASS IMPLEMENTATION
  72 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  73
  74 FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number, const struct Inputs& inputs) :
  75   FGForce(fdmex),
  76   GearNumber(number),
  77   SteerAngle(0.0),
  78   Castered(false),
  79   StaticFriction(false),
  80   in(inputs)
  81 {
  82   Element *force_table=0;
  83   Element *dampCoeff=0;
  84   Element *dampCoeffRebound=0;
  85   string force_type="";
  86
  87   kSpring = bDamp = bDampRebound = dynamicFCoeff = staticFCoeff = rollingFCoeff = maxSteerAngle = 0;
  88   sSteerType = sBrakeGroup = sSteerType = "";
  89   isRetractable = 0;
  90   eDampType = dtLinear;
  91   eDampTypeRebound = dtLinear;
  92
  93   name = el->GetAttributeValue("name");
  94   sContactType = el->GetAttributeValue("type");
  95   if (sContactType == "BOGEY") {
  96     eContactType = ctBOGEY;
  97   } else if (sContactType == "STRUCTURE") {
  98     eContactType = ctSTRUCTURE;
  99   } else {
 100     // Unknown contact point types will be treated as STRUCTURE.
 101     eContactType = ctSTRUCTURE;
 102   }
 103
 104   // Default values for structural contact points
 105   if (eContactType == ctSTRUCTURE) {
 106     kSpring = in.EmptyWeight;
 107     bDamp = kSpring;
 108     bDampRebound = kSpring * 10;
 109     staticFCoeff = 1.0;
 110     dynamicFCoeff = 1.0;
 111   }
 112
 113   if (el->FindElement("spring_coeff"))
 114     kSpring = el->FindElementValueAsNumberConvertTo("spring_coeff", "LBS/FT");
 115   if (el->FindElement("damping_coeff")) {
 116     dampCoeff = el->FindElement("damping_coeff");
 117     if (dampCoeff->GetAttributeValue("type") == "SQUARE") {
 118       eDampType = dtSquare;
 119       bDamp   = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT2/SEC2");
 120     } else {
 121       bDamp   = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT/SEC");
 122     }
 123   }
 124
 125   if (el->FindElement("damping_coeff_rebound")) {
 126     dampCoeffRebound = el->FindElement("damping_coeff_rebound");
 127     if (dampCoeffRebound->GetAttributeValue("type") == "SQUARE") {
 128       eDampTypeRebound = dtSquare;
 129       bDampRebound   = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT2/SEC2");
 130     } else {
 131       bDampRebound   = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT/SEC");
 132     }
 133   } else {
 134     bDampRebound   = bDamp;
 135     eDampTypeRebound = eDampType;
 136   }
 137
 138   if (el->FindElement("dynamic_friction"))
 139     dynamicFCoeff = el->FindElementValueAsNumber("dynamic_friction");
 140   if (el->FindElement("static_friction"))
 141     staticFCoeff = el->FindElementValueAsNumber("static_friction");
 142   if (el->FindElement("rolling_friction"))
 143     rollingFCoeff = el->FindElementValueAsNumber("rolling_friction");
 144   if (el->FindElement("max_steer"))
 145     maxSteerAngle = el->FindElementValueAsNumberConvertTo("max_steer", "DEG");
 146   if (el->FindElement("retractable"))
 147     isRetractable = ((unsigned int)el->FindElementValueAsNumber("retractable"))>0.0?true:false;
 148
 149   GroundReactions = fdmex->GetGroundReactions();
 150   PropertyManager = fdmex->GetPropertyManager();
 151
 152   ForceY_Table = 0;
 153   force_table = el->FindElement("table");
 154   while (force_table) {
 155     force_type = force_table->GetAttributeValue("type");
 156     if (force_type == "CORNERING_COEFF") {
 157       ForceY_Table = new FGTable(PropertyManager, force_table);
 158     } else {
 159       cerr << "Undefined force table for " << name << " contact point" << endl;
 160     }
 161     force_table = el->FindNextElement("table");
 162   }
 163
 164   sBrakeGroup = el->FindElementValue("brake_group");
 165
 166   if (maxSteerAngle == 360) sSteerType = "CASTERED";
 167   else if (maxSteerAngle == 0.0) sSteerType = "FIXED";
 168   else sSteerType = "STEERABLE";
 169
 170   Element* element = el->FindElement("location");
 171   if (element) vXYZn = element->FindElementTripletConvertTo("IN");
 172   else {cerr << "No location given for contact " << name << endl; exit(-1);}
 173   SetTransformType(FGForce::tCustom);
 174
 175   element = el->FindElement("orientation");
 176   if (element && (eContactType == ctBOGEY)) {
 177     vGearOrient = element->FindElementTripletConvertTo("RAD");
 178
 179     double cp,sp,cr,sr,cy,sy;
 180
 181     cp=cos(vGearOrient(ePitch)); sp=sin(vGearOrient(ePitch));
 182     cr=cos(vGearOrient(eRoll));  sr=sin(vGearOrient(eRoll));
 183     cy=cos(vGearOrient(eYaw));   sy=sin(vGearOrient(eYaw));
 184
 185     mTGear(1,1) =  cp*cy;
 186     mTGear(2,1) =  cp*sy;
 187     mTGear(3,1) = -sp;
 188
 189     mTGear(1,2) = sr*sp*cy - cr*sy;
 190     mTGear(2,2) = sr*sp*sy + cr*cy;
 191     mTGear(3,2) = sr*cp;
 192
 193     mTGear(1,3) = cr*sp*cy + sr*sy;
 194     mTGear(2,3) = cr*sp*sy - sr*cy;
 195     mTGear(3,3) = cr*cp;
 196   }
 197   else {
 198     mTGear(1,1) = 1.;
 199     mTGear(2,2) = 1.;
 200     mTGear(3,3) = 1.;
 201   }
 202
 203   if      (sBrakeGroup == "LEFT"  ) eBrakeGrp = bgLeft;
 204   else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
 205   else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
 206   else if (sBrakeGroup == "NOSE"  ) eBrakeGrp = bgNose;
 207   else if (sBrakeGroup == "TAIL"  ) eBrakeGrp = bgTail;
 208   else if (sBrakeGroup == "NONE"  ) eBrakeGrp = bgNone;
 209   else if (sBrakeGroup.empty()    ) {eBrakeGrp = bgNone;
 210                                      sBrakeGroup = "NONE (defaulted)";}
 211   else {
 212     cerr << "Improper braking group specification in config file: "
 213          << sBrakeGroup << " is undefined." << endl;
 214   }
 215
 216   if      (sSteerType == "STEERABLE") eSteerType = stSteer;
 217   else if (sSteerType == "FIXED"    ) eSteerType = stFixed;
 218   else if (sSteerType == "CASTERED" ) {eSteerType = stCaster; Castered = true;}
 219   else if (sSteerType.empty()       ) {eSteerType = stFixed;
 220                                        sSteerType = "FIXED (defaulted)";}
 221   else {
 222     cerr << "Improper steering type specification in config file: "
 223          << sSteerType << " is undefined." << endl;
 224   }
 225
 226   GearUp = false;
 227   GearDown = true;
 228   GearPos  = 1.0;
 229   useFCSGearPos = false;
 230   Servicable = true;
 231
 232 // Add some AI here to determine if gear is located properly according to its
 233 // brake group type ??
 234
 235   WOW = lastWOW = false;
 236   ReportEnable = true;
 237   FirstContact = false;
 238   StartedGroundRun = false;
 239   TakeoffReported = LandingReported = false;
 240   LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;
 241   MaximumStrutForce = MaximumStrutTravel = 0.0;
 242   SinkRate = GroundSpeed = 0.0;
 243
 244   vWhlVelVec.InitMatrix();
 245
 246   compressLength  = 0.0;
 247   compressSpeed   = 0.0;
 248   brakePct        = 0.0;
 249   maxCompLen      = 0.0;
 250
 251   WheelSlip = 0.0;
 252   TirePressureNorm = 1.0;
 253
 254   // Set Pacejka terms
 255
 256   Stiffness = 0.06;
 257   Shape = 2.8;
 258   Peak = staticFCoeff;
 259   Curvature = 1.03;
 260
 261   // Initialize Lagrange multipliers
 262   memset(LMultiplier, 0, sizeof(LMultiplier));
 263
 264   Debug(0);
 265 }
 266
 267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 268
 269 FGLGear::~FGLGear()
 270 {
 271   delete ForceY_Table;
 272   Debug(1);
 273 }
 274
 275 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 276
 277 FGColumnVector3& FGLGear::GetBodyForces(void)
 278 {
 279   double t = fdmex->GetSimTime();
 280
 281   vFn.InitMatrix();
 282
 283   if (isRetractable) ComputeRetractionState();
 284
 285   if (GearDown) {
 286     FGColumnVector3 terrainVel, dummy;
 287
 288     vLocalGear = in.Tb2l * in.vWhlBodyVec[GearNumber]; // Get local frame wheel location
 289
 290     gearLoc = in.Location.LocalToLocation(vLocalGear);
 291     // Compute the height of the theoretical location of the wheel (if strut is
 292     // not compressed) with respect to the ground level
 293     double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, terrainVel, dummy);
 294     vGroundNormal = in.Tec2b * normal;
 295
 296     // The height returned above is the AGL and is expressed in the Z direction
 297     // of the ECEF coordinate frame. We now need to transform this height in
 298     // actual compression of the strut (BOGEY) of in the normal direction to the
 299     // ground (STRUCTURE)
 300     double normalZ = (in.Tec2l*normal)(eZ);
 301     double LGearProj = -(mTGear.Transposed() * vGroundNormal)(eZ);
 302
 303     switch (eContactType) {
 304     case ctBOGEY:
 305       compressLength = LGearProj > 0.0 ? height * normalZ / LGearProj : 0.0;
 306       break;
 307     case ctSTRUCTURE:
 308       compressLength = height * normalZ / DotProduct(normal, normal);
 309       break;
 310     }
 311
 312     if (compressLength > 0.00) {
 313       WOW = true;
 314
 315       // The following equations use the vector to the tire contact patch
 316       // including the strut compression.
 317       FGColumnVector3 vWhlDisplVec;
 318
 319       switch(eContactType) {
 320       case ctBOGEY:
 321         vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength);
 322         break;
 323       case ctSTRUCTURE:
 324         vWhlDisplVec = compressLength * vGroundNormal;
 325         break;
 326       }
 327
 328       FGColumnVector3 vWhlContactVec = in.vWhlBodyVec[GearNumber] + vWhlDisplVec;
 329       vActingXYZn = vXYZn + Tb2s * vWhlDisplVec;
 330       FGColumnVector3 vBodyWhlVel = in.PQR * vWhlContactVec;
 331       vBodyWhlVel += in.UVW - in.Tec2b * terrainVel;
 332
 333       vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
 334
 335       InitializeReporting();
 336       ComputeSteeringAngle();
 337       ComputeGroundCoordSys();
 338
 339       vLocalWhlVel = Transform().Transposed() * vBodyWhlVel;
 340
 341       if (fdmex->GetTrimStatus())
 342         compressSpeed = 0.0; // Steady state is sought during trimming
 343       else {
 344         compressSpeed = -vLocalWhlVel(eX);
 345         if (eContactType == ctBOGEY)
 346           compressSpeed /= LGearProj;
 347       }
 348
 349       ComputeVerticalStrutForce();
 350
 351       // Compute the friction coefficients in the wheel ground plane.
 352       if (eContactType == ctBOGEY) {
 353         ComputeSlipAngle();
 354         ComputeBrakeForceCoefficient();
 355         ComputeSideForceCoefficient();
 356       }
 357
 358       // Prepare the Jacobians and the Lagrange multipliers for later friction
 359       // forces calculations.
 360       ComputeJacobian(vWhlContactVec);
 361
 362     } else { // Gear is NOT compressed
 363
 364       WOW = false;
 365       compressLength = 0.0;
 366       compressSpeed = 0.0;
 367       WheelSlip = 0.0;
 368       StrutForce = 0.0;
 369
 370       // Let wheel spin down slowly
 371       vWhlVelVec(eX) -= 13.0 * in.TotalDeltaT;
 372       if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0;
 373
 374       // Return to neutral position between 1.0 and 0.8 gear pos.
 375       SteerAngle *= max(GetGearUnitPos()-0.8, 0.0)/0.2;
 376
 377       ResetReporting();
 378     }
 379   }
 380
 381   if (!fdmex->GetTrimStatus()) {
 382     ReportTakeoffOrLanding();
 383
 384     // Require both WOW and LastWOW to be true before checking crash conditions
 385     // to allow the WOW flag to be used in terminating a scripted run.
 386     if (WOW && lastWOW) CrashDetect();
 387
 388     lastWOW = WOW;
 389   }
 390
 391   return FGForce::GetBodyForces();
 392 }
 393
 394 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 395 // Build a local "ground" coordinate system defined by
 396 //  eX : normal to the ground
 397 //  eY : projection of the rolling direction on the ground
 398 //  eZ : projection of the sliping direction on the ground
 399
 400 void FGLGear::ComputeGroundCoordSys(void)
 401 {
 402   // Euler angles are built up to create a local frame to describe the forces
 403   // applied to the gear by the ground. Here pitch, yaw and roll do not have
 404   // any physical meaning. It is just a convenient notation.
 405   // First, "pitch" and "yaw" are determined in order to align eX with the
 406   // ground normal.
 407   if (vGroundNormal(eZ) < -1.0)
 408     vOrient(ePitch) = 0.5*M_PI;
 409   else if (1.0 < vGroundNormal(eZ))
 410     vOrient(ePitch) = -0.5*M_PI;
 411   else
 412     vOrient(ePitch) = asin(-vGroundNormal(eZ));
 413
 414   if (fabs(vOrient(ePitch)) == 0.5*M_PI)
 415     vOrient(eYaw) = 0.;
 416   else
 417     vOrient(eYaw) = atan2(vGroundNormal(eY), vGroundNormal(eX));
 418
 419   vOrient(eRoll) = 0.;
 420   UpdateCustomTransformMatrix();
 421
 422   if (eContactType == ctBOGEY) {
 423     // In the case of a bogey, the third angle "roll" is used to align the axis eY and eZ
 424     // to the rolling and sliping direction respectively.
 425     FGColumnVector3 updatedRollingAxis = Transform().Transposed() * mTGear
 426                                        * FGColumnVector3(-sin(SteerAngle), cos(SteerAngle), 0.);
 427
 428     vOrient(eRoll) = atan2(updatedRollingAxis(eY), -updatedRollingAxis(eZ));
 429     UpdateCustomTransformMatrix();
 430   }
 431 }
 432
 433 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 434
 435 void FGLGear::ComputeRetractionState(void)
 436 {
 437   double gearPos = GetGearUnitPos();
 438   if (gearPos < 0.01) {
 439     GearUp   = true;
 440     WOW      = false;
 441     GearDown = false;
 442     vWhlVelVec.InitMatrix();
 443   } else if (gearPos > 0.99) {
 444     GearDown = true;
 445     GearUp   = false;
 446   } else {
 447     GearUp   = false;
 448     GearDown = false;
 449   }
 450 }
 451
 452 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 453 // Calculate tire slip angle.
 454
 455 void FGLGear::ComputeSlipAngle(void)
 456 {
 457 // Check that the speed is non-null otherwise use the current angle
 458   if (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)
 459     WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
 460 }
 461
 462 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 463 // Compute the steering angle in any case.
 464 // This will also make sure that animations will look right.
 465
 466 void FGLGear::ComputeSteeringAngle(void)
 467 {
 468   switch (eSteerType) {
 469   case stSteer:
 470     SteerAngle = degtorad * in.SteerPosDeg[GearNumber];
 471     break;
 472   case stFixed:
 473     SteerAngle = 0.0;
 474     break;
 475   case stCaster:
 476     if (!Castered)
 477       SteerAngle = degtorad * in.SteerPosDeg[GearNumber];
 478     else {
 479       // Check that the speed is non-null otherwise use the current angle
 480       if (vWhlVelVec.Magnitude(eX,eY) > 0.1)
 481         SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
 482     }
 483     break;
 484   default:
 485     cerr << "Improper steering type membership detected for this gear." << endl;
 486     break;
 487   }
 488 }
 489
 490 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 491 // Reset reporting functionality after takeoff
 492
 493 void FGLGear::ResetReporting(void)
 494 {
 495   if (in.DistanceAGL > 200.0) {
 496     FirstContact = false;
 497     StartedGroundRun = false;
 498     LandingReported = false;
 499     TakeoffReported = true;
 500     LandingDistanceTraveled = 0.0;
 501     MaximumStrutForce = MaximumStrutTravel = 0.0;
 502   }
 503 }
 504
 505 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 506
 507 void FGLGear::InitializeReporting(void)
 508 {
 509   // If this is the first time the wheel has made contact, remember some values
 510   // for later printout.
 511
 512   if (!FirstContact) {
 513     FirstContact  = true;
 514     SinkRate      =  compressSpeed;
 515     GroundSpeed   =  in.Vground;
 516     TakeoffReported = false;
 517   }
 518
 519   // If the takeoff run is starting, initialize.
 520
 521   if ((in.Vground > 0.1) &&
 522       (in.BrakePos[bgLeft] == 0) &&
 523       (in.BrakePos[bgRight] == 0) &&
 524       (in.TakeoffThrottle && !StartedGroundRun))
 525   {
 526     TakeoffDistanceTraveled = 0;
 527     TakeoffDistanceTraveled50ft = 0;
 528     StartedGroundRun = true;
 529   }
 530 }
 531
 532 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 533 // Takeoff and landing reporting functionality
 534
 535 void FGLGear::ReportTakeoffOrLanding(void)
 536 {
 537   if (FirstContact)
 538     LandingDistanceTraveled += in.Vground * in.TotalDeltaT;
 539
 540   if (StartedGroundRun) {
 541     TakeoffDistanceTraveled50ft += in.Vground * in.TotalDeltaT;
 542     if (WOW) TakeoffDistanceTraveled += in.Vground * in.TotalDeltaT;
 543   }
 544
 545   if ( ReportEnable
 546        && in.Vground <= 0.05
 547        && !LandingReported
 548        && in.WOW)
 549   {
 550     if (debug_lvl > 0) Report(erLand);
 551   }
 552
 553   if ( ReportEnable
 554        && !TakeoffReported
 555        && (in.DistanceAGL - vLocalGear(eZ)) > 50.0
 556        && !in.WOW)
 557   {
 558     if (debug_lvl > 0) Report(erTakeoff);
 559   }
 560
 561   if (lastWOW != WOW) PutMessage("GEAR_CONTACT: " + name, WOW);
 562 }
 563
 564 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 565 // Crash detection logic (really out-of-bounds detection)
 566
 567 void FGLGear::CrashDetect(void)
 568 {
 569   if ( (compressLength > 500.0 ||
 570       vFn.Magnitude() > 100000000.0 ||
 571       GetMoments().Magnitude() > 5000000000.0 ||
 572       SinkRate > 1.4666*30 ) && !fdmex->IntegrationSuspended())
 573   {
 574     PutMessage("Crash Detected: Simulation FREEZE.");
 575     fdmex->SuspendIntegration();
 576   }
 577 }
 578
 579 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 580 // The following needs work regarding friction coefficients and braking and
 581 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
 582 // It also assumes that we won't be turning and braking at the same time.
 583 // Will fix this later.
 584 // [JSB] The braking force coefficients include normal rolling coefficient +
 585 // a percentage of the static friction coefficient based on braking applied.
 586
 587 void FGLGear::ComputeBrakeForceCoefficient(void)
 588 {
 589   switch (eBrakeGrp) {
 590   case bgLeft:
 591     BrakeFCoeff =  ( rollingFCoeff * (1.0 - in.BrakePos[bgLeft]) +
 592                      staticFCoeff * in.BrakePos[bgLeft] );
 593     break;
 594   case bgRight:
 595     BrakeFCoeff =  ( rollingFCoeff * (1.0 - in.BrakePos[bgRight]) +
 596                      staticFCoeff * in.BrakePos[bgRight] );
 597     break;
 598   case bgCenter:
 599     BrakeFCoeff =  ( rollingFCoeff * (1.0 - in.BrakePos[bgCenter]) +
 600                      staticFCoeff * in.BrakePos[bgCenter] );
 601     break;
 602   case bgNose:
 603     BrakeFCoeff =  ( rollingFCoeff * (1.0 - in.BrakePos[bgCenter]) +
 604                      staticFCoeff * in.BrakePos[bgCenter] );
 605     break;
 606   case bgTail:
 607     BrakeFCoeff =  ( rollingFCoeff * (1.0 - in.BrakePos[bgCenter]) +
 608                      staticFCoeff * in.BrakePos[bgCenter] );
 609     break;
 610   case bgNone:
 611     BrakeFCoeff =  rollingFCoeff;
 612     break;
 613   default:
 614     cerr << "Improper brake group membership detected for this gear." << endl;
 615     break;
 616   }
 617 }
 618
 619 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 620 // Compute the sideforce coefficients using Pacejka's Magic Formula.
 621 //
 622 //   y(x) = D sin {C arctan [Bx - E(Bx - arctan Bx)]}
 623 //
 624 // Where: B = Stiffness Factor (0.06, here)
 625 //        C = Shape Factor (2.8, here)
 626 //        D = Peak Factor (0.8, here)
 627 //        E = Curvature Factor (1.03, here)
 628
 629 void FGLGear::ComputeSideForceCoefficient(void)
 630 {
 631   if (ForceY_Table) {
 632     FCoeff = ForceY_Table->GetValue(WheelSlip);
 633   } else {
 634     double StiffSlip = Stiffness*WheelSlip;
 635     FCoeff = Peak * sin(Shape*atan(StiffSlip - Curvature*(StiffSlip - atan(StiffSlip))));
 636   }
 637 }
 638
 639 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 640 // Compute the vertical force on the wheel using square-law damping (per comment
 641 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
 642 // allowing for both square and linear damping force calculation. Also need to
 643 // possibly give a "rebound damping factor" that differs from the compression
 644 // case.
 645
 646 void FGLGear::ComputeVerticalStrutForce(void)
 647 {
 648   double springForce = 0;
 649   double dampForce = 0;
 650
 651   springForce = -compressLength * kSpring;
 652
 653   if (compressSpeed >= 0.0) {
 654
 655     if (eDampType == dtLinear)   dampForce = -compressSpeed * bDamp;
 656     else         dampForce = -compressSpeed * compressSpeed * bDamp;
 657
 658   } else {
 659
 660     if (eDampTypeRebound == dtLinear)
 661       dampForce   = -compressSpeed * bDampRebound;
 662     else
 663       dampForce   =  compressSpeed * compressSpeed * bDampRebound;
 664
 665   }
 666
 667   StrutForce = min(springForce + dampForce, (double)0.0);
 668
 669   // The reaction force of the wheel is always normal to the ground
 670   switch (eContactType) {
 671   case ctBOGEY:
 672     // Project back the strut force in the local coordinate frame of the ground
 673     vFn(eX) = StrutForce / (mTGear.Transposed()*vGroundNormal)(eZ);
 674     break;
 675   case ctSTRUCTURE:
 676     vFn(eX) = -StrutForce;
 677     break;
 678   }
 679
 680   // Remember these values for reporting
 681   MaximumStrutForce = max(MaximumStrutForce, fabs(StrutForce));
 682   MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
 683 }
 684
 685 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 686
 687 double FGLGear::GetGearUnitPos(void)
 688 {
 689   // hack to provide backward compatibility to gear/gear-pos-norm property
 690   if( useFCSGearPos || in.FCSGearPos != 1.0 ) {
 691     useFCSGearPos = true;
 692     return in.FCSGearPos;
 693   }
 694   return GearPos;
 695 }
 696
 697 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 698 // Compute the jacobian entries for the friction forces resolution later
 699 // in FGPropagate
 700
 701 void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec)
 702 {
 703   // When the point of contact is moving, dynamic friction is used
 704   // This type of friction is limited to ctSTRUCTURE elements because their
 705   // friction coefficient is the same in every directions
 706   if ((eContactType == ctSTRUCTURE) && (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)) {
 707     FGColumnVector3 velocityDirection = vLocalWhlVel;
 708
 709     StaticFriction = false;
 710
 711     velocityDirection(eX) = 0.;
 712     velocityDirection.Normalize();
 713
 714     LMultiplier[ftDynamic].ForceJacobian = Transform()*velocityDirection;
 715     LMultiplier[ftDynamic].MomentJacobian = vWhlContactVec * LMultiplier[ftDynamic].ForceJacobian;
 716     LMultiplier[ftDynamic].Max = 0.;
 717     LMultiplier[ftDynamic].Min = -fabs(dynamicFCoeff * vFn(eX));
 718
 719     // The Lagrange multiplier value obtained from the previous iteration is kept
 720     // This is supposed to accelerate the convergence of the projected Gauss-Seidel
 721     // algorithm. The code just below is to make sure that the initial value
 722     // is consistent with the current friction coefficient and normal reaction.
 723     LMultiplier[ftDynamic].value = Constrain(LMultiplier[ftDynamic].Min, LMultiplier[ftDynamic].value, LMultiplier[ftDynamic].Max);
 724
 725     GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftDynamic]);
 726   }
 727   else {
 728     // Static friction is used for ctSTRUCTURE when the contact point is not moving.
 729     // It is always used for ctBOGEY elements because the friction coefficients
 730     // of a tyre depend on the direction of the movement (roll & side directions).
 731     // This cannot be handled properly by the so-called "dynamic friction".
 732     StaticFriction = true;
 733
 734     LMultiplier[ftRoll].ForceJacobian = Transform()*FGColumnVector3(0.,1.,0.);
 735     LMultiplier[ftSide].ForceJacobian = Transform()*FGColumnVector3(0.,0.,1.);
 736     LMultiplier[ftRoll].MomentJacobian = vWhlContactVec * LMultiplier[ftRoll].ForceJacobian;
 737     LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian;
 738
 739     switch(eContactType) {
 740     case ctBOGEY:
 741       LMultiplier[ftRoll].Max = fabs(BrakeFCoeff * vFn(eX));
 742       LMultiplier[ftSide].Max = fabs(FCoeff * vFn(eX));
 743       break;
 744     case ctSTRUCTURE:
 745       LMultiplier[ftRoll].Max = fabs(staticFCoeff * vFn(eX));
 746       LMultiplier[ftSide].Max = fabs(staticFCoeff * vFn(eX));
 747       break;
 748     }
 749
 750     LMultiplier[ftRoll].Min = -LMultiplier[ftRoll].Max;
 751     LMultiplier[ftSide].Min = -LMultiplier[ftSide].Max;
 752
 753     // The Lagrange multiplier value obtained from the previous iteration is kept
 754     // This is supposed to accelerate the convergence of the projected Gauss-Seidel
 755     // algorithm. The code just below is to make sure that the initial value
 756     // is consistent with the current friction coefficient and normal reaction.
 757     LMultiplier[ftRoll].value = Constrain(LMultiplier[ftRoll].Min, LMultiplier[ftRoll].value, LMultiplier[ftRoll].Max);
 758     LMultiplier[ftSide].value = Constrain(LMultiplier[ftSide].Min, LMultiplier[ftSide].value, LMultiplier[ftSide].Max);
 759
 760     GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftRoll]);
 761     GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftSide]);
 762   }
 763 }
 764
 765 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 766 // This routine is called after the Lagrange multiplier has been computed in
 767 // the FGAccelerations class. The friction forces of the landing gear are then
 768 // updated accordingly.
 769 void 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
 779 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 780
 781 void FGLGear::bind(void)
 782 {
 783   string property_name;
 784   string base_property_name;
 785
 786   switch(eContactType) {
 787   case ctBOGEY:
 788     base_property_name = CreateIndexedPropertyName("gear/unit", GearNumber);
 789     break;
 790   case ctSTRUCTURE:
 791     base_property_name = CreateIndexedPropertyName("contact/unit", GearNumber);
 792     break;
 793   default:
 794     return;
 795   }
 796
 797   property_name = base_property_name + "/WOW";
 798   PropertyManager->Tie( property_name.c_str(), &WOW );
 799   property_name = base_property_name + "/z-position";
 800   PropertyManager->Tie( property_name.c_str(), (FGForce*)this,
 801                           &FGForce::GetLocationZ, &FGForce::SetLocationZ);
 802   property_name = base_property_name + "/compression-ft";
 803   PropertyManager->Tie( property_name.c_str(), &compressLength );
 804   property_name = base_property_name + "/static_friction_coeff";
 805   PropertyManager->Tie( property_name.c_str(), &staticFCoeff );
 806   property_name = base_property_name + "/dynamic_friction_coeff";
 807   PropertyManager->Tie( property_name.c_str(), &dynamicFCoeff );
 808
 809   if (eContactType == ctBOGEY) {
 810     property_name = base_property_name + "/slip-angle-deg";
 811     PropertyManager->Tie( property_name.c_str(), &WheelSlip );
 812     property_name = base_property_name + "/wheel-speed-fps";
 813     PropertyManager->Tie( property_name.c_str(), (FGLGear*)this,
 814                           &FGLGear::GetWheelRollVel);
 815     property_name = base_property_name + "/side_friction_coeff";
 816     PropertyManager->Tie( property_name.c_str(), &FCoeff );
 817     property_name = base_property_name + "/rolling_friction_coeff";
 818     PropertyManager->Tie( property_name.c_str(), &rollingFCoeff );
 819
 820     if (eSteerType == stCaster) {
 821       property_name = base_property_name + "/steering-angle-deg";
 822       PropertyManager->Tie( property_name.c_str(), this, &FGLGear::GetSteerAngleDeg );
 823       property_name = base_property_name + "/castered";
 824       PropertyManager->Tie( property_name.c_str(), &Castered);
 825     }
 826   }
 827
 828   if( isRetractable ) {
 829     property_name = base_property_name + "/pos-norm";
 830     PropertyManager->Tie( property_name.c_str(), &GearPos );
 831   }
 832 }
 833
 834 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 835
 836 void FGLGear::Report(ReportType repType)
 837 {
 838   if (fabs(TakeoffDistanceTraveled) < 0.001) return; // Don't print superfluous reports
 839
 840   switch(repType) {
 841   case erLand:
 842     cout << endl << "Touchdown report for " << name << " (WOW at time: "
 843          << fdmex->GetSimTime() << " seconds)" << endl;
 844     cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
 845                                 << SinkRate*0.3048          << " mps"     << endl;
 846     cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
 847                                 << GroundSpeed*0.3048       << " mps"     << endl;
 848     cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
 849                                 << MaximumStrutForce*4.448  << " Newtons" << endl;
 850     cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
 851                                 << MaximumStrutTravel*30.48 << " cm"      << endl;
 852     cout << "  Distance traveled:     " << LandingDistanceTraveled        << " ft,     "
 853                                 << LandingDistanceTraveled*0.3048  << " meters"  << endl;
 854     LandingReported = true;
 855     break;
 856   case erTakeoff:
 857     cout << endl << "Takeoff report for " << name << " (Liftoff at time: "
 858         << fdmex->GetSimTime() << " seconds)" << endl;
 859     cout << "  Distance traveled:                " << TakeoffDistanceTraveled
 860          << " ft,     " << TakeoffDistanceTraveled*0.3048  << " meters"  << endl;
 861     cout << "  Distance traveled (over 50'):     " << TakeoffDistanceTraveled50ft
 862          << " ft,     " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
 863     cout << "  [Altitude (ASL): " << in.DistanceASL << " ft. / "
 864          << in.DistanceASL*FGJSBBase::fttom << " m  | Temperature: "
 865          << in.Temperature - 459.67 << " F / "
 866          << RankineToCelsius(in.Temperature) << " C]" << endl;
 867     cout << "  [Velocity (KCAS): " << in.VcalibratedKts << "]" << endl;
 868     TakeoffReported = true;
 869     break;
 870   case erNone:
 871     break;
 872   }
 873 }
 874
 875 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 876 //    The bitmasked value choices are as follows:
 877 //    unset: In this case (the default) JSBSim would only print
 878 //       out the normally expected messages, essentially echoing
 879 //       the config files as they are read. If the environment
 880 //       variable is not set, debug_lvl is set to 1 internally
 881 //    0: This requests JSBSim not to output any messages
 882 //       whatsoever.
 883 //    1: This value explicity requests the normal JSBSim
 884 //       startup messages
 885 //    2: This value asks for a message to be printed out when
 886 //       a class is instantiated
 887 //    4: When this value is set, a message is displayed when a
 888 //       FGModel object executes its Run() method
 889 //    8: When this value is set, various runtime state variables
 890 //       are printed out periodically
 891 //    16: When set various parameters are sanity checked and
 892 //       a message is printed out when they go out of bounds
 893
 894 void FGLGear::Debug(int from)
 895 {
 896   if (debug_lvl <= 0) return;
 897
 898   if (debug_lvl & 1) { // Standard console startup message output
 899     if (from == 0) { // Constructor - loading and initialization
 900       cout << "    " << sContactType << " " << name          << endl;
 901       cout << "      Location: "         << vXYZn          << endl;
 902       cout << "      Spring Constant:  " << kSpring       << endl;
 903
 904       if (eDampType == dtLinear)
 905         cout << "      Damping Constant: " << bDamp << " (linear)" << endl;
 906       else
 907         cout << "      Damping Constant: " << bDamp << " (square law)" << endl;
 908
 909       if (eDampTypeRebound == dtLinear)
 910         cout << "      Rebound Damping Constant: " << bDampRebound << " (linear)" << endl;
 911       else
 912         cout << "      Rebound Damping Constant: " << bDampRebound << " (square law)" << endl;
 913
 914       cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
 915       cout << "      Static Friction:  " << staticFCoeff  << endl;
 916       if (eContactType == ctBOGEY) {
 917         cout << "      Rolling Friction: " << rollingFCoeff << endl;
 918         cout << "      Steering Type:    " << sSteerType    << endl;
 919         cout << "      Grouping:         " << sBrakeGroup   << endl;
 920         cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
 921         cout << "      Retractable:      " << isRetractable  << endl;
 922       }
 923     }
 924   }
 925   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 926     if (from == 0) cout << "Instantiated: FGLGear" << endl;
 927     if (from == 1) cout << "Destroyed:    FGLGear" << endl;
 928   }
 929   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 930   }
 931   if (debug_lvl & 8 ) { // Runtime state variables
 932   }
 933   if (debug_lvl & 16) { // Sanity checking
 934   }
 935   if (debug_lvl & 64) {
 936     if (from == 0) { // Constructor
 937       cout << IdSrc << endl;
 938       cout << IdHdr << endl;
 939     }
 940   }
 941 }
 942
 943 } // namespace JSBSim
 944