JSBsim/FGTranslation.cpp

   1 /*******************************************************************************
   2
   3  Module:       FGTranslation.cpp
   4  Author:       Jon Berndt
   5  Date started: 12/02/98
   6  Purpose:      Integrates the translational EOM
   7  Called by:    FDMExec
   8
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30 This class integrates the translational EOM.
  31
  32 ARGUMENTS
  33 --------------------------------------------------------------------------------
  34
  35
  36 HISTORY
  37 --------------------------------------------------------------------------------
  38 12/02/98   JSB   Created
  39
  40 ********************************************************************************
  41 COMMENTS, REFERENCES,  and NOTES
  42 ********************************************************************************
  43 [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
  44     Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
  45     School, January 1994
  46 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
  47     JSC 12960, July 1977
  48 [3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
  49     NASA-Ames", NASA CR-2497, January 1975
  50 [4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
  51     Wiley & Sons, 1979 ISBN 0-471-03032-5
  52 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
  53     1982 ISBN 0-471-08936-2
  54
  55   The order of rotations used in this class corresponds to a 3-2-1 sequence,
  56   or Y-P-R, or Z-Y-X, if you prefer.
  57
  58 ********************************************************************************
  59 INCLUDES
  60 *******************************************************************************/
  61
  62 #include "FGTranslation.h"
  63
  64 /*******************************************************************************
  65 ************************************ CODE **************************************
  66 *******************************************************************************/
  67
  68
  69 FGTranslation::FGTranslation(void) : FGModel()
  70 {
  71   strcpy(Name, "FGTranslation");
  72 }
  73
  74
  75 FGTranslation::~FGTranslation(void)
  76 {
  77 }
  78
  79
  80 bool FGTranslation::Run(void)
  81 {
  82   if (!FGModel::Run()) {
  83
  84     GetState();
  85
  86     lastUdot = Udot;
  87     lastVdot = Vdot;
  88     lastWdot = Wdot;
  89
  90     Udot = V*R - W*Q + Fx/m;
  91     Vdot = W*P - U*R + Fy/m;
  92     Wdot = U*Q - V*P + Fz/m;
  93
  94     U += 0.5*dt*(lastUdot + Udot);
  95     V += 0.5*dt*(lastVdot + Vdot);
  96     W += 0.5*dt*(lastWdot + Wdot);
  97
  98     Vt = U*U+V*V+W*W > 0.0 ? sqrt(U*U + V*V + W*W) : 0.0;
  99
 100     if (W != 0.0)
 101       alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 102     if (V != 0.0)
 103       beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 104
 105     qbar = 0.5*rho*Vt*Vt;
 106
 107     PutState();
 108   } else {
 109   }
 110   return false;
 111 }
 112
 113
 114 void FGTranslation::GetState(void)
 115 {
 116   dt = State->Getdt();
 117
 118   P = State->GetP();
 119   Q = State->GetQ();
 120   R = State->GetR();
 121
 122   Fx = State->GetFx();
 123   Fy = State->GetFy();
 124   Fz = State->GetFz();
 125
 126   m = State->Getm();
 127   g = State->Getg();
 128   rho = State->Getrho();
 129
 130   phi = State->Getphi();
 131   tht = State->Gettht();
 132   psi = State->Getpsi();
 133
 134   U = State->GetU();
 135   V = State->GetV();
 136   W = State->GetW();
 137
 138   Udot = State->GetUdot();
 139   Vdot = State->GetVdot();
 140   Wdot = State->GetWdot();
 141
 142   alpha = State->Getalpha();
 143   beta = State->Getbeta();
 144 }
 145
 146
 147 void FGTranslation::PutState(void)
 148 {
 149   State->SetU(U);
 150   State->SetV(V);
 151   State->SetW(W);
 152
 153   State->SetVt(Vt);
 154   State->Setqbar(qbar);
 155
 156   State->SetUdot(Udot);
 157   State->SetVdot(Vdot);
 158   State->SetWdot(Wdot);
 159
 160   State->Setalpha(alpha);
 161   State->Setbeta(beta);
 162 }
 163