X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGPosition.cpp;h=e8e09b867c0fc43e1a38954c5a76b00fd97672b3;hb=9c0925744d27363955b3f26caf3311bc0a91ea69;hp=bdbd5668b9c1f2a40eede7feee9abb7f794d4a53;hpb=fb2a4f69094992af371e9cd7cd92746354b7409f;p=flightgear.git diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index bdbd5668b..e8e09b867 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -1,4 +1,4 @@ -/******************************************************************************* +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Module: FGPosition.cpp Author: Jon S. Berndt @@ -29,14 +29,14 @@ FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- This class encapsulates the integration of rates and accelerations to get the current position of the aircraft. - + HISTORY -------------------------------------------------------------------------------- 01/05/99 JSB Created - -******************************************************************************** + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMENTS, REFERENCES, and NOTES -******************************************************************************** +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate School, January 1994 @@ -48,20 +48,23 @@ COMMENTS, REFERENCES, and NOTES Wiley & Sons, 1979 ISBN 0-471-03032-5 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons, 1982 ISBN 0-471-08936-2 - -******************************************************************************** + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES -*******************************************************************************/ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #ifdef FGFS # include -# ifdef FG_HAVE_STD_INCLUDES +# ifdef SG_HAVE_STD_INCLUDES # include +# include # else # include +# include # endif #else # include +# include #endif #include "FGPosition.h" @@ -70,14 +73,20 @@ INCLUDES #include "FGFDMExec.h" #include "FGFCS.h" #include "FGAircraft.h" +#include "FGMassBalance.h" #include "FGTranslation.h" #include "FGRotation.h" #include "FGAuxiliary.h" #include "FGOutput.h" -/******************************************************************************* -************************************ CODE ************************************** -*******************************************************************************/ +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_POSITION; + +extern short debug_lvl; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ extern float globalTriNormal[3]; extern double globalSceneryAltitude; @@ -85,23 +94,40 @@ extern double globalSeaLevelRadius; FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), vUVW(3), - vVel(3) + vVel(3), + vVelDot(3), + vRunwayNormal(3) { Name = "FGPosition"; LongitudeDot = LatitudeDot = RadiusDot = 0.0; lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0; Longitude = Latitude = 0.0; - h = 0.0; - Radius = EARTHRAD + h; - gamma=Vt=0.0; - RunwayRadius = EARTHRAD; + gamma = Vt = Vground = 0.0; + h = 3.0; // Est. height of aircraft cg off runway + SeaLevelRadius = EARTHRAD; // For initialization ONLY + Radius = SeaLevelRadius + h; + RunwayRadius = SeaLevelRadius; + DistanceAGL = Radius - RunwayRadius; // Geocentric + vRunwayNormal(3) = -1.0; // Initialized for standalone mode + + if (debug_lvl & 2) cout << "Instantiated: " << Name << endl; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGPosition::~FGPosition(void) {} +FGPosition::~FGPosition() +{ + if (debug_lvl & 2) cout << "Destroyed: FGPosition" << endl; +} -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +/* +Purpose: Called on a schedule to perform Positioning algorithms +Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always + be the case + [JB] Run in standalone mode, SeaLevelRadius will be EARTHRAD. In FGFS, + SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass. +*/ bool FGPosition:: Run(void) { double cosLat; @@ -110,7 +136,14 @@ bool FGPosition:: Run(void) { if (!FGModel::Run()) { GetState(); - vVel = State->GetTl2b()*vUVW; + Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) ); + psigt = atan2(vVel(eEast), vVel(eNorth)); + if(psigt < 0.0) + psigt += 2*M_PI; + + invMass = 1.0 / MassBalance->GetMass(); + Radius = h + SeaLevelRadius; + invRadius = 1.0 / Radius; cosLat = cos(Latitude); if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat); @@ -122,22 +155,22 @@ bool FGPosition:: Run(void) { Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot); Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot); - h = Radius - EARTHRAD; // Geocentric + h = Radius - SeaLevelRadius; // Geocentric DistanceAGL = Radius - RunwayRadius; // Geocentric - hoverb = h/b; + hoverb = DistanceAGL/b; - if(Vt > 0) { - hdot_Vt=RadiusDot/Vt; - //make sure that -Vt <= hdot <= Vt, which, of course, should always be the case - if(fabs(hdot_Vt) <= 1) gamma= asin(hdot_Vt); - } else - gamma=0.0; + if (Vt > 0) { + hdot_Vt = RadiusDot/Vt; + if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt); + } else { + gamma = 0.0; + } - lastLatitudeDot = LatitudeDot; + lastLatitudeDot = LatitudeDot; lastLongitudeDot = LongitudeDot; - lastRadiusDot = RadiusDot; + lastRadiusDot = RadiusDot; return false; @@ -146,16 +179,39 @@ bool FGPosition:: Run(void) { } } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPosition::GetState(void) { dt = State->Getdt(); - vUVW = Translation->GetUVW(); - Vt = Translation->GetVt(); - invMass = 1.0 / Aircraft->GetMass(); - invRadius = 1.0 / (h + EARTHRAD); - Radius = h + EARTHRAD; + vUVW = Translation->GetUVW(); + Vt = Translation->GetVt(); + vVel = State->GetTb2l()*vUVW + Atmosphere->GetWindNED(); + vVelDot = State->GetTb2l() * Translation->GetUVWdot(); + b = Aircraft->GetWingSpan(); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::Seth(double tt) { + h = tt; + Radius = h + SeaLevelRadius; + DistanceAGL = Radius - RunwayRadius; // Geocentric +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::SetDistanceAGL(double tt) { + DistanceAGL=tt; + Radius = RunwayRadius + DistanceAGL; + h = Radius - SeaLevelRadius; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::Debug(void) +{ + //TODO: Add your source code here +} +