PAtch by Andreas Gaeb to eliminate NaN's in the location code

diff --git a/src/FDM/JSBSim/math/FGLocation.cpp b/src/FDM/JSBSim/math/FGLocation.cpp
index fcba3289e19de9132180e7758b50701944f5311c..ab88f3735173c34589982172dc78d4a5e18ea8bb 100644 (file)
--- a/src/FDM/JSBSim/math/FGLocation.cpp
+++ b/src/FDM/JSBSim/math/FGLocation.cpp
@@ -56,20 +56,14 @@ CLASS IMPLEMENTATION
 FGLocation::FGLocation(void)
 {
   mCacheValid = false;
-  initial_longitude = 0.0;
-  a = 0.0;
-  b = 0.0;
-  a2 = 0.0;
-  b2 = 0.0;
-  e2 = 1.0;
-  e = 1.0;
+
+  a = b = a2 = b2 = 0.0;
+  e = e2 = f = 1.0;
   eps2 = -1.0;
-  f = 1.0;
   epa = 0.0;
 
-  mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
-  
-//  initial_longitude = 0.0;
+  mLon = mLat = mRadius = 0.0;
+  mGeodLat = GeodeticAltitude = initial_longitude = 0.0;
 
   mTl2ec.InitMatrix();
   mTec2l.InitMatrix();
@@ -84,26 +78,117 @@ FGLocation::FGLocation(void)
 
 FGLocation::FGLocation(double lon, double lat, double radius)
 {
-  mCacheValid = false;
+
+  a = b = a2 = b2 = 0.0;
+  e = e2 = f = 1.0;
+  eps2 = -1.0;
+  epa = 0.0;
+
+  mLon = mLat = mRadius = 0.0;
+  mGeodLat = GeodeticAltitude = initial_longitude = 0.0;
+
+  mTl2ec.InitMatrix();
+  mTec2l.InitMatrix();
+  mTi2ec.InitMatrix();
+  mTec2i.InitMatrix();
+  mTi2l.InitMatrix();
+  mTl2i.InitMatrix();
 
   double sinLat = sin(lat);
   double cosLat = cos(lat);
   double sinLon = sin(lon);
   double cosLon = cos(lon);
-
-  a = 0.0;
-  b = 0.0;
-  a2 = 0.0;
-  b2 = 0.0;
-  e2 = 1.0;
-  e = 1.0;
-  eps2 = -1.0;
-  f = 1.0;
-  epa = 0.0;
   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
                             radius*cosLat*sinLon,
                             radius*sinLat );
-  mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGLocation::FGLocation(const FGColumnVector3& lv) : mECLoc(lv), mCacheValid(false)
+{
+  a = b = a2 = b2 = 0.0;
+  e = e2 = f = 1.0;
+  eps2 = -1.0;
+  epa = 0.0;
+
+  mLon = mLat = mRadius = 0.0;
+  mGeodLat = GeodeticAltitude = initial_longitude = 0.0;
+
+  mTl2ec.InitMatrix();
+  mTec2l.InitMatrix();
+  mTi2ec.InitMatrix();
+  mTec2i.InitMatrix();
+  mTi2l.InitMatrix();
+  mTl2i.InitMatrix();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGLocation::FGLocation(const FGLocation& l)
+  : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid)
+{
+
+  a = l.a;
+  b = l.b;
+  a2 = l.a2;
+  b2 = l.b2;
+  e2 = l.e2;
+  e = l.e;
+  eps2 = l.eps2;
+  f = l.f;
+
+  /*ag
+   * if the cache is not valid, all of the following values are unset.
+   * They will be calculated once ComputeDerivedUnconditional is called.
+   * If unset, they may possibly contain NaN and could thus trigger floating
+   * point exceptions.
+   */
+  if (!mCacheValid) return;
+
+  mLon = l.mLon;
+  mLat = l.mLat;
+  mRadius = l.mRadius;
+
+  mTl2ec = l.mTl2ec;
+  mTec2l = l.mTec2l;
+
+  initial_longitude = l.initial_longitude;
+  mGeodLat = l.mGeodLat;
+  GeodeticAltitude = l.GeodeticAltitude;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+const FGLocation& FGLocation::operator=(const FGLocation& l)
+{
+  mECLoc = l.mECLoc;
+  mCacheValid = l.mCacheValid;
+
+  a = l.a;
+  b = l.b;
+  a2 = l.a2;
+  b2 = l.b2;
+  e2 = l.e2;
+  e = l.e;
+  eps2 = l.eps2;
+  f = l.f;
+
+  //ag See comment in constructor above
+  if (!mCacheValid) return *this;
+
+  mLon = l.mLon;
+  mLat = l.mLat;
+  mRadius = l.mRadius;
+
+  mTl2ec = l.mTl2ec;
+  mTec2l = l.mTec2l;
+
+  initial_longitude = l.initial_longitude;
+  mGeodLat = l.mGeodLat;
+  GeodeticAltitude = l.GeodeticAltitude;
+
+  return *this;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -324,9 +409,9 @@ void FGLocation::ComputeDerivedUnconditional(void) const
     s  = r02/(e2*eps2);
     p2 = p*p;
     q  = p2 - b2 + s;
-    sqrt_q = sqrt(q);
     if (q>0)
     {
+      sqrt_q = sqrt(q);
       u  = p/sqrt_q;
       u2 = p2/q;
       v  = b2*u2/q;

diff --git a/src/FDM/JSBSim/math/FGLocation.h b/src/FDM/JSBSim/math/FGLocation.h
index 46a1b0d9994242f07bbf030529529c676771be72..23b3845e6fab08b407967138e0acf5a932a15601 100644 (file)
--- a/src/FDM/JSBSim/math/FGLocation.h
+++ b/src/FDM/JSBSim/math/FGLocation.h
@@ -163,42 +163,10 @@ public:
   FGLocation(double lon, double lat, double radius);
 
   /** Column constructor. */
-  FGLocation(const FGColumnVector3& lv) : mECLoc(lv), mCacheValid(false)
-  {
-    a = 0.0;
-    b = 0.0;
-    a2 = 0.0;
-    b2 = 0.0;
-    e2 = 1.0;
-    e = 1.0;
-    eps2 = -1.0;
-    f = 1.0;
-  }
+  FGLocation(const FGColumnVector3& lv);
 
   /** Copy constructor. */
-  FGLocation(const FGLocation& l)
-    : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid)
-  {
-//    if (!mCacheValid) return; // This doesn't seem right.
-
-    mLon = l.mLon;
-    mLat = l.mLat;
-    mRadius = l.mRadius;
-
-    mTl2ec = l.mTl2ec;
-    mTec2l = l.mTec2l;
-
-    a = l.a;
-    b = l.b;
-    a2 = l.a2;
-    b2 = l.b2;
-    e2 = l.e2;
-    e = l.e;
-    eps2 = l.eps2;
-    f = l.f;
-
-    initial_longitude = l.initial_longitude;
-  }
+  FGLocation(const FGLocation& l);
 
   /** Set the longitude.
       @param longitude Longitude in rad to set.
@@ -426,35 +394,7 @@ public:
   /** Sets this location via the supplied location object.
       @param v A location object reference. 
       @return a reference to the FGLocation object. */
-  const FGLocation& operator=(const FGLocation& l)
-  {
-    mECLoc = l.mECLoc;
-    mCacheValid = l.mCacheValid;
-
-//    if (!mCacheValid) return *this; // Why is this here for an assignment operator?
-
-    mLon = l.mLon;
-    mLat = l.mLat;
-    mRadius = l.mRadius;
-
-    mTl2ec = l.mTl2ec;
-    mTec2l = l.mTec2l;
-
-    a = l.a;
-    b = l.b;
-    a2 = l.a2;
-    b2 = l.b2;
-    e2 = l.e2;
-    e = l.e;
-    eps2 = l.eps2;
-    f = l.f;
-
-    initial_longitude = l.initial_longitude;
-    mGeodLat = l.mGeodLat;
-    GeodeticAltitude = l.GeodeticAltitude;
-
-    return *this;
-  }
+  const FGLocation& operator=(const FGLocation& l);
 
   /** This operator returns true if the ECEF location vectors for the two
       location objects are equal. */

diff --git a/src/FDM/JSBSim/models/FGAircraft.cpp b/src/FDM/JSBSim/models/FGAircraft.cpp
index 079748274e1e39fb0da61127a4da417f0d38332d..30649c6a42337b638b27a150219328983b4a9ad1 100644 (file)
--- a/src/FDM/JSBSim/models/FGAircraft.cpp
+++ b/src/FDM/JSBSim/models/FGAircraft.cpp
@@ -152,7 +152,10 @@ bool FGAircraft::Run(void)
 
 double FGAircraft::GetNlf(void) const
 {
+  if (FDMExec->GetMassBalance()->GetWeight() != 0)
   return (-FDMExec->GetAerodynamics()->GetvFw(3))/FDMExec->GetMassBalance()->GetWeight();
+  else
+    return 0.;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

diff --git a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp
index 87c76685fc4809eda32724c43b82d5ce55282c84..1a40501a199957c09b03443245a60565e3db6cff 100644 (file)
--- a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp
+++ b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp
@@ -124,6 +124,8 @@ FGRocket::~FGRocket(void)
 
 void FGRocket::Calculate(void)
 {
+  if (FDMExec->IntegrationSuspended()) return;
+
   double dT = FDMExec->GetDeltaT()*Propulsion->GetRate();
 
   RunPreFunctions();