Synced with latest JSBSim as of June 5, 2001.

[flightgear.git] / src / FDM / JSBSim / FGPosition.cpp

diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp
index bc73e33f55d27230f09d285ed6cbb4f421872f5c..e8e09b867c0fc43e1a38954c5a76b00fd97672b3 100644 (file)
--- 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 <simgear/compiler.h>
-#  ifdef FG_HAVE_STD_INCLUDES
+#  ifdef SG_HAVE_STD_INCLUDES
 #    include <cmath>
 #    include <iomanip>
 #  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
+}
+