1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
6 Purpose: Encapsulates the landing gear elements
9 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
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
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
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.
25 Further information about the GNU General Public License can also be found on
26 the world wide web at http://www.gnu.org.
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
32 --------------------------------------------------------------------------------
34 01/30/01 NHP Extended gear model to properly simulate steering and braking
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
45 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 static const char *IdSrc = "$Id$";
53 static const char *IdHdr = ID_LGEAR;
55 extern short debug_lvl;
57 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
61 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
67 *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
68 >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
69 >> rollingFCoeff >> sSteerType >> sBrakeGroup >> maxSteerAngle;
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;
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;
89 cerr << "Improper braking group specification in config file: "
90 << sBrakeGroup << " is undefined." << endl;
93 if (sSteerType == "STEERABLE") eSteerType = stSteer;
94 else if (sSteerType == "FIXED" ) eSteerType = stFixed;
95 else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
97 cerr << "Improper steering type specification in config file: "
98 << sSteerType << " is undefined." << endl;
101 // Add some AI here to determine if gear is located properly according to its
102 // brake group type ??
104 State = Exec->GetState();
105 Aircraft = Exec->GetAircraft();
106 Position = Exec->GetPosition();
107 Rotation = Exec->GetRotation();
108 FCS = Exec->GetFCS();
112 FirstContact = false;
114 DistanceTraveled = 0.0;
115 MaximumStrutForce = MaximumStrutTravel = 0.0;
117 vWhlBodyVec = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
118 vWhlBodyVec(eX) = -vWhlBodyVec(eX);
119 vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
121 vLocalGear = State->GetTb2l() * vWhlBodyVec;
123 if (debug_lvl & 2) cout << "Instantiated: FGLGear" << endl;
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128 FGLGear::FGLGear(const FGLGear& lgear)
131 Aircraft = lgear.Aircraft;
132 Position = lgear.Position;
133 Rotation = lgear.Rotation;
138 vMoment = lgear.vMoment;
139 vWhlBodyVec = lgear.vWhlBodyVec;
140 vLocalGear = lgear.vLocalGear;
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 eSteerType = lgear.eSteerType;
164 sBrakeGroup = lgear.sBrakeGroup;
165 eBrakeGrp = lgear.eBrakeGrp;
166 maxSteerAngle = lgear.maxSteerAngle;
169 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
173 if (debug_lvl & 2) cout << "Destroyed: FGLGear" << endl;
176 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
178 FGColumnVector FGLGear::Force(void)
180 float SteerGain, SteerAngle, BrakeFCoeff;
181 float SinWheel, CosWheel, SideWhlVel, RollingWhlVel;
182 float RudderPedal, RollingForce, SideForce, FCoeff;
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)
190 vWhlBodyVec = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
191 vWhlBodyVec(eX) = -vWhlBodyVec(eX);
192 vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
194 vLocalGear = State->GetTb2l() * vWhlBodyVec;
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.
199 compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
201 if (compressLength > 0.00) {
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.
208 vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
209 vWhlVelVec += Position->GetVel();
211 compressSpeed = vWhlVelVec(eZ);
215 SinkRate = compressSpeed;
216 GroundSpeed = Position->GetVel().Magnitude();
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.
226 SteerGain = -maxSteerAngle;
227 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
228 staticFCoeff*FCS->GetBrake(bgLeft);
231 SteerGain = -maxSteerAngle;
232 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
233 staticFCoeff*FCS->GetBrake(bgRight);
236 SteerGain = -maxSteerAngle;
237 BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
238 staticFCoeff*FCS->GetBrake(bgCenter);
241 SteerGain = maxSteerAngle;
242 BrakeFCoeff = rollingFCoeff;
245 SteerGain = -maxSteerAngle;
246 BrakeFCoeff = rollingFCoeff;
249 SteerGain = -maxSteerAngle;
250 BrakeFCoeff = rollingFCoeff;
253 cerr << "Improper brake group membership detected for this gear." << endl;
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.
260 switch (eSteerType) {
262 SteerAngle = SteerGain*FCS->GetDrCmd();
269 // Note to Jon: This is not correct for castering gear. I'll fix it later.
273 cerr << "Improper steering type membership detected for this gear." << endl;
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.
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;
287 // Calculate tire slip angle.
289 if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
292 WheelSlip = RADTODEG*atan2(SideWhlVel, RollingWhlVel);
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.
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;
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.
310 if (fabs(WheelSlip) <= 10.0) {
311 FCoeff = staticFCoeff*WheelSlip/10.0;
313 FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
316 // Compute the vertical force on the wheel.
318 vLocalForce(eZ) = min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0);
320 MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
321 MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
323 // Compute the forces in the wheel ground plane.
326 if(fabs(RollingWhlVel) > 1E-3) {
327 RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
329 SideForce = vLocalForce(eZ) * FCoeff;
331 // Transform these forces back to the local reference frame.
333 vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
334 vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel;
336 // Note to Jon: At this point the forces will be too big when the airplane is stopped or
337 // rolling to a stop. We need to make sure that the gear forces just balance out the non-gear forces
338 // when the airplane is stopped. That way the airplane won't start to accelerate until the non-gear
339 // forces are larger than the gear forces. I think that the proper fix should go into FGAircraft::FMGear.
340 // This routine would only compute the local strut forces and return them to FMGear. All of the gear
341 // forces would get adjusted in FMGear using the total non-gear forces. Then the gear moments would be
342 // calculated. If strange things start happening to the airplane during testing as it rolls to a stop,
343 // then we need to implement this change. I ran out of time to do it now but have the equations.
345 // Transform the forces back to the body frame and compute the moment.
347 vForce = State->GetTl2b() * vLocalForce;
348 vMoment = vWhlBodyVec * vForce;
354 if (Position->GetDistanceAGL() > 200.0) {
355 FirstContact = false;
357 DistanceTraveled = 0.0;
358 MaximumStrutForce = MaximumStrutTravel = 0.0;
362 vMoment.InitMatrix();
366 DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
369 if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
376 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
378 void FGLGear::Report(void)
380 cout << endl << "Touchdown report for " << name << endl;
381 cout << " Sink rate at contact: " << SinkRate << " fps, "
382 << SinkRate*0.3408 << " mps" << endl;
383 cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, "
384 << GroundSpeed*0.3408 << " mps" << endl;
385 cout << " Maximum contact force: " << MaximumStrutForce << " lbs, "
386 << MaximumStrutForce*4.448 << " Newtons" << endl;
387 cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, "
388 << MaximumStrutTravel*30.48 << " cm" << endl;
389 cout << " Distance traveled: " << DistanceTraveled << " ft, "
390 << DistanceTraveled*0.3408 << " meters" << endl;
394 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
396 void FGLGear::Debug(void)
398 // TODO: Add user code here