-/*******************************************************************************
-
- 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
-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
-[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
- JSC 12960, July 1977
-[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
- NASA-Ames", NASA CR-2497, January 1975
-[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
- 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 <simgear/compiler.h>
-# ifdef FG_HAVE_STD_INCLUDES
-# include <cmath>
-# else
-# include <math.h>
-# endif
-#else
-# include <cmath>
-#endif
-
-#include "FGPosition.h"
-#include "FGAtmosphere.h"
-#include "FGState.h"
-#include "FGFDMExec.h"
-#include "FGFCS.h"
-#include "FGAircraft.h"
-#include "FGTranslation.h"
-#include "FGRotation.h"
-#include "FGAuxiliary.h"
-#include "FGOutput.h"
-
-/*******************************************************************************
-************************************ CODE **************************************
-*******************************************************************************/
-
-
-FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex),
- vUVW(3),
- vVel(3)
-{
- Name = "FGPosition";
- LongitudeDot = LatitudeDot = RadiusDot = 0.0;
- lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0;
-}
-
-/******************************************************************************/
-
-FGPosition::~FGPosition(void)
-{
-}
-
-/******************************************************************************/
-
-bool FGPosition:: Run(void)
-{
- float cosLat;
-
- if (!FGModel::Run()) {
- GetState();
-
- vVel = State->GetTb2l()*vUVW;
-
- cosLat = cos(Latitude);
- if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
-
- 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);
-
- lastLatitudeDot = LatitudeDot;
- lastLongitudeDot = LongitudeDot;
- lastRadiusDot = RadiusDot;
-
- PutState();
- return false;
-
- } else {
- return true;
- }
-}
-
-/******************************************************************************/
-
-void FGPosition::GetState(void)
-{
- dt = State->Getdt();
-
- vUVW = Translation->GetUVW();
-
- Latitude = State->Getlatitude();
- Longitude = State->Getlongitude();
-
- invMass = 1.0 / Aircraft->GetMass();
- invRadius = 1.0 / (State->Geth() + EARTHRAD);
- Radius = State->Geth() + EARTHRAD;
-}
-
-/******************************************************************************/
-
-void FGPosition::PutState(void)
-{
- State->Setlatitude(Latitude);
- State->Setlongitude(Longitude);
- State->Seth(Radius - EARTHRAD);
-}
-
-/******************************************************************************/
-
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGPosition.cpp\r
+ Author: Jon S. Berndt\r
+ Date started: 01/05/99\r
+ Purpose: Integrate the EOM to determine instantaneous position\r
+ Called by: FGFDMExec\r
+\r
+ ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+This class encapsulates the integration of rates and accelerations to get the\r
+current position of the aircraft.\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+01/05/99 JSB Created\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+COMMENTS, REFERENCES, and NOTES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling\r
+ Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate\r
+ School, January 1994\r
+[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",\r
+ JSC 12960, July 1977\r
+[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at\r
+ NASA-Ames", NASA CR-2497, January 1975\r
+[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",\r
+ Wiley & Sons, 1979 ISBN 0-471-03032-5\r
+[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,\r
+ 1982 ISBN 0-471-08936-2\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#ifdef FGFS\r
+# include <simgear/compiler.h>\r
+# ifdef SG_HAVE_STD_INCLUDES\r
+# include <cmath>\r
+# include <iomanip>\r
+# else\r
+# include <math.h>\r
+# include <iomanip.h>\r
+# endif\r
+#else\r
+# if defined(sgi) && !defined(__GNUC__)\r
+# include <math.h>\r
+# if (_COMPILER_VERSION < 740)\r
+# include <iomanip.h>\r
+# else\r
+# include <iomanip>\r
+# endif\r
+# else\r
+# include <cmath>\r
+# include <iomanip>\r
+# endif\r
+#endif\r
+\r
+#include "FGPosition.h"\r
+#include "FGState.h"\r
+#include "FGFDMExec.h"\r
+#include "FGAircraft.h"\r
+#include "FGMassBalance.h"\r
+#include "FGTranslation.h"\r
+#include "FGRotation.h"\r
+#include "FGInertial.h"\r
+#include "FGPropertyManager.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_POSITION;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)\r
+{\r
+ Name = "FGPosition";\r
+ LongitudeDot = LatitudeDot = RadiusDot = 0.0;\r
+\r
+ for (int i=0;i<4;i++) {\r
+ LatitudeDot_prev[i] = 0.0;\r
+ LongitudeDot_prev[i] = 0.0;\r
+ RadiusDot_prev[i] = 0.0;\r
+ }\r
+\r
+ vVRPoffset.InitMatrix();\r
+\r
+ Longitude = Latitude = 0.0;\r
+ LongitudeVRP = LatitudeVRP = 0.0;\r
+ gamma = Vt = Vground = 0.0;\r
+ hoverbmac = hoverbcg = 0.0;\r
+ psigt = 0.0;\r
+ bind();\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGPosition::~FGPosition(void)\r
+{\r
+ unbind();\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+bool FGPosition::InitModel(void)\r
+{\r
+ FGModel::InitModel();\r
+\r
+ h = 3.0; // Est. height of aircraft cg off runway\r
+ SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY\r
+ Radius = SeaLevelRadius + h;\r
+ RunwayRadius = SeaLevelRadius;\r
+ DistanceAGL = Radius - RunwayRadius; // Geocentric\r
+ vRunwayNormal(3) = -1.0; // Initialized for standalone mode\r
+ b = 1;\r
+ return true;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+/*\r
+Purpose: Called on a schedule to perform Positioning algorithms\r
+Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always\r
+ be the case\r
+ [JB] Run in standalone mode, SeaLevelRadius will be reference radius.\r
+ In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.\r
+*/\r
+\r
+bool FGPosition::Run(void)\r
+{\r
+ double cosLat;\r
+ double hdot_Vt;\r
+\r
+ if (!FGModel::Run()) {\r
+ GetState();\r
+\r
+ Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );\r
+\r
+ if (vVel(eNorth) == 0) psigt = 0;\r
+ else psigt = atan2(vVel(eEast), vVel(eNorth));\r
+\r
+ if (psigt < 0.0) psigt += 2*M_PI;\r
+\r
+ Radius = h + SeaLevelRadius;\r
+\r
+ cosLat = cos(Latitude);\r
+ if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);\r
+ LatitudeDot = vVel(eNorth) / Radius;\r
+ RadiusDot = -vVel(eDown);\r
+\r
+ Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);\r
+ Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);\r
+ Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);\r
+\r
+ h = Radius - SeaLevelRadius; // Geocentric\r
+\r
+ vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp());\r
+\r
+ // vVRP - the vector to the Visual Reference Point - now contains the\r
+ // offset from the CG to the VRP, in units of feet, in the Local coordinate\r
+ // frame, where X points north, Y points East, and Z points down. This needs\r
+ // to be converted to Lat/Lon/Alt, now.\r
+\r
+ if (cosLat != 0)\r
+ LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude;\r
+\r
+ LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude;\r
+ hVRP = h - vVRPoffset(eDown);\r
+/*\r
+cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl;\r
+cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl;\r
+*/\r
+ DistanceAGL = Radius - RunwayRadius; // Geocentric\r
+\r
+ hoverbcg = DistanceAGL/b;\r
+\r
+ vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());\r
+ hoverbmac = (DistanceAGL + vMac(3)) / b;\r
+\r
+ if (Vt > 0) {\r
+ hdot_Vt = RadiusDot/Vt;\r
+ if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);\r
+ } else {\r
+ gamma = 0.0;\r
+ }\r
+\r
+ return false;\r
+\r
+ } else {\r
+ return true;\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::GetState(void)\r
+{\r
+ dt = State->Getdt();\r
+\r
+ Vt = Translation->GetVt();\r
+ vVel = State->GetTb2l() * Translation->GetUVW();\r
+ vVelDot = State->GetTb2l() * Translation->GetUVWdot();\r
+\r
+ b = Aircraft->GetWingSpan();\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::Seth(double tt)\r
+{\r
+ h = tt;\r
+ Radius = h + SeaLevelRadius;\r
+ DistanceAGL = Radius - RunwayRadius; // Geocentric\r
+ hoverbcg = DistanceAGL/b;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::SetDistanceAGL(double tt)\r
+{\r
+ DistanceAGL=tt;\r
+ Radius = RunwayRadius + DistanceAGL;\r
+ h = Radius - SeaLevelRadius;\r
+ hoverbcg = DistanceAGL/b;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::bind(void)\r
+{\r
+ PropertyManager->Tie("velocities/v-north-fps", this,\r
+ &FGPosition::GetVn);\r
+ PropertyManager->Tie("velocities/v-east-fps", this,\r
+ &FGPosition::GetVe);\r
+ PropertyManager->Tie("velocities/v-down-fps", this,\r
+ &FGPosition::GetVd);\r
+ PropertyManager->Tie("velocities/vg-fps", this,\r
+ &FGPosition::GetVground);\r
+ PropertyManager->Tie("flight-path/psi-gt-rad", this,\r
+ &FGPosition::GetGroundTrack);\r
+ PropertyManager->Tie("position/h-sl-ft", this,\r
+ &FGPosition::Geth,\r
+ &FGPosition::Seth,\r
+ true);\r
+ PropertyManager->Tie("velocities/h-dot-fps", this,\r
+ &FGPosition::Gethdot);\r
+ PropertyManager->Tie("position/lat-gc-rad", this,\r
+ &FGPosition::GetLatitude,\r
+ &FGPosition::SetLatitude);\r
+ PropertyManager->Tie("position/lat-dot-gc-rad", this,\r
+ &FGPosition::GetLatitudeDot);\r
+ PropertyManager->Tie("position/long-gc-rad", this,\r
+ &FGPosition::GetLongitude,\r
+ &FGPosition::SetLongitude,\r
+ true);\r
+ PropertyManager->Tie("position/long-dot-gc-rad", this,\r
+ &FGPosition::GetLongitudeDot);\r
+ PropertyManager->Tie("metrics/runway-radius", this,\r
+ &FGPosition::GetRunwayRadius,\r
+ &FGPosition::SetRunwayRadius);\r
+ PropertyManager->Tie("position/h-agl-ft", this,\r
+ &FGPosition::GetDistanceAGL,\r
+ &FGPosition::SetDistanceAGL);\r
+ PropertyManager->Tie("position/radius-to-vehicle-ft", this,\r
+ &FGPosition::GetRadius);\r
+ PropertyManager->Tie("flight-path/gamma-rad", this,\r
+ &FGPosition::GetGamma,\r
+ &FGPosition::SetGamma);\r
+ PropertyManager->Tie("aero/h_b-cg-ft", this,\r
+ &FGPosition::GetHOverBCG);\r
+ PropertyManager->Tie("aero/h_b-mac-ft", this,\r
+ &FGPosition::GetHOverBMAC);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::unbind(void)\r
+{\r
+ PropertyManager->Untie("velocities/v-north-fps");\r
+ PropertyManager->Untie("velocities/v-east-fps");\r
+ PropertyManager->Untie("velocities/v-down-fps");\r
+ PropertyManager->Untie("velocities/vg-fps");\r
+ PropertyManager->Untie("flight-path/psi-gt-rad");\r
+ PropertyManager->Untie("position/h-sl-ft");\r
+ PropertyManager->Untie("velocities/h-dot-fps");\r
+ PropertyManager->Untie("position/lat-gc-rad");\r
+ PropertyManager->Untie("position/lat-dot-gc-rad");\r
+ PropertyManager->Untie("position/long-gc-rad");\r
+ PropertyManager->Untie("position/long-dot-gc-rad");\r
+ PropertyManager->Untie("metrics/runway-radius");\r
+ PropertyManager->Untie("position/h-agl-ft");\r
+ PropertyManager->Untie("position/radius-to-vehicle-ft");\r
+ PropertyManager->Untie("flight-path/gamma-rad");\r
+ PropertyManager->Untie("aero/h_b-cg-ft");\r
+ PropertyManager->Untie("aero/h_b-mac-ft");\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGPosition::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGPosition" << endl;\r
+ if (from == 1) cout << "Destroyed: FGPosition" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+}\r
projects / flightgear.git / blobdiff