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   // Finaly make shure that the quaternion stays normalized.
 157   VState.vQtrn.Normalize();
 158 }
 159
 160 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 161 /*
 162 Purpose: Called on a schedule to perform EOM integration
 163 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 164          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 165
 166 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 167 set up for use later, rather than using the longer class->function() notation.
 168
 169 Here, propagation of state is done using a simple explicit Euler scheme (see the
 170 bottom of the function). This propagation is done using the current state values
 171 and current derivatives. Based on these values we compute an approximation to the
 172 state values for (now + dt).
 173
 174 */
 175
 176 bool FGPropagate::Run(void)
 177 {
 178   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 179
 180   double dt = State->Getdt()*rate;  // The 'stepsize'
 181   const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
 182   const FGColumnVector3& vForces = Aircraft->GetForces();     // current forces
 183   const FGColumnVector3& vMoments = Aircraft->GetMoments();   // current moments
 184
 185   double mass = MassBalance->GetMass();             // mass
 186   const FGMatrix33& J = MassBalance->GetJ();        // inertia matrix
 187   const FGMatrix33& Jinv = MassBalance->GetJinv();  // inertia matrix inverse
 188   double r = GetRadius();                           // radius
 189   if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
 190   double rInv = 1.0/r;
 191   FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
 192
 193   // The rotation matrices:
 194   const FGMatrix33& Tl2b = GetTl2b();  // local to body frame
 195   const FGMatrix33& Tb2l = GetTb2l();  // body to local frame
 196   const FGMatrix33& Tec2l = VState.vLocation.GetTec2l();  // earth centered to local frame
 197   const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec();  // local to earth centered frame
 198
 199   // Inertial angular velocity measured in the body frame.
 200   const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
 201
 202   // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
 203   vVel = Tb2l * VState.vUVW;
 204
 205   // First compute the time derivatives of the vehicle state values:
 206
 207   // Compute body frame rotational accelerations based on the current body moments
 208   vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
 209
 210   // Compute body frame accelerations based on the current body forces
 211   vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
 212
 213   // Coriolis acceleration.
 214   FGColumnVector3 ecVel = Tl2ec*vVel;
 215   FGColumnVector3 ace = 2.0*omega*ecVel;
 216   vUVWdot -= Tl2b*(Tec2l*ace);
 217
 218   // Centrifugal acceleration.
 219   FGColumnVector3 aeec = omega*(omega*VState.vLocation);
 220   vUVWdot -= Tl2b*(Tec2l*aeec);
 221
 222   // Gravitation accel
 223   vUVWdot += Tl2b*gAccel;
 224
 225   // Compute vehicle velocity wrt EC frame, expressed in EC frame
 226   FGColumnVector3 vLocationDot = Tl2ec * vVel;
 227
 228   FGColumnVector3 omegaLocal( rInv*vVel(eEast),
 229                               -rInv*vVel(eNorth),
 230                               -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 231
 232   // Compute quaternion orientation derivative on current body rates
 233   FGQuaternion vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
 234
 235   // Propagate velocities
 236   VState.vPQR += dt*vPQRdot;
 237   VState.vUVW += dt*vUVWdot;
 238
 239   // Propagate positions
 240   VState.vQtrn += dt*vQtrndot;
 241   VState.vLocation += dt*vLocationDot;
 242
 243   return false;
 244 }
 245
 246 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 247
 248 void FGPropagate::Seth(double tt)
 249 {
 250   VState.vLocation.SetRadius( tt + SeaLevelRadius );
 251 }
 252
 253 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 254
 255 void FGPropagate::SetDistanceAGL(double tt)
 256 {
 257   VState.vLocation.SetRadius( tt + RunwayRadius );
 258 }
 259
 260 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 261
 262 void FGPropagate::bind(void)
 263 {
 264   typedef double (FGPropagate::*PMF)(int) const;
 265   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 266
 267   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 268   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 269   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 270
 271   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 272   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 273   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 274
 275   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 276   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 277   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 278
 279   PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
 280   PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 281   PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
 282
 283   PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
 284   PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
 285   PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
 286
 287   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
 288   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 289   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 290   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 291   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 292
 293   PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius, &FGPropagate::SetRunwayRadius);
 294
 295   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 296   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 297   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 298
 299   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 300   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 301   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 302 }
 303
 304 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 305
 306 void FGPropagate::unbind(void)
 307 {
 308   PropertyManager->Untie("velocities/v-north-fps");
 309   PropertyManager->Untie("velocities/v-east-fps");
 310   PropertyManager->Untie("velocities/v-down-fps");
 311   PropertyManager->Untie("velocities/h-dot-fps");
 312   PropertyManager->Untie("velocities/u-fps");
 313   PropertyManager->Untie("velocities/v-fps");
 314   PropertyManager->Untie("velocities/w-fps");
 315   PropertyManager->Untie("velocities/p-rad_sec");
 316   PropertyManager->Untie("velocities/q-rad_sec");
 317   PropertyManager->Untie("velocities/r-rad_sec");
 318   PropertyManager->Untie("accelerations/udot-fps");
 319   PropertyManager->Untie("accelerations/vdot-fps");
 320   PropertyManager->Untie("accelerations/wdot-fps");
 321   PropertyManager->Untie("accelerations/pdot-rad_sec");
 322   PropertyManager->Untie("accelerations/qdot-rad_sec");
 323   PropertyManager->Untie("accelerations/rdot-rad_sec");
 324   PropertyManager->Untie("position/h-sl-ft");
 325   PropertyManager->Untie("position/lat-gc-rad");
 326   PropertyManager->Untie("position/long-gc-rad");
 327   PropertyManager->Untie("position/h-agl-ft");
 328   PropertyManager->Untie("position/radius-to-vehicle-ft");
 329   PropertyManager->Untie("metrics/runway-radius");
 330   PropertyManager->Untie("attitude/phi-rad");
 331   PropertyManager->Untie("attitude/theta-rad");
 332   PropertyManager->Untie("attitude/psi-rad");
 333   PropertyManager->Untie("attitude/roll-rad");
 334   PropertyManager->Untie("attitude/pitch-rad");
 335   PropertyManager->Untie("attitude/heading-true-rad");
 336 }
 337
 338 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 339 //    The bitmasked value choices are as follows:
 340 //    unset: In this case (the default) JSBSim would only print
 341 //       out the normally expected messages, essentially echoing
 342 //       the config files as they are read. If the environment
 343 //       variable is not set, debug_lvl is set to 1 internally
 344 //    0: This requests JSBSim not to output any messages
 345 //       whatsoever.
 346 //    1: This value explicity requests the normal JSBSim
 347 //       startup messages
 348 //    2: This value asks for a message to be printed out when
 349 //       a class is instantiated
 350 //    4: When this value is set, a message is displayed when a
 351 //       FGModel object executes its Run() method
 352 //    8: When this value is set, various runtime state variables
 353 //       are printed out periodically
 354 //    16: When set various parameters are sanity checked and
 355 //       a message is printed out when they go out of bounds
 356
 357 void FGPropagate::Debug(int from)
 358 {
 359   if (debug_lvl <= 0) return;
 360
 361   if (debug_lvl & 1) { // Standard console startup message output
 362     if (from == 0) { // Constructor
 363
 364     }
 365   }
 366   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 367     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 368     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 369   }
 370   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 371   }
 372   if (debug_lvl & 8 ) { // Runtime state variables
 373   }
 374   if (debug_lvl & 16) { // Sanity checking
 375   }
 376   if (debug_lvl & 64) {
 377     if (from == 0) { // Constructor
 378       cout << IdSrc << endl;
 379       cout << IdHdr << endl;
 380     }
 381   }
 382 }
 383 }