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 01a36c649eae0d284f6142d3fd70ee7e7ed237f8..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;
 
@@ -101,8 +107,17 @@ FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
 {
   Name = "FGPosition";
   LongitudeDot = LatitudeDot = RadiusDot = 0.0;
-  lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0;
+  
+  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;
@@ -143,10 +158,10 @@ Notes:   [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always
               In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 */
 
-bool FGPosition::Run(void) {
+bool FGPosition::Run(void)
+{
   double cosLat;
   double hdot_Vt;
-  FGColumnVector3 vMac;
 
   if (!FGModel::Run()) {
     GetState();
@@ -160,25 +175,37 @@ bool FGPosition::Run(void) {
 
     cosLat = cos(Latitude);
     if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
-
     LatitudeDot = vVel(eNorth) / Radius;
     RadiusDot   = -vVel(eDown);
 
-    Longitude += 0.5*dt*rate*(LongitudeDot + lastLongitudeDot);
-    Latitude  += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot);
-    Radius    += 0.5*dt*rate*(RadiusDot + lastRadiusDot);
+    Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);
+    Latitude  += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);
+    Radius    += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);
 
     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;
@@ -187,10 +214,6 @@ bool FGPosition::Run(void) {
       gamma = 0.0;
     }
 
-    lastLatitudeDot  = LatitudeDot;
-    lastLongitudeDot = LongitudeDot;
-    lastRadiusDot    = RadiusDot;
-
     return false;
 
   } else {
@@ -200,7 +223,8 @@ bool FGPosition::Run(void) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGPosition::GetState(void) {
+void FGPosition::GetState(void)
+{
   dt = State->Getdt();
 
   Vt        = Translation->GetVt();
@@ -212,7 +236,8 @@ void FGPosition::GetState(void) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGPosition::Seth(double tt) {
+void FGPosition::Seth(double tt)
+{
  h = tt;
  Radius    = h + SeaLevelRadius;
  DistanceAGL = Radius - RunwayRadius;   // Geocentric
@@ -221,7 +246,8 @@ void FGPosition::Seth(double tt) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGPosition::SetDistanceAGL(double tt) {
+void FGPosition::SetDistanceAGL(double tt)
+{
   DistanceAGL=tt;
   Radius = RunwayRadius + DistanceAGL;
   h = Radius - SeaLevelRadius;
@@ -344,4 +370,4 @@ void FGPosition::Debug(int from)
     }
   }
 }
-
+}