X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGPosition.cpp;h=e8e09b867c0fc43e1a38954c5a76b00fd97672b3;hb=9c0925744d27363955b3f26caf3311bc0a91ea69;hp=5849c934436744ac8115dd551dbd94e1f74a1899;hpb=1868e44ee44977069bd296afa30674318c7af161;p=flightgear.git diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index 5849c9344..e8e09b867 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -1,30 +1,30 @@ -/******************************************************************************* - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + Module: FGPosition.cpp Author: Jon S. Berndt Date started: 01/05/99 Purpose: Integrate the EOM to determine instantaneous position Called by: FGFDMExec - + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - + This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. - + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - + Further information about the GNU General Public License can also be found on the world wide web at http://www.gnu.org. - + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- This class encapsulates the integration of rates and accelerations to get the @@ -34,9 +34,9 @@ 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 @@ -49,19 +49,22 @@ COMMENTS, REFERENCES, and NOTES [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,43 +73,77 @@ 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; +extern double globalSeaLevelRadius; FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), - vUVW(3), - vVel(3) + vUVW(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; + 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) -{ - float cosLat; +bool FGPosition:: Run(void) { + double cosLat; + double hdot_Vt; if (!FGModel::Run()) { GetState(); - vVel = State->GetTb2l()*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); @@ -118,11 +155,22 @@ bool FGPosition:: Run(void) Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot); Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot); - h = Radius - EARTHRAD; - - lastLatitudeDot = LatitudeDot; + h = Radius - SeaLevelRadius; // Geocentric + + DistanceAGL = Radius - RunwayRadius; // Geocentric + + hoverb = DistanceAGL/b; + + if (Vt > 0) { + hdot_Vt = RadiusDot/Vt; + if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt); + } else { + gamma = 0.0; + } + + lastLatitudeDot = LatitudeDot; lastLongitudeDot = LongitudeDot; - lastRadiusDot = RadiusDot; + lastRadiusDot = RadiusDot; return false; @@ -131,16 +179,39 @@ bool FGPosition:: Run(void) } } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPosition::GetState(void) -{ +void FGPosition::GetState(void) { dt = State->Getdt(); - vUVW = Translation->GetUVW(); + vUVW = Translation->GetUVW(); + Vt = Translation->GetVt(); + vVel = State->GetTb2l()*vUVW + Atmosphere->GetWindNED(); + vVelDot = State->GetTb2l() * Translation->GetUVWdot(); - invMass = 1.0 / Aircraft->GetMass(); - invRadius = 1.0 / (h + EARTHRAD); - Radius = h + EARTHRAD; + 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 }