diff --git a/src/FDM/JSBSim/math/FGLocation.cpp b/src/FDM/JSBSim/math/FGLocation.cpp index fcba3289e..ab88f3735 100644 --- 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 46a1b0d99..23b3845e6 100644 --- 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 079748274..30649c6a4 100644 --- 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 87c76685f..1a40501a1 100644 --- 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();