src/FDM/JSBSim/FGLGear.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGLGear.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 11/18/99
   6  Purpose:      Encapsulates the landing gear elements
   7  Called by:    FGAircraft
   8
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30
  31 HISTORY
  32 --------------------------------------------------------------------------------
  33 11/18/99   JSB   Created
  34 01/30/01   NHP   Extended gear model to properly simulate steering and braking
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGLGear.h"
  41 #include <algorithm>
  42
  43 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  44 DEFINITIONS
  45 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  46
  47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  48 GLOBAL DATA
  49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  50
  51
  52 static const char *IdSrc = "$Id$";
  53 static const char *IdHdr = ID_LGEAR;
  54
  55 extern short debug_lvl;
  56
  57 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  58 CLASS IMPLEMENTATION
  59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  60
  61 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
  62                                                            vMoment(3),
  63                                                            vWhlBodyVec(3),
  64                                                            Exec(fdmex)
  65 {
  66   string tmp;
  67   *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
  68             >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
  69                   >> rollingFCoeff >> sSteerType >> sBrakeGroup >> maxSteerAngle;
  70
  71   cout << "    Name: " << name << endl;
  72   cout << "      Location: " << vXYZ << endl;
  73   cout << "      Spring Constant:  " << kSpring << endl;
  74   cout << "      Damping Constant: " << bDamp << endl;
  75   cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
  76   cout << "      Static Friction:  " << staticFCoeff << endl;
  77   cout << "      Rolling Friction: " << rollingFCoeff << endl;
  78   cout << "      Steering Type:    " << sSteerType << endl;
  79   cout << "      Grouping:         " << sBrakeGroup << endl;
  80   cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
  81
  82   if      (sBrakeGroup == "LEFT"  ) eBrakeGrp = bgLeft;
  83   else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
  84   else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
  85   else if (sBrakeGroup == "NOSE"  ) eBrakeGrp = bgNose;
  86   else if (sBrakeGroup == "TAIL"  ) eBrakeGrp = bgTail;
  87   else if (sBrakeGroup == "NONE"  ) eBrakeGrp = bgNone;
  88   else {
  89     cerr << "Improper braking group specification in config file: "
  90          << sBrakeGroup << " is undefined." << endl;
  91   }
  92
  93   if      (sSteerType == "STEERABLE") eSteerType = stSteer;
  94   else if (sSteerType == "FIXED"    ) eSteerType = stFixed;
  95   else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
  96   else {
  97     cerr << "Improper steering type specification in config file: "
  98          << sSteerType << " is undefined." << endl;
  99   }
 100
 101 // Add some AI here to determine if gear is located properly according to its
 102 // brake group type ??
 103
 104   State       = Exec->GetState();
 105   Aircraft    = Exec->GetAircraft();
 106   Position    = Exec->GetPosition();
 107   Rotation    = Exec->GetRotation();
 108   FCS         = Exec->GetFCS();
 109
 110   WOW = false;
 111   ReportEnable = true;
 112   FirstContact = false;
 113   Reported = false;
 114   DistanceTraveled = 0.0;
 115   MaximumStrutForce = MaximumStrutTravel = 0.0;
 116
 117   vWhlBodyVec     = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
 118   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
 119   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
 120
 121   vLocalGear = State->GetTb2l() * vWhlBodyVec;
 122
 123   if (debug_lvl & 2) cout << "Instantiated: FGLGear" << endl;
 124 }
 125
 126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 127
 128 FGLGear::FGLGear(const FGLGear& lgear)
 129 {
 130   State    = lgear.State;
 131   Aircraft = lgear.Aircraft;
 132   Position = lgear.Position;
 133   Rotation = lgear.Rotation;
 134   Exec     = lgear.Exec;
 135   FCS      = lgear.FCS;
 136
 137   vXYZ = lgear.vXYZ;
 138   vMoment = lgear.vMoment;
 139   vWhlBodyVec = lgear.vWhlBodyVec;
 140   vLocalGear = lgear.vLocalGear;
 141
 142   WOW                = lgear.WOW;
 143   ReportEnable       = lgear.ReportEnable;
 144   FirstContact       = lgear.FirstContact;
 145   DistanceTraveled   = lgear.DistanceTraveled;
 146   MaximumStrutForce  = lgear.MaximumStrutForce;
 147   MaximumStrutTravel = lgear.MaximumStrutTravel;
 148
 149   kSpring         = lgear.kSpring;
 150   bDamp           = lgear.bDamp;
 151   compressLength  = lgear.compressLength;
 152   compressSpeed   = lgear.compressSpeed;
 153   staticFCoeff    = lgear.staticFCoeff;
 154   dynamicFCoeff   = lgear.dynamicFCoeff;
 155   rollingFCoeff   = lgear.rollingFCoeff;
 156   brakePct        = lgear.brakePct;
 157   maxCompLen      = lgear.maxCompLen;
 158   SinkRate        = lgear.SinkRate;
 159   GroundSpeed     = lgear.GroundSpeed;
 160   Reported        = lgear.Reported;
 161   name            = lgear.name;
 162   sSteerType      = lgear.sSteerType;
 163   eSteerType      = lgear.eSteerType;
 164   sBrakeGroup     = lgear.sBrakeGroup;
 165   eBrakeGrp       = lgear.eBrakeGrp;
 166   maxSteerAngle   = lgear.maxSteerAngle;
 167 }
 168
 169 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 170
 171 FGLGear::~FGLGear()
 172 {
 173   if (debug_lvl & 2) cout << "Destroyed:    FGLGear" << endl;
 174 }
 175
 176 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 177
 178 FGColumnVector FGLGear::Force(void)
 179 {
 180   float SteerGain, SteerAngle, BrakeFCoeff;
 181   float SinWheel, CosWheel, SideWhlVel, RollingWhlVel;
 182   float RudderPedal, RollingForce, SideForce, FCoeff;
 183   float WheelSlip;
 184
 185   FGColumnVector vForce(3);
 186   FGColumnVector vLocalForce(3);
 187   //FGColumnVector vLocalGear(3);     // Vector: CG to this wheel (Local)
 188   FGColumnVector vWhlVelVec(3);     // Velocity of this wheel (Local)
 189
 190   vWhlBodyVec     = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
 191   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
 192   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
 193
 194   vLocalGear = State->GetTb2l() * vWhlBodyVec;
 195
 196 // For now, gear compression is assumed to happen in the Local Z axis,
 197 // not the strut axis as it should be.  Will fix this later.
 198
 199   compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
 200
 201   if (compressLength > 0.00) {
 202
 203     WOW = true;
 204
 205 // The next equation should really use the vector to the contact patch of the tire
 206 // including the strut compression and not vWhlBodyVec.  Will fix this later.
 207
 208     vWhlVelVec      =  State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
 209     vWhlVelVec     +=  Position->GetVel();
 210
 211     compressSpeed   =  vWhlVelVec(eZ);
 212
 213     if (!FirstContact) {
 214       FirstContact  = true;
 215       SinkRate      =  compressSpeed;
 216       GroundSpeed   =  Position->GetVel().Magnitude();
 217     }
 218
 219 // The following needs work regarding friction coefficients and braking and
 220 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
 221 // It also assumes that we won't be turning and braking at the same time.
 222 // Will fix this later.
 223
 224     switch (eBrakeGrp) {
 225     case bgLeft:
 226         SteerGain = -maxSteerAngle;
 227         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
 228                                             staticFCoeff*FCS->GetBrake(bgLeft);
 229       break;
 230     case bgRight:
 231         SteerGain = -maxSteerAngle;
 232         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
 233                                             staticFCoeff*FCS->GetBrake(bgRight);
 234       break;
 235     case bgCenter:
 236         SteerGain = -maxSteerAngle;
 237         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
 238                                             staticFCoeff*FCS->GetBrake(bgCenter);
 239       break;
 240     case bgNose:
 241         SteerGain = maxSteerAngle;
 242         BrakeFCoeff = rollingFCoeff;
 243       break;
 244     case bgTail:
 245         SteerGain = -maxSteerAngle;
 246         BrakeFCoeff = rollingFCoeff;
 247       break;
 248     case bgNone:
 249         SteerGain = -maxSteerAngle;
 250         BrakeFCoeff = rollingFCoeff;
 251       break;
 252     default:
 253       cerr << "Improper brake group membership detected for this gear." << endl;
 254       break;
 255     }
 256
 257 // Note to Jon: Need to substitute the correct variable for RudderPedal.
 258 // It is assumed that rudder pedal has a range of -1.0 to 1.0.
 259
 260     switch (eSteerType) {
 261     case stSteer:
 262       SteerAngle = SteerGain*RudderPedal;
 263       break;
 264     case stFixed:
 265       SteerAngle = 0.0;
 266       break;
 267     case stCaster:
 268
 269     // Note to Jon: This is not correct for castering gear.  I'll fix it later.
 270       SteerAngle = 0.0;
 271       break;
 272     default:
 273       cerr << "Improper steering type membership detected for this gear." << endl;
 274       break;
 275     }
 276
 277 // Transform the wheel velocities from the local axis system to the wheel axis system.
 278 // For now, steering angle is assumed to happen in the Local Z axis,
 279 // not the strut axis as it should be.  Will fix this later.
 280 // Note to Jon: Please substitute the correct variable for Deg2Rad conversion.
 281
 282     SinWheel      = sin(Rotation->Getpsi() + SteerAngle*DEGTORAD);
 283     CosWheel      = cos(Rotation->Getpsi() + SteerAngle*DEGTORAD);
 284     RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
 285     SideWhlVel    = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
 286
 287 // Calculate tire slip angle.
 288
 289     if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
 290       WheelSlip = 0.0;
 291     } else {
 292       WheelSlip = RADTODEG*atan2(SideWhlVel, RollingWhlVel);
 293     }
 294
 295     // The following code normalizes the wheel velocity vector, reverses it, and zeroes out
 296     // the z component of the velocity. The question is, should the Z axis velocity be zeroed
 297     // out first before the normalization takes place or not? Subsequent to that, the Wheel
 298     // Velocity vector now points as a unit vector backwards and parallel to the wheel
 299     // velocity vector. It acts AT the wheel.
 300
 301 // Note to Jon: I commented out this line because I wasn't sure we want to do this.
 302 //    vWhlVelVec      = -1.0 * vWhlVelVec.Normalize();
 303 //    vWhlVelVec(eZ)  =  0.00;
 304
 305 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
 306 // Allow a maximum of 10 degrees tire slip angle before wheel slides.  At that point,
 307 // transition from static to dynamic friction.  There are more complicated formulations
 308 // of this that avoid the discrete jump.  Will fix this later.
 309
 310     if (fabs(WheelSlip) <= 10.0) {
 311       FCoeff = staticFCoeff*WheelSlip/10.0;
 312     } else {
 313       FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
 314     }
 315
 316 // Compute the vertical force on the wheel.
 317
 318     vLocalForce(eZ) =  min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0);
 319
 320     MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
 321     MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
 322
 323 // Compute the forces in the wheel ground plane.
 324
 325     RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
 326     SideForce    = vLocalForce(eZ) * FCoeff;
 327
 328 // Transform these forces back to the local reference frame.
 329
 330     vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
 331     vLocalForce(eY) = SideForce*CosWheel    + RollingForce*SinWheel;
 332
 333 // Note to Jon: At this point the forces will be too big when the airplane is stopped or
 334 // rolling to a stop.  We need to make sure that the gear forces just balance out the non-gear forces
 335 // when the airplane is stopped.  That way the airplane won't start to accelerate until the non-gear
 336 // forces are larger than the gear forces.  I think that the proper fix should go into FGAircraft::FMGear.
 337 // This routine would only compute the local strut forces and return them to FMGear.  All of the gear
 338 // forces would get adjusted in FMGear using the total non-gear forces.  Then the gear moments would be
 339 // calculated.  If strange things start happening to the airplane during testing as it rolls to a stop,
 340 // then we need to implement this change.  I ran out of time to do it now but have the equations.
 341
 342 // Transform the forces back to the body frame and compute the moment.
 343
 344     vForce  = State->GetTl2b() * vLocalForce;
 345     vMoment = vWhlBodyVec * vForce;
 346
 347   } else {
 348
 349     WOW = false;
 350
 351     if (Position->GetDistanceAGL() > 200.0) {
 352       FirstContact = false;
 353       Reported = false;
 354       DistanceTraveled = 0.0;
 355       MaximumStrutForce = MaximumStrutTravel = 0.0;
 356     }
 357
 358     vForce.InitMatrix();
 359     vMoment.InitMatrix();
 360   }
 361
 362   if (FirstContact) {
 363     DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
 364   }
 365
 366   if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
 367     Report();
 368   }
 369
 370   return vForce;
 371 }
 372
 373 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 374
 375 void FGLGear::Report(void)
 376 {
 377   cout << endl << "Touchdown report for " << name << endl;
 378   cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
 379                               << SinkRate*0.3408          << " mps"     << endl;
 380   cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
 381                               << GroundSpeed*0.3408       << " mps"     << endl;
 382   cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
 383                               << MaximumStrutForce*4.448  << " Newtons" << endl;
 384   cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
 385                               << MaximumStrutTravel*30.48 << " cm"      << endl;
 386   cout << "  Distance traveled:     " << DistanceTraveled        << " ft,     "
 387                               << DistanceTraveled*0.3408  << " meters"  << endl;
 388   Reported = true;
 389 }
 390
 391 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 392
 393 void FGLGear::Debug(void)
 394 {
 395   // TODO: Add user code here
 396 }
 397