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