Encapsulate the interpolstion version of FGEnvironment and fix some bugs

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

diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp
index b717df1c11e32a1f8f3aef26b366f7eea74edee0..4bdb1f2feb146928c5e6bbb0fb4b8ec1f9ee9fbe 100644 (file)
--- a/src/FDM/JSBSim/FGLGear.cpp
+++ b/src/FDM/JSBSim/FGLGear.cpp
@@ -39,7 +39,7 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGLGear.h"
-#include <algorithm>
+//#include <algorithm>
 
 namespace JSBSim {
 
@@ -94,6 +94,7 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
   
   GearUp = false;
   GearDown = true;
+  Servicable = true;
 
 // Add some AI here to determine if gear is located properly according to its
 // brake group type ??
@@ -114,9 +115,7 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
   MaximumStrutForce = MaximumStrutTravel = 0.0;
   SinkRate = GroundSpeed = 0.0;
 
-  vWhlBodyVec     = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
-  vWhlBodyVec(eX) = -vWhlBodyVec(eX);
-  vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
+  vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
   
   vLocalGear = State->GetTb2l() * vWhlBodyVec;
 
@@ -132,6 +131,8 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
   brakePct        = 0.0;
   maxCompLen      = 0.0;
 
+  TirePressureNorm = 1.0;
+
   Debug(0);
 }
 
@@ -188,6 +189,8 @@ FGLGear::FGLGear(const FGLGear& lgear)
   GearDown        = lgear.GearDown;
   WheelSlip       = lgear.WheelSlip;
   lastWheelSlip   = lgear.lastWheelSlip;
+  TirePressureNorm = lgear.TirePressureNorm;
+  Servicable      = lgear.Servicable;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -203,7 +206,9 @@ FGColumnVector3& FGLGear::Force(void)
 {
   double SteerGain = 0;
   double SinWheel, CosWheel;
-  double deltaT;
+  double deltaSlip;
+  double deltaT = State->Getdt()*Aircraft->GetRate();
+  double maxdeltaSlip = 0.5*deltaT;
 
   vForce.InitMatrix();
   vMoment.InitMatrix();
@@ -226,9 +231,7 @@ FGColumnVector3& FGLGear::Force(void)
       
   if (GearDown) {
 
-    vWhlBodyVec     = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
-    vWhlBodyVec(eX) = -vWhlBodyVec(eX);
-    vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
+    vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
 
 // vWhlBodyVec now stores the vector from the cg to this wheel
 
@@ -245,7 +248,7 @@ FGColumnVector3& FGLGear::Force(void)
 
     if (compressLength > 0.00) {
 
-      WOW = true;// Weight-On-Wheels is true
+      WOW = true; // Weight-On-Wheels is true
 
 // The next equation should really use the vector to the contact patch of the tire
 // including the strut compression and not vWhlBodyVec.  Will fix this later.
@@ -295,31 +298,27 @@ FGColumnVector3& FGLGear::Force(void)
 
       switch (eBrakeGrp) {
       case bgLeft:
-        SteerGain = 0.10;
-        BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
-                                              staticFCoeff*FCS->GetBrake(bgLeft);
+         BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
+                        staticFCoeff*FCS->GetBrake(bgLeft) );
         break;
       case bgRight:
-        SteerGain = 0.10;
-        BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
-                                             staticFCoeff*FCS->GetBrake(bgRight);
+        BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
+                         staticFCoeff*FCS->GetBrake(bgRight) );
         break;
       case bgCenter:
-        SteerGain = 0.10;
-        BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
-                                             staticFCoeff*FCS->GetBrake(bgCenter);
+        BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
+                         staticFCoeff*FCS->GetBrake(bgCenter) );
         break;
       case bgNose:
-        SteerGain = -0.50;
-        BrakeFCoeff = rollingFCoeff;
+        BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
+                         staticFCoeff*FCS->GetBrake(bgCenter) );
         break;
       case bgTail:
-        SteerGain = -0.10;
-        BrakeFCoeff = rollingFCoeff;
+        BrakeFCoeff =  ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
+                         staticFCoeff*FCS->GetBrake(bgCenter) );
         break;
       case bgNone:
-        SteerGain = 0.0;
-        BrakeFCoeff = rollingFCoeff;
+        BrakeFCoeff =  rollingFCoeff;
         break;
       default:
         cerr << "Improper brake group membership detected for this gear." << endl;
@@ -328,7 +327,7 @@ FGColumnVector3& FGLGear::Force(void)
 
       switch (eSteerType) {
       case stSteer:
-        SteerAngle = SteerGain*FCS->GetDrCmd()*0.349; // 20 deg
+        SteerAngle = -maxSteerAngle * FCS->GetDrCmd() * 0.01745; 
         break;
       case stFixed:
         SteerAngle = 0.0;
@@ -355,18 +354,34 @@ FGColumnVector3& FGLGear::Force(void)
 
       if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
         WheelSlip = 0.0;
-      } else if (fabs(RollingWhlVel) < 0.10) {
+      } else if (fabs(RollingWhlVel) < 1.0) {
         WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;
       } else {
         WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
       }
+/*
+      if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
+        WheelSlip = 0.0;
+      } else if (RollingWhlVel < 1.0) {
+        WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
+        deltaSlip = WheelSlip - lastWheelSlip;
+        if (fabs(deltaSlip) > maxdeltaSlip) {
+          if (WheelSlip > lastWheelSlip) {
+            WheelSlip = lastWheelSlip + maxdeltaSlip;
+          } else if (WheelSlip < lastWheelSlip) {
+            WheelSlip = lastWheelSlip - maxdeltaSlip;
+          }
+        }
+      } else {
+        WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
+      }
 
       if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) ||
           (WheelSlip > 0.0 && lastWheelSlip < 0.0))
       {
         WheelSlip = 0.0;
       }
-      
+*/    
       lastWheelSlip = WheelSlip;
 
 // Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
@@ -400,7 +415,9 @@ FGColumnVector3& FGLGear::Force(void)
 
       RollingForce = 0;
       if (fabs(RollingWhlVel) > 1E-3) {
-        RollingForce = vLocalForce(eZ) * BrakeFCoeff * fabs(RollingWhlVel)/RollingWhlVel;
+        RollingForce = (1.0 - TirePressureNorm) * 30
+                       + vLocalForce(eZ) * BrakeFCoeff
+                       * fabs(RollingWhlVel)/RollingWhlVel;
       }
       SideForce    = vLocalForce(eZ) * FCoeff;
 
@@ -426,7 +443,7 @@ FGColumnVector3& FGLGear::Force(void)
       vForce  = State->GetTl2b() * vLocalForce;
       vMoment = vWhlBodyVec * vForce;
 
-    } else {
+    } else { // Gear is NOT compressed
 
       WOW = false;
 
@@ -441,8 +458,6 @@ FGColumnVector3& FGLGear::Force(void)
       compressLength = 0.0; // reset compressLength to zero for data output validity
     }
 
-    deltaT = State->Getdt()*Aircraft->GetRate();
-
     if (FirstContact) LandingDistanceTraveled += Position->GetVground()*deltaT;
   
     if (StartedGroundRun) {
@@ -488,23 +503,23 @@ void FGLGear::Report(ReportType repType)
   case erLand:
     cout << endl << "Touchdown report for " << name << endl;
     cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
-                                << SinkRate*0.3408          << " mps"     << endl;
+                                << SinkRate*0.3048          << " mps"     << endl;
     cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
-                                << GroundSpeed*0.3408       << " mps"     << endl;
+                                << GroundSpeed*0.3048       << " mps"     << endl;
     cout << "  Maximum contact force: " << MaximumStrutForce       << " lbs,    "
                                 << MaximumStrutForce*4.448  << " Newtons" << endl;
     cout << "  Maximum strut travel:  " << MaximumStrutTravel*12.0 << " inches, "
                                 << MaximumStrutTravel*30.48 << " cm"      << endl;
     cout << "  Distance traveled:     " << LandingDistanceTraveled        << " ft,     "
-                                << LandingDistanceTraveled*0.3408  << " meters"  << endl;
+                                << LandingDistanceTraveled*0.3048  << " meters"  << endl;
     LandingReported = true;
     break;
   case erTakeoff:
     cout << endl << "Takeoff report for " << name << endl;
     cout << "  Distance traveled:                " << TakeoffDistanceTraveled
-         << " ft,     " << TakeoffDistanceTraveled*0.3408  << " meters"  << endl;
+         << " ft,     " << TakeoffDistanceTraveled*0.3048  << " meters"  << endl;
     cout << "  Distance traveled (over 50'):     " << TakeoffDistanceTraveled50ft
-         << " ft,     " << TakeoffDistanceTraveled50ft*0.3408 << " meters" << endl;
+         << " ft,     " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
     TakeoffReported = true;
     break;
   }