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