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 extern short debug_lvl;
58 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
62 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
68 *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
69 >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
70 >> rollingFCoeff >> sSteerType >> sBrakeGroup >> maxSteerAngle;
72 cout << " Name: " << name << endl;
73 cout << " Location: " << vXYZ << endl;
74 cout << " Spring Constant: " << kSpring << endl;
75 cout << " Damping Constant: " << bDamp << endl;
76 cout << " Dynamic Friction: " << dynamicFCoeff << endl;
77 cout << " Static Friction: " << staticFCoeff << endl;
78 cout << " Rolling Friction: " << rollingFCoeff << endl;
79 cout << " Steering Type: " << sSteerType << endl;
80 cout << " Grouping: " << sBrakeGroup << endl;
81 cout << " Max Steer Angle: " << maxSteerAngle << endl;
83 if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft;
84 else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
85 else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
86 else if (sBrakeGroup == "NOSE" ) eBrakeGrp = bgNose;
87 else if (sBrakeGroup == "TAIL" ) eBrakeGrp = bgTail;
88 else if (sBrakeGroup == "NONE" ) eBrakeGrp = bgNone;
90 cerr << "Improper braking group specification in config file: "
91 << sBrakeGroup << " is undefined." << endl;
94 if (sSteerType == "STEERABLE") eSteerType = stSteer;
95 else if (sSteerType == "FIXED" ) eSteerType = stFixed;
96 else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
98 cerr << "Improper steering type specification in config file: "
99 << sSteerType << " is undefined." << endl;
102 // Add some AI here to determine if gear is located properly according to its
103 // brake group type ??
105 State = Exec->GetState();
106 Aircraft = Exec->GetAircraft();
107 Position = Exec->GetPosition();
108 Rotation = Exec->GetRotation();
109 FCS = Exec->GetFCS();
113 FirstContact = false;
115 DistanceTraveled = 0.0;
116 MaximumStrutForce = MaximumStrutTravel = 0.0;
118 vWhlBodyVec = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
119 vWhlBodyVec(eX) = -vWhlBodyVec(eX);
120 vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
122 vLocalGear = State->GetTb2l() * vWhlBodyVec;
124 if (debug_lvl & 2) cout << "Instantiated: FGLGear" << endl;
127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 FGLGear::FGLGear(const FGLGear& lgear)
132 Aircraft = lgear.Aircraft;
133 Position = lgear.Position;
134 Rotation = lgear.Rotation;
139 vMoment = lgear.vMoment;
140 vWhlBodyVec = lgear.vWhlBodyVec;
141 vLocalGear = lgear.vLocalGear;
144 ReportEnable = lgear.ReportEnable;
145 FirstContact = lgear.FirstContact;
146 DistanceTraveled = lgear.DistanceTraveled;
147 MaximumStrutForce = lgear.MaximumStrutForce;
148 MaximumStrutTravel = lgear.MaximumStrutTravel;
150 kSpring = lgear.kSpring;
152 compressLength = lgear.compressLength;
153 compressSpeed = lgear.compressSpeed;
154 staticFCoeff = lgear.staticFCoeff;
155 dynamicFCoeff = lgear.dynamicFCoeff;
156 rollingFCoeff = lgear.rollingFCoeff;
157 brakePct = lgear.brakePct;
158 maxCompLen = lgear.maxCompLen;
159 SinkRate = lgear.SinkRate;
160 GroundSpeed = lgear.GroundSpeed;
161 Reported = lgear.Reported;
163 sSteerType = lgear.sSteerType;
164 eSteerType = lgear.eSteerType;
165 sBrakeGroup = lgear.sBrakeGroup;
166 eBrakeGrp = lgear.eBrakeGrp;
167 maxSteerAngle = lgear.maxSteerAngle;
170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
174 if (debug_lvl & 2) cout << "Destroyed: FGLGear" << endl;
177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
179 FGColumnVector FGLGear::Force(void)
181 float SteerGain, SteerAngle, BrakeFCoeff;
182 float SinWheel, CosWheel, SideWhlVel, RollingWhlVel;
183 float RudderPedal, RollingForce, SideForce, FCoeff;
186 FGColumnVector vForce(3);
187 FGColumnVector vLocalForce(3);
188 FGColumnVector vWhlVelVec(3); // Velocity of this wheel (Local)
190 vWhlBodyVec = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
191 vWhlBodyVec(eX) = -vWhlBodyVec(eX);
192 vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
194 // vWhlBodyVec now stores the vector from the cg to this wheel
196 vLocalGear = State->GetTb2l() * vWhlBodyVec;
198 // vLocalGear now stores the vector from the cg to the wheel in local coords.
200 compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
202 // The compression length is currently measured in the Z-axis, only, at this time.
203 // It should be measured along the strut axis. If the local-frame gear position
204 // "hangs down" below the CG greater than the altitude, then the compressLength
205 // will be positive - i.e. the gear will have made contact.
207 if (compressLength > 0.00) {
209 WOW = true; // Weight-On-Wheels is true
211 // The next equation should really use the vector to the contact patch of the tire
212 // including the strut compression and not vWhlBodyVec. Will fix this later.
213 // As it stands, now, the following equation takes the aircraft body-frame
214 // rotational rate and calculates the cross-product with the vector from the CG
215 // to the wheel, thus producing the instantaneous velocity vector of the tire
216 // in Body coords. The frame is also converted to local coordinates. When the
217 // aircraft local-frame velocity is added to this quantity, the total velocity of
218 // the wheel in local frame is then known. Subsequently, the compression speed
219 // (used for calculating damping force) is found by taking the Z-component of the
222 vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
223 vWhlVelVec += Position->GetVel();
225 compressSpeed = vWhlVelVec(eZ);
227 // If this is the first time the wheel has made contact, remember some values
228 // for later printout.
232 SinkRate = compressSpeed;
233 GroundSpeed = Position->GetVel().Magnitude();
236 // The following needs work regarding friction coefficients and braking and
237 // steering The BrakeFCoeff formula assumes that an anti-skid system is used.
238 // It also assumes that we won't be turning and braking at the same time.
239 // Will fix this later.
240 // [JSB] The braking force coefficients include normal rolling coefficient +
241 // a percentage of the static friction coefficient based on braking applied.
245 SteerGain = -maxSteerAngle;
246 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
247 staticFCoeff*FCS->GetBrake(bgLeft);
250 SteerGain = -maxSteerAngle;
251 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
252 staticFCoeff*FCS->GetBrake(bgRight);
255 SteerGain = -maxSteerAngle;
256 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
257 staticFCoeff*FCS->GetBrake(bgCenter);
260 SteerGain = maxSteerAngle;
261 BrakeFCoeff = rollingFCoeff;
264 SteerGain = -maxSteerAngle;
265 BrakeFCoeff = rollingFCoeff;
268 SteerGain = -maxSteerAngle;
269 BrakeFCoeff = rollingFCoeff;
272 cerr << "Improper brake group membership detected for this gear." << endl;
276 switch (eSteerType) {
278 SteerAngle = SteerGain*FCS->GetDrCmd();
284 // Note to Jon: This is not correct for castering gear. I'll fix it later.
288 cerr << "Improper steering type membership detected for this gear." << endl;
292 // Transform the wheel velocities from the local axis system to the wheel axis system.
293 // For now, steering angle is assumed to happen in the Local Z axis,
294 // not the strut axis as it should be. Will fix this later.
296 SinWheel = sin(Rotation->Getpsi() + SteerAngle*DEGTORAD);
297 CosWheel = cos(Rotation->Getpsi() + SteerAngle*DEGTORAD);
298 RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
299 SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
301 // Calculate tire slip angle.
303 if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
306 WheelSlip = RADTODEG*atan2(SideWhlVel, RollingWhlVel);
309 // The following code normalizes the wheel velocity vector, reverses it, and zeroes out
310 // the z component of the velocity. The question is, should the Z axis velocity be zeroed
311 // out first before the normalization takes place or not? Subsequent to that, the Wheel
312 // Velocity vector now points as a unit vector backwards and parallel to the wheel
313 // velocity vector. It acts AT the wheel.
315 // Note to Jon: I commented out this line because I wasn't sure we want to do this.
316 // vWhlVelVec = -1.0 * vWhlVelVec.Normalize();
317 // vWhlVelVec(eZ) = 0.00;
319 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
320 // Allow a maximum of 10 degrees tire slip angle before wheel slides. At that point,
321 // transition from static to dynamic friction. There are more complicated formulations
322 // of this that avoid the discrete jump. Will fix this later.
324 if (fabs(WheelSlip) <= 10.0) {
325 FCoeff = staticFCoeff*WheelSlip/10.0;
327 FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
330 // Compute the vertical force on the wheel.
332 vLocalForce(eZ) = min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0);
334 MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
335 MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
337 // Compute the forces in the wheel ground plane.
340 if (fabs(RollingWhlVel) > 1E-3) {
341 RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
343 SideForce = vLocalForce(eZ) * FCoeff;
345 // Transform these forces back to the local reference frame.
347 vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
348 vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel;
350 // Note to Jon: At this point the forces will be too big when the airplane is stopped or
351 // rolling to a stop. We need to make sure that the gear forces just balance out the non-gear forces
352 // when the airplane is stopped. That way the airplane won't start to accelerate until the non-gear
353 // forces are larger than the gear forces. I think that the proper fix should go into FGAircraft::FMGear.
354 // This routine would only compute the local strut forces and return them to FMGear. All of the gear
355 // forces would get adjusted in FMGear using the total non-gear forces. Then the gear moments would be
356 // calculated. If strange things start happening to the airplane during testing as it rolls to a stop,
357 // then we need to implement this change. I ran out of time to do it now but have the equations.
359 // Transform the forces back to the body frame and compute the moment.
361 vForce = State->GetTl2b() * vLocalForce;
362 vMoment = vWhlBodyVec * vForce;
368 if (Position->GetDistanceAGL() > 200.0) {
369 FirstContact = false;
371 DistanceTraveled = 0.0;
372 MaximumStrutForce = MaximumStrutTravel = 0.0;
376 vMoment.InitMatrix();
380 DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
383 if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
390 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
392 void FGLGear::Report(void)
394 cout << endl << "Touchdown report for " << name << endl;
395 cout << " Sink rate at contact: " << SinkRate << " fps, "
396 << SinkRate*0.3408 << " mps" << endl;
397 cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, "
398 << GroundSpeed*0.3408 << " mps" << endl;
399 cout << " Maximum contact force: " << MaximumStrutForce << " lbs, "
400 << MaximumStrutForce*4.448 << " Newtons" << endl;
401 cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, "
402 << MaximumStrutTravel*30.48 << " cm" << endl;
403 cout << " Distance traveled: " << DistanceTraveled << " ft, "
404 << DistanceTraveled*0.3408 << " meters" << endl;
408 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
410 void FGLGear::Debug(void)
412 // TODO: Add user code here