JSBSim tweaks.

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

diff --git a/src/FDM/JSBSim/FGInertial.cpp b/src/FDM/JSBSim/FGInertial.cpp
index e72aa64ed334d562cee0b08e388afb31f9b6e7c2..db9ffe46ece93d04a0085fc43dee447ceb27443d 100644 (file)
--- a/src/FDM/JSBSim/FGInertial.cpp
+++ b/src/FDM/JSBSim/FGInertial.cpp
@@ -36,33 +36,92 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGInertial.h"
+#include "FGPosition.h"
+#include "FGMassBalance.h"
 
-static const char *IdSrc = "$Header$";
+static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_INERTIAL;
 
+extern short debug_lvl;
+
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 
-FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex)
+FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex),
+    vForces(3),
+    vOmegaLocal(3),
+    vRadius(3)
 {
+  Name = "FGInertial";
+
+  vRadius.InitMatrix();
 
+  if (debug_lvl & 2) cout << "Instantiated: FGInertial" << endl;
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGInertial::~FGInertial(void)
+{
+  if (debug_lvl & 2) cout << "Destroyed:    FGInertial" << endl;
+}
 
-bool FGInertial:: Run(void) {
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGInertial::Run(void)
+{
+  float stht, ctht, sphi, cphi;
 
   if (!FGModel::Run()) {
 
+    stht = sin(Rotation->GetEuler(eTht));
+    ctht = cos(Rotation->GetEuler(eTht));
+    sphi = sin(Rotation->GetEuler(ePhi));
+    cphi = cos(Rotation->GetEuler(ePhi));
+
+    vForces(eX) = -GRAVITY*stht;
+    vForces(eY) =  GRAVITY*sphi*ctht;
+    vForces(eZ) =  GRAVITY*cphi*ctht;
+    
+    // The following equation for vOmegaLocal terms shows the angular velocity
+    // calculation _for_the_local_frame_ given the earth's rotation (first set)
+    // at the current latitude, and also the component due to the aircraft
+    // motion over the curved surface of the earth (second set).
+
+    vOmegaLocal(eX) = OMEGA_EARTH * cos(Position->GetLatitude());
+    vOmegaLocal(eY) = 0.0;
+    vOmegaLocal(eZ) = OMEGA_EARTH * -sin(Position->GetLatitude());
+
+    vOmegaLocal(eX) +=  Position->GetVe() / Position->GetRadius();
+    vOmegaLocal(eY) += -Position->GetVn() / Position->GetRadius();
+    vOmegaLocal(eZ) +=  0.00;
+
+//    vForces = State->GetTl2b()*(-2.0*vOmegaLocal * Position->GetVel());
+
+    vRadius(3) = Position->GetRadius();
+    vForces += State->GetTl2b()*(vOmegaLocal * (vOmegaLocal * vRadius));
+
+    vForces *= MassBalance->GetMass(); // IMPORTANT !!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    
     return false;
   } else {
     return true;
   }
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 bool FGInertial::LoadInertial(FGConfigFile* AC_cfg)
 {
-//
+  return true;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGInertial::Debug(void)
+{
+    //TODO: Add your source code here
 }