fly/flightgear
1
0
Fork 0
Code Activity

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

Browse source
This commit is contained in:
Erik Hofman 2010-11-29 09:57:45 +01:00
parent 1cf207e054
commit c44948041b
4 changed files with 117 additions and 87 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

133
src/FDM/JSBSim/math/FGLocation.cpp
View file

@ -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;

66
src/FDM/JSBSim/math/FGLocation.h
View file

@ -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. */

3
src/FDM/JSBSim/models/FGAircraft.cpp
View file

@ -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.;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/propulsion/FGRocket.cpp
View file

@ -124,6 +124,8 @@ FGRocket::~FGRocket(void)
void FGRocket::Calculate(void)
{
if (FDMExec->IntegrationSuspended()) return;
double dT = FDMExec->GetDeltaT()*Propulsion->GetRate();
RunPreFunctions();