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