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 05417b0d2afd0fc85b4bf9bd47cc7785f61080ed..6858c969cf92bf664c2b9b865112e7c90eff1281 100644 (file)
--- a/src/FDM/JSBSim/FGLGear.cpp
+++ b/src/FDM/JSBSim/FGLGear.cpp
@@ -37,26 +37,150 @@ INCLUDES
 *******************************************************************************/
 
 #include "FGLGear.h"
-#include "FGState.h"
+#include <algorithm>
+
+static const char *IdSrc = "$Header$";
+static const char *IdHdr = ID_LGEAR;
 
 /*******************************************************************************
 ************************************ CODE **************************************
 *******************************************************************************/
 
 
-FGLGear::FGLGear(ifstream& acfile)
+FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
+                                                           vMoment(3),
+                                                           vWhlBodyVec(3),
+                                                           Exec(fdmex)
 {
-  acfile >> name >> X >> Y >> Z >> kSpring >> bDamp >> statFCoeff >> brakeCoeff;
+  string tmp;
+  *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;
 }
 
 
+/******************************************************************************/
+
 FGLGear::~FGLGear(void)
 {
+  cout << "Destructing Landing Gear ..." << endl;
 }
 
-float FGLGear::Force(void)
+/******************************************************************************/
+
+FGColumnVector FGLGear::Force(void)
 {
-  return 0.0;
+  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);
+
+  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();
+    }
+
+    // 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.
+
+    vWhlVelVec      = -1.0 * vWhlVelVec.Normalize();
+    vWhlVelVec(eZ)  =  0.00;
+
+// the following needs work regarding friction coefficients and braking and steering
+
+    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;
+}