src/FDM/JSBSim/models/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 (jon@jsbsim.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser 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 Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser 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 Lesser 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 #include <cmath>
  57 #include <iomanip>
  58
  59 #include "FGPropagate.h"
  60 #include <FGFDMExec.h>
  61 #include <FGState.h>
  62 #include "FGAircraft.h"
  63 #include "FGMassBalance.h"
  64 #include "FGInertial.h"
  65 #include <input_output/FGPropertyManager.h>
  66
  67 namespace JSBSim {
  68
  69 static const char *IdSrc = "$Id$";
  70 static const char *IdHdr = ID_PROPAGATE;
  71
  72 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  73 CLASS IMPLEMENTATION
  74 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  75
  76 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
  77 {
  78   Debug(0);
  79   Name = "FGPropagate";
  80
  81   last2_vPQRdot.InitMatrix();
  82   last_vPQRdot.InitMatrix();
  83   vPQRdot.InitMatrix();
  84
  85   last2_vQtrndot = FGQuaternion(0,0,0);
  86   last_vQtrndot = FGQuaternion(0,0,0);
  87   vQtrndot = FGQuaternion(0,0,0);
  88
  89   last2_vUVWdot.InitMatrix();
  90   last_vUVWdot.InitMatrix();
  91   vUVWdot.InitMatrix();
  92
  93   last2_vLocationDot.InitMatrix();
  94   last_vLocationDot.InitMatrix();
  95   vLocationDot.InitMatrix();
  96
  97   vOmegaLocal.InitMatrix();
  98
  99   integrator_rotational_rate = eAdamsBashforth2;
 100   integrator_translational_rate = eTrapezoidal;
 101   integrator_rotational_position = eAdamsBashforth2;
 102   integrator_translational_position = eTrapezoidal;
 103
 104   bind();
 105   Debug(0);
 106 }
 107
 108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 109
 110 FGPropagate::~FGPropagate(void)
 111 {
 112   Debug(1);
 113 }
 114
 115 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 116
 117 bool FGPropagate::InitModel(void)
 118 {
 119   if (!FGModel::InitModel()) return false;
 120
 121   // For initialization ONLY:
 122   SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
 123
 124   VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
 125   VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
 126   vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
 127
 128   last2_vPQRdot.InitMatrix();
 129   last_vPQRdot.InitMatrix();
 130   vPQRdot.InitMatrix();
 131
 132   last2_vQtrndot = FGQuaternion(0,0,0);
 133   last_vQtrndot = FGQuaternion(0,0,0);
 134   vQtrndot = FGQuaternion(0,0,0);
 135
 136   last2_vUVWdot.InitMatrix();
 137   last_vUVWdot.InitMatrix();
 138   vUVWdot.InitMatrix();
 139
 140   last2_vLocationDot.InitMatrix();
 141   last_vLocationDot.InitMatrix();
 142   vLocationDot.InitMatrix();
 143
 144   vOmegaLocal.InitMatrix();
 145
 146   integrator_rotational_rate = eAdamsBashforth2;
 147   integrator_translational_rate = eTrapezoidal;
 148   integrator_rotational_position = eAdamsBashforth2;
 149   integrator_translational_position = eTrapezoidal;
 150
 151   return true;
 152 }
 153
 154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 155
 156 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 157 {
 158   SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
 159   SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
 160
 161   // Set the position lat/lon/radius
 162   VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
 163                           FGIC->GetLatitudeRadIC(),
 164                           FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 165
 166   VehicleRadius = GetRadius();
 167   radInv = 1.0/VehicleRadius;
 168
 169   // Set the Orientation from the euler angles
 170   VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
 171                         FGIC->GetThetaRadIC(),
 172                         FGIC->GetPsiRadIC() );
 173
 174   // Set the velocities in the instantaneus body frame
 175   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
 176                           FGIC->GetVBodyFpsIC(),
 177                           FGIC->GetWBodyFpsIC() );
 178
 179   // Set the angular velocities in the instantaneus body frame.
 180   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
 181                           FGIC->GetQRadpsIC(),
 182                           FGIC->GetRRadpsIC() );
 183
 184   // Compute the local frame ECEF velocity
 185   vVel = GetTb2l()*VState.vUVW;
 186
 187   // Finally, make sure that the quaternion stays normalized.
 188   VState.vQtrn.Normalize();
 189
 190   // Recompute the LocalTerrainRadius.
 191   RecomputeLocalTerrainRadius();
 192
 193   // These local copies of the transformation matrices are for use for
 194   // initial conditions only.
 195
 196   Tl2b = GetTl2b();           // local to body frame transform
 197   Tb2l = Tl2b.Transposed();   // body to local frame transform
 198   Tl2ec = GetTl2ec();         // local to ECEF transform
 199   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 200   Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
 201   Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
 202   Ti2ec = GetTi2ec();         // ECI to ECEF transform
 203   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 204   Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
 205   Tb2i  = Ti2b.Transposed();  // body to ECI frame transform
 206 }
 207
 208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 209 /*
 210 Purpose: Called on a schedule to perform EOM integration
 211 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 212          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 213
 214 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 215 set up for use later, rather than using the longer class->function() notation.
 216
 217 This propagation is done using the current state values
 218 and current derivatives. Based on these values we compute an approximation to the
 219 state values for (now + dt).
 220
 221 In the code below, variables named beginning with a small "v" refer to a
 222 a column vector, variables named beginning with a "T" refer to a transformation
 223 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
 224 Inertial.
 225
 226 */
 227
 228 bool FGPropagate::Run(void)
 229 {
 230   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 231   if (FDMExec->Holding()) return false;
 232
 233   RecomputeLocalTerrainRadius();
 234
 235   // Calculate current aircraft radius from center of planet
 236
 237   VehicleRadius = GetRadius();
 238   radInv = 1.0/VehicleRadius;
 239
 240   // These local copies of the transformation matrices are for use this
 241   // pass through Run() only.
 242
 243   Tl2b = GetTl2b();           // local to body frame transform
 244   Tb2l = Tl2b.Transposed();   // body to local frame transform
 245   Tl2ec = GetTl2ec();         // local to ECEF transform
 246   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 247   Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
 248   Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
 249   Ti2ec = GetTi2ec();         // ECI to ECEF transform
 250   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 251   Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
 252   Tb2i  = Ti2b.Transposed();  // body to ECI frame transform
 253
 254   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 255   vVel = Tb2l * VState.vUVW;
 256
 257   // Inertial angular velocity measured in the body frame.
 258   vPQRi = VState.vPQR + Tec2b*vOmega;
 259
 260   // Calculate state derivatives
 261   CalculatePQRdot();      // Angular rate derivative
 262   CalculateUVWdot();      // Translational rate derivative
 263   CalculateQuatdot();     // Angular orientation derivative
 264   CalculateLocationdot(); // Translational position derivative
 265
 266   // Integrate to propagate the state
 267
 268   double dt = State->Getdt()*rate;  // The 'stepsize'
 269
 270   // Propagate rotational velocity
 271
 272   switch(integrator_rotational_rate) {
 273   case eRectEuler:       VState.vPQR += dt*vPQRdot;
 274     break;
 275   case eTrapezoidal:     VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot);
 276     break;
 277   case eAdamsBashforth2: VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
 278     break;
 279   case eAdamsBashforth3: VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
 280     break;
 281   case eNone: // do nothing, freeze angular rate
 282     break;
 283   }
 284
 285   // Propagate translational velocity
 286
 287   switch(integrator_translational_rate) {
 288   case eRectEuler:       VState.vUVW += dt*vUVWdot;
 289     break;
 290   case eTrapezoidal:     VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot);
 291     break;
 292   case eAdamsBashforth2: VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot);
 293     break;
 294   case eAdamsBashforth3: VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot);
 295     break;
 296   case eNone: // do nothing, freeze translational rate
 297     break;
 298   }
 299
 300   // Propagate angular position
 301
 302   switch(integrator_rotational_position) {
 303   case eRectEuler:       VState.vQtrn += dt*vQtrndot;
 304     break;
 305   case eTrapezoidal:     VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot);
 306     break;
 307   case eAdamsBashforth2: VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot);
 308     break;
 309   case eAdamsBashforth3: VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot);
 310     break;
 311   case eNone: // do nothing, freeze angular position
 312     break;
 313   }
 314
 315   // Propagate translational position
 316
 317   switch(integrator_translational_position) {
 318   case eRectEuler:       VState.vLocation += dt*vLocationDot;
 319     break;
 320   case eTrapezoidal:     VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot);
 321     break;
 322   case eAdamsBashforth2: VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot);
 323     break;
 324   case eAdamsBashforth3: VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot);
 325     break;
 326   case eNone: // do nothing, freeze translational position
 327     break;
 328   }
 329
 330   // Set past values
 331
 332   last2_vPQRdot = last_vPQRdot;
 333   last_vPQRdot = vPQRdot;
 334
 335   last2_vUVWdot = last_vUVWdot;
 336   last_vUVWdot = vUVWdot;
 337
 338   last2_vQtrndot = last_vQtrndot;
 339   last_vQtrndot = vQtrndot;
 340
 341   last2_vLocationDot = last_vLocationDot;
 342   last_vLocationDot = vLocationDot;
 343
 344   Debug(2);
 345   return false;
 346 }
 347
 348 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 349 // Compute body frame rotational accelerations based on the current body moments
 350 //
 351 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
 352 // (body rate with respect to the inertial frame), expressed in the body frame,
 353 // where the derivative is taken in the body frame.
 354 // J is the inertia matrix
 355 // Jinv is the inverse inertia matrix
 356 // vMoments is the moment vector in the body frame
 357 // vPQRi is the total inertial angular velocity of the vehicle
 358 // expressed in the body frame.
 359 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 360 //            Second edition (2004), eqn 1.5-16e (page 50)
 361
 362 void FGPropagate::CalculatePQRdot(void)
 363 {
 364   const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
 365   const FGMatrix33& J = MassBalance->GetJ();                // inertia matrix
 366   const FGMatrix33& Jinv = MassBalance->GetJinv();          // inertia matrix inverse
 367
 368   // Compute body frame rotational accelerations based on the current body
 369   // moments and the total inertial angular velocity expressed in the body
 370   // frame.
 371
 372   vPQRdot = Jinv*(vMoments - vPQRi*(J*vPQRi));
 373 }
 374
 375 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 376 // Compute the quaternion orientation derivative
 377 //
 378 // vQtrndot is the quaternion derivative.
 379 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 380 //            Second edition (2004), eqn 1.5-16b (page 50)
 381
 382 void FGPropagate::CalculateQuatdot(void)
 383 {
 384   vOmegaLocal.InitMatrix( radInv*vVel(eEast),
 385                          -radInv*vVel(eNorth),
 386                          -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 387
 388   // Compute quaternion orientation derivative on current body rates
 389   vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*vOmegaLocal);
 390 }
 391
 392 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 393 // This set of calculations results in the body frame accelerations being
 394 // computed.
 395 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 396 //            Second edition (2004), eqn 1.5-16d (page 50)
 397
 398 void FGPropagate::CalculateUVWdot(void)
 399 {
 400   double mass = MassBalance->GetMass();                      // mass
 401   const FGColumnVector3& vForces = Aircraft->GetForces();    // current forces
 402
 403   const FGColumnVector3 vGravAccel( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
 404
 405   // Begin to compute body frame accelerations based on the current body forces
 406   vUVWdot = vForces/mass - VState.vPQR * VState.vUVW;
 407
 408   // Include Coriolis acceleration.
 409   vUVWdot -= 2.0 * (Ti2b *vOmega) * VState.vUVW;
 410
 411   // Include Centrifugal acceleration.
 412   if (!GroundReactions->GetWOW()) {
 413     vUVWdot -= Ti2b*(vOmega*(vOmega*(Tec2i*VState.vLocation)));
 414   }
 415
 416   // Include Gravitation accel
 417   FGColumnVector3 gravAccel = Tl2b*vGravAccel;
 418   vUVWdot += gravAccel;
 419 }
 420
 421 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 422
 423 void FGPropagate::CalculateLocationdot(void)
 424 {
 425   // Transform the vehicle velocity relative to the ECEF frame, expressed
 426   // in the body frame, to be expressed in the ECEF frame.
 427   vLocationDot = Tb2ec * VState.vUVW;
 428
 429   // Now, transform the velocity vector of the body relative to the origin (Earth
 430   // center) to be expressed in the inertial frame, and add the vehicle velocity
 431   // contribution due to the rotation of the planet. The above velocity is only
 432   // relative to the rotating ECEF frame.
 433   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 434   //            Second edition (2004), eqn 1.5-16c (page 50)
 435
 436   vInertialVelocity = Tec2i * vLocationDot + (vOmega * (Tec2i * VState.vLocation));
 437 }
 438
 439 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 440
 441 void FGPropagate::RecomputeLocalTerrainRadius(void)
 442 {
 443   double t = State->Getsim_time();
 444
 445   // Get the LocalTerrain radius.
 446   FGLocation contactloc;
 447   FGColumnVector3 dv;
 448   FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 449   LocalTerrainRadius = contactloc.GetRadius();
 450 }
 451
 452 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 453
 454 void FGPropagate::SetTerrainElevation(double terrainElev)
 455 {
 456   LocalTerrainRadius = terrainElev + SeaLevelRadius;
 457   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
 458 }
 459
 460 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 461
 462 double FGPropagate::GetTerrainElevation(void) const
 463 {
 464   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
 465 }
 466
 467 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 468
 469 const FGMatrix33& FGPropagate::GetTi2ec(void)
 470 {
 471   return VState.vLocation.GetTi2ec(Inertial->GetEarthPositionAngle());
 472 }
 473
 474 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 475
 476 const FGMatrix33& FGPropagate::GetTec2i(void)
 477 {
 478   return VState.vLocation.GetTec2i(Inertial->GetEarthPositionAngle());
 479 }
 480
 481 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 482
 483 void FGPropagate::SetAltitudeASL(double altASL)
 484 {
 485   VState.vLocation.SetRadius( altASL + SeaLevelRadius );
 486 }
 487
 488 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 489
 490 double FGPropagate::GetLocalTerrainRadius(void) const
 491 {
 492   return LocalTerrainRadius;
 493 }
 494
 495 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 496
 497 double FGPropagate::GetDistanceAGL(void) const
 498 {
 499   return VState.vLocation.GetRadius() - LocalTerrainRadius;
 500 }
 501
 502 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 503
 504 void FGPropagate::SetDistanceAGL(double tt)
 505 {
 506   VState.vLocation.SetRadius( tt + LocalTerrainRadius );
 507 }
 508
 509 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 510
 511 void FGPropagate::bind(void)
 512 {
 513   typedef double (FGPropagate::*PMF)(int) const;
 514 //  typedef double (FGPropagate::*dPMF)() const;
 515   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 516
 517   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 518   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 519   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 520
 521   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 522   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 523   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 524
 525   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 526   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 527   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 528
 529   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 530   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 531   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 532
 533   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 534
 535   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
 536   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 537   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
 538
 539   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
 540   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
 541   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
 542
 543   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 544   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 545   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 546   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 547   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
 548   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
 549   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 550   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 551   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 552   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 553   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 554   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 555                           &FGPropagate::GetTerrainElevation,
 556                           &FGPropagate::SetTerrainElevation, false);
 557
 558   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 559
 560   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 561   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 562   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 563
 564   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 565   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 566   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 567
 568   PropertyManager->Tie("simulation/integrator/rate/rotational", &integrator_rotational_rate);
 569   PropertyManager->Tie("simulation/integrator/rate/translational", &integrator_translational_rate);
 570   PropertyManager->Tie("simulation/integrator/position/rotational", &integrator_rotational_position);
 571   PropertyManager->Tie("simulation/integrator/position/translational", &integrator_translational_position);
 572 }
 573
 574 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 575 //    The bitmasked value choices are as follows:
 576 //    unset: In this case (the default) JSBSim would only print
 577 //       out the normally expected messages, essentially echoing
 578 //       the config files as they are read. If the environment
 579 //       variable is not set, debug_lvl is set to 1 internally
 580 //    0: This requests JSBSim not to output any messages
 581 //       whatsoever.
 582 //    1: This value explicity requests the normal JSBSim
 583 //       startup messages
 584 //    2: This value asks for a message to be printed out when
 585 //       a class is instantiated
 586 //    4: When this value is set, a message is displayed when a
 587 //       FGModel object executes its Run() method
 588 //    8: When this value is set, various runtime state variables
 589 //       are printed out periodically
 590 //    16: When set various parameters are sanity checked and
 591 //       a message is printed out when they go out of bounds
 592
 593 void FGPropagate::Debug(int from)
 594 {
 595   if (debug_lvl <= 0) return;
 596
 597   if (debug_lvl & 1) { // Standard console startup message output
 598     if (from == 0) { // Constructor
 599
 600     }
 601   }
 602   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 603     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 604     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 605   }
 606   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 607   }
 608   if (debug_lvl & 8 ) { // Runtime state variables
 609   }
 610   if (debug_lvl & 16) { // Sanity checking
 611     if (from == 2) { // State sanity checking
 612       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 613         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 614         exit(-1);
 615       }
 616       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 617         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 618         exit(-1);
 619       }
 620       if (fabs(GetDistanceAGL()) > 1e10) {
 621         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 622         exit(-1);
 623       }
 624     }
 625   }
 626   if (debug_lvl & 64) {
 627     if (from == 0) { // Constructor
 628       cout << IdSrc << endl;
 629       cout << IdHdr << endl;
 630     }
 631   }
 632 }
 633 }