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