src/FDM/JSBSim/FGPropagate.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGPropagate.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 01/05/99
   6  Purpose:      Integrate the EOM to determine instantaneous position
   7  Called by:    FGFDMExec
   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 encapsulates the integration of rates and accelerations to get the
  31 current position of the aircraft.
  32
  33 HISTORY
  34 --------------------------------------------------------------------------------
  35 01/05/99   JSB   Created
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 COMMENTS, REFERENCES,  and NOTES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40 [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
  41     Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
  42     School, January 1994
  43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
  44     JSC 12960, July 1977
  45 [3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
  46     NASA-Ames", NASA CR-2497, January 1975
  47 [4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
  48     Wiley & Sons, 1979 ISBN 0-471-03032-5
  49 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
  50     1982 ISBN 0-471-08936-2
  51
  52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53 INCLUDES
  54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  55
  56 #ifdef FGFS
  57 #  include <simgear/compiler.h>
  58 #  ifdef SG_HAVE_STD_INCLUDES
  59 #    include <cmath>
  60 #    include <iomanip>
  61 #  else
  62 #    include <math.h>
  63 #    include <iomanip.h>
  64 #  endif
  65 #else
  66 #  if defined(sgi) && !defined(__GNUC__)
  67 #    include <math.h>
  68 #    if (_COMPILER_VERSION < 740)
  69 #      include <iomanip.h>
  70 #    else
  71 #      include <iomanip>
  72 #    endif
  73 #  else
  74 #    include <cmath>
  75 #    include <iomanip>
  76 #  endif
  77 #endif
  78
  79 #include "FGPropagate.h"
  80 #include "FGState.h"
  81 #include "FGFDMExec.h"
  82 #include "FGAircraft.h"
  83 #include "FGMassBalance.h"
  84 #include "FGInertial.h"
  85 #include "FGPropertyManager.h"
  86
  87 namespace JSBSim {
  88
  89 static const char *IdSrc = "$Id$";
  90 static const char *IdHdr = ID_PROPAGATE;
  91
  92 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  93 CLASS IMPLEMENTATION
  94 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  95
  96 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
  97 {
  98   Name = "FGPropagate";
  99
 100   bind();
 101   Debug(0);
 102 }
 103
 104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 105
 106 FGPropagate::~FGPropagate(void)
 107 {
 108   unbind();
 109   Debug(1);
 110 }
 111
 112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 113
 114 bool FGPropagate::InitModel(void)
 115 {
 116   FGModel::InitModel();
 117
 118   SeaLevelRadius = Inertial->RefRadius();          // For initialization ONLY
 119   RunwayRadius   = SeaLevelRadius;
 120
 121   VState.vLocation.SetRadius( SeaLevelRadius + 4.0 );
 122
 123   return true;
 124 }
 125
 126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 127
 128 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 129 {
 130   SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
 131   RunwayRadius = FGIC->GetSeaLevelRadiusFtIC() + FGIC->GetTerrainAltitudeFtIC();
 132
 133   // Set the position lat/lon/radius
 134   VState.vLocation = FGLocation( FGIC->GetLongitudeRadIC(),
 135                           FGIC->GetLatitudeRadIC(),
 136                           FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 137
 138   // Set the Orientation from the euler angles
 139   VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
 140                         FGIC->GetThetaRadIC(),
 141                         FGIC->GetPsiRadIC() );
 142
 143   // Set the velocities in the instantaneus body frame
 144   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
 145                           FGIC->GetVBodyFpsIC(),
 146                           FGIC->GetWBodyFpsIC() );
 147
 148   // Set the angular velocities in the instantaneus body frame.
 149   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
 150                           FGIC->GetQRadpsIC(),
 151                           FGIC->GetRRadpsIC() );
 152
 153   // Compute some derived values.
 154   vVel = VState.vQtrn.GetTInv()*VState.vUVW;
 155 }
 156
 157 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 158 /*
 159 Purpose: Called on a schedule to perform EOM integration
 160 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 161          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 162
 163 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 164 set up for use later, rather than using the longer class->function() notation.
 165
 166 Here, propagation of state is done using a simple explicit Euler scheme (see the
 167 bottom of the function). This propagation is done using the current state values
 168 and current derivatives. Based on these values we compute an approximation to the
 169 state values for (now + dt).
 170
 171 */
 172
 173 bool FGPropagate::Run(void)
 174 {
 175   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 176
 177   double dt = State->Getdt()*rate;  // The 'stepsize'
 178   const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
 179   const FGColumnVector3& vForces = Aircraft->GetForces();     // current forces
 180   const FGColumnVector3& vMoments = Aircraft->GetMoments();   // current moments
 181
 182   double mass = MassBalance->GetMass();             // mass
 183   const FGMatrix33& J = MassBalance->GetJ();        // inertia matrix
 184   const FGMatrix33& Jinv = MassBalance->GetJinv();  // inertia matrix inverse
 185   double r = GetRadius();                           // radius
 186   if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
 187   double rInv = 1.0/r;
 188   FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
 189
 190   // The rotation matrices:
 191   const FGMatrix33& Tl2b = GetTl2b();  // local to body frame
 192   const FGMatrix33& Tb2l = GetTb2l();  // body to local frame
 193   const FGMatrix33& Tec2l = VState.vLocation.GetTec2l();  // earth centered to local frame
 194   const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec();  // local to earth centered frame
 195
 196   // Inertial angular velocity measured in the body frame.
 197   const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
 198
 199   // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
 200   vVel = Tb2l * VState.vUVW;
 201
 202   // First compute the time derivatives of the vehicle state values:
 203
 204   // Compute body frame rotational accelerations based on the current body moments
 205   vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
 206
 207   // Compute body frame accelerations based on the current body forces
 208   vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
 209
 210   // Centrifugal acceleration.
 211   FGColumnVector3 ecVel = Tl2ec*vVel;
 212   FGColumnVector3 ace = 2.0*omega*ecVel;
 213   vUVWdot -= Tl2b*(Tec2l*ace);
 214
 215   // Coriolis acceleration.
 216   FGColumnVector3 aeec = omega*(omega*VState.vLocation);
 217   vUVWdot -= Tl2b*(Tec2l*aeec);
 218
 219   // Gravitation accel
 220   vUVWdot += Tl2b*gAccel;
 221
 222   // Compute vehicle velocity wrt EC frame, expressed in EC frame
 223   FGColumnVector3 vLocationDot = Tl2ec * vVel;
 224
 225   FGColumnVector3 omegaLocal( rInv*vVel(eEast),
 226                               -rInv*vVel(eNorth),
 227                               -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 228
 229   // Compute quaternion orientation derivative on current body rates
 230   FGQuaternion vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
 231
 232   // Propagate velocities
 233   VState.vPQR += dt*vPQRdot;
 234   VState.vUVW += dt*vUVWdot;
 235
 236   // Propagate positions
 237   VState.vQtrn += dt*vQtrndot;
 238   VState.vLocation += dt*vLocationDot;
 239
 240   return false;
 241 }
 242
 243 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 244
 245 void FGPropagate::Seth(double tt)
 246 {
 247   VState.vLocation.SetRadius( tt + SeaLevelRadius );
 248 }
 249
 250 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 251
 252 void FGPropagate::SetDistanceAGL(double tt)
 253 {
 254   VState.vLocation.SetRadius( tt + RunwayRadius );
 255 }
 256
 257 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 258
 259 void FGPropagate::bind(void)
 260 {
 261   typedef double (FGPropagate::*PMF)(int) const;
 262   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 263
 264   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 265   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 266   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 267
 268   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 269   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 270   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 271
 272   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 273   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 274   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 275
 276   PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
 277   PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 278   PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
 279
 280   PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
 281   PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
 282   PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
 283
 284   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
 285   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 286   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 287   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 288   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 289
 290   PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius, &FGPropagate::SetRunwayRadius);
 291
 292   PropertyManager->Tie("attitude/phi-rad", this, &FGPropagate::Getphi);
 293   PropertyManager->Tie("attitude/theta-rad", this, &FGPropagate::Gettht);
 294   PropertyManager->Tie("attitude/psi-rad", this, &FGPropagate::Getpsi);
 295
 296   PropertyManager->Tie("attitude/roll-rad", this, &FGPropagate::Getphi);
 297   PropertyManager->Tie("attitude/pitch-rad", this, &FGPropagate::Gettht);
 298   PropertyManager->Tie("attitude/heading-true-rad", this, &FGPropagate::Getpsi);
 299 }
 300
 301 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 302
 303 void FGPropagate::unbind(void)
 304 {
 305   PropertyManager->Untie("velocities/v-north-fps");
 306   PropertyManager->Untie("velocities/v-east-fps");
 307   PropertyManager->Untie("velocities/v-down-fps");
 308   PropertyManager->Untie("velocities/h-dot-fps");
 309   PropertyManager->Untie("velocities/u-fps");
 310   PropertyManager->Untie("velocities/v-fps");
 311   PropertyManager->Untie("velocities/w-fps");
 312   PropertyManager->Untie("velocities/p-rad_sec");
 313   PropertyManager->Untie("velocities/q-rad_sec");
 314   PropertyManager->Untie("velocities/r-rad_sec");
 315   PropertyManager->Untie("accelerations/udot-fps");
 316   PropertyManager->Untie("accelerations/vdot-fps");
 317   PropertyManager->Untie("accelerations/wdot-fps");
 318   PropertyManager->Untie("accelerations/pdot-rad_sec");
 319   PropertyManager->Untie("accelerations/qdot-rad_sec");
 320   PropertyManager->Untie("accelerations/rdot-rad_sec");
 321   PropertyManager->Untie("position/h-sl-ft");
 322   PropertyManager->Untie("position/lat-gc-rad");
 323   PropertyManager->Untie("position/long-gc-rad");
 324   PropertyManager->Untie("position/h-agl-ft");
 325   PropertyManager->Untie("position/radius-to-vehicle-ft");
 326   PropertyManager->Untie("metrics/runway-radius");
 327   PropertyManager->Untie("attitude/phi-rad");
 328   PropertyManager->Untie("attitude/theta-rad");
 329   PropertyManager->Untie("attitude/psi-rad");
 330   PropertyManager->Untie("attitude/roll-rad");
 331   PropertyManager->Untie("attitude/pitch-rad");
 332   PropertyManager->Untie("attitude/heading-true-rad");
 333 }
 334
 335 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 336 //    The bitmasked value choices are as follows:
 337 //    unset: In this case (the default) JSBSim would only print
 338 //       out the normally expected messages, essentially echoing
 339 //       the config files as they are read. If the environment
 340 //       variable is not set, debug_lvl is set to 1 internally
 341 //    0: This requests JSBSim not to output any messages
 342 //       whatsoever.
 343 //    1: This value explicity requests the normal JSBSim
 344 //       startup messages
 345 //    2: This value asks for a message to be printed out when
 346 //       a class is instantiated
 347 //    4: When this value is set, a message is displayed when a
 348 //       FGModel object executes its Run() method
 349 //    8: When this value is set, various runtime state variables
 350 //       are printed out periodically
 351 //    16: When set various parameters are sanity checked and
 352 //       a message is printed out when they go out of bounds
 353
 354 void FGPropagate::Debug(int from)
 355 {
 356   if (debug_lvl <= 0) return;
 357
 358   if (debug_lvl & 1) { // Standard console startup message output
 359     if (from == 0) { // Constructor
 360
 361     }
 362   }
 363   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 364     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 365     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 366   }
 367   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 368   }
 369   if (debug_lvl & 8 ) { // Runtime state variables
 370   }
 371   if (debug_lvl & 16) { // Sanity checking
 372   }
 373   if (debug_lvl & 64) {
 374     if (from == 0) { // Constructor
 375       cout << IdSrc << endl;
 376       cout << IdHdr << endl;
 377     }
 378   }
 379 }
 380 }