Merge branch 'next' into durk-atc

[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 75a699f990689f73cbd9a9c1fe8fef835cc9141b..94aa291c3346653f8f37024f4bad96b49bdd3141 100644 (file)
--- a/src/FDM/JSBSim/models/FGInertial.cpp
+++ b/src/FDM/JSBSim/models/FGInertial.cpp
@@ -5,7 +5,7 @@
  Date started: 09/13/00
  Purpose:      Encapsulates the inertial frame forces (coriolis and centrifugal)
 
- ------------- Copyright (C) 2000  Jon S. Berndt (jsb@hal-pc.org) -------------
+ ------------- Copyright (C) 2000  Jon S. Berndt (jon@jsbsim.org) -------------
 
  This program is free software; you can redistribute it and/or modify it under
  the terms of the GNU Lesser General Public License as published by the Free Software
@@ -36,13 +36,16 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGInertial.h"
+#include "FGFDMExec.h"
 #include "FGPropagate.h"
-#include "FGState.h"
 #include "FGMassBalance.h"
+#include <iostream>
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGInertial.cpp,v 1.20 2010/11/18 12:38:06 jberndt Exp $";
 static const char *IdHdr = ID_INERTIAL;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -54,7 +57,7 @@ FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex)
 {
   Name = "FGInertial";
 
-  // Defaults
+  // Earth defaults
   RotationRate    = 0.00007292115;
   GM              = 14.07644180E15;     // WGS84 value
   RadiusReference = 20925650.00;        // Equatorial radius (WGS84)
@@ -64,6 +67,18 @@ FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex)
   b               = 20855486.5951;      // WGS84 semiminor axis length in feet
   earthPosAngle   = 0.0;
 
+  // Lunar defaults
+  /*
+  RotationRate    = 0.0000026617;
+  GM              = 1.7314079E14;         // Lunar GM
+  RadiusReference = 5702559.05;           // Equatorial radius
+  C2_0            = 0;                    // value for the C2,0 coefficient
+  J2              = 2.033542482111609E-4; // value for J2
+  a               = 5702559.05;           // semimajor axis length in feet 
+  b               = 5695439.63;           // semiminor axis length in feet
+  earthPosAngle   = 0.0;
+  */
+
   gAccelReference = GM/(RadiusReference*RadiusReference);
   gAccel          = GM/(RadiusReference*RadiusReference);
 
@@ -98,10 +113,14 @@ bool FGInertial::Run(void)
   if (FGModel::Run()) return true;
   if (FDMExec->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();
 
   return false;
 }
@@ -113,6 +132,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)