Fix stall widths for the "auxilliary" (reverse flow) stalls so they

[flightgear.git] / src / FDM / JSBSim / FGLGear.cpp

diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp
index dd0325b109ff9251ecd689b46725dc9cda460a8e..70f3606ad12cb0e73fcc877683113284c5c59199 100644 (file)
--- a/src/FDM/JSBSim/FGLGear.cpp
+++ b/src/FDM/JSBSim/FGLGear.cpp
@@ -57,11 +57,11 @@ static const char *IdHdr = ID_LGEAR;
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
+FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex, int number) : Exec(fdmex)
 {
-  string tmp;
+  GearNumber = number;
 
-  *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
+  *AC_cfg >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
             >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
                   >> rollingFCoeff >> sSteerType >> sBrakeGroup
                      >> maxSteerAngle >> sRetractable;
@@ -140,6 +140,8 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
 
 FGLGear::FGLGear(const FGLGear& lgear)
 {
+  GearNumber = lgear.GearNumber;
+
   State    = lgear.State;
   Aircraft = lgear.Aircraft;
   Propagate = lgear.Propagate;
@@ -207,8 +209,7 @@ FGLGear::~FGLGear()
 FGColumnVector3& FGLGear::Force(void)
 {
   double SinWheel, CosWheel;
-  double deltaSlip;
-  double deltaT = State->Getdt()*Aircraft->GetRate();
+  double deltaT = State->Getdt()*Exec->GetGroundReactions()->GetRate();
 
   vForce.InitMatrix();
   vMoment.InitMatrix();
@@ -229,7 +230,26 @@ FGColumnVector3& FGLGear::Force(void)
       GearDown = true;
   }
 
+  // Compute the steering angle in any case.
+  // Will make shure that animations will look right.
+  switch (eSteerType) {
+  case stSteer:
+    SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
+    break;
+  case stFixed:
+    SteerAngle = 0.0;
+    break;
+  case stCaster:
+    // Note to Jon: This is not correct for castering gear.  I'll fix it later.
+    SteerAngle = 0.0;
+    break;
+  default:
+    cerr << "Improper steering type membership detected for this gear." << endl;
+    break;
+  }
+
   if (GearDown) {
+    double t = Exec->GetState()->Getsim_time();
 
     vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
 
@@ -239,7 +259,10 @@ FGColumnVector3& FGLGear::Force(void)
 
 // vLocalGear now stores the vector from the cg to the wheel in local coords.
 
-    compressLength = vLocalGear(eZ) - Propagate->GetDistanceAGL();
+    FGColumnVector3 normal, cvel;
+    FGLocation contact;
+    FGLocation gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear);
+    compressLength = - Exec->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel);
 
 // The compression length is currently measured in the Z-axis, only, at this time.
 // It should be measured along the strut axis. If the local-frame gear position
@@ -262,7 +285,7 @@ FGColumnVector3& FGLGear::Force(void)
 // wheel velocity.
 
       vWhlVelVec      =  Propagate->GetTb2l() * (Propagate->GetPQR() * vWhlBodyVec);
-      vWhlVelVec     +=  Propagate->GetVel();
+      vWhlVelVec     +=  Propagate->GetVel() - cvel;
       compressSpeed   =  vWhlVelVec(eZ);
 
 // If this is the first time the wheel has made contact, remember some values
@@ -323,28 +346,12 @@ FGColumnVector3& FGLGear::Force(void)
         break;
       }
 
-      switch (eSteerType) {
-      case stSteer:
-        SteerAngle = -maxSteerAngle * FCS->GetDrCmd() * 0.01745;
-        break;
-      case stFixed:
-        SteerAngle = 0.0;
-        break;
-      case stCaster:
-// Note to Jon: This is not correct for castering gear.  I'll fix it later.
-        SteerAngle = 0.0;
-        break;
-      default:
-        cerr << "Improper steering type membership detected for this gear." << endl;
-        break;
-      }
-
 // Transform the wheel velocities from the local axis system to the wheel axis system.
 // For now, steering angle is assumed to happen in the Local Z axis,
 // not the strut axis as it should be.  Will fix this later.
 
-      SinWheel      = sin(Propagate->Getpsi() + SteerAngle);
-      CosWheel      = cos(Propagate->Getpsi() + SteerAngle);
+      SinWheel      = sin(Propagate->GetEuler(ePsi) + SteerAngle);
+      CosWheel      = cos(Propagate->GetEuler(ePsi) + SteerAngle);
       RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
       SideWhlVel    = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
 
@@ -353,9 +360,9 @@ FGColumnVector3& FGLGear::Force(void)
       if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
         WheelSlip = 0.0;
       } else if (fabs(RollingWhlVel) < 1.0) {
-        WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;
+        WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, fabs(RollingWhlVel)) + 0.95*WheelSlip;
       } else {
-        WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
+        WheelSlip = radtodeg*atan2(SideWhlVel, fabs(RollingWhlVel));
       }
 /*
       double maxdeltaSlip = 0.5*deltaT;
@@ -415,9 +422,10 @@ FGColumnVector3& FGLGear::Force(void)
 
       RollingForce = 0;
       if (fabs(RollingWhlVel) > 1E-3) {
-        RollingForce = (1.0 - TirePressureNorm) * 30
-                       + vLocalForce(eZ) * BrakeFCoeff
-                       * fabs(RollingWhlVel)/RollingWhlVel;
+        double badPresResis = (1.0 - TirePressureNorm) * 30;
+        RollingForce = (badPresResis * min(fabs(RollingWhlVel), 1.0)
+                        + vLocalForce(eZ) * BrakeFCoeff)
+                        * fabs(RollingWhlVel)/RollingWhlVel;
       }
       SideForce    = vLocalForce(eZ) * FCoeff;
 
@@ -447,6 +455,9 @@ FGColumnVector3& FGLGear::Force(void)
 
       WOW = false;
 
+      // Return to neutral position between 1.0 and 0.8 gear pos.
+      SteerAngle *= max(FCS->GetGearPos()-0.8, 0.0)/0.2;
+
       if (Propagate->GetDistanceAGL() > 200.0) {
         FirstContact = false;
         StartedGroundRun = false;