Encapsulate the interpolstion version of FGEnvironment and fix some bugs

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

diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp
index 268af3aced03bec71d5ee2c3b3504ec3fef60e2c..4042a02d0c60f7939d45867b924cd75a5a9369a1 100644 (file)
--- a/src/FDM/JSBSim/FGTranslation.cpp
+++ b/src/FDM/JSBSim/FGTranslation.cpp
@@ -70,6 +70,8 @@ INCLUDES
 #include "FGOutput.h"
 #include "FGPropertyManager.h"
 
+namespace JSBSim {
+
 static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_TRANSLATION;
 
@@ -82,10 +84,19 @@ FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex)
 {
   Name = "FGTranslation";
   qbar = 0;
+  qbarUW = 0.0;
+  qbarUV = 0.0;
   Vt = 0.0;
   Mach = 0.0;
   alpha = beta = 0.0;
   adot = bdot = 0.0;
+
+  vUVWdot.InitMatrix();
+  vUVWdot_prev[0].InitMatrix();
+  vUVWdot_prev[1].InitMatrix();
+  vUVWdot_prev[2].InitMatrix();
+  vUVWdot_prev[3].InitMatrix();
+
   bind();
   Debug(0);
 }
@@ -102,8 +113,6 @@ FGTranslation::~FGTranslation(void)
 
 bool FGTranslation::Run(void)
 {
-  double Tc = 0.5*State->Getdt()*rate;
-
   if (!FGModel::Run()) {
 
     mVel(1,1) =  0.0;
@@ -118,7 +127,7 @@ bool FGTranslation::Run(void)
 
     vUVWdot = mVel*Rotation->GetPQR() + Aircraft->GetBodyAccel();
 
-    vUVW += Tc*(vUVWdot + vlastUVWdot);
+    vUVW += State->Integrate(FGState::TRAPZ, State->Getdt()*rate, vUVWdot, vUVWdot_prev);
 
     vAeroUVW = vUVW + State->GetTl2b()*Atmosphere->GetWindNED();
 
@@ -149,9 +158,12 @@ bool FGTranslation::Run(void)
     }
 
     qbar = 0.5*Atmosphere->GetDensity()*Vt*Vt;
+    qbarUW = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
+    qbarUV = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eV)*vAeroUVW(eV));
     Mach = Vt / State->Geta();
-
-    vlastUVWdot = vUVWdot;
+    vMachUVW(eU) = vAeroUVW(eU) / State->Geta();
+    vMachUVW(eV) = vAeroUVW(eV) / State->Geta();
+    vMachUVW(eW) = vAeroUVW(eW) / State->Geta();
 
     if (debug_lvl > 1) Debug(1);
 
@@ -204,6 +216,14 @@ void FGTranslation::bind(void)
                        &FGTranslation::Getqbar,
                        &FGTranslation::Setqbar,
                        true);
+  PropertyManager->Tie("aero/qbarUW-psf", this,
+                       &FGTranslation::GetqbarUW,
+                       &FGTranslation::SetqbarUW,
+                       true);
+  PropertyManager->Tie("aero/qbarUV-psf", this,
+                       &FGTranslation::GetqbarUV,
+                       &FGTranslation::SetqbarUV,
+                       true);
   PropertyManager->Tie("velocities/vt-fps", this,
                        &FGTranslation::GetVt,
                        &FGTranslation::SetVt,
@@ -238,10 +258,13 @@ void FGTranslation::unbind(void)
   PropertyManager->Untie("aero/alpha-rad");
   PropertyManager->Untie("aero/beta-rad");
   PropertyManager->Untie("aero/qbar-psf");
+  PropertyManager->Untie("aero/qbarUW-psf");
+  PropertyManager->Untie("aero/qbarUV-psf");
   PropertyManager->Untie("velocities/vt-fps");
   PropertyManager->Untie("velocities/mach-norm");
   PropertyManager->Untie("aero/alphadot-rad_sec");
   PropertyManager->Untie("aero/betadot-rad_sec");
+  PropertyManager->Untie("aero/mag-beta-rad");
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -305,4 +328,4 @@ void FGTranslation::Debug(int from)
     }
   }
 }
-
+}