Removed useless divisions - As a side effect, it removes the risk of divisions by...

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

diff --git a/src/FDM/JSBSim/models/FGInertial.cpp b/src/FDM/JSBSim/models/FGInertial.cpp
index 4ff657a51e8774471cab7986cd67374a10d56371..93874c12064cbcffef39f728d0626fcb2da75ed1 100644 (file)
--- a/src/FDM/JSBSim/models/FGInertial.cpp
+++ b/src/FDM/JSBSim/models/FGInertial.cpp
@@ -38,7 +38,6 @@ INCLUDES
 #include "FGInertial.h"
 #include "FGFDMExec.h"
 #include "FGPropagate.h"
-#include "FGState.h"
 #include "FGMassBalance.h"
 #include <iostream>
 
@@ -46,7 +45,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGInertial.cpp,v 1.21 2011/05/20 03:18:36 jberndt Exp $";
 static const char *IdHdr = ID_INERTIAL;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -99,8 +98,6 @@ FGInertial::~FGInertial(void)
 
 bool FGInertial::InitModel(void)
 {
-  if (!FGModel::InitModel()) return false;
-
   earthPosAngle   = 0.0;
 
   return true;
@@ -108,18 +105,18 @@ bool FGInertial::InitModel(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-bool FGInertial::Run(void)
+bool FGInertial::Run(bool Holding)
 {
   // Fast return if we have nothing to do ...
-  if (FGModel::Run()) return true;
-  if (FDMExec->Holding()) return false;
+  if (FGModel::Run(Holding)) return true;
+  if (Holding) return false;
 
   RunPreFunctions();
 
   // Gravitation accel
-  double r = Propagate->GetRadius();
+  double r = FDMExec->GetPropagate()->GetRadius();
   gAccel = GetGAccel(r);
-  earthPosAngle += State->Getdt()*RotationRate;
+  earthPosAngle += FDMExec->GetDeltaT()*RotationRate;
 
   RunPostFunctions();
 
@@ -133,6 +130,35 @@ double FGInertial::GetGAccel(double r) const
   return GM/(r*r);
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+//
+// Calculate the WGS84 gravitation value in ECEF frame. Pass in the ECEF position
+// via the position parameter. The J2Gravity value returned is in ECEF frame,
+// and therefore may need to be expressed (transformed) in another frame,
+// depending on how it is used. See Stevens and Lewis eqn. 1.4-16.
+
+FGColumnVector3 FGInertial::GetGravityJ2(FGColumnVector3 position) const
+{
+  FGColumnVector3 J2Gravity;
+
+  // Gravitation accel
+  double r = position.Magnitude();
+  double lat = FDMExec->GetPropagate()->GetLatitude();
+  double sinLat = sin(lat);
+
+  double adivr = a/r;
+  double preCommon = 1.5*J2*adivr*adivr;
+  double xy = 1.0 - 5.0*(sinLat*sinLat);
+  double z = 3.0 - 5.0*(sinLat*sinLat);
+  double GMOverr2 = GM/(r*r);
+
+  J2Gravity(1) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eX)/r);
+  J2Gravity(2) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eY)/r);
+  J2Gravity(3) = -GMOverr2 * ((1.0 + (preCommon *  z)) * position(eZ)/r);
+
+  return J2Gravity;
+}
+
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGInertial::bind(void)