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