Check return value of FDM::init().

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

diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp
index 865fbe6bf0eca0f54d0eb08169d9915d91f16626..6858c969cf92bf664c2b9b865112e7c90eff1281 100644 (file)
--- a/src/FDM/JSBSim/FGLGear.cpp
+++ b/src/FDM/JSBSim/FGLGear.cpp
@@ -39,6 +39,9 @@ INCLUDES
 #include "FGLGear.h"
 #include <algorithm>
 
+static const char *IdSrc = "$Header$";
+static const char *IdHdr = ID_LGEAR;
+
 /*******************************************************************************
 ************************************ CODE **************************************
 *******************************************************************************/
@@ -46,17 +49,36 @@ INCLUDES
 
 FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
                                                            vMoment(3),
+                                                           vWhlBodyVec(3),
                                                            Exec(fdmex)
 {
   string tmp;
-  *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3) >> kSpring >> bDamp
-                                                    >> statFCoeff >> brakeCoeff;
+  *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)  
+            >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
+                 >> SteerType >> BrakeType >> GroupMember >> maxSteerAngle;
+    
+  cout << "    Name: " << name << endl;
+  cout << "      Location: " << vXYZ << endl;
+  cout << "      Spring Constant:  " << kSpring << endl;
+  cout << "      Damping Constant: " << bDamp << endl;
+  cout << "      Dynamic Friction: " << dynamicFCoeff << endl;
+  cout << "      Static Friction:  " << staticFCoeff << endl;
+  cout << "      Brake Type:       " << BrakeType << endl;
+  cout << "      Grouping:         " << GroupMember << endl;
+  cout << "      Steering Type:    " << SteerType << endl;
+  cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
+  
   State       = Exec->GetState();
   Aircraft    = Exec->GetAircraft();
   Position    = Exec->GetPosition();
   Rotation    = Exec->GetRotation();
   
   WOW = false;
+  ReportEnable=true;
+  FirstContact = false;
+  Reported = false;
+  DistanceTraveled = 0.0;
+  MaximumStrutForce = MaximumStrutTravel = 0.0;
 }
 
 
@@ -64,53 +86,101 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
 
 FGLGear::~FGLGear(void)
 {
+  cout << "Destructing Landing Gear ..." << endl;
 }
 
 /******************************************************************************/
 
 FGColumnVector FGLGear::Force(void)
 {
-  static FGColumnVector vForce(3);
-  static FGColumnVector vLocalForce(3);
-  static FGColumnVector vLocalGear(3);
-  static FGColumnVector vWhlBodyVec(3);
-  static FGColumnVector vWhlVelVec(3);
-
-  vWhlBodyVec     = vXYZ - Aircraft->GetXYZcg();
+  FGColumnVector vForce(3);
+  FGColumnVector vLocalForce(3);
+  FGColumnVector vLocalGear(3);     // Vector: CG to this wheel (Local)
+  FGColumnVector vWhlVelVec(3);     // Velocity of this wheel (Local)
+  
+  vWhlBodyVec     = (vXYZ - Aircraft->GetXYZcg()) / 12.0;
   vWhlBodyVec(eX) = -vWhlBodyVec(eX);
   vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
-  vWhlBodyVec     = vWhlBodyVec/12.0;
 
   vLocalGear = State->GetTb2l() * vWhlBodyVec;
-
+  
   compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
 
   if (compressLength > 0.00) {
-
+     
     WOW = true;
+    vWhlVelVec      =  State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
+    vWhlVelVec     +=  Position->GetVel();
+
+    compressSpeed   =  vWhlVelVec(eZ);
+
+    if (!FirstContact) {
+      FirstContact  = true;
+      SinkRate      =  compressSpeed;
+      GroundSpeed   =  Position->GetVel().Magnitude();
+    }
 
-    vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
-    compressSpeed = vWhlVelVec(eZ) + Position->GetVd();
+    // The following code normalizes the wheel velocity vector, reverses it, and zeroes out
+    // the z component of the velocity. The question is, should the Z axis velocity be zeroed
+    // out first before the normalization takes place or not? Subsequent to that, the Wheel
+    // Velocity vector now points as a unit vector backwards and parallel to the wheel
+    // velocity vector. It acts AT the wheel.
 
-    vLocalForce(eZ) = min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0);
+    vWhlVelVec      = -1.0 * vWhlVelVec.Normalize();
+    vWhlVelVec(eZ)  =  0.00;
 
-    vForce = State->GetTl2b() * vLocalForce ;
+// the following needs work regarding friction coefficients and braking and steering
 
-    // currently only aircraft body axis Z-force modeled
-    vMoment(eX) = vForce(eZ) * vWhlBodyVec(eY);
-    vMoment(eY) = -vForce(eZ) * vWhlBodyVec(eX);
-    vMoment(eZ) = 0.0;
+    vLocalForce(eZ) =  min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0);
+    vLocalForce(eX) =  fabs(vLocalForce(eZ) * staticFCoeff) * vWhlVelVec(eX);
+    vLocalForce(eY) =  fabs(vLocalForce(eZ) * staticFCoeff) * vWhlVelVec(eY);
+
+    MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
+    MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
+
+    vForce  = State->GetTl2b() * vLocalForce ;
+    vMoment = vWhlBodyVec * vForce;
 
   } else {
 
     WOW = false;
+
+    if (Position->GetDistanceAGL() > 200.0) {
+      FirstContact = false;
+      Reported = false;
+      DistanceTraveled = 0.0;
+      MaximumStrutForce = MaximumStrutTravel = 0.0;
+    }
+
     vForce.InitMatrix();
     vMoment.InitMatrix();
   }
 
+  if (FirstContact) {
+    DistanceTraveled += Position->GetVel().Magnitude()*State->Getdt()*Aircraft->GetRate();
+  }
 
+  if (ReportEnable && Position->GetVel().Magnitude() <= 0.05 && !Reported) {
+    Report();
+  }
   return vForce;
 }
 
 /******************************************************************************/
 
+void FGLGear::Report(void)
+{
+  cout << endl << "Touchdown report for " << name << endl;
+  cout << "  Sink rate at contact:  " << SinkRate                << " fps,    "
+                              << SinkRate*0.3408          << " mps"     << endl;
+  cout << "  Contact ground speed:  " << GroundSpeed*.5925       << " knots,  "
+                              << GroundSpeed*0.3408       << " 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:     " << DistanceTraveled        << " ft,     "
+                              << DistanceTraveled*0.3408  << " meters"  << endl;
+  Reported = true;
+}
+