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