Encapsulate the interpolstion version of FGEnvironment and fix some bugs

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

diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp
index 74cd727bb56134bb4e3c25a20be000043b838d74..932c511b2ce6387b48e0d417bbb4cf830e3874e7 100644 (file)
--- a/src/FDM/JSBSim/FGPosition.cpp
+++ b/src/FDM/JSBSim/FGPosition.cpp
@@ -65,7 +65,11 @@ INCLUDES
 #else
 #  if defined(sgi) && !defined(__GNUC__)
 #    include <math.h>
-#    include <iomanip.h>
+#    if (_COMPILER_VERSION < 740)
+#      include <iomanip.h>
+#    else
+#      include <iomanip>
+#    endif
 #  else
 #    include <cmath>
 #    include <iomanip>
@@ -86,6 +90,8 @@ INCLUDES
 #include "FGPropertyManager.h"
 
 
+namespace JSBSim {
+
 static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_POSITION;
 
@@ -102,13 +108,16 @@ FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
   Name = "FGPosition";
   LongitudeDot = LatitudeDot = RadiusDot = 0.0;
   
-  for (int i=0;i<3;i++) {
+  for (int i=0;i<4;i++) {
     LatitudeDot_prev[i]  = 0.0;
     LongitudeDot_prev[i] = 0.0;
     RadiusDot_prev[i]    = 0.0;
   }
-  
+
+  vVRPoffset.InitMatrix();
+
   Longitude = Latitude = 0.0;
+  LongitudeVRP = LatitudeVRP = 0.0;
   gamma = Vt = Vground = 0.0;
   hoverbmac = hoverbcg = 0.0;
   psigt = 0.0;
@@ -153,7 +162,6 @@ bool FGPosition::Run(void)
 {
   double cosLat;
   double hdot_Vt;
-  FGColumnVector3 vMac;
 
   if (!FGModel::Run()) {
     GetState();
@@ -176,14 +184,28 @@ bool FGPosition::Run(void)
 
     h = Radius - SeaLevelRadius;           // Geocentric
 
+    vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp());
+
+    // vVRP  - the vector to the Visual Reference Point - now contains the 
+    // offset from the CG to the VRP, in units of feet, in the Local coordinate
+    // frame, where X points north, Y points East, and Z points down. This needs
+    // to be converted to Lat/Lon/Alt, now.
+
+    if (cosLat != 0)
+      LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude;
+
+    LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude;
+    hVRP = h - vVRPoffset(eDown);
+/*
+cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl;
+cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl;
+*/
     DistanceAGL = Radius - RunwayRadius;   // Geocentric
-    
+
     hoverbcg = DistanceAGL/b;
-    
-    vMac = State->GetTb2l()*Aircraft->GetXYZrp();
-    
-    vMac *= inchtoft;
-    hoverbmac = (DistanceAGL + vMac(3))/b;
+
+    vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());
+    hoverbmac = (DistanceAGL + vMac(3)) / b;
 
     if (Vt > 0) {
       hdot_Vt = RadiusDot/Vt;
@@ -348,4 +370,4 @@ void FGPosition::Debug(int from)
     }
   }
 }
-
+}