1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 Purpose: Encapsulates the landing gear elements
10 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
12 This program is free software; you can redistribute it and/or modify it under
13 the terms of the GNU General Public License as published by the Free Software
14 Foundation; either version 2 of the License, or (at your option) any later
17 This program is distributed in the hope that it will be useful, but WITHOUT
18 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
19 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
22 You should have received a copy of the GNU General Public License along with
23 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
24 Place - Suite 330, Boston, MA 02111-1307, USA.
26 Further information about the GNU General Public License can also be found on
27 the world wide web at http://www.gnu.org.
29 FUNCTIONAL DESCRIPTION
30 --------------------------------------------------------------------------------
33 --------------------------------------------------------------------------------
35 01/30/01 NHP Extended gear model to properly simulate steering and braking
37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
44 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
46 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
53 static const char *IdSrc = "$Id$";
54 static const char *IdHdr = ID_LGEAR;
56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
60 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
64 *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
65 >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
66 >> rollingFCoeff >> sSteerType >> sBrakeGroup
67 >> maxSteerAngle >> sRetractable;
69 if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft;
70 else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
71 else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
72 else if (sBrakeGroup == "NOSE" ) eBrakeGrp = bgNose;
73 else if (sBrakeGroup == "TAIL" ) eBrakeGrp = bgTail;
74 else if (sBrakeGroup == "NONE" ) eBrakeGrp = bgNone;
76 cerr << "Improper braking group specification in config file: "
77 << sBrakeGroup << " is undefined." << endl;
80 if (sSteerType == "STEERABLE") eSteerType = stSteer;
81 else if (sSteerType == "FIXED" ) eSteerType = stFixed;
82 else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
84 cerr << "Improper steering type specification in config file: "
85 << sSteerType << " is undefined." << endl;
88 if ( sRetractable == "RETRACT" ) {
91 isRetractable = false;
97 // Add some AI here to determine if gear is located properly according to its
98 // brake group type ??
100 State = Exec->GetState();
101 Aircraft = Exec->GetAircraft();
102 Position = Exec->GetPosition();
103 Rotation = Exec->GetRotation();
104 FCS = Exec->GetFCS();
105 MassBalance = Exec->GetMassBalance();
107 WOW = lastWOW = false;
109 FirstContact = false;
111 DistanceTraveled = 0.0;
112 MaximumStrutForce = MaximumStrutTravel = 0.0;
113 SinkRate = GroundSpeed = 0.0;
115 vWhlBodyVec = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
116 vWhlBodyVec(eX) = -vWhlBodyVec(eX);
117 vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
119 vLocalGear = State->GetTb2l() * vWhlBodyVec;
124 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
126 FGLGear::FGLGear(const FGLGear& lgear)
129 Aircraft = lgear.Aircraft;
130 Position = lgear.Position;
131 Rotation = lgear.Rotation;
134 MassBalance = lgear.MassBalance;
137 vMoment = lgear.vMoment;
138 vWhlBodyVec = lgear.vWhlBodyVec;
139 vLocalGear = lgear.vLocalGear;
142 lastWOW = lgear.lastWOW;
143 ReportEnable = lgear.ReportEnable;
144 FirstContact = lgear.FirstContact;
145 DistanceTraveled = lgear.DistanceTraveled;
146 MaximumStrutForce = lgear.MaximumStrutForce;
147 MaximumStrutTravel = lgear.MaximumStrutTravel;
149 kSpring = lgear.kSpring;
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;
162 sSteerType = lgear.sSteerType;
163 sRetractable = lgear.sRetractable;
164 eSteerType = lgear.eSteerType;
165 sBrakeGroup = lgear.sBrakeGroup;
166 eBrakeGrp = lgear.eBrakeGrp;
167 maxSteerAngle = lgear.maxSteerAngle;
168 isRetractable = lgear.isRetractable;
169 GearUp = lgear.GearUp;
170 GearDown = lgear.GearDown;
173 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
180 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
182 FGColumnVector3& FGLGear::Force(void)
185 vMoment.InitMatrix();
188 if (FCS->GetGearPos() < 0.01) {
191 } else if (FCS->GetGearPos() > 0.99) {
204 double SteerGain = 0;
205 double SinWheel, CosWheel, SideWhlVel, RollingWhlVel;
206 double RollingForce, SideForce, FCoeff;
209 vWhlBodyVec = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
210 vWhlBodyVec(eX) = -vWhlBodyVec(eX);
211 vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
213 // vWhlBodyVec now stores the vector from the cg to this wheel
215 vLocalGear = State->GetTb2l() * vWhlBodyVec;
217 // vLocalGear now stores the vector from the cg to the wheel in local coords.
219 compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
221 // The compression length is currently measured in the Z-axis, only, at this time.
222 // It should be measured along the strut axis. If the local-frame gear position
223 // "hangs down" below the CG greater than the altitude, then the compressLength
224 // will be positive - i.e. the gear will have made contact.
226 if (compressLength > 0.00) {
228 WOW = true;// Weight-On-Wheels is true
230 // The next equation should really use the vector to the contact patch of the tire
231 // including the strut compression and not vWhlBodyVec. Will fix this later.
232 // As it stands, now, the following equation takes the aircraft body-frame
233 // rotational rate and calculates the cross-product with the vector from the CG
234 // to the wheel, thus producing the instantaneous velocity vector of the tire
235 // in Body coords. The frame is also converted to local coordinates. When the
236 // aircraft local-frame velocity is added to this quantity, the total velocity of
237 // the wheel in local frame is then known. Subsequently, the compression speed
238 // (used for calculating damping force) is found by taking the Z-component of the
241 vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
243 vWhlVelVec += Position->GetVel();
245 compressSpeed = vWhlVelVec(eZ);
247 // If this is the first time the wheel has made contact, remember some values
248 // for later printout.
252 SinkRate = compressSpeed;
253 GroundSpeed = Position->GetVel().Magnitude();
256 // The following needs work regarding friction coefficients and braking and
257 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
258 // It also assumes that we won't be turning and braking at the same time.
259 // Will fix this later.
260 // [JSB] The braking force coefficients include normal rolling coefficient +
261 // a percentage of the static friction coefficient based on braking applied.
266 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
267 staticFCoeff*FCS->GetBrake(bgLeft);
271 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
272 staticFCoeff*FCS->GetBrake(bgRight);
276 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
277 staticFCoeff*FCS->GetBrake(bgCenter);
281 BrakeFCoeff = rollingFCoeff;
285 BrakeFCoeff = rollingFCoeff;
289 BrakeFCoeff = rollingFCoeff;
292 cerr << "Improper brake group membership detected for this gear." << endl;
296 switch (eSteerType) {
298 SteerAngle = SteerGain*FCS->GetDrPos();
304 // Note to Jon: This is not correct for castering gear. I'll fix it later.
308 cerr << "Improper steering type membership detected for this gear." << endl;
312 // Transform the wheel velocities from the local axis system to the wheel axis system.
313 // For now, steering angle is assumed to happen in the Local Z axis,
314 // not the strut axis as it should be. Will fix this later.
316 SinWheel = sin(Rotation->Getpsi() + SteerAngle);
317 CosWheel = cos(Rotation->Getpsi() + SteerAngle);
318 RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
319 SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
321 // Calculate tire slip angle.
323 if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
326 WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
329 // The following code normalizes the wheel velocity vector, reverses it, and zeroes out
330 // the z component of the velocity. The question is, should the Z axis velocity be zeroed
331 // out first before the normalization takes place or not? Subsequent to that, the Wheel
332 // Velocity vector now points as a unit vector backwards and parallel to the wheel
333 // velocity vector. It acts AT the wheel.
335 // Note to Jon: I commented out this line because I wasn't sure we want to do this.
336 // vWhlVelVec = -1.0 * vWhlVelVec.Normalize();
337 // vWhlVelVec(eZ) = 0.00;
339 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
340 // Allow a maximum of 10 degrees tire slip angle before wheel slides. At that point,
341 // transition from static to dynamic friction. There are more complicated formulations
342 // of this that avoid the discrete jump. Will fix this later.
344 if (fabs(WheelSlip) <= 10.0) {
345 FCoeff = staticFCoeff*WheelSlip/10.0;
347 FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
350 // Compute the vertical force on the wheel using square-law damping (per comment
351 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
352 // allowing for both square and linear damping force calculation. Also need to
353 // possibly give a "rebound damping factor" that differs from the compression
354 // case. NOTE: SQUARE LAW DAMPING NO GOOD!
356 vLocalForce(eZ) = min(-compressLength * kSpring
357 - compressSpeed * bDamp, (double)0.0);
359 MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
360 MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
362 // Compute the forces in the wheel ground plane.
365 if (fabs(RollingWhlVel) > 1E-3) {
366 RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
368 SideForce = vLocalForce(eZ) * FCoeff;
370 // Transform these forces back to the local reference frame.
372 vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
373 vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel;
375 // Note to Jon: At this point the forces will be too big when the airplane is
376 // stopped or rolling to a stop. We need to make sure that the gear forces just
377 // balance out the non-gear forces when the airplane is stopped. That way the
378 // airplane won't start to accelerate until the non-gear/ forces are larger than
379 // the gear forces. I think that the proper fix should go into FGAircraft::FMGear.
380 // This routine would only compute the local strut forces and return them to
381 // FMGear. All of the gear forces would get adjusted in FMGear using the total
382 // non-gear forces. Then the gear moments would be calculated. If strange things
383 // start happening to the airplane during testing as it rolls to a stop, then we
384 // need to implement this change. I ran out of time to do it now but have the
387 // Transform the forces back to the body frame and compute the moment.
389 vForce = State->GetTl2b() * vLocalForce;
390 vMoment = vWhlBodyVec * vForce;
396 if (Position->GetDistanceAGL() > 200.0) {
397 FirstContact = false;
399 DistanceTraveled = 0.0;
400 MaximumStrutForce = MaximumStrutTravel = 0.0;
403 compressLength = 0.0; // reset compressLength to zero for data output validity
407 DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
410 if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
411 if (debug_lvl > 0) Report();
414 if (lastWOW != WOW) {
415 PutMessage("GEAR_CONTACT", WOW);
420 // Crash detection logic (really out-of-bounds detection)
422 if (compressLength > 500.0 ||
423 vForce.Magnitude() > 100000000.0 ||
424 vMoment.Magnitude() > 5000000000.0 ||
425 SinkRate > 1.4666*30)
427 PutMessage("Crash Detected");
434 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
436 void FGLGear::Report(void)
438 cout << endl << "Touchdown report for " << name << endl;
439 cout << " Sink rate at contact: " << SinkRate << " fps, "
440 << SinkRate*0.3408 << " mps" << endl;
441 cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, "
442 << GroundSpeed*0.3408 << " mps" << endl;
443 cout << " Maximum contact force: " << MaximumStrutForce << " lbs, "
444 << MaximumStrutForce*4.448 << " Newtons" << endl;
445 cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, "
446 << MaximumStrutTravel*30.48 << " cm" << endl;
447 cout << " Distance traveled: " << DistanceTraveled << " ft, "
448 << DistanceTraveled*0.3408 << " meters" << endl;
452 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
453 // The bitmasked value choices are as follows:
454 // unset: In this case (the default) JSBSim would only print
455 // out the normally expected messages, essentially echoing
456 // the config files as they are read. If the environment
457 // variable is not set, debug_lvl is set to 1 internally
458 // 0: This requests JSBSim not to output any messages
460 // 1: This value explicity requests the normal JSBSim
462 // 2: This value asks for a message to be printed out when
463 // a class is instantiated
464 // 4: When this value is set, a message is displayed when a
465 // FGModel object executes its Run() method
466 // 8: When this value is set, various runtime state variables
467 // are printed out periodically
468 // 16: When set various parameters are sanity checked and
469 // a message is printed out when they go out of bounds
471 void FGLGear::Debug(int from)
473 if (debug_lvl <= 0) return;
475 if (debug_lvl & 1) { // Standard console startup message output
476 if (from == 0) { // Constructor
477 cout << " Name: " << name << endl;
478 cout << " Location: " << vXYZ << endl;
479 cout << " Spring Constant: " << kSpring << endl;
480 cout << " Damping Constant: " << bDamp << endl;
481 cout << " Dynamic Friction: " << dynamicFCoeff << endl;
482 cout << " Static Friction: " << staticFCoeff << endl;
483 cout << " Rolling Friction: " << rollingFCoeff << endl;
484 cout << " Steering Type: " << sSteerType << endl;
485 cout << " Grouping: " << sBrakeGroup << endl;
486 cout << " Max Steer Angle: " << maxSteerAngle << endl;
487 cout << " Retractable: " << sRetractable << endl;
490 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
491 if (from == 0) cout << "Instantiated: FGLGear" << endl;
492 if (from == 1) cout << "Destroyed: FGLGear" << endl;
494 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
496 if (debug_lvl & 8 ) { // Runtime state variables
498 if (debug_lvl & 16) { // Sanity checking
500 if (debug_lvl & 64) {
501 if (from == 0) { // Constructor
502 cout << IdSrc << endl;
503 cout << IdHdr << endl;