X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGPosition.cpp;h=e8e09b867c0fc43e1a38954c5a76b00fd97672b3;hb=9c0925744d27363955b3f26caf3311bc0a91ea69;hp=bc73e33f55d27230f09d285ed6cbb4f421872f5c;hpb=76b4de7f1fd97e042834ca711abdfabb154be9ea;p=flightgear.git diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index bc73e33f5..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,14 +48,14 @@ 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 @@ -73,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; @@ -89,7 +95,8 @@ extern double globalSeaLevelRadius; FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), vUVW(3), vVel(3), - vVelDot(3) + vVelDot(3), + vRunwayNormal(3) { Name = "FGPosition"; LongitudeDot = LatitudeDot = RadiusDot = 0.0; @@ -101,13 +108,20 @@ FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), 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; +} -/*************************************************************************** Run +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +/* 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 @@ -123,8 +137,11 @@ bool FGPosition:: Run(void) { GetState(); Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) ); - - invMass = 1.0 / Aircraft->GetMass(); + 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; @@ -162,28 +179,39 @@ bool FGPosition:: Run(void) { } } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPosition::GetState(void) { dt = State->Getdt(); vUVW = Translation->GetUVW(); Vt = Translation->GetVt(); - vVel = State->GetTb2l()*vUVW; + vVel = State->GetTb2l()*vUVW + Atmosphere->GetWindNED(); vVelDot = State->GetTb2l() * Translation->GetUVWdot(); b = Aircraft->GetWingSpan(); - } -void FGPosition::Seth(double tt) { - h=tt; +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::Seth(double tt) { + h = tt; Radius = h + SeaLevelRadius; - DistanceAGL = Radius - RunwayRadius; // Geocentric -} + 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 +} +