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 [6] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
  52
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  54 INCLUDES
  55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  56
  57 #include <cmath>
  58 #include <cstdlib>
  59 #include <iostream>
  60 #include <iomanip>
  61
  62 #include "FGPropagate.h"
  63 #include "FGGroundReactions.h"
  64 #include "FGFDMExec.h"
  65 #include "FGAircraft.h"
  66 #include "FGMassBalance.h"
  67 #include "FGInertial.h"
  68 #include "input_output/FGPropertyManager.h"
  69
  70 using namespace std;
  71
  72 namespace JSBSim {
  73
  74 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.59 2010/07/30 11:50:01 jberndt Exp $";
  75 static const char *IdHdr = ID_PROPAGATE;
  76
  77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  78 CLASS IMPLEMENTATION
  79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  80
  81 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
  82 {
  83   Debug(0);
  84   Name = "FGPropagate";
  85   gravType = gtWGS84;
  86
  87   vPQRdot.InitMatrix();
  88   vQtrndot = FGQuaternion(0,0,0);
  89   vUVWdot.InitMatrix();
  90   vInertialVelocity.InitMatrix();
  91
  92   integrator_rotational_rate = eAdamsBashforth2;
  93   integrator_translational_rate = eTrapezoidal;
  94   integrator_rotational_position = eAdamsBashforth2;
  95   integrator_translational_position = eTrapezoidal;
  96
  97   VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
  98   VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
  99   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
 100   VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
 101
 102   bind();
 103   Debug(0);
 104 }
 105
 106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 107
 108 FGPropagate::~FGPropagate(void)
 109 {
 110   Debug(1);
 111 }
 112
 113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114
 115 bool FGPropagate::InitModel(void)
 116 {
 117   if (!FGModel::InitModel()) return false;
 118
 119   // For initialization ONLY:
 120   SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
 121
 122   VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
 123   VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
 124   vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
 125
 126   vPQRdot.InitMatrix();
 127   vQtrndot = FGQuaternion(0,0,0);
 128   vUVWdot.InitMatrix();
 129   vInertialVelocity.InitMatrix();
 130
 131   VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 132   VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 133   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
 134   VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 135
 136   integrator_rotational_rate = eAdamsBashforth2;
 137   integrator_translational_rate = eTrapezoidal;
 138   integrator_rotational_position = eAdamsBashforth2;
 139   integrator_translational_position = eTrapezoidal;
 140
 141   return true;
 142 }
 143
 144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 145
 146 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 147 {
 148   SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
 149   SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
 150
 151   VehicleRadius = GetRadius();
 152   radInv = 1.0/VehicleRadius;
 153
 154   // Initialize the State Vector elements and the transformation matrices
 155
 156   // Set the position lat/lon/radius
 157   VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
 158                                 FGIC->GetLatitudeRadIC(),
 159                                 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 160
 161   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
 162
 163   Ti2ec = GetTi2ec();         // ECI to ECEF transform
 164   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 165
 166   Tl2ec = GetTl2ec();         // local to ECEF transform
 167   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 168
 169   Ti2l  = GetTi2l();
 170   Tl2i  = Ti2l.Transposed();
 171
 172   // Set the orientation from the euler angles (is normalized within the
 173   // constructor). The Euler angles represent the orientation of the body
 174   // frame relative to the local frame.
 175   VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
 176                                         FGIC->GetThetaRadIC(),
 177                                         FGIC->GetPsiRadIC() );
 178
 179   VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
 180
 181   Ti2b  = GetTi2b();           // ECI to body frame transform
 182   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
 183
 184   Tl2b  = VState.qAttitudeLocal; // local to body frame transform
 185   Tb2l  = Tl2b.Transposed();     // body to local frame transform
 186
 187   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
 188   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
 189
 190   // Set the velocities in the instantaneus body frame
 191   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
 192                                  FGIC->GetVBodyFpsIC(),
 193                                  FGIC->GetWBodyFpsIC() );
 194
 195   VState.vInertialPosition = Tec2i * VState.vLocation;
 196
 197   // Compute the local frame ECEF velocity
 198   vVel = Tb2l * VState.vUVW;
 199
 200   // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
 201   // This is the rotation rate of the "Local" frame, expressed in the local frame.
 202
 203   FGColumnVector3 vOmegaLocal = FGColumnVector3(
 204      radInv*vVel(eEast),
 205     -radInv*vVel(eNorth),
 206     -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 207
 208   // Set the angular velocities of the body frame relative to the ECEF frame,
 209   // expressed in the body frame. Effectively, this is:
 210   //   w_b/e = w_b/l + w_l/e
 211   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
 212                                  FGIC->GetQRadpsIC(),
 213                                  FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
 214
 215   VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
 216
 217   // Make an initial run and set past values
 218   CalculatePQRdot();           // Angular rate derivative
 219   CalculateUVWdot();           // Translational rate derivative
 220   ResolveFrictionForces(0.);   // Update rate derivatives with friction forces
 221   CalculateQuatdot();          // Angular orientation derivative
 222   CalculateInertialVelocity(); // Translational position derivative
 223
 224   // Initialize past values deques
 225   VState.dqPQRdot.clear();
 226   VState.dqUVWdot.clear();
 227   VState.dqInertialVelocity.clear();
 228   VState.dqQtrndot.clear();
 229   for (int i=0; i<4; i++) {
 230     VState.dqPQRdot.push_front(vPQRdot);
 231     VState.dqUVWdot.push_front(vUVWdot);
 232     VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
 233     VState.dqQtrndot.push_front(vQtrndot);
 234   }
 235
 236   // Recompute the LocalTerrainRadius.
 237   RecomputeLocalTerrainRadius();
 238 }
 239
 240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 241 /*
 242 Purpose: Called on a schedule to perform EOM integration
 243 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 244          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 245
 246 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 247 set up for use later, rather than using the longer class->function() notation.
 248
 249 This propagation is done using the current state values
 250 and current derivatives. Based on these values we compute an approximation to the
 251 state values for (now + dt).
 252
 253 In the code below, variables named beginning with a small "v" refer to a
 254 a column vector, variables named beginning with a "T" refer to a transformation
 255 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
 256 Inertial.
 257
 258 */
 259
 260 bool FGPropagate::Run(void)
 261 {
 262 static int ctr;
 263
 264   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 265   if (FDMExec->Holding()) return false;
 266
 267   double dt = FDMExec->GetDeltaT()*rate;  // The 'stepsize'
 268
 269   RunPreFunctions();
 270
 271   // Calculate state derivatives
 272   CalculatePQRdot();           // Angular rate derivative
 273   CalculateUVWdot();           // Translational rate derivative
 274   ResolveFrictionForces(dt);   // Update rate derivatives with friction forces
 275   CalculateQuatdot();          // Angular orientation derivative
 276   CalculateInertialVelocity(); // Translational position derivative
 277
 278   // Propagate rotational / translational velocity, angular /translational position, respectively.
 279   Integrate(VState.vPQRi,             vPQRdot,           VState.dqPQRdot,           dt, integrator_rotational_rate);
 280   Integrate(VState.vUVW,              vUVWdot,           VState.dqUVWdot,           dt, integrator_translational_rate);
 281   Integrate(VState.qAttitudeECI,      vQtrndot,          VState.dqQtrndot,          dt, integrator_rotational_position);
 282   Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
 283
 284   VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
 285
 286   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
 287
 288   // Update the "Location-based" transformation matrices from the vLocation vector.
 289
 290   Ti2ec = GetTi2ec();          // ECI to ECEF transform
 291   Tec2i = Ti2ec.Transposed();  // ECEF to ECI frame transform
 292   Tl2ec = GetTl2ec();          // local to ECEF transform
 293   Tec2l = Tl2ec.Transposed();  // ECEF to local frame transform
 294   Ti2l  = GetTi2l();
 295   Tl2i  = Ti2l.Transposed();
 296
 297   // Update the "Orientation-based" transformation matrices from the orientation quaternion
 298
 299   Ti2b  = GetTi2b();           // ECI to body frame transform
 300   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
 301   Tl2b  = Ti2b*Tl2i;           // local to body frame transform
 302   Tb2l  = Tl2b.Transposed();   // body to local frame transform
 303   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
 304   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
 305
 306   // Set auxililary state variables
 307   VState.vLocation = Ti2ec*VState.vInertialPosition;
 308   RecomputeLocalTerrainRadius();
 309
 310   VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
 311   radInv = 1.0/VehicleRadius;
 312
 313   VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
 314
 315   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 316
 317   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 318   vVel = Tb2l * VState.vUVW;
 319
 320   RunPostFunctions();
 321
 322   Debug(2);
 323   return false;
 324 }
 325
 326 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 327 // Compute body frame rotational accelerations based on the current body moments
 328 //
 329 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
 330 // (body rate with respect to the inertial frame), expressed in the body frame,
 331 // where the derivative is taken in the body frame.
 332 // J is the inertia matrix
 333 // Jinv is the inverse inertia matrix
 334 // vMoments is the moment vector in the body frame
 335 // VState.vPQRi is the total inertial angular velocity of the vehicle
 336 // expressed in the body frame.
 337 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 338 //            Second edition (2004), eqn 1.5-16e (page 50)
 339
 340 void FGPropagate::CalculatePQRdot(void)
 341 {
 342   const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
 343   const FGMatrix33& J = MassBalance->GetJ();                // inertia matrix
 344   const FGMatrix33& Jinv = MassBalance->GetJinv();          // inertia matrix inverse
 345
 346   // Compute body frame rotational accelerations based on the current body
 347   // moments and the total inertial angular velocity expressed in the body
 348   // frame.
 349
 350   vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
 351 }
 352
 353 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 354 // Compute the quaternion orientation derivative
 355 //
 356 // vQtrndot is the quaternion derivative.
 357 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 358 //            Second edition (2004), eqn 1.5-16b (page 50)
 359
 360 void FGPropagate::CalculateQuatdot(void)
 361 {
 362   // Compute quaternion orientation derivative on current body rates
 363   vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
 364 }
 365
 366 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 367 // This set of calculations results in the body frame accelerations being
 368 // computed.
 369 // Compute body frame accelerations based on the current body forces.
 370 // Include centripetal and coriolis accelerations.
 371 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
 372 //   so it has to be transformed to the body frame. More completely,
 373 //   vOmegaEarth is the rate of the ECEF frame relative to the Inertial
 374 //   frame (ECI), expressed in the Inertial frame.
 375 // vForces is the total force on the vehicle in the body frame.
 376 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
 377 //   in the body frame.
 378 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
 379 //   in the body frame.
 380 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 381 //            Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
 382
 383 void FGPropagate::CalculateUVWdot(void)
 384 {
 385   double mass = MassBalance->GetMass();                      // mass
 386   const FGColumnVector3& vForces = Aircraft->GetForces();    // current forces
 387
 388   vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
 389
 390   // Include Centripetal acceleration.
 391   vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
 392
 393   // Include Gravitation accel
 394   switch (gravType) {
 395     case gtStandard:
 396       vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
 397       break;
 398     case gtWGS84:
 399       vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
 400       break;
 401   }
 402
 403   vUVWdot += vGravAccel;
 404 }
 405
 406 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 407   // Transform the velocity vector of the body relative to the origin (Earth
 408   // center) to be expressed in the inertial frame, and add the vehicle velocity
 409   // contribution due to the rotation of the planet.
 410   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 411   //            Second edition (2004), eqn 1.5-16c (page 50)
 412
 413 void FGPropagate::CalculateInertialVelocity(void)
 414 {
 415   VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
 416 }
 417
 418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 419
 420 void FGPropagate::Integrate( FGColumnVector3& Integrand,
 421                              FGColumnVector3& Val,
 422                              deque <FGColumnVector3>& ValDot,
 423                              double dt,
 424                              eIntegrateType integration_type)
 425 {
 426   ValDot.push_front(Val);
 427   ValDot.pop_back();
 428
 429   switch(integration_type) {
 430   case eRectEuler:       Integrand += dt*ValDot[0];
 431     break;
 432   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 433     break;
 434   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 435     break;
 436   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 437     break;
 438   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 439     break;
 440   case eNone: // do nothing, freeze translational rate
 441     break;
 442   }
 443 }
 444
 445 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 446
 447 void FGPropagate::Integrate( FGQuaternion& Integrand,
 448                              FGQuaternion& Val,
 449                              deque <FGQuaternion>& ValDot,
 450                              double dt,
 451                              eIntegrateType integration_type)
 452 {
 453   ValDot.push_front(Val);
 454   ValDot.pop_back();
 455
 456   switch(integration_type) {
 457   case eRectEuler:       Integrand += dt*ValDot[0];
 458     break;
 459   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 460     break;
 461   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 462     break;
 463   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 464     break;
 465   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 466     break;
 467   case eNone: // do nothing, freeze rotational rate
 468     break;
 469   }
 470 }
 471
 472 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 473 // Resolves the contact forces just before integrating the EOM.
 474 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
 475 // (PGS) method.
 476 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
 477 //            February 22, 2005
 478 // In JSBSim there is only one rigid body (the aircraft) and there can be
 479 // multiple points of contact between the aircraft and the ground. As a
 480 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
 481 // in Catto's paper has been adapted accordingly.
 482
 483 void FGPropagate::ResolveFrictionForces(double dt)
 484 {
 485   const double invMass = 1.0 / MassBalance->GetMass();
 486   const FGMatrix33& Jinv = MassBalance->GetJinv();
 487   vector <FGColumnVector3> JacF, JacM;
 488   FGColumnVector3 vdot, wdot;
 489   FGColumnVector3 Fc, Mc;
 490   int n = 0, i;
 491
 492   // Compiles data from the ground reactions to build up the jacobian matrix
 493   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
 494     JacF.push_back((*it)->ForceJacobian);
 495     JacM.push_back((*it)->MomentJacobian);
 496   }
 497
 498   // If no gears are in contact with the ground then return
 499   if (!n) return;
 500
 501   double *a = new double[n*n]; // Will contain J*M^-1*J^T
 502   double *eta = new double[n];
 503   double *lambda = new double[n];
 504   double *lambdaMin = new double[n];
 505   double *lambdaMax = new double[n];
 506
 507   // Initializes the Lagrange multipliers
 508   i = 0;
 509   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
 510     lambda[i] = (*it)->value;
 511     lambdaMax[i] = (*it)->Max;
 512     lambdaMin[i] = (*it)->Min;
 513   }
 514
 515   vdot = vUVWdot;
 516   wdot = vPQRdot;
 517
 518   if (dt > 0.) {
 519     // First compute the ground velocity below the aircraft center of gravity
 520     FGLocation contact;
 521     FGColumnVector3 normal, cvel;
 522     double t = FDMExec->GetSimTime();
 523     double height = FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contact, normal, cvel);
 524
 525     // Instruct the algorithm to zero out the relative movement between the
 526     // aircraft and the ground.
 527     vdot += (VState.vUVW - Tec2b * cvel) / dt;
 528     wdot += VState.vPQR / dt;
 529   }
 530
 531   // Assemble the linear system of equations
 532   for (i=0; i < n; i++) {
 533     for (int j=0; j < i; j++)
 534       a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
 535     for (int j=i; j < n; j++)
 536       a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
 537   }
 538
 539   // Prepare the linear system for the Gauss-Seidel algorithm :
 540   // divide every line of 'a' and eta by a[i,i]. This is in order to save
 541   // a division computation at each iteration of Gauss-Seidel.
 542   for (i=0; i < n; i++) {
 543     double d = 1.0 / a[i*n+i];
 544
 545     eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
 546     for (int j=0; j < n; j++)
 547       a[i*n+j] *= d;
 548   }
 549
 550   // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
 551   for (int iter=0; iter < 50; iter++) {
 552     double norm = 0.;
 553
 554     for (i=0; i < n; i++) {
 555       double lambda0 = lambda[i];
 556       double dlambda = eta[i];
 557
 558       for (int j=0; j < n; j++)
 559         dlambda -= a[i*n+j]*lambda[j];
 560
 561       lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
 562       dlambda = lambda[i] - lambda0;
 563
 564       norm += fabs(dlambda);
 565     }
 566
 567     if (norm < 1E-5) break;
 568   }
 569
 570   // Calculate the total friction forces and moments
 571
 572   Fc.InitMatrix();
 573   Mc.InitMatrix();
 574
 575   for (i=0; i< n; i++) {
 576     Fc += lambda[i]*JacF[i];
 577     Mc += lambda[i]*JacM[i];
 578   }
 579
 580   vUVWdot += invMass * Fc;
 581   vPQRdot += Jinv * Mc;
 582
 583   // Save the value of the Lagrange multipliers to accelerate the convergence
 584   // of the Gauss-Seidel algorithm at next iteration.
 585   i = 0;
 586   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
 587     (*it)->value = lambda[i++];
 588
 589   GroundReactions->UpdateForcesAndMoments();
 590
 591   delete a, eta, lambda, lambdaMin, lambdaMax;
 592 }
 593
 594 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 595
 596 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
 597   VState.qAttitudeECI = Qi;
 598 }
 599
 600 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 601
 602 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
 603   VState.vInertialVelocity = Vi;
 604 }
 605
 606 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 607
 608 void FGPropagate::RecomputeLocalTerrainRadius(void)
 609 {
 610   double t = FDMExec->GetSimTime();
 611
 612   // Get the LocalTerrain radius.
 613   FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 614   LocalTerrainRadius = contactloc.GetRadius();
 615 }
 616
 617 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 618
 619 void FGPropagate::SetTerrainElevation(double terrainElev)
 620 {
 621   LocalTerrainRadius = terrainElev + SeaLevelRadius;
 622   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
 623 }
 624
 625 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 626
 627 double FGPropagate::GetTerrainElevation(void) const
 628 {
 629   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
 630 }
 631
 632 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 633 //Todo: when should this be called - when should the new EPA be used to
 634 // calculate the transformation matrix, so that the matrix is not a step
 635 // ahead of the sim and the associated calculations?
 636 const FGMatrix33& FGPropagate::GetTi2ec(void)
 637 {
 638   return VState.vLocation.GetTi2ec();
 639 }
 640
 641 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 642
 643 const FGMatrix33& FGPropagate::GetTec2i(void)
 644 {
 645   return VState.vLocation.GetTec2i();
 646 }
 647
 648 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 649
 650 void FGPropagate::SetAltitudeASL(double altASL)
 651 {
 652   VState.vLocation.SetRadius( altASL + SeaLevelRadius );
 653 }
 654
 655 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 656
 657 double FGPropagate::GetLocalTerrainRadius(void) const
 658 {
 659   return LocalTerrainRadius;
 660 }
 661
 662 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 663
 664 double FGPropagate::GetDistanceAGL(void) const
 665 {
 666   return VState.vLocation.GetRadius() - LocalTerrainRadius;
 667 }
 668
 669 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 670
 671 void FGPropagate::SetDistanceAGL(double tt)
 672 {
 673   VState.vLocation.SetRadius( tt + LocalTerrainRadius );
 674 }
 675
 676 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 677
 678 void FGPropagate::bind(void)
 679 {
 680   typedef double (FGPropagate::*PMF)(int) const;
 681
 682   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 683
 684   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 685   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 686   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 687
 688   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 689   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 690   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 691
 692   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 693   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 694   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 695
 696   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 697   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 698   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 699
 700   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 701
 702   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
 703   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 704   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
 705
 706   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
 707   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
 708   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
 709
 710   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 711   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 712   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 713   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 714   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
 715   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
 716   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 717   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 718   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 719   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 720   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 721   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 722                           &FGPropagate::GetTerrainElevation,
 723                           &FGPropagate::SetTerrainElevation, false);
 724
 725   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 726
 727   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 728   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 729   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 730
 731   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 732   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 733   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 734
 735   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
 736   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
 737   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
 738   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
 739   PropertyManager->Tie("simulation/gravity-model", &gravType);
 740 }
 741
 742 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 743 //    The bitmasked value choices are as follows:
 744 //    unset: In this case (the default) JSBSim would only print
 745 //       out the normally expected messages, essentially echoing
 746 //       the config files as they are read. If the environment
 747 //       variable is not set, debug_lvl is set to 1 internally
 748 //    0: This requests JSBSim not to output any messages
 749 //       whatsoever.
 750 //    1: This value explicity requests the normal JSBSim
 751 //       startup messages
 752 //    2: This value asks for a message to be printed out when
 753 //       a class is instantiated
 754 //    4: When this value is set, a message is displayed when a
 755 //       FGModel object executes its Run() method
 756 //    8: When this value is set, various runtime state variables
 757 //       are printed out periodically
 758 //    16: When set various parameters are sanity checked and
 759 //       a message is printed out when they go out of bounds
 760
 761 void FGPropagate::Debug(int from)
 762 {
 763   if (debug_lvl <= 0) return;
 764
 765   if (debug_lvl & 1) { // Standard console startup message output
 766     if (from == 0) { // Constructor
 767
 768     }
 769   }
 770   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 771     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 772     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 773   }
 774   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 775   }
 776   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 777     cout << endl << fgblue << highint << left
 778          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
 779          << reset << endl;
 780     cout << endl;
 781     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
 782                     << Inertial->GetEarthPositionAngleDeg() << endl;
 783     cout << endl;
 784     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
 785     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
 786     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
 787     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
 788     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
 789     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
 790     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
 791     cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
 792     cout << endl;
 793     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
 794                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
 795     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 796                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
 797                     << endl << endl;
 798
 799     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
 800                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
 801     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 802                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
 803                     << endl << endl;
 804
 805     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
 806                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
 807     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 808                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
 809                     << endl << endl;
 810
 811     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
 812                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
 813     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 814                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
 815                     << endl << endl;
 816
 817     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
 818                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
 819     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 820                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
 821                     << endl << endl;
 822
 823     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
 824                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
 825     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 826                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
 827                     << endl << endl;
 828
 829     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
 830                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
 831     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 832                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
 833                     << endl << endl;
 834
 835     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
 836                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
 837     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 838                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
 839                     << endl << endl;
 840
 841     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
 842                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
 843     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 844                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
 845                     << endl << endl;
 846
 847     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
 848                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
 849     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 850                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
 851                     << endl << endl;
 852
 853     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
 854                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
 855     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 856                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
 857                     << endl << endl;
 858
 859     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
 860                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
 861     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 862                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
 863                     << endl << endl;
 864
 865     cout << setprecision(6); // reset the output stream
 866   }
 867   if (debug_lvl & 16) { // Sanity checking
 868     if (from == 2) { // State sanity checking
 869       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 870         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 871         exit(-1);
 872       }
 873       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 874         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 875         exit(-1);
 876       }
 877       if (fabs(GetDistanceAGL()) > 1e10) {
 878         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 879         exit(-1);
 880       }
 881     }
 882   }
 883   if (debug_lvl & 64) {
 884     if (from == 0) { // Constructor
 885       cout << IdSrc << endl;
 886       cout << IdHdr << endl;
 887     }
 888   }
 889 }
 890 }