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 bdbd5668b9c1f2a40eede7feee9abb7f794d4a53..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,20 +48,23 @@ 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
 #    include <math.h>
+#    include <iomanip.h>
 #  endif
 #else
 #  include <cmath>
+#  include <iomanip>
 #endif
 
 #include "FGPosition.h"
@@ -70,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;
@@ -85,23 +94,40 @@ extern double globalSeaLevelRadius;
 
 FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex),
     vUVW(3),
-    vVel(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;
-  Radius = EARTHRAD + h;
-  gamma=Vt=0.0;
-  RunwayRadius = EARTHRAD;
+  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) {
   double cosLat;
@@ -110,7 +136,14 @@ bool FGPosition:: Run(void) {
   if (!FGModel::Run()) {
     GetState();
 
-    vVel = State->GetTl2b()*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);
@@ -122,22 +155,22 @@ bool FGPosition:: Run(void) {
     Latitude  += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot);
     Radius    += 0.5*dt*rate*(RadiusDot + lastRadiusDot);
 
-    h = Radius - EARTHRAD;                 // Geocentric
+    h = Radius - SeaLevelRadius;           // Geocentric
 
     DistanceAGL = Radius - RunwayRadius;   // Geocentric
 
-    hoverb = h/b;
+    hoverb = DistanceAGL/b;
 
-    if(Vt > 0) {
-      hdot_Vt=RadiusDot/Vt;
-      //make sure that -Vt <= hdot <= Vt, which, of course, should always be the case
-      if(fabs(hdot_Vt) <= 1) gamma= asin(hdot_Vt);
-    } else
-      gamma=0.0;
+    if (Vt > 0) {
+      hdot_Vt = RadiusDot/Vt;
+      if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);
+    } else {
+      gamma = 0.0;
+    }
 
-    lastLatitudeDot = LatitudeDot;
+    lastLatitudeDot  = LatitudeDot;
     lastLongitudeDot = LongitudeDot;
-    lastRadiusDot = RadiusDot;
+    lastRadiusDot    = RadiusDot;
 
     return false;
 
@@ -146,16 +179,39 @@ bool FGPosition:: Run(void) {
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGPosition::GetState(void) {
   dt = State->Getdt();
 
-  vUVW = Translation->GetUVW();
-  Vt = Translation->GetVt();
-  invMass = 1.0 / Aircraft->GetMass();
-  invRadius = 1.0 / (h + EARTHRAD);
-  Radius = h + EARTHRAD;
+  vUVW      = Translation->GetUVW();
+  Vt        = Translation->GetVt();
+  vVel      = State->GetTb2l()*vUVW + Atmosphere->GetWindNED();
+  vVelDot   = State->GetTb2l() * Translation->GetUVWdot();
+
   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
+}
+