Fixes to jsbsim.

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

diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp
index d2b60d93469652a9418aac29fb63bfc4f63cdef2..82eff48edefefcdf0d947ab78ad399fea34a3bd3 100644 (file)
--- a/src/FDM/JSBSim/FGTranslation.cpp
+++ b/src/FDM/JSBSim/FGTranslation.cpp
@@ -1,40 +1,40 @@
 /*******************************************************************************
-
+ 
  Module:       FGTranslation.cpp
  Author:       Jon Berndt
  Date started: 12/02/98
  Purpose:      Integrates the translational EOM
  Called by:    FDMExec
-
+ 
  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
-
+ 
  This program is free software; you can redistribute it and/or modify it under
  the terms of the GNU General Public License as published by the Free Software
  Foundation; either version 2 of the License, or (at your option) any later
  version.
-
+ 
  This program is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  details.
-
+ 
  You should have received a copy of the GNU General Public License along with
  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  Place - Suite 330, Boston, MA  02111-1307, USA.
-
+ 
  Further information about the GNU General Public License can also be found on
  the world wide web at http://www.gnu.org.
-
+ 
 FUNCTIONAL DESCRIPTION
 --------------------------------------------------------------------------------
 This class integrates the translational EOM.
-
+ 
 HISTORY
 --------------------------------------------------------------------------------
 12/02/98   JSB   Created
  7/23/99   TP    Added data member and modified Run and PutState to calcuate 
                               Mach number
-
+ 
 ********************************************************************************
 COMMENTS, REFERENCES,  and NOTES
 ********************************************************************************
@@ -49,10 +49,10 @@ 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
-
+ 
   The order of rotations used in this class corresponds to a 3-2-1 sequence,
   or Y-P-R, or Z-Y-X, if you prefer.
-
+ 
 ********************************************************************************
 INCLUDES
 *******************************************************************************/
@@ -74,31 +74,30 @@ INCLUDES
 
 
 FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex),
-                                                 vUVW(3),
-                                                 vPQR(3),
-                                                 vForces(3),
-                                                 vEuler(3) 
+    vUVW(3),
+    vWindUVW(3),
+    vUVWdot(3),
+    vNcg(3),
+    vPQR(3),
+    vForces(3),
+    vEuler(3)
 {
   Name = "FGTranslation";
   qbar = 0;
   Vt = 0.0;
   Mach = 0.0;
-  alpha = beta = gamma = 0.0;
+  alpha = beta = 0.0;
   rho = 0.002378;
 }
 
 /******************************************************************************/
 
-FGTranslation::~FGTranslation(void)
-{
-}
+FGTranslation::~FGTranslation(void) {}
 
 /******************************************************************************/
 
-bool FGTranslation::Run(void)
-{
+bool FGTranslation::Run(void) {
   static FGColumnVector vlastUVWdot(3);
-  static FGColumnVector vUVWdot(3);
   static FGMatrix       mVel(3,3);
 
   if (!FGModel::Run()) {
@@ -116,15 +115,18 @@ bool FGTranslation::Run(void)
     mVel(3,3) =  0.0;
 
     vUVWdot = mVel*vPQR + vForces/Mass;
+    
+    vNcg=vUVWdot*INVGRAVITY;
 
-    vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot);
-
+    vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot) + vWindUVW;
+    
     Vt = vUVW.Magnitude();
 
     if (vUVW(eW) != 0.0)
       alpha = vUVW(eU)*vUVW(eU) > 0.0 ? atan2(vUVW(eW), vUVW(eU)) : 0.0;
     if (vUVW(eV) != 0.0)
-      beta = vUVW(eU)*vUVW(eU)+vUVW(eW)*vUVW(eW) > 0.0 ? atan2(vUVW(eV), (fabs(vUVW(eU))/vUVW(eU))*sqrt(vUVW(eU)*vUVW(eU) + vUVW(eW)*vUVW(eW))) : 0.0;
+      beta = vUVW(eU)*vUVW(eU)+vUVW(eW)*vUVW(eW) > 0.0 ? atan2(vUVW(eV),
+             sqrt(vUVW(eU)*vUVW(eU) + vUVW(eW)*vUVW(eW))) : 0.0;
 
     qbar = 0.5*rho*Vt*Vt;
 
@@ -132,15 +134,14 @@ bool FGTranslation::Run(void)
 
     vlastUVWdot = vUVWdot;
 
-  } else {
-  }
+  } else {}
+
   return false;
 }
 
 /******************************************************************************/
 
-void FGTranslation::GetState(void)
-{
+void FGTranslation::GetState(void) {
   dt = State->Getdt();
 
   vPQR = Rotation->GetPQR();
@@ -150,5 +151,7 @@ void FGTranslation::GetState(void)
   rho = Atmosphere->GetDensity();
 
   vEuler = Rotation->GetEuler();
+
+  vWindUVW = Atmosphere->GetWindUVW();
 }