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