]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGLGear.cpp
Fix a compile error.
[flightgear.git] / src / FDM / JSBSim / FGLGear.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3  Module:       FGLGear.cpp
4  Author:       Jon S. Berndt
5                Norman H. Princen
6  Date started: 11/18/99
7  Purpose:      Encapsulates the landing gear elements
8  Called by:    FGAircraft
9
10  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
11
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
15  version.
16
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
20  details.
21
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.
25
26  Further information about the GNU General Public License can also be found on
27  the world wide web at http://www.gnu.org.
28
29 FUNCTIONAL DESCRIPTION
30 --------------------------------------------------------------------------------
31
32 HISTORY
33 --------------------------------------------------------------------------------
34 11/18/99   JSB   Created
35 01/30/01   NHP   Extended gear model to properly simulate steering and braking
36
37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 INCLUDES
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
40
41 #include "FGLGear.h"
42 #include <algorithm>
43
44 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
45 DEFINITIONS
46 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
47
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 GLOBAL DATA
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
51
52
53 static const char *IdSrc = "$Id$";
54 static const char *IdHdr = ID_LGEAR;
55
56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
57 CLASS IMPLEMENTATION
58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
59
60 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
61 {
62   string tmp;
63   
64   *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
65             >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
66                   >> rollingFCoeff >> sSteerType >> sBrakeGroup 
67                      >> maxSteerAngle >> sRetractable;
68
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;
75   else {
76     cerr << "Improper braking group specification in config file: "
77          << sBrakeGroup << " is undefined." << endl;
78   }
79
80   if      (sSteerType == "STEERABLE") eSteerType = stSteer;
81   else if (sSteerType == "FIXED"    ) eSteerType = stFixed;
82   else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
83   else {
84     cerr << "Improper steering type specification in config file: "
85          << sSteerType << " is undefined." << endl;
86   }
87   
88   if ( sRetractable == "RETRACT" ) {
89     isRetractable = true;
90   } else  {
91     isRetractable = false;
92   }  
93   
94   GearUp = false;
95   GearDown = true;
96
97 // Add some AI here to determine if gear is located properly according to its
98 // brake group type ??
99
100   State       = Exec->GetState();
101   Aircraft    = Exec->GetAircraft();
102   Position    = Exec->GetPosition();
103   Rotation    = Exec->GetRotation();
104   FCS         = Exec->GetFCS();
105   MassBalance = Exec->GetMassBalance();
106
107   WOW = lastWOW = true; // should the value be initialized to true?
108   ReportEnable = true;
109   FirstContact = false;
110   Reported = false;
111   DistanceTraveled = 0.0;
112   MaximumStrutForce = MaximumStrutTravel = 0.0;
113   SinkRate = GroundSpeed = 0.0;
114
115   vWhlBodyVec     = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
116   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
117   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
118   
119   vLocalGear = State->GetTb2l() * vWhlBodyVec;
120
121   compressLength  = 0.0;
122   compressSpeed   = 0.0;
123   brakePct        = 0.0;
124   maxCompLen      = 0.0;
125
126   WheelSlip = lastWheelSlip = 0.0;
127
128   compressLength  = 0.0;
129   compressSpeed   = 0.0;
130   brakePct        = 0.0;
131   maxCompLen      = 0.0;
132
133   Debug(0);
134 }
135
136 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
137
138 FGLGear::FGLGear(const FGLGear& lgear)
139 {
140   State    = lgear.State;
141   Aircraft = lgear.Aircraft;
142   Position = lgear.Position;
143   Rotation = lgear.Rotation;
144   Exec     = lgear.Exec;
145   FCS      = lgear.FCS;
146   MassBalance = lgear.MassBalance;
147
148   vXYZ = lgear.vXYZ;
149   vMoment = lgear.vMoment;
150   vWhlBodyVec = lgear.vWhlBodyVec;
151   vLocalGear = lgear.vLocalGear;
152
153   WOW                = lgear.WOW;
154   lastWOW            = lgear.lastWOW;
155   ReportEnable       = lgear.ReportEnable;
156   FirstContact       = lgear.FirstContact;
157   DistanceTraveled   = lgear.DistanceTraveled;
158   MaximumStrutForce  = lgear.MaximumStrutForce;
159   MaximumStrutTravel = lgear.MaximumStrutTravel;
160
161   kSpring         = lgear.kSpring;
162   bDamp           = lgear.bDamp;
163   compressLength  = lgear.compressLength;
164   compressSpeed   = lgear.compressSpeed;
165   staticFCoeff    = lgear.staticFCoeff;
166   dynamicFCoeff   = lgear.dynamicFCoeff;
167   rollingFCoeff   = lgear.rollingFCoeff;
168   brakePct        = lgear.brakePct;
169   maxCompLen      = lgear.maxCompLen;
170   SinkRate        = lgear.SinkRate;
171   GroundSpeed     = lgear.GroundSpeed;
172   Reported        = lgear.Reported;
173   name            = lgear.name;
174   sSteerType      = lgear.sSteerType;
175   sRetractable    = lgear.sRetractable;
176   eSteerType      = lgear.eSteerType;
177   sBrakeGroup     = lgear.sBrakeGroup;
178   eBrakeGrp       = lgear.eBrakeGrp;
179   maxSteerAngle   = lgear.maxSteerAngle;
180   isRetractable   = lgear.isRetractable;
181   GearUp          = lgear.GearUp;
182   GearDown        = lgear.GearDown;
183   WheelSlip       = lgear.WheelSlip;
184   lastWheelSlip   = lgear.lastWheelSlip;
185 }
186
187 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188
189 FGLGear::~FGLGear()
190 {
191   Debug(1);
192 }
193
194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
195
196 FGColumnVector3& FGLGear::Force(void)
197 {
198   double SteerGain = 0;
199   double SinWheel, CosWheel;
200
201   vForce.InitMatrix();
202   vMoment.InitMatrix();
203
204   if (isRetractable) {
205     if (FCS->GetGearPos() < 0.01) {
206       GearUp   = true;
207       GearDown = false;
208      } else if (FCS->GetGearPos() > 0.99) {
209       GearDown = true;
210       GearUp   = false;
211      } else {
212       GearUp   = false;
213       GearDown = false;
214      }
215   } else {
216       GearUp   = false;
217       GearDown = true;
218   }         
219       
220   if (GearDown) {
221
222     vWhlBodyVec     = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
223     vWhlBodyVec(eX) = -vWhlBodyVec(eX);
224     vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
225
226 // vWhlBodyVec now stores the vector from the cg to this wheel
227
228     vLocalGear = State->GetTb2l() * vWhlBodyVec;
229
230 // vLocalGear now stores the vector from the cg to the wheel in local coords.
231
232     compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
233
234 // The compression length is currently measured in the Z-axis, only, at this time.
235 // It should be measured along the strut axis. If the local-frame gear position
236 // "hangs down" below the CG greater than the altitude, then the compressLength
237 // will be positive - i.e. the gear will have made contact.
238
239     if (compressLength > 0.00) {
240
241       WOW = true;// Weight-On-Wheels is true
242
243 // The next equation should really use the vector to the contact patch of the tire
244 // including the strut compression and not vWhlBodyVec.  Will fix this later.
245 // As it stands, now, the following equation takes the aircraft body-frame
246 // rotational rate and calculates the cross-product with the vector from the CG
247 // to the wheel, thus producing the instantaneous velocity vector of the tire
248 // in Body coords. The frame is also converted to local coordinates. When the
249 // aircraft local-frame velocity is added to this quantity, the total velocity of
250 // the wheel in local frame is then known. Subsequently, the compression speed
251 // (used for calculating damping force) is found by taking the Z-component of the
252 // wheel velocity.
253
254       vWhlVelVec      =  State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
255
256       vWhlVelVec     +=  Position->GetVel();
257
258       compressSpeed   =  vWhlVelVec(eZ);
259
260 // If this is the first time the wheel has made contact, remember some values
261 // for later printout.
262
263       if (!FirstContact) {
264         FirstContact  = true;
265         SinkRate      =  compressSpeed;
266         GroundSpeed   =  Position->GetVel().Magnitude();
267       }
268
269 // The following needs work regarding friction coefficients and braking and
270 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
271 // It also assumes that we won't be turning and braking at the same time.
272 // Will fix this later.
273 // [JSB] The braking force coefficients include normal rolling coefficient +
274 // a percentage of the static friction coefficient based on braking applied.
275
276       switch (eBrakeGrp) {
277       case bgLeft:
278         SteerGain = 0.10;
279         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
280                                               staticFCoeff*FCS->GetBrake(bgLeft);
281         break;
282       case bgRight:
283         SteerGain = 0.10;
284         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
285                                              staticFCoeff*FCS->GetBrake(bgRight);
286         break;
287       case bgCenter:
288         SteerGain = 0.10;
289         BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
290                                              staticFCoeff*FCS->GetBrake(bgCenter);
291         break;
292       case bgNose:
293         SteerGain = -0.50;
294         BrakeFCoeff = rollingFCoeff;
295         break;
296       case bgTail:
297         SteerGain = -0.10;
298         BrakeFCoeff = rollingFCoeff;
299         break;
300       case bgNone:
301         SteerGain = 0.0;
302         BrakeFCoeff = rollingFCoeff;
303         break;
304       default:
305         cerr << "Improper brake group membership detected for this gear." << endl;
306         break;
307       }
308
309       switch (eSteerType) {
310       case stSteer:
311         SteerAngle = SteerGain*FCS->GetDrPos();
312         break;
313       case stFixed:
314         SteerAngle = 0.0;
315         break;
316       case stCaster:
317 // Note to Jon: This is not correct for castering gear.  I'll fix it later.
318         SteerAngle = 0.0;
319         break;
320       default:
321         cerr << "Improper steering type membership detected for this gear." << endl;
322         break;
323       }
324
325 // Transform the wheel velocities from the local axis system to the wheel axis system.
326 // For now, steering angle is assumed to happen in the Local Z axis,
327 // not the strut axis as it should be.  Will fix this later.
328
329       SinWheel      = sin(Rotation->Getpsi() + SteerAngle);
330       CosWheel      = cos(Rotation->Getpsi() + SteerAngle);
331       RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
332       SideWhlVel    = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
333
334 // Calculate tire slip angle.
335
336       if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
337         WheelSlip = 0.0;
338       } else if (fabs(RollingWhlVel) < 0.10) {
339         WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;
340       } else {
341         WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
342       }
343
344       if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) ||
345           (WheelSlip > 0.0 && lastWheelSlip < 0.0))
346       {
347         WheelSlip = 0.0;
348       }
349       
350       lastWheelSlip = WheelSlip;
351
352 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
353 // Allow a maximum of 10 degrees tire slip angle before wheel slides.  At that point,
354 // transition from static to dynamic friction.  There are more complicated formulations
355 // of this that avoid the discrete jump.  Will fix this later.
356
357       if (fabs(WheelSlip) <= 20.0) {
358         FCoeff = staticFCoeff*WheelSlip/20.0;
359       } else if (fabs(WheelSlip) <= 40.0) {
360 //        FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
361         FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 20.0)/20.0 + 
362                   staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip;
363       } else {
364         FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
365       }
366
367 // Compute the vertical force on the wheel using square-law damping (per comment
368 // in paper AIAA-2000-4303 - see header prologue comments). We might consider
369 // allowing for both square and linear damping force calculation. Also need to
370 // possibly give a "rebound damping factor" that differs from the compression
371 // case.
372
373       vLocalForce(eZ) =  min(-compressLength * kSpring
374                              - compressSpeed * bDamp, (double)0.0);
375
376       MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
377       MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
378
379 // Compute the forces in the wheel ground plane.
380
381       RollingForce = 0;
382       if (fabs(RollingWhlVel) > 1E-3) {
383         RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
384       }
385       SideForce    = vLocalForce(eZ) * FCoeff;
386
387 // Transform these forces back to the local reference frame.
388
389       vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
390       vLocalForce(eY) = SideForce*CosWheel    + RollingForce*SinWheel;
391
392 // Note to Jon: At this point the forces will be too big when the airplane is
393 // stopped or rolling to a stop.  We need to make sure that the gear forces just
394 // balance out the non-gear forces when the airplane is stopped.  That way the
395 // airplane won't start to accelerate until the non-gear/ forces are larger than
396 // the gear forces.  I think that the proper fix should go into FGAircraft::FMGear.
397 // This routine would only compute the local strut forces and return them to
398 // FMGear. All of the gear forces would get adjusted in FMGear using the total
399 // non-gear forces. Then the gear moments would be calculated. If strange things
400 // start happening to the airplane during testing as it rolls to a stop, then we
401 // need to implement this change.  I ran out of time to do it now but have the
402 // equations.
403
404 // Transform the forces back to the body frame and compute the moment.
405
406       vForce  = State->GetTl2b() * vLocalForce;
407       vMoment = vWhlBodyVec * vForce;
408
409     } else {
410
411       WOW = false;
412
413       if (Position->GetDistanceAGL() > 200.0) {
414         FirstContact = false;
415         Reported = false;
416         DistanceTraveled = 0.0;
417         MaximumStrutForce = MaximumStrutTravel = 0.0;
418       }
419
420       compressLength = 0.0; // reset compressLength to zero for data output validity
421     }
422
423     if (FirstContact) {
424       DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
425     }
426   
427     if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
428       if (debug_lvl > 0) Report();
429     }
430
431     if (lastWOW != WOW) {
432       PutMessage("GEAR_CONTACT: " + name, WOW);
433     }
434
435     lastWOW = WOW;
436
437 // Crash detection logic (really out-of-bounds detection)
438
439     if (compressLength > 500.0 ||
440         vForce.Magnitude() > 100000000.0 ||
441         vMoment.Magnitude() > 5000000000.0 ||
442         SinkRate > 1.4666*30)
443     {
444       PutMessage("Crash Detected: Simulation FREEZE.");
445       Exec->Freeze();
446     }
447   } 
448   return vForce; 
449 }
450
451 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
452
453 void FGLGear::Report(void)
454 {
455   cout << endl << "Touchdown report for " << name << endl;
456   cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
457                               << SinkRate*0.3408          << " mps"     << endl;
458   cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
459                               << GroundSpeed*0.3408       << " mps"     << endl;
460   cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
461                               << MaximumStrutForce*4.448  << " Newtons" << endl;
462   cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
463                               << MaximumStrutTravel*30.48 << " cm"      << endl;
464   cout << "  Distance traveled:     " << DistanceTraveled        << " ft,     "
465                               << DistanceTraveled*0.3408  << " meters"  << endl;
466   Reported = true;
467 }
468
469 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
470 //    The bitmasked value choices are as follows:
471 //    unset: In this case (the default) JSBSim would only print
472 //       out the normally expected messages, essentially echoing
473 //       the config files as they are read. If the environment
474 //       variable is not set, debug_lvl is set to 1 internally
475 //    0: This requests JSBSim not to output any messages
476 //       whatsoever.
477 //    1: This value explicity requests the normal JSBSim
478 //       startup messages
479 //    2: This value asks for a message to be printed out when
480 //       a class is instantiated
481 //    4: When this value is set, a message is displayed when a
482 //       FGModel object executes its Run() method
483 //    8: When this value is set, various runtime state variables
484 //       are printed out periodically
485 //    16: When set various parameters are sanity checked and
486 //       a message is printed out when they go out of bounds
487
488 void FGLGear::Debug(int from)
489 {
490   if (debug_lvl <= 0) return;
491
492   if (debug_lvl & 1) { // Standard console startup message output
493     if (from == 0) { // Constructor
494       cout << "    Name: "               << name          << endl;
495       cout << "      Location: "         << vXYZ          << endl;
496       cout << "      Spring Constant:  " << kSpring       << endl;
497       cout << "      Damping Constant: " << bDamp         << endl;
498       cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
499       cout << "      Static Friction:  " << staticFCoeff  << endl;
500       cout << "      Rolling Friction: " << rollingFCoeff << endl;
501       cout << "      Steering Type:    " << sSteerType    << endl;
502       cout << "      Grouping:         " << sBrakeGroup   << endl;
503       cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
504       cout << "      Retractable:      " << sRetractable  << endl;
505     }
506   }
507   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
508     if (from == 0) cout << "Instantiated: FGLGear" << endl;
509     if (from == 1) cout << "Destroyed:    FGLGear" << endl;
510   }
511   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
512   }
513   if (debug_lvl & 8 ) { // Runtime state variables
514   }
515   if (debug_lvl & 16) { // Sanity checking
516   }
517   if (debug_lvl & 64) {
518     if (from == 0) { // Constructor
519       cout << IdSrc << endl;
520       cout << IdHdr << endl;
521     }
522   }
523 }
524