X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGPosition.cpp;h=e8e09b867c0fc43e1a38954c5a76b00fd97672b3;hb=9c0925744d27363955b3f26caf3311bc0a91ea69;hp=3489bc0f1a19f962b8d3e7a697ad61935039cc53;hpb=c19302134669adefac1303a441a21c8835fc7fa1;p=flightgear.git diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index 3489bc0f1..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 +# include +# ifdef SG_HAVE_STD_INCLUDES # include +# include # else # include +# include # endif #else # include +# include #endif #include "FGPosition.h" @@ -70,120 +73,145 @@ 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; -FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex) +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +extern float globalTriNormal[3]; +extern double globalSceneryAltitude; +extern double globalSeaLevelRadius; + +FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), + vUVW(3), + vVel(3), + vVelDot(3), + vRunwayNormal(3) { Name = "FGPosition"; - AccelN = AccelE = AccelD = 0.0; LongitudeDot = LatitudeDot = RadiusDot = 0.0; + lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0; + Longitude = Latitude = 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 tanLat, cosLat; +bool FGPosition:: Run(void) { + double cosLat; + double hdot_Vt; if (!FGModel::Run()) { GetState(); - T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3; // Page A-11 - T[1][2] = 2*(Q1*Q2 + Q0*Q3); // From - T[1][3] = 2*(Q1*Q3 - Q0*Q2); // Reference [2] - T[2][1] = 2*(Q1*Q2 - Q0*Q3); - T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3; - T[2][3] = 2*(Q2*Q3 + Q0*Q1); - T[3][1] = 2*(Q1*Q3 + Q0*Q2); - T[3][2] = 2*(Q2*Q3 - Q0*Q1); - T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3; - - Fn = T[1][1]*Fx + T[2][1]*Fy + T[3][1]*Fz; // Eqn. 3.5 - Fe = T[1][2]*Fx + T[2][2]*Fy + T[3][2]*Fz; // From - Fd = T[1][3]*Fx + T[2][3]*Fy + T[3][3]*Fz; // Reference [3] - - tanLat = tan(Latitude); // I made this up - cosLat = cos(Latitude); - - lastAccelN = AccelN; - lastAccelE = AccelE; - lastAccelD = AccelD; - - Vn = T[1][1]*U + T[2][1]*V + T[3][1]*W; - Ve = T[1][2]*U + T[2][2]*V + T[3][2]*W; - Vd = T[1][3]*U + T[2][3]*V + T[3][3]*W; -/* AccelN = invMass * Fn + invRadius * (Vn*Vd - Ve*Ve*tanLat); // Eqn. 3.6 - AccelE = invMass * Fe + invRadius * (Ve*Vd + Vn*Ve*tanLat); // From - AccelD = invMass * Fd - invRadius * (Vn*Vn + Ve*Ve); // Reference [3] + Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) ); + psigt = atan2(vVel(eEast), vVel(eNorth)); + if(psigt < 0.0) + psigt += 2*M_PI; - Vn += 0.5*dt*rate*(3.0*AccelN - lastAccelN); // Eqn. 3.7 - Ve += 0.5*dt*rate*(3.0*AccelE - lastAccelE); // From - Vd += 0.5*dt*rate*(3.0*AccelD - lastAccelD); // Reference [3] + invMass = 1.0 / MassBalance->GetMass(); + Radius = h + SeaLevelRadius; + invRadius = 1.0 / Radius; - Vee = Ve - OMEGAEARTH * (Radius) * cosLat; // From Eq. 3.8 -*/ // Reference [3] - lastLatitudeDot = LatitudeDot; - lastLongitudeDot = LongitudeDot; - lastRadiusDot = RadiusDot; + cosLat = cos(Latitude); + if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat); - if (cosLat != 0) LongitudeDot = Ve / (Radius * cosLat); - LatitudeDot = Vn * invRadius; - RadiusDot = -Vd; + LatitudeDot = vVel(eNorth) * invRadius; + RadiusDot = -vVel(eDown); Longitude += 0.5*dt*rate*(LongitudeDot + lastLongitudeDot); Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot); Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot); - PutState(); + 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; + return false; + } else { return true; } } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPosition::GetState(void) -{ +void FGPosition::GetState(void) { dt = State->Getdt(); - Q0 = Rotation->GetQ0(); - Q1 = Rotation->GetQ1(); - Q2 = Rotation->GetQ2(); - Q3 = Rotation->GetQ3(); + vUVW = Translation->GetUVW(); + Vt = Translation->GetVt(); + vVel = State->GetTb2l()*vUVW + Atmosphere->GetWindNED(); + vVelDot = State->GetTb2l() * Translation->GetUVWdot(); - Fx = Aircraft->GetFx(); - Fy = Aircraft->GetFy(); - Fz = Aircraft->GetFz(); + b = Aircraft->GetWingSpan(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - U = Translation->GetU(); - V = Translation->GetV(); - W = Translation->GetW(); +void FGPosition::Seth(double tt) { + h = tt; + Radius = h + SeaLevelRadius; + DistanceAGL = Radius - RunwayRadius; // Geocentric +} - Latitude = State->Getlatitude(); - Longitude = State->Getlongitude(); +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - invMass = 1.0 / Aircraft->GetMass(); - invRadius = 1.0 / (State->Geth() + EARTHRAD); - Radius = State->Geth() + EARTHRAD; +void FGPosition::SetDistanceAGL(double tt) { + DistanceAGL=tt; + Radius = RunwayRadius + DistanceAGL; + h = Radius - SeaLevelRadius; } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPosition::PutState(void) +void FGPosition::Debug(void) { - State->Setlatitude(Latitude); - State->Setlongitude(Longitude); - State->Seth(Radius - EARTHRAD); + //TODO: Add your source code here }